diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-06 09:15:24 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-08-06 09:15:24 +0200 |
commit | 461b2eb3667838d455efc8946e81498f6a0d58f1 (patch) | |
tree | 61eefabd6488f24443706a3e47d7f776be68f0e0 /src/drivers/meas_airspeed/meas_airspeed.cpp | |
parent | ec2e02d50ea2f518051b50e4e83e12736526fbc2 (diff) | |
parent | 338e506a28e4233bc8a16493530f3b82a0dd67e9 (diff) | |
download | px4-firmware-461b2eb3667838d455efc8946e81498f6a0d58f1.tar.gz px4-firmware-461b2eb3667838d455efc8946e81498f6a0d58f1.tar.bz2 px4-firmware-461b2eb3667838d455efc8946e81498f6a0d58f1.zip |
Merge branch 'mpu6k_queue' of github.com:PX4/Firmware into fmuv2_bringup
Diffstat (limited to 'src/drivers/meas_airspeed/meas_airspeed.cpp')
-rw-r--r-- | src/drivers/meas_airspeed/meas_airspeed.cpp | 34 |
1 files changed, 22 insertions, 12 deletions
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index b19a1a0e6..3dc6159ef 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 */ @@ -122,7 +120,7 @@ protected: extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); MEASAirspeed::MEASAirspeed(int bus, int address) : Airspeed(bus, address, - CONVERSION_INTERVAL) + CONVERSION_INTERVAL) { } @@ -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); @@ -171,25 +169,37 @@ MEASAirspeed::collect() if (status == 2) { log("err: stale data"); + } else if (status == 3) { 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 */ |