PX4 Firmware
PX4 Autopilot Software http://px4.io
airspeed.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file airspeed.cpp
36  * @author Simon Wilks <simon@px4.io>
37  * @author Lorenz Meier <lorenz@px4.io>
38  *
39  */
40 
41 #include <px4_platform_common/px4_config.h>
42 #include <drivers/device/device.h>
43 
44 #include <drivers/device/i2c.h>
45 
46 #include <systemlib/err.h>
47 #include <parameters/param.h>
48 #include <perf/perf_counter.h>
49 
50 #include <drivers/drv_airspeed.h>
51 #include <drivers/drv_hrt.h>
53 
54 #include <uORB/uORB.h>
56 
58 
59 Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const char *path) :
60  I2C("Airspeed", path, bus, address, 100000),
61  ScheduledWorkItem(MODULE_NAME, px4::device_bus_to_wq(get_device_id())),
62  _sensor_ok(false),
63  _measure_interval(0),
64  _collect_phase(false),
65  _diff_pres_offset(0.0f),
66  _airspeed_pub(nullptr),
67  _airspeed_orb_class_instance(-1),
68  _class_instance(-1),
69  _conversion_interval(conversion_interval),
70  _sample_perf(perf_alloc(PC_ELAPSED, "aspd_read")),
71  _comms_errors(perf_alloc(PC_COUNT, "aspd_com_err"))
72 {
73 }
74 
76 {
77  /* make sure we are truly inactive */
78  stop();
79 
80  if (_class_instance != -1) {
81  unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance);
82  }
83 
85 
86  // free perf counters
89 }
90 
91 int
93 {
94  /* do I2C init (and probe) first */
95  if (I2C::init() != PX4_OK) {
96  return PX4_ERROR;
97  }
98 
99  /* register alternate interfaces if we have to */
100  _class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH);
101 
102  /* advertise sensor topic, measure manually to initialize valid report */
103  measure();
104  differential_pressure_s arp = {};
105 
106  /* measurement will have generated a report, publish */
107  _airspeed_pub = orb_advertise_multi(ORB_ID(differential_pressure), &arp, &_airspeed_orb_class_instance,
109 
110  if (_airspeed_pub == nullptr) {
111  PX4_WARN("uORB started?");
112  }
113 
114  return PX4_OK;
115 }
116 
117 int
119 {
120  /* on initial power up the device may need more than one retry
121  for detection. Once it is running the number of retries can
122  be reduced
123  */
124  _retries = 4;
125  int ret = measure();
126 
127  // drop back to 2 retries once initialised
128  _retries = 2;
129  return ret;
130 }
131 
132 int
133 Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg)
134 {
135  switch (cmd) {
136 
137  case SENSORIOCSPOLLRATE: {
138  switch (arg) {
139 
140  /* zero would be bad */
141  case 0:
142  return -EINVAL;
143 
144  /* set default polling rate */
146  /* do we need to start internal polling? */
147  bool want_start = (_measure_interval == 0);
148 
149  /* set interval for next measurement to minimum legal value */
151 
152  /* if we need to start the poll state machine, do it */
153  if (want_start) {
154  start();
155  }
156 
157  return OK;
158  }
159 
160  /* adjust to a legal polling interval in Hz */
161  default: {
162  /* do we need to start internal polling? */
163  bool want_start = (_measure_interval == 0);
164 
165  /* convert hz to tick interval via microseconds */
166  unsigned interval = (1000000 / arg);
167 
168  /* check against maximum rate */
169  if (interval < _conversion_interval) {
170  return -EINVAL;
171  }
172 
173  /* update interval for next measurement */
174  _measure_interval = interval;
175 
176  /* if we need to start the poll state machine, do it */
177  if (want_start) {
178  start();
179  }
180 
181  return OK;
182  }
183  }
184  }
185  break;
186 
187  case AIRSPEEDIOCSSCALE: {
188  struct airspeed_scale *s = (struct airspeed_scale *)arg;
190  return OK;
191  }
192 
193  default:
194  /* give it to the superclass */
195  return I2C::ioctl(filp, cmd, arg);
196  }
197 }
198 
199 void
201 {
202  /* reset the report ring and state machine */
203  _collect_phase = false;
204 
205  /* schedule a cycle to start things */
206  ScheduleNow();
207 }
208 
209 void
211 {
212  ScheduleClear();
213 }
orb_advert_t _airspeed_pub
Definition: airspeed.h:81
#define AIRSPEED_BASE_DEVICE_PATH
Definition: drv_airspeed.h:51
void stop()
Stop the automatic measurement state machine.
Definition: airspeed.cpp:210
measure the time elapsed performing an event
Definition: perf_counter.h:56
API for the uORB lightweight object broker.
unsigned _conversion_interval
Definition: airspeed.h:86
virtual int init()
Definition: airspeed.cpp:92
#define SENSOR_POLLRATE_DEFAULT
poll at driver normal rate
Definition: drv_sensor.h:136
int _airspeed_orb_class_instance
Definition: airspeed.h:82
count the number of times an event occurs
Definition: perf_counter.h:55
High-resolution timer with callouts and timekeeping.
virtual int probe()
Definition: airspeed.cpp:118
Global flash based parameter store.
#define SENSORIOCSPOLLRATE
Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE constants. ...
Definition: drv_sensor.h:134
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
airspeed scaling factors; out = (in * Vscale) + offset
Definition: drv_airspeed.h:67
void perf_free(perf_counter_t handle)
Free a counter.
virtual int ioctl(device::file_t *filp, int cmd, unsigned long arg)
Definition: airspeed.cpp:133
void init()
Activates/configures the hardware registers.
#define perf_alloc(a, b)
Definition: px4io.h:59
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
perf_counter_t _sample_perf
Definition: airspeed.h:88
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
virtual ~Airspeed()
Definition: airspeed.cpp:75
Airspeed(int bus, int address, unsigned conversion_interval, const char *path)
Definition: airspeed.cpp:59
int _measure_interval
Definition: airspeed.h:77
#define AIRSPEEDIOCSSCALE
Definition: drv_airspeed.h:64
float _diff_pres_offset
Definition: airspeed.h:79
int orb_unadvertise(orb_advert_t handle)
Definition: uORB.cpp:65
Definition: bst.cpp:62
#define OK
Definition: uavcan_main.cpp:71
bool _collect_phase
Definition: airspeed.h:78
virtual int measure()=0
Airspeed driver interface.
orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority)
Definition: uORB.cpp:53
perf_counter_t _comms_errors
Definition: airspeed.h:89
void start()
Initialise the automatic measurement state machine and start it.
Definition: airspeed.cpp:200
int _class_instance
Definition: airspeed.h:84
Performance measuring tools.
Base class for devices connected via I2C.