PX4 Firmware
PX4 Autopilot Software http://px4.io
ms4525_airspeed.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2013, 2014 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 meas_airspeed.cpp
36  * @author Lorenz Meier
37  * @author Sarthak Kaingade
38  * @author Simon Wilks
39  * @author Thomas Gubler
40  *
41  * Driver for the MEAS Spec series connected via I2C.
42  *
43  * Supported sensors:
44  *
45  * - MS4525DO (http://www.meas-spec.com/downloads/MS4525DO.pdf)
46  *
47  * Interface application notes:
48  *
49  * - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf)
50  */
51 
53 #include <px4_platform_common/getopt.h>
55 
57 
61 };
62 
63 /* I2C bus address is 1010001x */
64 #define I2C_ADDRESS_MS4515DO 0x46
65 #define I2C_ADDRESS_MS4525DO 0x28 /**< 7-bit address. Depends on the order code (this is for code "I") */
66 #define PATH_MS4525 "/dev/ms4525"
67 
68 /* Register address */
69 #define ADDR_READ_MR 0x00 /* write to this address to start conversion */
70 
71 /* Measurement rate is 100Hz */
72 #define MEAS_RATE 100
73 #define MEAS_DRIVER_FILTER_FREQ 1.2f
74 #define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */
75 
76 
77 class MEASAirspeed : public Airspeed
78 {
79 public:
80  MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char *path = PATH_MS4525);
81 
82 protected:
83 
84  /**
85  * Perform a poll cycle; collect from the previous measurement
86  * and start a new one.
87  */
88  void Run() override;
89  int measure() override;
90  int collect() override;
91 
93 
94  /**
95  * Correct for 5V rail voltage variations
96  */
97  void voltage_correction(float &diff_pres_pa, float &temperature);
98 
99  int _t_system_power{-1};
101 };
102 
103 /*
104  * Driver 'main' command.
105  */
106 extern "C" __EXPORT int ms4525_airspeed_main(int argc, char *argv[]);
107 
108 MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, CONVERSION_INTERVAL, path)
109 {
110  _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_MS4525;
111 }
112 
113 int
115 {
116  // Send the command to begin a measurement.
117  uint8_t cmd = 0;
118  int ret = transfer(&cmd, 1, nullptr, 0);
119 
120  if (OK != ret) {
122  }
123 
124  return ret;
125 }
126 
127 int
129 {
130  /* read from the sensor */
131  uint8_t val[4] = {0, 0, 0, 0};
132 
134 
135  int ret = transfer(nullptr, 0, &val[0], 4);
136 
137  if (ret < 0) {
140  return ret;
141  }
142 
143  uint8_t status = (val[0] & 0xC0) >> 6;
144 
145  switch (status) {
146  case 0:
147  // Normal Operation. Good Data Packet
148  break;
149 
150  case 1:
151  // Reserved
152  return -EAGAIN;
153 
154  case 2:
155  // Stale Data. Data has been fetched since last measurement cycle.
156  return -EAGAIN;
157 
158  case 3:
159  // Fault Detected
162  return -EAGAIN;
163  }
164 
165  int16_t dp_raw = 0, dT_raw = 0;
166  dp_raw = (val[0] << 8) + val[1];
167  /* mask the used bits */
168  dp_raw = 0x3FFF & dp_raw;
169  dT_raw = (val[2] << 8) + val[3];
170  dT_raw = (0xFFE0 & dT_raw) >> 5;
171 
172  // dT max is almost certainly an invalid reading
173  if (dT_raw == 2047) {
175  return -EAGAIN;
176  }
177 
178  float temperature = ((200.0f * dT_raw) / 2047) - 50;
179 
180  // Calculate differential pressure. As its centered around 8000
181  // and can go positive or negative
182  const float P_min = -1.0f;
183  const float P_max = 1.0f;
184  const float PSI_to_Pa = 6894.757f;
185  /*
186  this equation is an inversion of the equation in the
187  pressure transfer function figure on page 4 of the datasheet
188 
189  We negate the result so that positive differential pressures
190  are generated when the bottom port is used as the static
191  port on the pitot and top port is used as the dynamic port
192  */
193  float diff_press_PSI = -((dp_raw - 0.1f * 16383) * (P_max - P_min) / (0.8f * 16383) + P_min);
194  float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa;
195 
196  // correct for 5V rail voltage if possible
197  voltage_correction(diff_press_pa_raw, temperature);
198 
199  /*
200  With the above calculation the MS4525 sensor will produce a
201  positive number when the top port is used as a dynamic port
202  and bottom port is used as the static port
203  */
204 
205  struct differential_pressure_s report;
206 
207  report.timestamp = hrt_absolute_time();
209  report.temperature = temperature;
211  report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset;
212  report.device_id = _device_id.devid;
213 
214  if (_airspeed_pub != nullptr && !(_pub_blocked)) {
215  /* publish it */
216  orb_publish(ORB_ID(differential_pressure), _airspeed_pub, &report);
217  }
218 
219  ret = OK;
220 
222 
223  return ret;
224 }
225 
226 void
228 {
229  int ret;
230 
231  /* collection phase? */
232  if (_collect_phase) {
233 
234  /* perform collection */
235  ret = collect();
236 
237  if (OK != ret) {
238  /* restart the measurement state machine */
239  start();
240  _sensor_ok = false;
241  return;
242  }
243 
244  /* next phase is measurement */
245  _collect_phase = false;
246 
247  /*
248  * Is there a collect->measure gap?
249  */
251 
252  /* schedule a fresh cycle call when we are ready to measure again */
253  ScheduleDelayed(_measure_interval - CONVERSION_INTERVAL);
254 
255  return;
256  }
257  }
258 
259  /* measurement phase */
260  ret = measure();
261 
262  if (OK != ret) {
263  DEVICE_DEBUG("measure error");
264  }
265 
266  _sensor_ok = (ret == OK);
267 
268  /* next phase is collection */
269  _collect_phase = true;
270 
271  /* schedule a fresh cycle call when the measurement is done */
272  ScheduleDelayed(CONVERSION_INTERVAL);
273 }
274 
275 /**
276  correct for 5V rail voltage if the system_power ORB topic is
277  available
278 
279  See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of
280  offset versus voltage for 3 sensors
281  */
282 void
283 MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature)
284 {
285 #if defined(ADC_SCALED_V5_SENSE)
286 
287  if (_t_system_power == -1) {
289  }
290 
291  if (_t_system_power == -1) {
292  // not available
293  return;
294  }
295 
296  bool updated = false;
297  orb_check(_t_system_power, &updated);
298 
299  if (updated) {
301  }
302 
303  if (system_power.voltage5v_v < 3.0f || system_power.voltage5v_v > 6.0f) {
304  // not valid, skip correction
305  return;
306  }
307 
308  const float slope = 65.0f;
309  /*
310  apply a piecewise linear correction, flattening at 0.5V from 5V
311  */
312  float voltage_diff = system_power.voltage5v_v - 5.0f;
313 
314  if (voltage_diff > 0.5f) {
315  voltage_diff = 0.5f;
316  }
317 
318  if (voltage_diff < -0.5f) {
319  voltage_diff = -0.5f;
320  }
321 
322  diff_press_pa -= voltage_diff * slope;
323 
324  /*
325  the temperature masurement varies as well
326  */
327  const float temp_slope = 0.887f;
328  voltage_diff = system_power.voltage5v_v - 5.0f;
329 
330  if (voltage_diff > 0.5f) {
331  voltage_diff = 0.5f;
332  }
333 
334  if (voltage_diff < -1.0f) {
335  voltage_diff = -1.0f;
336  }
337 
338  temperature -= voltage_diff * temp_slope;
339 #endif // defined(ADC_SCALED_V5_SENSE)
340 }
341 
342 /**
343  * Local functions in support of the shell command.
344  */
345 namespace meas_airspeed
346 {
347 
348 MEASAirspeed *g_dev = nullptr;
349 
350 int start();
351 int start_bus(int i2c_bus, int address);
352 int stop();
353 int reset();
354 
355 /**
356 * Attempt to start driver on all available I2C busses.
357 *
358 * This function will return as soon as the first sensor
359 * is detected on one of the available busses or if no
360 * sensors are detected.
361 *
362 */
363 int
365 {
366  for (unsigned i = 0; i < NUM_I2C_BUS_OPTIONS; i++) {
367  if (start_bus(i2c_bus_options[i], I2C_ADDRESS_MS4525DO) == PX4_OK) {
368  return PX4_OK;
369  }
370  }
371 
372  return PX4_ERROR;
373 
374 }
375 
376 /**
377  * Start the driver on a specific bus.
378  *
379  * This function call only returns once the driver is up and running
380  * or failed to detect the sensor.
381  */
382 int
383 start_bus(int i2c_bus, int address)
384 {
385  int fd;
386 
387  if (g_dev != nullptr) {
388  PX4_ERR("already started");
389  return PX4_ERROR;
390  }
391 
392  /* create the driver, try the MS4525DO first */
393  g_dev = new MEASAirspeed(i2c_bus, address, PATH_MS4525);
394 
395  /* check if the MS4525DO was instantiated */
396  if (g_dev == nullptr) {
397  goto fail;
398  }
399 
400  if (OK != g_dev->init()) {
401  goto fail;
402  }
403 
404  /* set the poll rate to default, starts automatic data collection */
405  fd = px4_open(PATH_MS4525, O_RDONLY);
406 
407  if (fd < 0) {
408  goto fail;
409  }
410 
412  goto fail;
413  }
414 
415  return PX4_OK;
416 
417 fail:
418 
419  if (g_dev != nullptr) {
420  delete g_dev;
421  g_dev = nullptr;
422  }
423 
424  return PX4_ERROR;
425 }
426 
427 /**
428  * Stop the driver
429  */
430 int
432 {
433  if (g_dev != nullptr) {
434  delete g_dev;
435  g_dev = nullptr;
436 
437  } else {
438  PX4_ERR("driver not running");
439  return PX4_ERROR;
440  }
441 
442  return PX4_OK;
443 }
444 
445 /**
446  * Reset the driver.
447  */
448 int
450 {
451  int fd = px4_open(PATH_MS4525, O_RDONLY);
452 
453  if (fd < 0) {
454  PX4_ERR("failed");
455  return PX4_ERROR;
456  }
457 
458  if (px4_ioctl(fd, SENSORIOCRESET, 0) < 0) {
459  PX4_ERR("driver reset failed");
460  return PX4_ERROR;
461  }
462 
464  PX4_ERR("driver poll restart failed");
465  return PX4_ERROR;
466  }
467 
468  return PX4_OK;
469 }
470 
471 } // namespace
472 
473 
474 static void
476 {
477  PX4_INFO("usage: ms4525 command [options]");
478  PX4_INFO("options:");
479  PX4_INFO("\t-b --bus i2cbus (%d)", PX4_I2C_BUS_DEFAULT);
480  PX4_INFO("\t-a --all");
481  PX4_INFO("command:");
482  PX4_INFO("\tstart|stop|reset");
483 }
484 
485 int
486 ms4525_airspeed_main(int argc, char *argv[])
487 {
488  int i2c_bus = PX4_I2C_BUS_DEFAULT;
489 
490  int myoptind = 1;
491  int ch;
492  const char *myoptarg = nullptr;
493 
494  int device_type = DEVICE_TYPE_MS4525;
495 
496  bool start_all = false;
497 
498  while ((ch = px4_getopt(argc, argv, "ab:T:", &myoptind, &myoptarg)) != EOF) {
499  switch (ch) {
500  case 'b':
501  i2c_bus = atoi(myoptarg);
502  break;
503 
504  case 'a':
505  start_all = true;
506  break;
507 
508  case 'T':
509  device_type = atoi(myoptarg);
510  break;
511 
512  default:
514  return 0;
515  }
516  }
517 
518  if (myoptind >= argc) {
520  return -1;
521  }
522 
523  /*
524  * Start/load the driver.
525  */
526  if (!strcmp(argv[myoptind], "start")) {
527  if (start_all) {
528  return meas_airspeed::start();
529 
530  } else if (device_type == DEVICE_TYPE_MS4515) {
532 
533  } else if (device_type == DEVICE_TYPE_MS4525) {
535  }
536  }
537 
538  /*
539  * Stop the driver
540  */
541  if (!strcmp(argv[myoptind], "stop")) {
542  return meas_airspeed::stop();
543  }
544 
545  /*
546  * Reset the driver.
547  */
548  if (!strcmp(argv[myoptind], "reset")) {
549  return meas_airspeed::reset();
550  }
551 
553  return 0;
554 }
orb_advert_t _airspeed_pub
Definition: airspeed.h:81
#define I2C_ADDRESS_MS4515DO
#define MEAS_RATE
int orb_copy(const struct orb_metadata *meta, int handle, void *buffer)
Definition: uORB.cpp:90
void stop()
Stop the automatic measurement state machine.
Definition: airspeed.cpp:210
static struct vehicle_status_s status
Definition: Commander.cpp:138
__EXPORT int ms4525_airspeed_main(int argc, char *argv[])
MS_DEVICE_TYPE
virtual int init()
Definition: airspeed.cpp:92
#define MEAS_DRIVER_FILTER_FREQ
int collect() override
#define SENSOR_POLLRATE_DEFAULT
poll at driver normal rate
Definition: drv_sensor.h:136
Definition: I2C.hpp:51
static void meas_airspeed_usage()
int stop()
Stop the driver.
int measure() override
int start()
Attempt to start driver on all available I2C busses.
int start_bus(int i2c_bus, int address)
Start the driver on a specific bus.
#define CONVERSION_INTERVAL
system_power_s system_power
#define PATH_MS4525
#define SENSORIOCSPOLLRATE
Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE constants. ...
Definition: drv_sensor.h:134
int orb_subscribe(const struct orb_metadata *meta)
Definition: uORB.cpp:75
static constexpr uint8_t PX4_I2C_BUS_DEFAULT
Definition: airspeed.h:48
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
void perf_count(perf_counter_t handle)
Count a performance event.
MEASAirspeed * g_dev
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
void Run() override
Perform a poll cycle; collect from the previous measurement and start a new one.
void perf_end(perf_counter_t handle)
End a performance event.
#define I2C_ADDRESS_MS4525DO
7-bit address.
int fd
Definition: dataman.cpp:146
int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, const void *data)
Definition: uORB.cpp:70
int _measure_interval
Definition: airspeed.h:77
static const int i2c_bus_options[]
Definition: i2c.h:46
math::LowPassFilter2p _filter
MEASAirspeed(int bus, int address=I2C_ADDRESS_MS4525DO, const char *path=PATH_MS4525)
int reset()
Reset the driver.
#define DRV_DIFF_PRESS_DEVTYPE_MS4525
Definition: drv_sensor.h:97
float apply(float sample)
Add a new raw value to the filter.
int px4_open(const char *path, int flags,...)
float _diff_pres_offset
Definition: airspeed.h:79
int orb_check(int handle, bool *updated)
Definition: uORB.cpp:95
#define SENSORIOCRESET
Reset the sensor to its default configuration.
Definition: drv_sensor.h:141
uint64_t perf_event_count(perf_counter_t handle)
Return current event_count.
#define OK
Definition: uavcan_main.cpp:71
bool _collect_phase
Definition: airspeed.h:78
Local functions in support of the shell command.
#define NUM_I2C_BUS_OPTIONS
Definition: i2c.h:61
bool _sensor_ok
Definition: airspeed.h:76
void voltage_correction(float &diff_pres_pa, float &temperature)
Correct for 5V rail voltage variations.
void perf_begin(perf_counter_t handle)
Begin a performance event.
#define DEVICE_DEBUG(FMT,...)
Definition: Device.hpp:52
perf_counter_t _comms_errors
Definition: airspeed.h:89
__EXPORT hrt_abstime hrt_absolute_time(void)
Get absolute time in [us] (does not wrap).
void start()
Initialise the automatic measurement state machine and start it.
Definition: airspeed.cpp:200
int px4_ioctl(int fd, int cmd, unsigned long arg)