diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-05 12:44:20 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-05 12:44:38 +0200 |
commit | 901a9c3e35456445465e7008d3c69b0bd3481e9e (patch) | |
tree | f3644ac02b9a51157557566073f47021b8522a9f | |
parent | 5ddbe24d8e5383a19f51957463211a4d8922d366 (diff) | |
download | px4-firmware-901a9c3e35456445465e7008d3c69b0bd3481e9e.tar.gz px4-firmware-901a9c3e35456445465e7008d3c69b0bd3481e9e.tar.bz2 px4-firmware-901a9c3e35456445465e7008d3c69b0bd3481e9e.zip |
Hotfix: MEAS Airspeed sensor fixes from Sarthak Kaingade
-rw-r--r-- | src/drivers/meas_airspeed/meas_airspeed.cpp | 31 |
1 files changed, 20 insertions, 11 deletions
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 15cae7d70..ebae21cf7 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -34,6 +34,7 @@ /** * @file meas_airspeed.cpp * @author Lorenz Meier + * @author Sarthak Kaingade * @author Simon Wilks * * Driver for the MEAS Spec series connected via I2C. @@ -92,9 +93,6 @@ /* Register address */ #define ADDR_READ_MR 0x00 /* write to this address to start conversion */ -#define ADDR_READ_DF2 0x00 /* read from this address to read pressure only */ -#define ADDR_READ_DF3 0x01 -#define ADDR_READ_DF4 0x02 /* read from this address to read pressure and temp */ /* Measurement rate is 100Hz */ #define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ @@ -160,7 +158,7 @@ MEASAirspeed::collect() perf_begin(_sample_perf); - ret = transfer(nullptr, 0, &val[0], 2); + ret = transfer(nullptr, 0, &val[0], 4); if (ret < 0) { log("error reading from sensor: %d", ret); @@ -176,21 +174,32 @@ MEASAirspeed::collect() log("err: fault"); } - uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8); + //uint16_t diff_pres_pa = (val[1]) | ((val[0] & ~(0xC0)) << 8); uint16_t temp = (val[3] & 0xE0) << 8 | val[2]; - diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f)); - diff_pres_pa -= _diff_pres_offset; + // XXX leaving this in until new calculation method has been cross-checked + //diff_pres_pa = abs(diff_pres_pa - (16384 / 2.0f)); + //diff_pres_pa -= _diff_pres_offset; + int16_t dp_raw = 0, dT_raw = 0; + dp_raw = (val[0] << 8) + val[1]; + dp_raw = 0x3FFF & dp_raw; + dT_raw = (val[2] << 8) + val[3]; + dT_raw = (0xFFE0 & dT_raw) >> 5; + float temperature = ((200 * dT_raw) / 2047) - 50; // XXX we may want to smooth out the readings to remove noise. + // Calculate differential pressure. As its centered around 8000 + // and can go positive or negative, enforce absolute value + uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f)); + _reports[_next_report].timestamp = hrt_absolute_time(); - _reports[_next_report].temperature = temp; - _reports[_next_report].differential_pressure_pa = diff_pres_pa; + _reports[_next_report].temperature = temperature; + _reports[_next_report].differential_pressure_pa = diff_press_pa; // Track maximum differential pressure measured (so we can work out top speed). - if (diff_pres_pa > _reports[_next_report].max_differential_pressure_pa) { - _reports[_next_report].max_differential_pressure_pa = diff_pres_pa; + if (diff_press_pa > _reports[_next_report].max_differential_pressure_pa) { + _reports[_next_report].max_differential_pressure_pa = diff_press_pa; } /* announce the airspeed if needed, just publish else */ |