From 28536682aa664b5dae74b96688cc66b919fccffe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Feb 2014 21:00:48 +0100 Subject: Fix airspeed sensor --- src/drivers/meas_airspeed/meas_airspeed.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) (limited to 'src/drivers/meas_airspeed/meas_airspeed.cpp') diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 05ae21c1f..c5394d9b4 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -89,8 +89,10 @@ /* I2C bus address is 1010001x */ #define I2C_ADDRESS_MS4525DO 0x28 //0x51 /* 7-bit address. */ +#define PATH_MS4525 "/dev/ms4525" /* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */ #define I2C_ADDRESS_MS5525DSO 0x77 //0x77/* 7-bit address, addr. pin pulled low */ +#define PATH_MS5525 "/dev/ms5525" /* Register address */ #define ADDR_READ_MR 0x00 /* write to this address to start conversion */ @@ -120,8 +122,8 @@ protected: */ extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); -MEASAirspeed::MEASAirspeed(int bus, int address) : Airspeed(bus, address, - CONVERSION_INTERVAL) +MEASAirspeed::MEASAirspeed(int bus, int address, const char* path) : Airspeed(bus, address, + CONVERSION_INTERVAL, path) { } @@ -304,7 +306,7 @@ start(int i2c_bus) errx(1, "already started"); /* create the driver, try the MS4525DO first */ - g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO); + g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525); /* check if the MS4525DO was instantiated */ if (g_dev == nullptr) @@ -313,7 +315,7 @@ start(int i2c_bus) /* try the MS5525DSO next if init fails */ if (OK != g_dev->Airspeed::init()) { delete g_dev; - g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO); + g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS4425); /* check if the MS5525DSO was instantiated */ if (g_dev == nullptr) -- cgit v1.2.3 From 4bd83dcaeb48cb6629d205b28532f49ca9440e1c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Feb 2014 21:20:02 +0100 Subject: Fix compile errors --- src/drivers/meas_airspeed/meas_airspeed.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) (limited to 'src/drivers/meas_airspeed/meas_airspeed.cpp') diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index c5394d9b4..eb65c049b 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -103,7 +103,7 @@ class MEASAirspeed : public Airspeed { public: - MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO); + MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char* path = PATH_MS4525); protected: @@ -315,7 +315,7 @@ start(int i2c_bus) /* try the MS5525DSO next if init fails */ if (OK != g_dev->Airspeed::init()) { delete g_dev; - g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS4425); + g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525); /* check if the MS5525DSO was instantiated */ if (g_dev == nullptr) -- cgit v1.2.3 From 387bfad1cacc2f9929e7590130ca93f27c688655 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 15 Feb 2014 22:19:14 +0100 Subject: Fixed status printing for airspeed sensor --- src/drivers/meas_airspeed/meas_airspeed.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) (limited to 'src/drivers/meas_airspeed/meas_airspeed.cpp') diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 05ae21c1f..6fe6c2221 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -50,6 +50,7 @@ * - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf) */ + #include #include @@ -386,7 +387,7 @@ test() err(1, "immediate read failed"); warnx("single read"); - warnx("diff pressure: %d pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) @@ -411,7 +412,7 @@ test() err(1, "periodic read failed"); warnx("periodic read %u", i); - warnx("diff pressure: %d pa", report.differential_pressure_pa); + warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature); } -- cgit v1.2.3 From 0784ef918925a546e0e4294571859150f0789561 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 14 Mar 2014 01:13:22 +0100 Subject: add low pass filter to meas airspeed driver including logging of filtered value --- src/drivers/meas_airspeed/meas_airspeed.cpp | 9 ++++++++- src/modules/sdlog2/sdlog2.c | 1 + src/modules/sdlog2/sdlog2_messages.h | 3 ++- src/modules/sensors/sensors.cpp | 1 + src/modules/uORB/topics/differential_pressure.h | 5 +++-- src/modules/uORB/topics/sensor_combined.h | 5 +++-- 6 files changed, 18 insertions(+), 6 deletions(-) (limited to 'src/drivers/meas_airspeed/meas_airspeed.cpp') diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 06d89abf7..39ee1e882 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -79,6 +79,8 @@ #include #include +#include + #include #include @@ -99,6 +101,8 @@ #define ADDR_READ_MR 0x00 /* write to this address to start conversion */ /* Measurement rate is 100Hz */ +#define MEAS_RATE 100.0f +#define MEAS_DRIVER_FILTER_FREQ 3.0f #define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ class MEASAirspeed : public Airspeed @@ -116,6 +120,7 @@ protected: virtual int measure(); virtual int collect(); + math::LowPassFilter2p _filter; }; /* @@ -124,7 +129,8 @@ protected: extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); MEASAirspeed::MEASAirspeed(int bus, int address, const char* path) : Airspeed(bus, address, - CONVERSION_INTERVAL, path) + CONVERSION_INTERVAL, path), + _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ) { } @@ -208,6 +214,7 @@ MEASAirspeed::collect() report.error_count = perf_event_count(_comms_errors); report.temperature = temperature; report.differential_pressure_pa = diff_press_pa; + report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 1365d9e70..a128f46c7 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1142,6 +1142,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_SENS.baro_alt = buf.sensor.baro_alt_meter; log_msg.body.log_SENS.baro_temp = buf.sensor.baro_temp_celcius; log_msg.body.log_SENS.diff_pres = buf.sensor.differential_pressure_pa; + log_msg.body.log_SENS.diff_pres_filtered = buf.sensor.differential_pressure_filtered_pa; LOGBUFFER_WRITE_AND_COUNT(SENS); } } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 16bfc355d..8a8c3c84c 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -92,6 +92,7 @@ struct log_SENS_s { float baro_alt; float baro_temp; float diff_pres; + float diff_pres_filtered; }; /* --- LPOS - LOCAL POSITION --- */ @@ -306,7 +307,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate,GX,GY,GZ"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), - LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), + LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"), LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"), LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"), LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b176d7417..4ee04d462 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1028,6 +1028,7 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &_diff_pres); raw.differential_pressure_pa = _diff_pres.differential_pressure_pa; + raw.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa; raw.differential_pressure_counter++; _airspeed.indicated_airspeed_m_s = calc_indicated_airspeed(_diff_pres.differential_pressure_pa); diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index 5d94d4288..3592c023c 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -54,8 +54,9 @@ struct differential_pressure_s { uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ uint64_t error_count; - float differential_pressure_pa; /**< Differential pressure reading */ - float max_differential_pressure_pa; /**< Maximum differential pressure reading */ + float differential_pressure_pa; /**< Differential pressure reading */ + float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */ + float max_differential_pressure_pa; /**< Maximum differential pressure reading */ float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ float temperature; /**< Temperature provided by sensor */ diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h index ad164555e..c84fd5db7 100644 --- a/src/modules/uORB/topics/sensor_combined.h +++ b/src/modules/uORB/topics/sensor_combined.h @@ -103,8 +103,9 @@ struct sensor_combined_s { float mcu_temp_celcius; /**< Internal temperature measurement of MCU */ uint32_t baro_counter; /**< Number of raw baro measurements taken */ - float differential_pressure_pa; /**< Airspeed sensor differential pressure */ - uint32_t differential_pressure_counter; /**< Number of raw differential pressure measurements taken */ + float differential_pressure_pa; /**< Airspeed sensor differential pressure */ + float differential_pressure_filtered_pa; /**< Low pass filtered airspeed sensor differential pressure reading */ + uint32_t differential_pressure_counter; /**< Number of raw differential pressure measurements taken */ }; /** -- cgit v1.2.3 From c5a1f4617c6d3e1782dde5d4552122059200db84 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 18 Mar 2014 18:30:05 +0100 Subject: Fixed MS4525 error flag handling --- src/drivers/meas_airspeed/meas_airspeed.cpp | 115 ++++++++++++++++++---------- 1 file changed, 73 insertions(+), 42 deletions(-) (limited to 'src/drivers/meas_airspeed/meas_airspeed.cpp') diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 06d89abf7..7baa8738f 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -104,7 +104,7 @@ class MEASAirspeed : public Airspeed { public: - MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char* path = PATH_MS4525); + MEASAirspeed(int bus, int address = I2C_ADDRESS_MS4525DO, const char *path = PATH_MS4525); protected: @@ -123,7 +123,7 @@ protected: */ extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); -MEASAirspeed::MEASAirspeed(int bus, int address, const char* path) : Airspeed(bus, address, +MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, CONVERSION_INTERVAL, path) { @@ -161,23 +161,30 @@ MEASAirspeed::collect() ret = transfer(nullptr, 0, &val[0], 4); if (ret < 0) { - perf_count(_comms_errors); - perf_end(_sample_perf); + perf_count(_comms_errors); + perf_end(_sample_perf); return ret; } - uint8_t status = val[0] & 0xC0; + uint8_t status = (val[0] & 0xC0) >> 6; - if (status == 2) { - log("err: stale data"); - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; - } else if (status == 3) { - log("err: fault"); - perf_count(_comms_errors); - perf_end(_sample_perf); - return ret; + switch (status) { + case 0: break; + + case 1: + perf_count(_comms_errors); + perf_end(_sample_perf); + return -EAGAIN; + + case 2: + perf_count(_comms_errors); + perf_end(_sample_perf); + return -EAGAIN; + + case 3: + perf_count(_comms_errors); + perf_end(_sample_perf); + return -EAGAIN; } int16_t dp_raw = 0, dT_raw = 0; @@ -193,19 +200,21 @@ MEASAirspeed::collect() */ const float P_min = -1.0f; const float P_max = 1.0f; - float diff_press_pa = fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; - if (diff_press_pa < 0.0f) - diff_press_pa = 0.0f; + float diff_press_pa = fabsf((((float)dp_raw - 0.1f * 16383.0f) * (P_max - P_min) / (0.8f * 16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; + + if (diff_press_pa < 0.0f) { + diff_press_pa = 0.0f; + } struct differential_pressure_s report; /* track maximum differential pressure measured (so we can work out top speed). */ if (diff_press_pa > _max_differential_pressure_pa) { - _max_differential_pressure_pa = diff_press_pa; + _max_differential_pressure_pa = diff_press_pa; } report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); + report.error_count = perf_event_count(_comms_errors); report.temperature = temperature; report.differential_pressure_pa = diff_press_pa; report.voltage = 0; @@ -261,8 +270,9 @@ MEASAirspeed::cycle() } /* measurement phase */ - if (OK != measure()) + if (OK != measure()) { log("measure error"); + } /* next phase is collection */ _collect_phase = true; @@ -303,15 +313,17 @@ start(int i2c_bus) { int fd; - if (g_dev != nullptr) + if (g_dev != nullptr) { errx(1, "already started"); + } /* create the driver, try the MS4525DO first */ g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS4525DO, PATH_MS4525); /* check if the MS4525DO was instantiated */ - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } /* try the MS5525DSO next if init fails */ if (OK != g_dev->Airspeed::init()) { @@ -319,22 +331,26 @@ start(int i2c_bus) g_dev = new MEASAirspeed(i2c_bus, I2C_ADDRESS_MS5525DSO, PATH_MS5525); /* check if the MS5525DSO was instantiated */ - if (g_dev == nullptr) + if (g_dev == nullptr) { goto fail; + } /* both versions failed if the init for the MS5525DSO fails, give up */ - if (OK != g_dev->Airspeed::init()) + if (OK != g_dev->Airspeed::init()) { goto fail; + } } /* set the poll rate to default, starts automatic data collection */ fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { goto fail; + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { goto fail; + } exit(0); @@ -379,21 +395,24 @@ test() int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "%s open failed (try 'meas_airspeed start' if the driver is not running", AIRSPEED_DEVICE_PATH); + } /* do a simple demand read */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "immediate read failed"); + } warnx("single read"); warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { errx(1, "failed to set 2Hz poll rate"); + } /* read the sensor 5x and report each value */ for (unsigned i = 0; i < 5; i++) { @@ -404,14 +423,16 @@ test() fds.events = POLLIN; ret = poll(&fds, 1, 2000); - if (ret != 1) + if (ret != 1) { errx(1, "timed out waiting for sensor data"); + } /* now go get it */ sz = read(fd, &report, sizeof(report)); - if (sz != sizeof(report)) + if (sz != sizeof(report)) { err(1, "periodic read failed"); + } warnx("periodic read %u", i); warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); @@ -419,8 +440,9 @@ test() } /* reset the sensor polling to its default rate */ - if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) + if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) { errx(1, "failed to set default rate"); + } errx(0, "PASS"); } @@ -433,14 +455,17 @@ reset() { int fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) + if (fd < 0) { err(1, "failed "); + } - if (ioctl(fd, SENSORIOCRESET, 0) < 0) + if (ioctl(fd, SENSORIOCRESET, 0) < 0) { err(1, "driver reset failed"); + } - if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) + if (ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT) < 0) { err(1, "driver poll restart failed"); + } exit(0); } @@ -451,8 +476,9 @@ reset() void info() { - if (g_dev == nullptr) + if (g_dev == nullptr) { errx(1, "driver not running"); + } printf("state @ %p\n", g_dev); g_dev->print_info(); @@ -491,32 +517,37 @@ meas_airspeed_main(int argc, char *argv[]) /* * Start/load the driver. */ - if (!strcmp(argv[1], "start")) + if (!strcmp(argv[1], "start")) { meas_airspeed::start(i2c_bus); + } /* * Stop the driver */ - if (!strcmp(argv[1], "stop")) + if (!strcmp(argv[1], "stop")) { meas_airspeed::stop(); + } /* * Test the driver/device. */ - if (!strcmp(argv[1], "test")) + if (!strcmp(argv[1], "test")) { meas_airspeed::test(); + } /* * Reset the driver. */ - if (!strcmp(argv[1], "reset")) + if (!strcmp(argv[1], "reset")) { meas_airspeed::reset(); + } /* * Print driver information. */ - if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) + if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) { meas_airspeed::info(); + } meas_airspeed_usage(); exit(0); -- cgit v1.2.3 From 98ef3b9ab882da077c67f81eb2f7852c44badb36 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Mar 2014 08:07:56 +0100 Subject: Make driver start less verbose --- src/drivers/ets_airspeed/ets_airspeed.cpp | 1 - src/drivers/meas_airspeed/meas_airspeed.cpp | 13 ++++--------- 2 files changed, 4 insertions(+), 10 deletions(-) (limited to 'src/drivers/meas_airspeed/meas_airspeed.cpp') diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index b6e80ce1d..e77f31e87 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -132,7 +132,6 @@ ETSAirspeed::measure() if (OK != ret) { perf_count(_comms_errors); - log("i2c::transfer returned %d", ret); } return ret; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 7baa8738f..4f64d8585 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -169,18 +169,13 @@ MEASAirspeed::collect() uint8_t status = (val[0] & 0xC0) >> 6; switch (status) { - case 0: break; + case 0: + break; case 1: - perf_count(_comms_errors); - perf_end(_sample_perf); - return -EAGAIN; - + /* fallthrough */ case 2: - perf_count(_comms_errors); - perf_end(_sample_perf); - return -EAGAIN; - + /* fallthrough */ case 3: perf_count(_comms_errors); perf_end(_sample_perf); -- cgit v1.2.3 From 8af83f70747d104511cc100e4f94ff4d5c6ac6d4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 21 Mar 2014 08:11:28 +0100 Subject: Make error message less dramatic, as its part of a scanning progress --- src/drivers/ets_airspeed/ets_airspeed.cpp | 2 +- src/drivers/meas_airspeed/meas_airspeed.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) (limited to 'src/drivers/meas_airspeed/meas_airspeed.cpp') diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index e77f31e87..d27ab9727 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -307,7 +307,7 @@ fail: g_dev = nullptr; } - errx(1, "driver start failed"); + errx(1, "no ETS airspeed sensor connected"); } /** diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 4f64d8585..beca28e7a 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -356,7 +356,7 @@ fail: g_dev = nullptr; } - errx(1, "driver start failed"); + errx(1, "no MS4525 airspeed sensor connected"); } /** -- cgit v1.2.3 From 00c82f0836388ffa1e1c4a5827eefe28c0521df8 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Feb 2014 12:18:20 +1100 Subject: Merged airspeed changes --- src/drivers/meas_airspeed/meas_airspeed.cpp | 62 +++++++++++++++++++++++++++-- 1 file changed, 59 insertions(+), 3 deletions(-) (limited to 'src/drivers/meas_airspeed/meas_airspeed.cpp') diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 2c3efdc35..2324475ce 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -87,6 +87,7 @@ #include #include #include +#include #include @@ -121,6 +122,14 @@ protected: virtual int collect(); math::LowPassFilter2p _filter; + + /** + * Correct for 5V rail voltage variations + */ + void voltage_correction(float &diff_pres_pa); + + int _t_system_power; + struct system_power_s system_power; }; /* @@ -129,10 +138,11 @@ protected: extern "C" __EXPORT int meas_airspeed_main(int argc, char *argv[]); MEASAirspeed::MEASAirspeed(int bus, int address, const char *path) : Airspeed(bus, address, - CONVERSION_INTERVAL, path), - _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ) + CONVERSION_INTERVAL, path), + _filter(MEAS_RATE, MEAS_DRIVER_FILTER_FREQ), + _t_system_power(-1) { - + memset(&system_power, 0, sizeof(system_power)); } int @@ -207,6 +217,9 @@ MEASAirspeed::collect() diff_press_pa = 0.0f; } + // correct for 5V rail voltage if possible + voltage_correction(diff_press_pa); + struct differential_pressure_s report; /* track maximum differential pressure measured (so we can work out top speed). */ @@ -287,6 +300,49 @@ MEASAirspeed::cycle() USEC2TICK(CONVERSION_INTERVAL)); } +/** + correct for 5V rail voltage if the system_power ORB topic is + available + + See http://uav.tridgell.net/MS4525/MS4525-offset.png for a graph of + offset versus voltage for 3 sensors + */ +void +MEASAirspeed::voltage_correction(float &diff_press_pa) +{ +#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 + if (_t_system_power == -1) { + _t_system_power = orb_subscribe(ORB_ID(system_power)); + } + if (_t_system_power == -1) { + // not available + return; + } + bool updated = false; + orb_check(_t_system_power, &updated); + if (updated) { + orb_copy(ORB_ID(system_power), _t_system_power, &system_power); + } + if (system_power.voltage5V_v < 3.0f || system_power.voltage5V_v > 6.0f) { + // not valid, skip correction + return; + } + + const float slope = 70.0f; + /* + apply a piecewise linear correction, flattening at 0.5V from 5V + */ + float voltage_diff = system_power.voltage5V_v - 5.0f; + if (voltage_diff > 0.5f) { + voltage_diff = 0.5f; + } + if (voltage_diff < -0.5f) { + voltage_diff = -0.5f; + } + diff_press_pa -= voltage_diff * slope; +#endif // CONFIG_ARCH_BOARD_PX4FMU_V2 +} + /** * Local functions in support of the shell command. */ -- cgit v1.2.3 From 9719af623d92ceab0c5eebe21906b4d5cf515682 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 12 Feb 2014 13:48:37 +1100 Subject: Merged voltage compensation --- src/drivers/meas_airspeed/meas_airspeed.cpp | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) (limited to 'src/drivers/meas_airspeed/meas_airspeed.cpp') diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 2324475ce..0b7986383 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -126,7 +126,7 @@ protected: /** * Correct for 5V rail voltage variations */ - void voltage_correction(float &diff_pres_pa); + void voltage_correction(float &diff_pres_pa, float &temperature); int _t_system_power; struct system_power_s system_power; @@ -204,7 +204,7 @@ MEASAirspeed::collect() 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; + float temperature = ((200.0f * dT_raw) / 2047) - 50; /* calculate differential pressure. As its centered around 8000 * and can go positive or negative, enforce absolute value @@ -218,7 +218,7 @@ MEASAirspeed::collect() } // correct for 5V rail voltage if possible - voltage_correction(diff_press_pa); + voltage_correction(diff_press_pa, temperature); struct differential_pressure_s report; @@ -308,7 +308,7 @@ MEASAirspeed::cycle() offset versus voltage for 3 sensors */ void -MEASAirspeed::voltage_correction(float &diff_press_pa) +MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) { #ifdef CONFIG_ARCH_BOARD_PX4FMU_V2 if (_t_system_power == -1) { @@ -340,6 +340,19 @@ MEASAirspeed::voltage_correction(float &diff_press_pa) voltage_diff = -0.5f; } diff_press_pa -= voltage_diff * slope; + + /* + the temperature masurement varies as well + */ + const float temp_slope = 0.887f; + voltage_diff = system_power.voltage5V_v - 5.0f; + if (voltage_diff > 0.5f) { + voltage_diff = 0.5f; + } + if (voltage_diff < -1.0f) { + voltage_diff = -1.0f; + } + temperature -= voltage_diff * temp_slope; #endif // CONFIG_ARCH_BOARD_PX4FMU_V2 } -- cgit v1.2.3 From cdec5e1d052b587a2e2a5d9d3dfef0bc507cc5b9 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 13 Feb 2014 14:25:52 +1100 Subject: Included raw differential pressure field --- src/drivers/meas_airspeed/meas_airspeed.cpp | 49 +++++++++++++++++++------ src/modules/uORB/topics/differential_pressure.h | 9 +++-- 2 files changed, 43 insertions(+), 15 deletions(-) (limited to 'src/drivers/meas_airspeed/meas_airspeed.cpp') diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 0b7986383..58b128948 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -206,20 +206,46 @@ MEASAirspeed::collect() dT_raw = (0xFFE0 & dT_raw) >> 5; float temperature = ((200.0f * dT_raw) / 2047) - 50; - /* calculate differential pressure. As its centered around 8000 - * and can go positive or negative, enforce absolute value - */ + // Calculate differential pressure. As its centered around 8000 + // and can go positive or negative const float P_min = -1.0f; const float P_max = 1.0f; - float diff_press_pa = fabsf((((float)dp_raw - 0.1f * 16383.0f) * (P_max - P_min) / (0.8f * 16383.0f) + P_min) * 6894.8f) - _diff_pres_offset; + const float PSI_to_Pa = 6894.757f; + /* + this equation is an inversion of the equation in the + pressure transfer function figure on page 4 of the datasheet - if (diff_press_pa < 0.0f) { - diff_press_pa = 0.0f; - } + We negate the result so that positive differential pressures + are generated when the bottom port is used as the static + port on the pitot and top port is used as the dynamic port + */ + float diff_press_PSI = -((dp_raw - 0.1f*16383) * (P_max-P_min)/(0.8f*16383) + P_min); + float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa; // correct for 5V rail voltage if possible - voltage_correction(diff_press_pa, temperature); + voltage_correction(diff_press_pa_raw, temperature); + float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset); + + /* + note that we return both the absolute value with offset + applied and a raw value without the offset applied. This + makes it possible for higher level code to detect if the + user has the tubes connected backwards, and also makes it + possible to correctly use offsets calculated by a higher + level airspeed driver. + + With the above calculation the MS4525 sensor will produce a + positive number when the top port is used as a dynamic port + and bottom port is used as the static port + + Also note that the _diff_pres_offset is applied before the + fabsf() not afterwards. It needs to be done this way to + prevent a bias at low speeds, but this also means that when + setting a offset you must set it based on the raw value, not + the offset value + */ + struct differential_pressure_s report; /* track maximum differential pressure measured (so we can work out top speed). */ @@ -232,6 +258,7 @@ MEASAirspeed::collect() report.temperature = temperature; report.differential_pressure_pa = diff_press_pa; report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); + report.differential_pressure_raw_pa = diff_press_pa_raw; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; @@ -328,7 +355,7 @@ MEASAirspeed::voltage_correction(float &diff_press_pa, float &temperature) return; } - const float slope = 70.0f; + const float slope = 65.0f; /* apply a piecewise linear correction, flattening at 0.5V from 5V */ @@ -478,7 +505,7 @@ test() } warnx("single read"); - warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %d pa", (int)report.differential_pressure_pa); /* start the sensor polling at 2Hz */ if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) { @@ -506,7 +533,7 @@ test() } warnx("periodic read %u", i); - warnx("diff pressure: %8.4f pa", (double)report.differential_pressure_pa); + warnx("diff pressure: %d pa", (int)report.differential_pressure_pa); warnx("temperature: %d C (0x%02x)", (int)report.temperature, (unsigned) report.temperature); } diff --git a/src/modules/uORB/topics/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h index ff88b04c6..01e14cda9 100644 --- a/src/modules/uORB/topics/differential_pressure.h +++ b/src/modules/uORB/topics/differential_pressure.h @@ -52,13 +52,14 @@ * Differential pressure. */ struct differential_pressure_s { - uint64_t timestamp; /**< microseconds since system boot, needed to integrate */ - uint64_t error_count; + uint64_t timestamp; /**< Microseconds since system boot, needed to integrate */ + uint64_t error_count; /**< Number of errors detected by driver */ float differential_pressure_pa; /**< Differential pressure reading */ + float differential_pressure_raw_pa; /**< Raw differential pressure reading (may be negative) */ float differential_pressure_filtered_pa; /**< Low pass filtered differential pressure reading */ float max_differential_pressure_pa; /**< Maximum differential pressure reading */ - float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ - float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ + float voltage; /**< Voltage from analog airspeed sensors (voltage divider already compensated) */ + float temperature; /**< Temperature provided by sensor, -1000.0f if unknown */ }; -- cgit v1.2.3 From fc39af08a1d6673aa727a84b17afd6c4485dff19 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 5 Apr 2014 17:02:37 +0200 Subject: airspeed: Prevent the filter from overshooting into the negative airspeed range --- src/drivers/meas_airspeed/meas_airspeed.cpp | 6 ++++++ src/lib/mathlib/math/filter/LowPassFilter2p.cpp | 7 ++++++- src/lib/mathlib/math/filter/LowPassFilter2p.hpp | 20 ++++++++++++++++---- 3 files changed, 28 insertions(+), 5 deletions(-) (limited to 'src/drivers/meas_airspeed/meas_airspeed.cpp') diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 58b128948..1ad383ee0 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -258,6 +258,12 @@ MEASAirspeed::collect() report.temperature = temperature; report.differential_pressure_pa = diff_press_pa; report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa); + + /* the dynamics of the filter can make it overshoot into the negative range */ + if (report.differential_pressure_filtered_pa < 0.0f) { + report.differential_pressure_filtered_pa = _filter.reset(diff_press_pa); + } + report.differential_pressure_raw_pa = diff_press_pa_raw; report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp index 3699d9bce..6f640c9f9 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.cpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp @@ -69,7 +69,7 @@ float LowPassFilter2p::apply(float sample) // do the filtering float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2; if (isnan(delay_element_0) || isinf(delay_element_0)) { - // don't allow bad values to propogate via the filter + // don't allow bad values to propagate via the filter delay_element_0 = sample; } float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2; @@ -81,5 +81,10 @@ float LowPassFilter2p::apply(float sample) return output; } +float LowPassFilter2p::reset(float sample) { + _delay_element_1 = _delay_element_2 = sample; + return apply(sample); +} + } // namespace math diff --git a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp index 208ec98d4..74cd5d78c 100644 --- a/src/lib/mathlib/math/filter/LowPassFilter2p.hpp +++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp @@ -52,18 +52,30 @@ public: _delay_element_1 = _delay_element_2 = 0; } - // change parameters + /** + * Change filter parameters + */ void set_cutoff_frequency(float sample_freq, float cutoff_freq); - // apply - Add a new raw value to the filter - // and retrieve the filtered result + /** + * Add a new raw value to the filter + * + * @return retrieve the filtered result + */ float apply(float sample); - // return the cutoff frequency + /** + * Return the cutoff frequency + */ float get_cutoff_freq(void) const { return _cutoff_freq; } + /** + * Reset the filter state to this value + */ + float reset(float sample); + private: float _cutoff_freq; float _a1; -- cgit v1.2.3 From 709d104de44b1cf7e338f4ae25e8b1fdc336a279 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 21:43:18 +0200 Subject: airspeed driver: Do not spam the console on error, report the system change only once --- src/drivers/airspeed/airspeed.cpp | 41 ++++++++++++++++++----------- src/drivers/airspeed/airspeed.h | 7 +++++ src/drivers/ets_airspeed/ets_airspeed.cpp | 14 +++++++--- src/drivers/meas_airspeed/meas_airspeed.cpp | 13 ++++++--- 4 files changed, 53 insertions(+), 22 deletions(-) (limited to 'src/drivers/meas_airspeed/meas_airspeed.cpp') diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp index 524151c90..109c50287 100644 --- a/src/drivers/airspeed/airspeed.cpp +++ b/src/drivers/airspeed/airspeed.cpp @@ -82,10 +82,12 @@ Airspeed::Airspeed(int bus, int address, unsigned conversion_interval, const cha _buffer_overflows(perf_alloc(PC_COUNT, "airspeed_buffer_overflows")), _max_differential_pressure_pa(0), _sensor_ok(false), + _last_published_sensor_ok(true), /* initialize differently to force publication */ _measure_ticks(0), _collect_phase(false), _diff_pres_offset(0.0f), _airspeed_pub(-1), + _subsys_pub(-1), _class_instance(-1), _conversion_interval(conversion_interval), _sample_perf(perf_alloc(PC_ELAPSED, "airspeed_read")), @@ -344,22 +346,6 @@ Airspeed::start() /* schedule a cycle to start things */ work_queue(HPWORK, &_work, (worker_t)&Airspeed::cycle_trampoline, this, 1); - - /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - true, - SUBSYSTEM_TYPE_DIFFPRESSURE - }; - static orb_advert_t pub = -1; - - if (pub > 0) { - orb_publish(ORB_ID(subsystem_info), pub, &info); - - } else { - pub = orb_advertise(ORB_ID(subsystem_info), &info); - } } void @@ -368,12 +354,35 @@ Airspeed::stop() work_cancel(HPWORK, &_work); } +void +Airspeed::update_status() +{ + if (_sensor_ok != _last_published_sensor_ok) { + /* notify about state change */ + struct subsystem_info_s info = { + true, + true, + _sensor_ok, + SUBSYSTEM_TYPE_DIFFPRESSURE + }; + + if (_subsys_pub > 0) { + orb_publish(ORB_ID(subsystem_info), _subsys_pub, &info); + } else { + _subsys_pub = orb_advertise(ORB_ID(subsystem_info), &info); + } + + _last_published_sensor_ok = _sensor_ok; + } +} + void Airspeed::cycle_trampoline(void *arg) { Airspeed *dev = (Airspeed *)arg; dev->cycle(); + dev->update_status(); } void diff --git a/src/drivers/airspeed/airspeed.h b/src/drivers/airspeed/airspeed.h index 186602eda..0b8e949c9 100644 --- a/src/drivers/airspeed/airspeed.h +++ b/src/drivers/airspeed/airspeed.h @@ -118,14 +118,21 @@ protected: virtual int measure() = 0; virtual int collect() = 0; + /** + * Update the subsystem status + */ + void update_status(); + work_s _work; float _max_differential_pressure_pa; bool _sensor_ok; + bool _last_published_sensor_ok; int _measure_ticks; bool _collect_phase; float _diff_pres_offset; orb_advert_t _airspeed_pub; + orb_advert_t _subsys_pub; int _class_instance; diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index d873a1132..146a06e7c 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -207,14 +207,18 @@ ETSAirspeed::collect() void ETSAirspeed::cycle() { + int ret; + /* collection phase? */ if (_collect_phase) { /* perform collection */ - if (OK != collect()) { + ret = collect(); + if (OK != ret) { perf_count(_comms_errors); /* restart the measurement state machine */ start(); + _sensor_ok = false; return; } @@ -238,8 +242,12 @@ ETSAirspeed::cycle() } /* measurement phase */ - if (OK != measure()) - log("measure error"); + ret = measure(); + if (OK != ret) { + debug("measure error"); + } + + _sensor_ok = (ret == OK); /* next phase is collection */ _collect_phase = true; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 1ad383ee0..c0f3c28e0 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -288,13 +288,17 @@ MEASAirspeed::collect() void MEASAirspeed::cycle() { + int ret; + /* collection phase? */ if (_collect_phase) { /* perform collection */ - if (OK != collect()) { + ret = collect(); + if (OK != ret) { /* restart the measurement state machine */ start(); + _sensor_ok = false; return; } @@ -318,10 +322,13 @@ MEASAirspeed::cycle() } /* measurement phase */ - if (OK != measure()) { - log("measure error"); + ret = measure(); + if (OK != ret) { + debug("measure error"); } + _sensor_ok = (ret == OK); + /* next phase is collection */ _collect_phase = true; -- cgit v1.2.3 From 56e1fe382b77200b3826b1ba438fc49ebab8f8ee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 11 Jul 2014 09:10:15 +0200 Subject: Remove voltage field for MEAS sensor --- src/drivers/meas_airspeed/meas_airspeed.cpp | 1 - 1 file changed, 1 deletion(-) (limited to 'src/drivers/meas_airspeed/meas_airspeed.cpp') diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index c0f3c28e0..7763f1057 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -265,7 +265,6 @@ MEASAirspeed::collect() } report.differential_pressure_raw_pa = diff_press_pa_raw; - report.voltage = 0; report.max_differential_pressure_pa = _max_differential_pressure_pa; if (_airspeed_pub > 0 && !(_pub_blocked)) { -- cgit v1.2.3 From f3549d775cb049bcde93c3e860c3adbad3763364 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 14 Jul 2014 08:33:35 +0200 Subject: Airspeed driver: Use the known sensor offset for raw value as well --- src/drivers/ets_airspeed/ets_airspeed.cpp | 3 +++ src/drivers/meas_airspeed/meas_airspeed.cpp | 5 ++++- 2 files changed, 7 insertions(+), 1 deletion(-) (limited to 'src/drivers/meas_airspeed/meas_airspeed.cpp') diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp index d1a9fa57c..c15a0cee4 100644 --- a/src/drivers/ets_airspeed/ets_airspeed.cpp +++ b/src/drivers/ets_airspeed/ets_airspeed.cpp @@ -172,6 +172,9 @@ ETSAirspeed::collect() diff_pres_pa = diff_pres_pa_raw - _diff_pres_offset; } + // The raw value still should be compensated for the known offset + diff_pres_pa_raw -= _diff_pres_offset; + // Track maximum differential pressure measured (so we can work out top speed). if (diff_pres_pa > _max_differential_pressure_pa) { _max_differential_pressure_pa = diff_pres_pa; diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp index 7763f1057..07611f903 100644 --- a/src/drivers/meas_airspeed/meas_airspeed.cpp +++ b/src/drivers/meas_airspeed/meas_airspeed.cpp @@ -225,7 +225,10 @@ MEASAirspeed::collect() // correct for 5V rail voltage if possible voltage_correction(diff_press_pa_raw, temperature); - float diff_press_pa = fabsf(diff_press_pa_raw - _diff_pres_offset); + // the raw value still should be compensated for the known offset + diff_press_pa_raw -= _diff_pres_offset; + + float diff_press_pa = fabsf(diff_press_pa_raw); /* note that we return both the absolute value with offset -- cgit v1.2.3