From beb51a219fbc77f0f48eeb3d13266274f4c62e6a Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 11 Jan 2021 19:24:08 -0500 Subject: [PATCH] delete drv_airspeed.h (all IOCTLs), airspeed driver lib - differential pressure now processed downstream like other sensor data (accel/gyro/mag/etc) - rename msg differential_pressure -> sensor_differential_pressure - add device id and timestamp_sample to all messages - calibration performend directly on raw data and SENS_DPRES_OFF parameter updated - remove Airspeed base class from existing differential pressure drivers - name differential pressure drivers consistently (no _airspeed suffix) --- ROMFS/px4fmu_common/init.d/rc.sensors | 20 +- ROMFS/px4fmu_common/init.d/rcS | 6 +- ROMFS/px4fmu_test/init.d/rc.sensors | 4 +- msg/CMakeLists.txt | 2 +- msg/airspeed_validated.msg | 5 +- msg/differential_pressure.msg | 6 - msg/sensor_differential_pressure.msg | 10 + msg/tools/uorb_rtps_message_ids.yaml | 2 +- .../differential_pressure/ets/CMakeLists.txt | 4 +- .../ets/ets_airspeed.cpp | 115 +++++----- .../ms4525/CMakeLists.txt | 2 +- .../ms4525/ms4525_airspeed.cpp | 135 ++++++------ .../ms5525/CMakeLists.txt | 9 +- .../differential_pressure/ms5525/MS5525.cpp | 54 ++--- .../differential_pressure/ms5525/MS5525.hpp | 37 ++-- .../{MS5525_main.cpp => ms5525_main.cpp} | 11 +- .../sdp3x/CMakeLists.txt | 9 +- .../differential_pressure/sdp3x/SDP3X.cpp | 54 ++--- .../differential_pressure/sdp3x/SDP3X.hpp | 34 +-- .../sdp3x/{SDP3X_main.cpp => sdp3x_main.cpp} | 11 +- src/drivers/drv_airspeed.h | 72 ------- src/drivers/uavcan/sensors/airspeed.cpp | 30 +-- src/drivers/uavcan/sensors/airspeed.hpp | 1 - .../uavcan/sensors/differential_pressure.cpp | 24 +-- .../uavcan/sensors/differential_pressure.hpp | 8 +- .../uavcannode/Publishers/RawAirData.hpp | 8 +- src/lib/drivers/CMakeLists.txt | 1 - src/lib/drivers/airspeed/CMakeLists.txt | 35 --- src/lib/drivers/airspeed/airspeed.cpp | 130 ------------ src/lib/drivers/airspeed/airspeed.h | 87 -------- .../commander/airspeed_calibration.cpp | 195 +++++------------ src/modules/mavlink/mavlink_messages.cpp | 22 +- src/modules/mavlink/mavlink_receiver.cpp | 9 +- src/modules/mavlink/mavlink_receiver.h | 4 +- src/modules/sensors/airspeed/Airspeed.cpp | 36 +--- src/modules/sensors/airspeed/Airspeed.hpp | 14 +- src/modules/sensors/sensors.cpp | 199 +++++------------- src/modules/sensors/voted_sensors_update.cpp | 13 +- src/modules/sensors/voted_sensors_update.h | 13 +- src/modules/simulator/simulator.h | 4 +- src/modules/simulator/simulator_mavlink.cpp | 9 +- 41 files changed, 460 insertions(+), 984 deletions(-) delete mode 100644 msg/differential_pressure.msg create mode 100644 msg/sensor_differential_pressure.msg rename src/drivers/differential_pressure/ms5525/{MS5525_main.cpp => ms5525_main.cpp} (93%) rename src/drivers/differential_pressure/sdp3x/{SDP3X_main.cpp => sdp3x_main.cpp} (94%) delete mode 100644 src/drivers/drv_airspeed.h delete mode 100644 src/lib/drivers/airspeed/CMakeLists.txt delete mode 100644 src/lib/drivers/airspeed/airspeed.cpp delete mode 100644 src/lib/drivers/airspeed/airspeed.h diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 75f739e80a..a2da67cf8d 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -126,21 +126,11 @@ then then if param compare CBRK_AIRSPD_CHK 0 then - sdp3x_airspeed start -X -q - sdp3x_airspeed start -X -a 0x22 -q + sdp3x start -X -q + sdp3x start -X -a 0x22 -q - # Pixhawk 2.1 has a MS5611 on I2C which gets wrongly - # detected as MS5525 because the chip manufacturer was so - # clever to assign the same I2C address and skip a WHO_AM_I - # register. - if [ $BOARD_FMUV3 = 21 ] - then - ms5525_airspeed start -X -b 2 -q - else - ms5525_airspeed start -X -q - fi - - ms4525_airspeed start -X -q + ms4525 start -X -q + ms5525 start -X -q ets_airspeed start -X -q fi @@ -150,5 +140,3 @@ fi ############################################################################### # End Optional drivers # ############################################################################### - -sensors start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 3c49fee229..39c8f8fe9b 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -322,11 +322,14 @@ else # Sensors System (start before Commander so Preflight checks are properly run). # Commander needs to be this early for in-air-restarts. # + sensors start + if param greater SYS_HITL 0 then set OUTPUT_MODE hil - sensors start -h + commander start -h + # disable GPS param set GPS_1_CONFIG 0 @@ -371,7 +374,6 @@ else set AUX_MODE pwm4 fi - # Check if ATS is enabled if param compare FD_EXT_ATS_EN 1 then diff --git a/ROMFS/px4fmu_test/init.d/rc.sensors b/ROMFS/px4fmu_test/init.d/rc.sensors index 4291deaf29..989815b3e9 100644 --- a/ROMFS/px4fmu_test/init.d/rc.sensors +++ b/ROMFS/px4fmu_test/init.d/rc.sensors @@ -16,11 +16,11 @@ if board_adc start then fi -if sdp3x_airspeed start -X +if sdp3x start -X then fi -if ms5525_airspeed start -X +if ms5525 start -X then fi diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 388b8ea70e..cba17cbda8 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -51,7 +51,6 @@ set(msg_files commander_state.msg control_allocator_status.msg cpuload.msg - differential_pressure.msg distance_sensor.msg ekf2_timestamps.msg ekf_gps_drift.msg @@ -127,6 +126,7 @@ set(msg_files sensor_baro.msg sensor_combined.msg sensor_correction.msg + sensor_differential_pressure.msg sensor_gps.msg sensor_gyro.msg sensor_gyro_fft.msg diff --git a/msg/airspeed_validated.msg b/msg/airspeed_validated.msg index 06731cc410..0da96cb6d2 100644 --- a/msg/airspeed_validated.msg +++ b/msg/airspeed_validated.msg @@ -1,4 +1,7 @@ -uint64 timestamp # time since system start (microseconds) +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles float32 indicated_airspeed_m_s # indicated airspeed in m/s (IAS), set to NAN if invalid float32 calibrated_airspeed_m_s # calibrated airspeed in m/s (CAS, accounts for instrumentation errors), set to NAN if invalid diff --git a/msg/differential_pressure.msg b/msg/differential_pressure.msg deleted file mode 100644 index bfb8f85478..0000000000 --- a/msg/differential_pressure.msg +++ /dev/null @@ -1,6 +0,0 @@ -uint64 timestamp # time since system start (microseconds) -uint64 error_count # Number of errors detected by driver -float32 differential_pressure_raw_pa # Raw differential pressure reading (may be negative) -float32 differential_pressure_filtered_pa # Low pass filtered differential pressure reading -float32 temperature # Temperature provided by sensor, -1000.0f if unknown -uint32 device_id # unique device ID for the sensor that does not change between power cycles diff --git a/msg/sensor_differential_pressure.msg b/msg/sensor_differential_pressure.msg new file mode 100644 index 0000000000..4a98804486 --- /dev/null +++ b/msg/sensor_differential_pressure.msg @@ -0,0 +1,10 @@ +uint64 timestamp # time since system start (microseconds) +uint64 timestamp_sample + +uint32 device_id # unique device ID for the sensor that does not change between power cycles + +float32 differential_pressure_pa # differential pressure reading + +float32 temperature # temperature in degrees Celsius + +uint32 error_count diff --git a/msg/tools/uorb_rtps_message_ids.yaml b/msg/tools/uorb_rtps_message_ids.yaml index 94655e8b2b..3b19aed0bb 100644 --- a/msg/tools/uorb_rtps_message_ids.yaml +++ b/msg/tools/uorb_rtps_message_ids.yaml @@ -33,7 +33,7 @@ rtps: - msg: debug_vect id: 15 receive: true - - msg: differential_pressure + - msg: sensor_differential_pressure id: 16 - msg: distance_sensor id: 17 diff --git a/src/drivers/differential_pressure/ets/CMakeLists.txt b/src/drivers/differential_pressure/ets/CMakeLists.txt index fe77c1172c..dd0010a7c1 100644 --- a/src/drivers/differential_pressure/ets/CMakeLists.txt +++ b/src/drivers/differential_pressure/ets/CMakeLists.txt @@ -31,11 +31,11 @@ # ############################################################################ px4_add_module( - MODULE drivers__ets_airspeed + MODULE drivers__differential_pressure__ets_airspeed MAIN ets_airspeed COMPILE_FLAGS SRCS ets_airspeed.cpp DEPENDS - drivers__airspeed + px4_work_queue ) diff --git a/src/drivers/differential_pressure/ets/ets_airspeed.cpp b/src/drivers/differential_pressure/ets/ets_airspeed.cpp index 6c9601b3fd..9090ac3cf1 100644 --- a/src/drivers/differential_pressure/ets/ets_airspeed.cpp +++ b/src/drivers/differential_pressure/ets/ets_airspeed.cpp @@ -37,13 +37,14 @@ * * Driver for the Eagle Tree Airspeed V3 connected via I2C. */ - -#include - -#include #include #include #include +#include +#include +#include + +#include /* I2C bus address */ #define I2C_ADDRESS 0x75 /* 7-bit address. 8-bit address is 0xEA */ @@ -60,45 +61,63 @@ /* Measurement rate is 100Hz */ #define CONVERSION_INTERVAL (1000000 / 100) /* microseconds */ -class ETSAirspeed : public Airspeed, public I2CSPIDriver +class ETSAirspeed : public device::I2C, public I2CSPIDriver { public: ETSAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS); - virtual ~ETSAirspeed() = default; + virtual ~ETSAirspeed(); static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, int runtime_instance); static void print_usage(); void RunImpl(); -protected: - int measure() override; - int collect() override; + +private: + int probe() override; + + int measure(); + int collect(); + + bool _sensor_ok{false}; + int _measure_interval{0}; + bool _collect_phase{false}; + unsigned _conversion_interval{0}; + + uORB::PublicationMulti _differential_pressure_pub{ORB_ID(sensor_differential_pressure)}; + + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")}; }; -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]); - -ETSAirspeed::ETSAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address) - : Airspeed(bus, bus_frequency, address, CONVERSION_INTERVAL), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address) +ETSAirspeed::ETSAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address) : + I2C(DRV_DIFF_PRESS_DEVTYPE_ETS3, MODULE_NAME, bus, address, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address) { - _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_MS4525; } -int -ETSAirspeed::measure() +ETSAirspeed::~ETSAirspeed() { - int ret; + perf_free(_sample_perf); + perf_free(_comms_errors); +} +int ETSAirspeed::probe() +{ + uint8_t cmd = READ_CMD; + int ret = transfer(&cmd, 1, nullptr, 0); + + return ret; +} + +int ETSAirspeed::measure() +{ /* * Send the command to begin a measurement. */ uint8_t cmd = READ_CMD; - ret = transfer(&cmd, 1, nullptr, 0); + int ret = transfer(&cmd, 1, nullptr, 0); if (OK != ret) { perf_count(_comms_errors); @@ -107,59 +126,46 @@ ETSAirspeed::measure() return ret; } -int -ETSAirspeed::collect() +int ETSAirspeed::collect() { - int ret = -EIO; - /* read from the sensor */ - uint8_t val[2] = {0, 0}; - perf_begin(_sample_perf); - ret = transfer(nullptr, 0, &val[0], 2); + const hrt_abstime timestamp_sample = hrt_absolute_time(); + uint8_t val[2] {}; + int ret = transfer(nullptr, 0, &val[0], 2); if (ret < 0) { perf_count(_comms_errors); return ret; } - float diff_pres_pa_raw = (float)(val[1] << 8 | val[0]); + float diff_press_pa_raw = (float)(val[1] << 8 | val[0]); - differential_pressure_s report{}; - report.timestamp = hrt_absolute_time(); - - if (diff_pres_pa_raw < FLT_EPSILON) { + if (diff_press_pa_raw < FLT_EPSILON) { // a zero value indicates no measurement // since the noise floor has been arbitrarily killed // it defeats our stuck sensor detection - the best we // can do is to output some numerical noise to show // that we are still correctly sampling. - diff_pres_pa_raw = 0.001f * (report.timestamp & 0x01); + diff_press_pa_raw = 0.001f * (hrt_absolute_time() & 0x01); } - // The raw value still should be compensated for the known offset - diff_pres_pa_raw -= _diff_pres_offset; - - report.error_count = perf_event_count(_comms_errors); - - // XXX we may want to smooth out the readings to remove noise. - report.differential_pressure_filtered_pa = diff_pres_pa_raw; - report.differential_pressure_raw_pa = diff_pres_pa_raw; + sensor_differential_pressure_s report{}; + report.timestamp_sample = timestamp_sample; + report.device_id = get_device_id(); + report.differential_pressure_pa = diff_press_pa_raw; report.temperature = NAN; - report.device_id = _device_id.devid; - - _airspeed_pub.publish(report); - - ret = OK; + report.error_count = perf_event_count(_comms_errors); + report.timestamp = hrt_absolute_time(); + _differential_pressure_pub.publish(report); perf_end(_sample_perf); - return ret; + return PX4_OK; } -void -ETSAirspeed::RunImpl() +void ETSAirspeed::RunImpl() { int ret; @@ -228,9 +234,7 @@ I2CSPIDriverBase *ETSAirspeed::instantiate(const BusCLIArguments &cli, const Bus return instance; } - -void -ETSAirspeed::print_usage() +void ETSAirspeed::print_usage() { PRINT_MODULE_USAGE_NAME("ets_airspeed", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); @@ -239,8 +243,7 @@ ETSAirspeed::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -int -ets_airspeed_main(int argc, char *argv[]) +extern "C" __EXPORT int ets_airspeed_main(int argc, char *argv[]) { using ThisDriver = ETSAirspeed; BusCLIArguments cli{true, false}; diff --git a/src/drivers/differential_pressure/ms4525/CMakeLists.txt b/src/drivers/differential_pressure/ms4525/CMakeLists.txt index bac360d01e..28dee7a210 100644 --- a/src/drivers/differential_pressure/ms4525/CMakeLists.txt +++ b/src/drivers/differential_pressure/ms4525/CMakeLists.txt @@ -37,6 +37,6 @@ px4_add_module( SRCS ms4525_airspeed.cpp DEPENDS - drivers__airspeed mathlib + px4_work_queue ) diff --git a/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp b/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp index f5b982f682..23200f4605 100644 --- a/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp +++ b/src/drivers/differential_pressure/ms4525/ms4525_airspeed.cpp @@ -49,12 +49,12 @@ * - Interfacing to MEAS Digital Pressure Modules (http://www.meas-spec.com/downloads/Interfacing_to_MEAS_Digital_Pressure_Modules.pdf) */ -#include #include #include #include - -#include +#include +#include +#include enum MS_DEVICE_TYPE { DEVICE_TYPE_MS4515 = 4515, @@ -70,11 +70,9 @@ enum MS_DEVICE_TYPE { /* Measurement rate is 100Hz */ #define MEAS_RATE 100 -#define MEAS_DRIVER_FILTER_FREQ 1.2f #define CONVERSION_INTERVAL (1000000 / MEAS_RATE) /* microseconds */ - -class MEASAirspeed : public Airspeed, public I2CSPIDriver +class MEASAirspeed : public device::I2C, public I2CSPIDriver { public: MEASAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS_MS4525DO); @@ -87,28 +85,31 @@ public: void RunImpl(); -protected: +private: + int measure(); + int collect(); - int measure() override; - int collect() override; + bool _sensor_ok{false}; + int _measure_interval{0}; + bool _collect_phase{false}; + unsigned _conversion_interval{0}; - math::LowPassFilter2p _filter{MEAS_RATE, MEAS_DRIVER_FILTER_FREQ}; + int16_t _dp_raw_prev{0}; + int16_t _dT_raw_prev{0}; + + uORB::PublicationMulti _differential_pressure_pub{ORB_ID(sensor_differential_pressure)}; + + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")}; }; -/* - * Driver 'main' command. - */ -extern "C" __EXPORT int ms4525_airspeed_main(int argc, char *argv[]); - -MEASAirspeed::MEASAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address) - : Airspeed(bus, bus_frequency, address, CONVERSION_INTERVAL), - I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address) +MEASAirspeed::MEASAirspeed(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address) : + I2C(DRV_DIFF_PRESS_DEVTYPE_MS4525, MODULE_NAME, bus, address, bus_frequency), + I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address) { - _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_MS4525; } -int -MEASAirspeed::measure() +int MEASAirspeed::measure() { // Send the command to begin a measurement. uint8_t cmd = 0; @@ -121,14 +122,15 @@ MEASAirspeed::measure() return ret; } -int -MEASAirspeed::collect() +int MEASAirspeed::collect() { /* read from the sensor */ - uint8_t val[4] = {0, 0, 0, 0}; + uint8_t val[4] {}; perf_begin(_sample_perf); + const hrt_abstime timestamp_sample = hrt_absolute_time(); + int ret = transfer(nullptr, 0, &val[0], 4); if (ret < 0) { @@ -159,12 +161,9 @@ MEASAirspeed::collect() return -EAGAIN; } - int16_t dp_raw = 0, dT_raw = 0; - dp_raw = (val[0] << 8) + val[1]; /* mask the used bits */ - dp_raw = 0x3FFF & dp_raw; - dT_raw = (val[2] << 8) + val[3]; - dT_raw = (0xFFE0 & dT_raw) >> 5; + int16_t dp_raw = (0x3FFF & ((val[0] << 8) + val[1])); + int16_t dT_raw = (0xFFE0 & ((val[2] << 8) + val[3])) >> 5; // dT max is almost certainly an invalid reading if (dT_raw == 2047) { @@ -172,50 +171,51 @@ MEASAirspeed::collect() return -EAGAIN; } - float temperature = ((200.0f * dT_raw) / 2047) - 50; + // only publish changes + if ((dp_raw != _dp_raw_prev) && (dT_raw != _dT_raw_prev)) { - // 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; - 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 + _dp_raw_prev = dp_raw; + _dT_raw_prev = dT_raw; - 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; + float temperature = ((200.0f * dT_raw) / 2047) - 50; - /* - 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 - */ + // 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; + 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 - differential_pressure_s report{}; + 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; - report.timestamp = hrt_absolute_time(); - report.error_count = perf_event_count(_comms_errors); - report.temperature = temperature; - report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset; - report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset; - report.device_id = _device_id.devid; - - _airspeed_pub.publish(report); - - ret = OK; + /* + 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 + */ + sensor_differential_pressure_s report{}; + report.timestamp_sample = timestamp_sample; + report.device_id = get_device_id(); + report.differential_pressure_pa = diff_press_pa_raw; + report.temperature = temperature; + report.error_count = perf_event_count(_comms_errors); + report.timestamp = hrt_absolute_time(); + _differential_pressure_pub.publish(report); + } perf_end(_sample_perf); - return ret; + return PX4_OK; } -void -MEASAirspeed::RunImpl() +void MEASAirspeed::RunImpl() { int ret; @@ -251,7 +251,7 @@ MEASAirspeed::RunImpl() /* measurement phase */ ret = measure(); - if (OK != ret) { + if (PX4_OK != ret) { DEVICE_DEBUG("measure error"); } @@ -284,9 +284,7 @@ I2CSPIDriverBase *MEASAirspeed::instantiate(const BusCLIArguments &cli, const Bu return instance; } - -void -MEASAirspeed::print_usage() +void MEASAirspeed::print_usage() { PRINT_MODULE_USAGE_NAME("ms4525_airspeed", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); @@ -296,8 +294,7 @@ MEASAirspeed::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -int -ms4525_airspeed_main(int argc, char *argv[]) +extern "C" __EXPORT int ms4525_airspeed_main(int argc, char *argv[]) { int ch; using ThisDriver = MEASAirspeed; diff --git a/src/drivers/differential_pressure/ms5525/CMakeLists.txt b/src/drivers/differential_pressure/ms5525/CMakeLists.txt index 21e4195034..8a5ef95916 100644 --- a/src/drivers/differential_pressure/ms5525/CMakeLists.txt +++ b/src/drivers/differential_pressure/ms5525/CMakeLists.txt @@ -31,13 +31,14 @@ # ############################################################################ px4_add_module( - MODULE drivers__ms5525_airspeed - MAIN ms5525_airspeed + MODULE drivers__differential_pressure__ms5525 + MAIN ms5525 COMPILE_FLAGS SRCS + ms5525_main.cpp MS5525.cpp - MS5525_main.cpp + MS5525.hpp DEPENDS - drivers__airspeed mathlib + px4_work_queue ) diff --git a/src/drivers/differential_pressure/ms5525/MS5525.cpp b/src/drivers/differential_pressure/ms5525/MS5525.cpp index aa5e3f6ea5..12fa8552f7 100644 --- a/src/drivers/differential_pressure/ms5525/MS5525.cpp +++ b/src/drivers/differential_pressure/ms5525/MS5525.cpp @@ -33,8 +33,18 @@ #include "MS5525.hpp" -int -MS5525::measure() +MS5525::~MS5525() +{ + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +int MS5525::probe() +{ + return init_ms5525() ? PX4_OK : PX4_ERROR; +} + +int MS5525::measure() { int ret = PX4_ERROR; @@ -58,8 +68,7 @@ MS5525::measure() return ret; } -bool -MS5525::init_ms5525() +bool MS5525::init_ms5525() { // Step 1 - reset uint8_t cmd = CMD_RESET; @@ -128,8 +137,7 @@ MS5525::init_ms5525() } } -uint8_t -MS5525::prom_crc4(uint16_t n_prom[]) const +uint8_t MS5525::prom_crc4(uint16_t n_prom[]) const { // see Measurement Specialties AN520 @@ -166,11 +174,12 @@ MS5525::prom_crc4(uint16_t n_prom[]) const return (n_rem ^ 0x00); } -int -MS5525::collect() +int MS5525::collect() { perf_begin(_sample_perf); + const hrt_abstime timestamp_sample = hrt_absolute_time(); + // read ADC uint8_t cmd = CMD_ADC_READ; int ret = transfer(&cmd, 1, nullptr, 0); @@ -181,7 +190,7 @@ MS5525::collect() } // read 24 bits from the sensor - uint8_t val[3]; + uint8_t val[3] {}; ret = transfer(nullptr, 0, &val[0], 3); if (ret != PX4_OK) { @@ -248,28 +257,23 @@ MS5525::collect() static constexpr float PSI_to_Pa = 6894.757f; const float diff_press_pa_raw = diff_press_PSI * PSI_to_Pa; - const float temperature_c = TEMP * 0.01f; + const float temperature = TEMP * 0.01f; - differential_pressure_s diff_pressure = { - .timestamp = hrt_absolute_time(), - .error_count = perf_event_count(_comms_errors), - .differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset, - .differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset, - .temperature = temperature_c, - .device_id = _device_id.devid - }; - - _airspeed_pub.publish(diff_pressure); - - ret = OK; + sensor_differential_pressure_s report{}; + report.timestamp_sample = timestamp_sample; + report.device_id = get_device_id(); + report.differential_pressure_pa = diff_press_pa_raw; + report.temperature = temperature; + report.error_count = perf_event_count(_comms_errors); + report.timestamp = hrt_absolute_time(); + _differential_pressure_pub.publish(report); perf_end(_sample_perf); - return ret; + return PX4_OK; } -void -MS5525::RunImpl() +void MS5525::RunImpl() { int ret = PX4_ERROR; diff --git a/src/drivers/differential_pressure/ms5525/MS5525.hpp b/src/drivers/differential_pressure/ms5525/MS5525.hpp index de05204123..8069a4e107 100644 --- a/src/drivers/differential_pressure/ms5525/MS5525.hpp +++ b/src/drivers/differential_pressure/ms5525/MS5525.hpp @@ -33,31 +33,29 @@ #pragma once -#include -#include -#include #include #include #include +#include +#include +#include /* The MS5525DSO address is 111011Cx, where C is the complementary value of the pin CSB */ static constexpr uint8_t I2C_ADDRESS_1_MS5525DSO = 0x76; /* Measurement rate is 100Hz */ static constexpr unsigned MEAS_RATE = 100; -static constexpr float MEAS_DRIVER_FILTER_FREQ = 1.2f; static constexpr int64_t CONVERSION_INTERVAL = (1000000 / MEAS_RATE); /* microseconds */ -class MS5525 : public Airspeed, public I2CSPIDriver +class MS5525 : public device::I2C, public I2CSPIDriver { public: MS5525(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS_1_MS5525DSO) : - Airspeed(bus, bus_frequency, address, CONVERSION_INTERVAL), + I2C(DRV_DIFF_PRESS_DEVTYPE_MS5525, MODULE_NAME, bus, address, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address) - { - } + {} - virtual ~MS5525() = default; + virtual ~MS5525(); static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, int runtime_instance); @@ -66,12 +64,14 @@ public: void RunImpl(); private: + int probe() override; - int measure() override; - int collect() override; + bool init_ms5525(); - // temperature is read once every 10 cycles - math::LowPassFilter2p _filter{MEAS_RATE * 0.9, MEAS_DRIVER_FILTER_FREQ}; + uint8_t prom_crc4(uint16_t n_prom[]) const; + + int measure(); + int collect(); static constexpr uint8_t CMD_RESET = 0x1E; // ADC reset command static constexpr uint8_t CMD_ADC_READ = 0x00; // ADC read command @@ -121,9 +121,16 @@ private: uint32_t D1{0}; uint32_t D2{0}; - bool init_ms5525(); bool _inited{false}; - uint8_t prom_crc4(uint16_t n_prom[]) const; + bool _sensor_ok{false}; + int _measure_interval{0}; + bool _collect_phase{false}; + unsigned _conversion_interval{0}; + + uORB::PublicationMulti _differential_pressure_pub{ORB_ID(sensor_differential_pressure)}; + + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")}; }; diff --git a/src/drivers/differential_pressure/ms5525/MS5525_main.cpp b/src/drivers/differential_pressure/ms5525/ms5525_main.cpp similarity index 93% rename from src/drivers/differential_pressure/ms5525/MS5525_main.cpp rename to src/drivers/differential_pressure/ms5525/ms5525_main.cpp index 991736558f..6107cdf625 100644 --- a/src/drivers/differential_pressure/ms5525/MS5525_main.cpp +++ b/src/drivers/differential_pressure/ms5525/ms5525_main.cpp @@ -33,8 +33,6 @@ #include "MS5525.hpp" -extern "C" __EXPORT int ms5525_airspeed_main(int argc, char *argv[]); - I2CSPIDriverBase *MS5525::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, int runtime_instance) { @@ -54,19 +52,16 @@ I2CSPIDriverBase *MS5525::instantiate(const BusCLIArguments &cli, const BusInsta return instance; } - -void -MS5525::print_usage() +void MS5525::print_usage() { - PRINT_MODULE_USAGE_NAME("ms5525_airspeed", "driver"); + PRINT_MODULE_USAGE_NAME("ms5525", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -int -ms5525_airspeed_main(int argc, char *argv[]) +extern "C" __EXPORT int ms5525_main(int argc, char *argv[]) { using ThisDriver = MS5525; BusCLIArguments cli{true, false}; diff --git a/src/drivers/differential_pressure/sdp3x/CMakeLists.txt b/src/drivers/differential_pressure/sdp3x/CMakeLists.txt index 3ba5eba2fe..911009429e 100644 --- a/src/drivers/differential_pressure/sdp3x/CMakeLists.txt +++ b/src/drivers/differential_pressure/sdp3x/CMakeLists.txt @@ -31,13 +31,14 @@ # ############################################################################ px4_add_module( - MODULE drivers__sdp3x_airspeed - MAIN sdp3x_airspeed + MODULE drivers__differential_pressure__sdp3x + MAIN sdp3x COMPILE_FLAGS SRCS + sdp3x_main.cpp SDP3X.cpp - SDP3X_main.cpp + SDP3X.hpp DEPENDS - drivers__airspeed mathlib + px4_work_queue ) diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X.cpp b/src/drivers/differential_pressure/sdp3x/SDP3X.cpp index 4d7a89d9de..975d1b1e40 100644 --- a/src/drivers/differential_pressure/sdp3x/SDP3X.cpp +++ b/src/drivers/differential_pressure/sdp3x/SDP3X.cpp @@ -42,8 +42,13 @@ using namespace time_literals; -int -SDP3X::probe() +SDP3X::~SDP3X() +{ + perf_free(_sample_perf); + perf_free(_comms_errors); +} + +int SDP3X::probe() { bool require_initialization = !init_sdp3x(); @@ -63,14 +68,12 @@ int SDP3X::write_command(uint16_t command) return transfer(&cmd[0], 2, nullptr, 0); } -bool -SDP3X::init_sdp3x() +bool SDP3X::init_sdp3x() { return configure() == 0; } -int -SDP3X::configure() +int SDP3X::configure() { int ret = write_command(SDP3X_CONT_MEAS_AVG_MODE); @@ -86,8 +89,7 @@ SDP3X::configure() return ret; } -int -SDP3X::read_scale() +int SDP3X::read_scale() { // get scale uint8_t val[9]; @@ -109,15 +111,15 @@ SDP3X::read_scale() switch (_scale) { case SDP3X_SCALE_PRESSURE_SDP31: - _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SDP31; + set_device_type(DRV_DIFF_PRESS_DEVTYPE_SDP31); break; case SDP3X_SCALE_PRESSURE_SDP32: - _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SDP32; + set_device_type(DRV_DIFF_PRESS_DEVTYPE_SDP32); break; case SDP3X_SCALE_PRESSURE_SDP33: - _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SDP33; + set_device_type(DRV_DIFF_PRESS_DEVTYPE_SDP33); break; } @@ -130,13 +132,14 @@ void SDP3X::start() ScheduleDelayed(10_ms); } -int -SDP3X::collect() +int SDP3X::collect() { perf_begin(_sample_perf); + const hrt_abstime timestamp_sample = hrt_absolute_time(); + // read 6 bytes from the sensor - uint8_t val[6]; + uint8_t val[6] {}; int ret = transfer(nullptr, 0, &val[0], sizeof(val)); if (ret != PX4_OK) { @@ -154,26 +157,23 @@ SDP3X::collect() int16_t temp = (((int16_t)val[3]) << 8) | val[4]; float diff_press_pa_raw = static_cast(P) / static_cast(_scale); - float temperature_c = temp / static_cast(SDP3X_SCALE_TEMPERATURE); + float temperature = temp / static_cast(SDP3X_SCALE_TEMPERATURE); - differential_pressure_s report{}; - - report.timestamp = hrt_absolute_time(); + sensor_differential_pressure_s report{}; + report.timestamp_sample = timestamp_sample; + report.device_id = get_device_id(); + report.differential_pressure_pa = diff_press_pa_raw; + report.temperature = temperature; report.error_count = perf_event_count(_comms_errors); - report.temperature = temperature_c; - report.differential_pressure_filtered_pa = _filter.apply(diff_press_pa_raw) - _diff_pres_offset; - report.differential_pressure_raw_pa = diff_press_pa_raw - _diff_pres_offset; - report.device_id = _device_id.devid; - - _airspeed_pub.publish(report); + report.timestamp = hrt_absolute_time(); + _differential_pressure_pub.publish(report); perf_end(_sample_perf); - return ret; + return PX4_OK; } -void -SDP3X::RunImpl() +void SDP3X::RunImpl() { switch (_state) { case State::RequireConfig: diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X.hpp b/src/drivers/differential_pressure/sdp3x/SDP3X.hpp index 755d42429c..92a9f8d387 100644 --- a/src/drivers/differential_pressure/sdp3x/SDP3X.hpp +++ b/src/drivers/differential_pressure/sdp3x/SDP3X.hpp @@ -36,17 +36,17 @@ * * Driver for Sensirion SDP3X Differential Pressure Sensor * - * Datasheet: https://www.sensirion.com/fileadmin/user_upload/customers/sensirion/Dokumente/8_Differential_Pressure/Sensirion_Differential_Pressure_Sensors_SDP3x_Digital_Datasheet_V0.8.pdf + * Datasheet: https://www.sensirion.com/fileadmin/user_upload/customers/sensirion/Dokumente/8_Differential_Pressure/Sensirion_sensor_differential_pressure_sensors_SDP3x_Digital_Datasheet_V0.8.pdf */ #pragma once -#include -#include -#include #include #include #include +#include +#include +#include #define I2C_ADDRESS_1_SDP3X 0x21 #define I2C_ADDRESS_2_SDP3X 0x22 @@ -63,27 +63,26 @@ // Measurement rate is 20Hz #define SPD3X_MEAS_RATE 100 -#define SDP3X_MEAS_DRIVER_FILTER_FREQ 3.0f #define CONVERSION_INTERVAL (1000000 / SPD3X_MEAS_RATE) /* microseconds */ -class SDP3X : public Airspeed, public I2CSPIDriver +class SDP3X : public device::I2C, public I2CSPIDriver { public: SDP3X(I2CSPIBusOption bus_option, const int bus, int bus_frequency, int address = I2C_ADDRESS_1_SDP3X, bool keep_retrying = false) : - Airspeed(bus, bus_frequency, address, CONVERSION_INTERVAL), + I2C(DRV_DIFF_PRESS_DEVTYPE_SDP33, MODULE_NAME, bus, address, bus_frequency), I2CSPIDriver(MODULE_NAME, px4::device_bus_to_wq(get_device_id()), bus_option, bus, address), _keep_retrying{keep_retrying} { } - virtual ~SDP3X() = default; + virtual ~SDP3X(); static I2CSPIDriverBase *instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, int runtime_instance); static void print_usage(); - void RunImpl(); + void RunImpl(); void start(); @@ -94,14 +93,13 @@ private: Running }; - int measure() override { return 0; } - int collect() override; + int measure() { return 0; } + int collect(); + int probe() override; int configure(); int read_scale(); - math::LowPassFilter2p _filter{SPD3X_MEAS_RATE, SDP3X_MEAS_DRIVER_FILTER_FREQ}; - bool init_sdp3x(); /** @@ -114,7 +112,17 @@ private: */ int write_command(uint16_t command); + bool _sensor_ok{false}; + int _measure_interval{0}; + bool _collect_phase{false}; + unsigned _conversion_interval{0}; + uint16_t _scale{0}; const bool _keep_retrying; State _state{State::RequireConfig}; + + uORB::PublicationMulti _differential_pressure_pub{ORB_ID(sensor_differential_pressure)}; + + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")}; + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com err")}; }; diff --git a/src/drivers/differential_pressure/sdp3x/SDP3X_main.cpp b/src/drivers/differential_pressure/sdp3x/sdp3x_main.cpp similarity index 94% rename from src/drivers/differential_pressure/sdp3x/SDP3X_main.cpp rename to src/drivers/differential_pressure/sdp3x/sdp3x_main.cpp index c24e5b6bd2..1c896e3540 100644 --- a/src/drivers/differential_pressure/sdp3x/SDP3X_main.cpp +++ b/src/drivers/differential_pressure/sdp3x/sdp3x_main.cpp @@ -33,8 +33,6 @@ #include "SDP3X.hpp" -extern "C" __EXPORT int sdp3x_airspeed_main(int argc, char *argv[]); - I2CSPIDriverBase *SDP3X::instantiate(const BusCLIArguments &cli, const BusInstanceIterator &iterator, int runtime_instance) { @@ -55,11 +53,9 @@ I2CSPIDriverBase *SDP3X::instantiate(const BusCLIArguments &cli, const BusInstan return instance; } - -void -SDP3X::print_usage() +void SDP3X::print_usage() { - PRINT_MODULE_USAGE_NAME("sdp3x_airspeed", "driver"); + PRINT_MODULE_USAGE_NAME("sdp3x", "driver"); PRINT_MODULE_USAGE_SUBCATEGORY("airspeed_sensor"); PRINT_MODULE_USAGE_COMMAND("start"); PRINT_MODULE_USAGE_PARAMS_I2C_SPI_DRIVER(true, false); @@ -68,8 +64,7 @@ SDP3X::print_usage() PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } -int -sdp3x_airspeed_main(int argc, char *argv[]) +extern "C" __EXPORT int sdp3x_main(int argc, char *argv[]) { using ThisDriver = SDP3X; BusCLIArguments cli{true, false}; diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h deleted file mode 100644 index 763cab3524..0000000000 --- a/src/drivers/drv_airspeed.h +++ /dev/null @@ -1,72 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file drv_airspeed.h - * - * Airspeed driver interface. - * - * @author Simon Wilks - */ - -#ifndef _DRV_AIRSPEED_H -#define _DRV_AIRSPEED_H - -#include -#include - -#include "drv_sensor.h" -#include "drv_orb_dev.h" - -#define AIRSPEED_BASE_DEVICE_PATH "/dev/airspeed" -#define AIRSPEED0_DEVICE_PATH "/dev/airspeed0" - -/* - * ioctl() definitions - * - * Airspeed drivers also implement the generic sensor driver - * interfaces from drv_sensor.h - */ - -#define _AIRSPEEDIOCBASE (0x7700) -#define __AIRSPEEDIOC(_n) (_PX4_IOC(_AIRSPEEDIOCBASE, _n)) - -#define AIRSPEEDIOCSSCALE __AIRSPEEDIOC(0) - -/** airspeed scaling factors; out = (in * Vscale) + offset */ -struct airspeed_scale { - float offset_pa; - float scale; -}; - -#endif /* _DRV_AIRSPEED_H */ diff --git a/src/drivers/uavcan/sensors/airspeed.cpp b/src/drivers/uavcan/sensors/airspeed.cpp index e9f170bdfc..7d894bc9b9 100644 --- a/src/drivers/uavcan/sensors/airspeed.cpp +++ b/src/drivers/uavcan/sensors/airspeed.cpp @@ -47,7 +47,7 @@ UavcanAirspeedBridge::UavcanAirspeedBridge(uavcan::INode &node) : _sub_ias_data(node), _sub_tas_data(node), _sub_oat_data(node) -{ } +{} int UavcanAirspeedBridge::init() { @@ -75,36 +75,26 @@ int UavcanAirspeedBridge::init() return 0; } -void -UavcanAirspeedBridge::oat_sub_cb(const - uavcan::ReceivedDataStructure &msg) +void UavcanAirspeedBridge::oat_sub_cb(const + uavcan::ReceivedDataStructure &msg) { _last_outside_air_temp_k = msg.static_temperature; } -void -UavcanAirspeedBridge::tas_sub_cb(const - uavcan::ReceivedDataStructure &msg) +void UavcanAirspeedBridge::tas_sub_cb(const uavcan::ReceivedDataStructure + &msg) { _last_tas_m_s = msg.true_airspeed; } -void -UavcanAirspeedBridge::ias_sub_cb(const - uavcan::ReceivedDataStructure &msg) + +void UavcanAirspeedBridge::ias_sub_cb(const + uavcan::ReceivedDataStructure &msg) { airspeed_s report{}; - - /* - * FIXME HACK - * This code used to rely on msg.getMonotonicTimestamp().toUSec() instead of HRT. - * It stopped working when the time sync feature has been introduced, because it caused libuavcan - * to use an independent time source (based on hardware TIM5) instead of HRT. - * The proper solution is to be developed. - */ - report.timestamp = hrt_absolute_time(); + report.timestamp_sample = hrt_absolute_time(); report.indicated_airspeed_m_s = msg.indicated_airspeed; report.true_airspeed_m_s = _last_tas_m_s; report.air_temperature_celsius = _last_outside_air_temp_k + CONSTANTS_ABSOLUTE_NULL_CELSIUS; - + report.timestamp = hrt_absolute_time(); publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/drivers/uavcan/sensors/airspeed.hpp b/src/drivers/uavcan/sensors/airspeed.hpp index c2f142d230..29f0194f06 100644 --- a/src/drivers/uavcan/sensors/airspeed.hpp +++ b/src/drivers/uavcan/sensors/airspeed.hpp @@ -38,7 +38,6 @@ #pragma once #include "sensor_bridge.hpp" -#include #include #include diff --git a/src/drivers/uavcan/sensors/differential_pressure.cpp b/src/drivers/uavcan/sensors/differential_pressure.cpp index 8ce01b2715..879feb89f7 100644 --- a/src/drivers/uavcan/sensors/differential_pressure.cpp +++ b/src/drivers/uavcan/sensors/differential_pressure.cpp @@ -37,7 +37,6 @@ #include "differential_pressure.hpp" -#include #include #include #include @@ -46,16 +45,13 @@ const char *const UavcanDifferentialPressureBridge::NAME = "differential_pressure"; UavcanDifferentialPressureBridge::UavcanDifferentialPressureBridge(uavcan::INode &node) : - UavcanSensorBridgeBase("uavcan_differential_pressure", ORB_ID(differential_pressure)), + UavcanSensorBridgeBase("uavcan_differential_pressure", ORB_ID(sensor_differential_pressure)), _sub_air(node) { } int UavcanDifferentialPressureBridge::init() { - // Initialize the calibration offset - param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset); - int res = _sub_air.start(AirCbBinder(this, &UavcanDifferentialPressureBridge::air_sub_cb)); if (res < 0) { @@ -72,17 +68,13 @@ void UavcanDifferentialPressureBridge::air_sub_cb(const _device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_UAVCAN; _device_id.devid_s.address = msg.getSrcNodeID().get() & 0xFF; - float diff_press_pa = msg.differential_pressure; - float temperature_c = msg.static_air_temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS; - - differential_pressure_s report = { - .timestamp = hrt_absolute_time(), - .error_count = 0, - .differential_pressure_raw_pa = diff_press_pa - _diff_pres_offset, - .differential_pressure_filtered_pa = _filter.apply(diff_press_pa) - _diff_pres_offset, /// TODO: Create filter - .temperature = temperature_c, - .device_id = _device_id.devid - }; + sensor_differential_pressure_s report{}; + report.timestamp_sample = hrt_absolute_time(); + report.device_id = get_device_id(); + report.differential_pressure_pa = msg.differential_pressure; + report.temperature = msg.static_air_temperature + CONSTANTS_ABSOLUTE_NULL_CELSIUS; + report.error_count = 0; + report.timestamp = hrt_absolute_time(); publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/drivers/uavcan/sensors/differential_pressure.hpp b/src/drivers/uavcan/sensors/differential_pressure.hpp index 5364940e17..2212a03d24 100644 --- a/src/drivers/uavcan/sensors/differential_pressure.hpp +++ b/src/drivers/uavcan/sensors/differential_pressure.hpp @@ -37,9 +37,7 @@ #pragma once -#include -#include -#include +#include #include "sensor_bridge.hpp" @@ -57,10 +55,6 @@ public: int init() override; private: - float _diff_pres_offset{0.f}; - - math::LowPassFilter2p _filter{10.f, 1.1f}; /// Adapted from MS5525 driver - void air_sub_cb(const uavcan::ReceivedDataStructure &msg); typedef uavcan::MethodBinder < UavcanDifferentialPressureBridge *, diff --git a/src/drivers/uavcannode/Publishers/RawAirData.hpp b/src/drivers/uavcannode/Publishers/RawAirData.hpp index 98035e3a46..fff008ede4 100644 --- a/src/drivers/uavcannode/Publishers/RawAirData.hpp +++ b/src/drivers/uavcannode/Publishers/RawAirData.hpp @@ -38,7 +38,7 @@ #include #include -#include +#include namespace uavcannode { @@ -51,7 +51,7 @@ class RawAirData : public: RawAirData(px4::WorkItem *work_item, uavcan::INode &node) : UavcanPublisherBase(uavcan::equipment::air_data::RawAirData::DefaultDataTypeID), - uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(differential_pressure)), + uORB::SubscriptionCallbackWorkItem(work_item, ORB_ID(sensor_differential_pressure)), uavcan::Publisher(node) {} @@ -68,13 +68,13 @@ public: void BroadcastAnyUpdates() override { // differential_pressure -> uavcan::equipment::air_data::RawAirData - differential_pressure_s diff_press; + sensor_differential_pressure_s diff_press; if (uORB::SubscriptionCallbackWorkItem::update(&diff_press)) { uavcan::equipment::air_data::RawAirData raw_air_data{}; // raw_air_data.static_pressure = - raw_air_data.differential_pressure = diff_press.differential_pressure_raw_pa; + raw_air_data.differential_pressure = diff_press.differential_pressure_pa; // raw_air_data.static_pressure_sensor_temperature = raw_air_data.differential_pressure_sensor_temperature = diff_press.temperature - CONSTANTS_ABSOLUTE_NULL_CELSIUS; raw_air_data.static_air_temperature = diff_press.temperature - CONSTANTS_ABSOLUTE_NULL_CELSIUS; diff --git a/src/lib/drivers/CMakeLists.txt b/src/lib/drivers/CMakeLists.txt index b74962c5b9..f288207028 100644 --- a/src/lib/drivers/CMakeLists.txt +++ b/src/lib/drivers/CMakeLists.txt @@ -32,7 +32,6 @@ ############################################################################ add_subdirectory(accelerometer) -add_subdirectory(airspeed) add_subdirectory(barometer) add_subdirectory(device) add_subdirectory(gyroscope) diff --git a/src/lib/drivers/airspeed/CMakeLists.txt b/src/lib/drivers/airspeed/CMakeLists.txt deleted file mode 100644 index 4161c5faca..0000000000 --- a/src/lib/drivers/airspeed/CMakeLists.txt +++ /dev/null @@ -1,35 +0,0 @@ -############################################################################ -# -# Copyright (c) 2015 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -px4_add_library(drivers__airspeed airspeed.cpp) -target_link_libraries(drivers__airspeed PRIVATE drivers__device) diff --git a/src/lib/drivers/airspeed/airspeed.cpp b/src/lib/drivers/airspeed/airspeed.cpp deleted file mode 100644 index 556f1ceda7..0000000000 --- a/src/lib/drivers/airspeed/airspeed.cpp +++ /dev/null @@ -1,130 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file airspeed.cpp - * @author Simon Wilks - * @author Lorenz Meier - * - */ - -#include -#include - -#include - -#include -#include -#include - -#include -#include - -#include -#include - -#include - -Airspeed::Airspeed(int bus, int bus_frequency, int address, unsigned conversion_interval) : - I2C(0, "Airspeed", bus, address, bus_frequency), - _sensor_ok(false), - _measure_interval(conversion_interval), - _collect_phase(false), - _diff_pres_offset(0.0f), - _airspeed_orb_class_instance(-1), - _class_instance(-1), - _conversion_interval(conversion_interval), - _sample_perf(perf_alloc(PC_ELAPSED, "aspd_read")), - _comms_errors(perf_alloc(PC_COUNT, "aspd_com_err")) -{ -} - -Airspeed::~Airspeed() -{ - if (_class_instance != -1) { - unregister_class_devname(AIRSPEED_BASE_DEVICE_PATH, _class_instance); - } - - // free perf counters - perf_free(_sample_perf); - perf_free(_comms_errors); -} - -int -Airspeed::init() -{ - /* do I2C init (and probe) first */ - if (I2C::init() != PX4_OK) { - return PX4_ERROR; - } - - /* register alternate interfaces if we have to */ - _class_instance = register_class_devname(AIRSPEED_BASE_DEVICE_PATH); - - /* advertise sensor topic, measure manually to initialize valid report */ - measure(); - - return PX4_OK; -} - -int -Airspeed::probe() -{ - /* on initial power up the device may need more than one retry - for detection. Once it is running the number of retries can - be reduced - */ - _retries = 4; - int ret = measure(); - - // drop back to 2 retries once initialised - _retries = 2; - return ret; -} - -int -Airspeed::ioctl(device::file_t *filp, int cmd, unsigned long arg) -{ - switch (cmd) { - case AIRSPEEDIOCSSCALE: { - struct airspeed_scale *s = (struct airspeed_scale *)arg; - _diff_pres_offset = s->offset_pa; - return OK; - } - - default: - /* give it to the superclass */ - return I2C::ioctl(filp, cmd, arg); - } -} - diff --git a/src/lib/drivers/airspeed/airspeed.h b/src/lib/drivers/airspeed/airspeed.h deleted file mode 100644 index f60a59e511..0000000000 --- a/src/lib/drivers/airspeed/airspeed.h +++ /dev/null @@ -1,87 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#pragma once - -#include -#include -#include -#include -#include -#include -#include -#include -#include - -class __EXPORT Airspeed : public device::I2C -{ -public: - Airspeed(int bus, int bus_frequency, int address, unsigned conversion_interval); - virtual ~Airspeed(); - - int init() override; - - int ioctl(device::file_t *filp, int cmd, unsigned long arg) override; - -private: - Airspeed(const Airspeed &) = delete; - Airspeed &operator=(const Airspeed &) = delete; - -protected: - int probe() override; - - /** - * Perform a poll cycle; collect from the previous measurement - * and start a new one. - */ - virtual int measure() = 0; - virtual int collect() = 0; - - bool _sensor_ok; - int _measure_interval; - bool _collect_phase; - float _diff_pres_offset; - - uORB::PublicationMulti _airspeed_pub{ORB_ID(differential_pressure)}; - - int _airspeed_orb_class_instance; - - int _class_instance; - - unsigned _conversion_interval; - - perf_counter_t _sample_perf; - perf_counter_t _comms_errors; -}; - - diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index 24266698f1..44d7e7bfbe 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -44,18 +44,16 @@ #include #include #include -#include -#include -#include -#include #include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include -static const char *sensor_name = "airspeed"; +using namespace time_literals; + +static constexpr char sensor_name[] {"airspeed"}; static void feedback_calibration_failed(orb_advert_t *mavlink_log_pub) { @@ -67,114 +65,58 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) { const hrt_abstime calibration_started = hrt_absolute_time(); - int result = PX4_OK; - unsigned calibration_counter = 0; - const unsigned maxcount = 2400; + static constexpr unsigned MAX_COUNT = 2400; + static constexpr unsigned CALIBRATION_COUNT = (MAX_COUNT * 2) / 3; /* give directions */ calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); - const unsigned calibration_count = (maxcount * 2) / 3; + uORB::SubscriptionBlocking diff_pres_sub{ORB_ID(sensor_differential_pressure)}; + sensor_differential_pressure_s diff_pres{}; + diff_pres_sub.copy(&diff_pres); - int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - struct differential_pressure_s diff_pres; + // store initial error count + uint32_t error_count = diff_pres.error_count; - float diff_pres_offset = 0.0f; + float diff_pres_total = 0.0f; + unsigned calibration_counter = 0; - /* Reset sensor parameters */ - struct airspeed_scale airscale = { - diff_pres_offset, - 1.0f, - }; - - bool paramreset_successful = false; - int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0); - - if (fd >= 0) { - if (PX4_OK == px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { - paramreset_successful = true; - - } else { - calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset zero failed"); - } - - px4_close(fd); - } - - if (!paramreset_successful) { - - /* only warn if analog scaling is zero */ - float analog_scaling = 0.0f; - param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); - - if (fabsf(analog_scaling) < 0.1f) { - calibration_log_critical(mavlink_log_pub, "[cal] No airspeed sensor found"); - goto error_return; - } - - /* set scaling offset parameter */ - if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG); - goto error_return; - } - } - - calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind"); px4_usleep(500 * 1000); + calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind"); - while (calibration_counter < calibration_count) { + while (calibration_counter < CALIBRATION_COUNT) { if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) { - goto error_return; + return PX4_ERROR; } - /* wait blocking for new data */ - px4_pollfd_struct_t fds[1]; - fds[0].fd = diff_pres_sub; - fds[0].events = POLLIN; + if (diff_pres_sub.updateBlocking(diff_pres, 1_s)) { - int poll_ret = px4_poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - - diff_pres_offset += diff_pres.differential_pressure_raw_pa; + diff_pres_total += diff_pres.differential_pressure_pa; calibration_counter++; /* any differential pressure failure a reason to abort */ - if (diff_pres.error_count != 0) { - calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor is reporting errors (%" PRIu64 ")", - diff_pres.error_count); + if (diff_pres.error_count > error_count) { + calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor is reporting errors (%d)", diff_pres.error_count); calibration_log_critical(mavlink_log_pub, "[cal] Check wiring, reboot vehicle, and try again"); feedback_calibration_failed(mavlink_log_pub); - goto error_return; + return PX4_ERROR; } - if (calibration_counter % (calibration_count / 20) == 0) { - calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / calibration_count); + if (calibration_counter % (CALIBRATION_COUNT / 20) == 0) { + calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, (calibration_counter * 80) / CALIBRATION_COUNT); } - } else if (poll_ret == 0) { + } else { /* any poll failure for 1s is a reason to abort */ feedback_calibration_failed(mavlink_log_pub); - goto error_return; + return PX4_ERROR; } } - diff_pres_offset = diff_pres_offset / calibration_count; + float diff_pres_offset = diff_pres_total / calibration_counter; - if (PX4_ISFINITE(diff_pres_offset)) { - - int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0); - airscale.offset_pa = diff_pres_offset; - - if (fd_scale >= 0) { - if (PX4_OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { - calibration_log_critical(mavlink_log_pub, "[cal] airspeed offset update failed"); - } - - px4_close(fd_scale); - } + if ((calibration_counter > 0) && PX4_ISFINITE(diff_pres_offset)) { // Prevent a completely zero param // since this is used to detect a missing calibration @@ -184,89 +126,80 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) diff_pres_offset = 0.00000001f; } - if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + if (param_set(param_find("SENS_DPRES_OFF"), &diff_pres_offset)) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG); - goto error_return; + return PX4_ERROR; } } else { feedback_calibration_failed(mavlink_log_pub); - goto error_return; + return PX4_ERROR; } - calibration_log_info(mavlink_log_pub, "[cal] Offset of %d Pascal", (int)diff_pres_offset); + calibration_log_info(mavlink_log_pub, "[cal] Offset of %.1f Pascal", (double)diff_pres_offset); - /* wait 500 ms to ensure parameter propagated through the system */ - px4_usleep(500 * 1000); + /* wait 100 ms to ensure parameter propagated through the system */ + px4_usleep(100 * 1000); calibration_log_critical(mavlink_log_pub, "[cal] Blow across front of pitot without touching"); + AlphaFilter diff_pres_filtered{0.1f}; calibration_counter = 0; /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ - while (calibration_counter < maxcount) { + while (calibration_counter < MAX_COUNT) { if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) { - goto error_return; + return PX4_ERROR; } - /* wait blocking for new data */ - px4_pollfd_struct_t fds[1]; - fds[0].fd = diff_pres_sub; - fds[0].events = POLLIN; + if (diff_pres_sub.updateBlocking(diff_pres, 1_s)) { - int poll_ret = px4_poll(fds, 1, 1000); + diff_pres_filtered.update(diff_pres.differential_pressure_pa - diff_pres_offset); - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - - if (fabsf(diff_pres.differential_pressure_filtered_pa) > 50.0f) { - if (diff_pres.differential_pressure_filtered_pa > 0) { - calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)", - (int)diff_pres.differential_pressure_filtered_pa); + if (fabsf(diff_pres_filtered.getState()) > 50.0f) { + if (diff_pres_filtered.getState() > 0) { + calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%.1f Pa)", (double)diff_pres_filtered.getState()); break; } else { /* do not allow negative values */ - calibration_log_critical(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)", - (int)diff_pres.differential_pressure_filtered_pa); + calibration_log_critical(mavlink_log_pub, "[cal] Negative pressure difference detected (%.1f Pa)", + (double)diff_pres_filtered.getState()); calibration_log_critical(mavlink_log_pub, "[cal] Swap static and dynamic ports!"); /* the user setup is wrong, wipe the calibration to force a proper re-calibration */ - diff_pres_offset = 0.0f; - - if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + if (param_reset(param_find("SENS_DPRES_OFF")) != PX4_OK) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG); - goto error_return; + return PX4_ERROR; } /* save */ calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0); - param_save_default(); feedback_calibration_failed(mavlink_log_pub); - goto error_return; + return PX4_ERROR; } } if (calibration_counter % 500 == 0) { - calibration_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %d, wanted: 50 Pa)", - (int)diff_pres.differential_pressure_filtered_pa); + calibration_log_info(mavlink_log_pub, "[cal] Create air pressure! (got %.1f, wanted: 50 Pa)", + (double)diff_pres_filtered.getState()); tune_neutral(true); } calibration_counter++; - } else if (poll_ret == 0) { + } else { /* any poll failure for 1s is a reason to abort */ feedback_calibration_failed(mavlink_log_pub); - goto error_return; + return PX4_ERROR; } } - if (calibration_counter == maxcount) { + if (calibration_counter == MAX_COUNT) { feedback_calibration_failed(mavlink_log_pub); - goto error_return; + return PX4_ERROR; } calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100); @@ -274,19 +207,5 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name); tune_neutral(true); - /* Wait 2sec for the airflow to stop and ensure the driver filter has caught up, otherwise - * the followup preflight checks might fail. */ - px4_usleep(2e6); - -normal_return: - px4_close(diff_pres_sub); - - // This give a chance for the log messages to go out of the queue before someone else stomps on then - px4_sleep(1); - - return result; - -error_return: - result = PX4_ERROR; - goto normal_return; + return PX4_OK; } diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 904979ee30..47bc8ba461 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -63,7 +63,7 @@ #include #include #include -#include +#include #include #include #include @@ -923,7 +923,7 @@ private: uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)}; uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)}; uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)}; - uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure)}; + uORB::Subscription _sensor_differential_pressure_sub{ORB_ID(sensor_differential_pressure)}; uORB::Subscription _magnetometer_sub{ORB_ID(vehicle_magnetometer)}; uORB::Subscription _air_data_sub{ORB_ID(vehicle_air_data)}; @@ -1026,14 +1026,14 @@ protected: _air_data_sub.copy(&air_data); } - differential_pressure_s differential_pressure{}; + sensor_differential_pressure_s differential_pressure{}; - if (_differential_pressure_sub.update(&differential_pressure)) { + if (_sensor_differential_pressure_sub.update(&differential_pressure)) { /* mark fourth group (dpres field) dimensions as changed */ fields_updated |= (1 << 10); } else { - _differential_pressure_sub.copy(&differential_pressure); + _sensor_differential_pressure_sub.copy(&differential_pressure); } const float accel_dt_inv = 1.e6f / (float)imu.delta_velocity_dt; @@ -1055,7 +1055,7 @@ protected: msg.ymag = mag(1); msg.zmag = mag(2); msg.abs_pressure = air_data.baro_pressure_pa; - msg.diff_pressure = differential_pressure.differential_pressure_raw_pa; + msg.diff_pressure = differential_pressure.differential_pressure_pa; msg.pressure_alt = air_data.baro_alt_meter; msg.temperature = air_data.baro_temp_celcius; msg.fields_updated = fields_updated; @@ -1089,7 +1089,7 @@ public: } private: - uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure)}; + uORB::Subscription _sensor_differential_pressure_sub{ORB_ID(sensor_differential_pressure)}; uORB::Subscription _sensor_baro_sub{ORB_ID(sensor_baro), N}; /* do not allow top copying this class */ @@ -1102,16 +1102,16 @@ protected: bool send() override { - if (_sensor_baro_sub.updated() || _differential_pressure_sub.updated()) { + if (_sensor_baro_sub.updated() || _sensor_differential_pressure_sub.updated()) { sensor_baro_s sensor_baro{}; - differential_pressure_s differential_pressure{}; + sensor_differential_pressure_s differential_pressure{}; _sensor_baro_sub.copy(&sensor_baro); - _differential_pressure_sub.copy(&differential_pressure); + _sensor_differential_pressure_sub.copy(&differential_pressure); typename Derived::mav_msg_type msg{}; msg.time_boot_ms = sensor_baro.timestamp / 1000; msg.press_abs = sensor_baro.pressure; - msg.press_diff = differential_pressure.differential_pressure_raw_pa; + msg.press_diff = differential_pressure.differential_pressure_pa; msg.temperature = sensor_baro.temperature; Derived::send(_mavlink->get_channel(), &msg); diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index ccb1a33180..078dc89333 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2345,12 +2345,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) // differential pressure if ((hil_sensor.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS) { - differential_pressure_s report{}; - report.timestamp = timestamp; + sensor_differential_pressure_s report{}; + report.timestamp_sample = timestamp; report.temperature = hil_sensor.temperature; - report.differential_pressure_filtered_pa = hil_sensor.diff_pressure * 100.0f; // convert from millibar to bar; - report.differential_pressure_raw_pa = hil_sensor.diff_pressure * 100.0f; // convert from millibar to bar; - + report.differential_pressure_pa = hil_sensor.diff_pressure * 100.0f; // convert from millibar to bar; + report.timestamp = hrt_absolute_time(); _differential_pressure_pub.publish(report); } diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h index 8922860d28..af015f9beb 100644 --- a/src/modules/mavlink/mavlink_receiver.h +++ b/src/modules/mavlink/mavlink_receiver.h @@ -63,7 +63,7 @@ #include #include #include -#include +#include #include #include #include @@ -356,7 +356,7 @@ private: uORB::Publication _battery_pub{ORB_ID(battery_status)}; uORB::Publication _cellular_status_pub{ORB_ID(cellular_status)}; uORB::Publication _collision_report_pub{ORB_ID(collision_report)}; - uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; + uORB::Publication _differential_pressure_pub{ORB_ID(sensor_differential_pressure)}; uORB::Publication _follow_target_pub{ORB_ID(follow_target)}; uORB::Publication _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)}; uORB::Publication _gimbal_manager_set_manual_control_pub{ORB_ID(gimbal_manager_set_manual_control)}; diff --git a/src/modules/sensors/airspeed/Airspeed.cpp b/src/modules/sensors/airspeed/Airspeed.cpp index 4e4f0814b5..c3d287edb1 100644 --- a/src/modules/sensors/airspeed/Airspeed.cpp +++ b/src/modules/sensors/airspeed/Airspeed.cpp @@ -34,11 +34,12 @@ #include "Airspeed.hpp" #include -#include #include #include +#include + namespace sensors { @@ -94,23 +95,6 @@ void Airspeed::ParametersUpdate(bool force) _parameter_update_sub.copy(¶m_update); updateParams(); - - /* update airspeed scale */ - int fd = px4_open(AIRSPEED0_DEVICE_PATH, 0); - - /* this sensor is optional, abort without error */ - if (fd >= 0) { - struct airspeed_scale airscale = { - _param_sens_dpres_off.get(), - 1.0f, - }; - - if (OK != px4_ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) { - PX4_ERR("failed to set offset for differential pressure sensor"); - } - - px4_close(fd); - } } } @@ -169,15 +153,17 @@ void Airspeed::Run() } if (_advertised[uorb_index]) { - differential_pressure_s diff_pres; + sensor_differential_pressure_s diff_pres; while (_sensor_sub[uorb_index].update(&diff_pres)) { updated[uorb_index] = true; _device_id[uorb_index] = diff_pres.device_id; - float vect[3] {diff_pres.differential_pressure_raw_pa, diff_pres.temperature, 0.f}; - _voter.put(uorb_index, diff_pres.timestamp, vect, diff_pres.error_count, _priority[uorb_index]); + float differential_pressure_pa = diff_pres.differential_pressure_pa - _param_sens_dpres_off.get(); + + float vect[3] {differential_pressure_pa, diff_pres.temperature, 0.f}; + _voter.put(uorb_index, diff_pres.timestamp_sample, vect, diff_pres.error_count, _priority[uorb_index]); float air_temperature_celsius = NAN; @@ -195,8 +181,8 @@ void Airspeed::Run() } // average raw data for all instances - _timestamp_sample_sum[uorb_index] += diff_pres.timestamp; - _differential_pressure_sum[uorb_index] += diff_pres.differential_pressure_filtered_pa; + _timestamp_sample_sum[uorb_index] += diff_pres.timestamp_sample; + _sensor_differential_pressure_sum[uorb_index] += differential_pressure_pa; _temperature_sum[uorb_index] += air_temperature_celsius; _sum_count[uorb_index]++; } @@ -288,13 +274,13 @@ void Airspeed::Publish(uint8_t instance, bool multi) if ((_param_sens_dpres_rate.get() > 0) && hrt_elapsed_time(&_last_publication_timestamp[instance]) >= (1e6f / _param_sens_dpres_rate.get())) { - const float differential_pressure = _differential_pressure_sum[instance] / _sum_count[instance]; + const float differential_pressure = _sensor_differential_pressure_sum[instance] / _sum_count[instance]; const float temperature = _temperature_sum[instance] / _sum_count[instance]; const hrt_abstime timestamp_sample = _timestamp_sample_sum[instance] / _sum_count[instance]; // reset _timestamp_sample_sum[instance] = 0; - _differential_pressure_sum[instance] = 0; + _sensor_differential_pressure_sum[instance] = 0; _temperature_sum[instance] = 0; _sum_count[instance] = 0; diff --git a/src/modules/sensors/airspeed/Airspeed.hpp b/src/modules/sensors/airspeed/Airspeed.hpp index 28c89df85d..7c5556dc21 100644 --- a/src/modules/sensors/airspeed/Airspeed.hpp +++ b/src/modules/sensors/airspeed/Airspeed.hpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include #include @@ -88,10 +88,10 @@ private: uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { - {this, ORB_ID(differential_pressure), 0}, - {this, ORB_ID(differential_pressure), 1}, - {this, ORB_ID(differential_pressure), 2}, - {this, ORB_ID(differential_pressure), 3}, + {this, ORB_ID(sensor_differential_pressure), 0}, + {this, ORB_ID(sensor_differential_pressure), 1}, + {this, ORB_ID(sensor_differential_pressure), 2}, + {this, ORB_ID(sensor_differential_pressure), 3}, }; perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; @@ -105,11 +105,11 @@ private: uint32_t _device_id[MAX_SENSOR_COUNT] {}; uint64_t _timestamp_sample_sum[MAX_SENSOR_COUNT] {0}; hrt_abstime _last_publication_timestamp[MAX_SENSOR_COUNT] {}; - float _differential_pressure_sum[MAX_SENSOR_COUNT] {}; + float _sensor_differential_pressure_sum[MAX_SENSOR_COUNT] {}; float _temperature_sum[MAX_SENSOR_COUNT] {}; int _sum_count[MAX_SENSOR_COUNT] {}; - differential_pressure_s _last_data[MAX_SENSOR_COUNT] {}; + sensor_differential_pressure_s _last_data[MAX_SENSOR_COUNT] {}; bool _advertised[MAX_SENSOR_COUNT] {}; uint8_t _priority[MAX_SENSOR_COUNT] {100, 100, 100, 100}; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 8ddaf12717..00babd9b53 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -42,7 +42,6 @@ */ #include -#include #include #include #include @@ -59,13 +58,12 @@ #include #include #include -#include -#include #include +#include #include #include -#include #include +#include #include "voted_sensors_update.h" #include "airspeed/Airspeed.hpp" @@ -82,7 +80,7 @@ using namespace time_literals; class Sensors : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: - explicit Sensors(bool hil_enabled); + explicit Sensors(); ~Sensors() override; /** @see ModuleBase */ @@ -103,8 +101,8 @@ public: bool init(); private: - const bool _hil_enabled; /**< if true, HIL is active */ - bool _armed{false}; /**< arming status of the vehicle */ + bool _armed{false}; /**< arming status of the vehicle */ + bool _hil_enabled{false}; /**< if true, HIL is active */ hrt_abstime _last_config_update{0}; hrt_abstime _sensor_combined_prev_timestamp{0}; @@ -120,38 +118,17 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _vcontrol_mode_sub{ORB_ID(vehicle_control_mode)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Publication _sensor_pub{ORB_ID(sensor_combined)}; perf_counter_t _loop_perf; /**< loop performance counter */ -#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL - - hrt_abstime _last_adc{0}; /**< last time we took input from the ADC */ - - uORB::Subscription _adc_report_sub{ORB_ID(adc_report)}; /**< adc_report sub */ - differential_pressure_s _diff_pres {}; - uORB::PublicationMulti _diff_pres_pub{ORB_ID(differential_pressure)}; /**< differential_pressure */ +#if defined(ADC_AIRSPEED_VOLTAGE_CHANNEL) + uORB::Subscription _adc_report_sub {ORB_ID(adc_report)}; + uORB::PublicationMulti _diff_pres_pub{ORB_ID(sensor_differential_pressure)}; /**< differential_pressure */ #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - - struct Parameters { - float diff_pres_offset_pa; -#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL - float diff_pres_analog_scale; -#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - - } _parameters{}; /**< local copies of interesting parameters */ - - struct ParameterHandles { - param_t diff_pres_offset_pa; -#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL - param_t diff_pres_analog_scale; -#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - - } _parameter_handles{}; /**< handles for interesting parameters */ - VotedSensorsUpdate _voted_sensors_update; VehicleAcceleration _vehicle_acceleration; @@ -164,11 +141,6 @@ private: VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {}; - /** - * Update our local parameter cache. - */ - int parameters_update(); - /** * Check for changes in parameters. */ @@ -189,25 +161,22 @@ private: void InitializeVehicleMagnetometer(); DEFINE_PARAMETERS( +#if defined(ADC_AIRSPEED_VOLTAGE_CHANNEL) + (ParamFloat) _param_sens_dpres_ansc, +#endif // ADC_AIRSPEED_VOLTAGE_CHANNEL + (ParamBool) _param_sys_has_baro, (ParamBool) _param_sys_has_mag, (ParamBool) _param_sens_imu_mode ) }; -Sensors::Sensors(bool hil_enabled) : +Sensors::Sensors() : ModuleParams(nullptr), ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), - _hil_enabled(hil_enabled), _loop_perf(perf_alloc(PC_ELAPSED, "sensors")), - _voted_sensors_update(hil_enabled, _vehicle_imu_sub) + _voted_sensors_update(_vehicle_imu_sub) { - /* Differential pressure offset */ - _parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF"); -#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL - _parameter_handles.diff_pres_analog_scale = param_find("SENS_DPRES_ANSC"); -#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - param_find("SYS_FAC_CAL_MODE"); // Parameters controlling the on-board sensor thermal calibrator @@ -266,25 +235,7 @@ bool Sensors::init() return true; } -int Sensors::parameters_update() -{ - if (_armed) { - return 0; - } - - /* Airspeed offset */ - param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa)); -#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL - param_get(_parameter_handles.diff_pres_analog_scale, &(_parameters.diff_pres_analog_scale)); -#endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ - - _voted_sensors_update.parametersUpdate(); - - return PX4_OK; -} - -void -Sensors::parameter_update_poll(bool forced) +void Sensors::parameter_update_poll(bool forced) { // check for parameter updates if (_parameter_update_sub.updated() || forced) { @@ -293,8 +244,11 @@ Sensors::parameter_update_poll(bool forced) _parameter_update_sub.copy(&pupdate); // update parameters from storage - parameters_update(); updateParams(); + + if (!_armed) { + _voted_sensors_update.parametersUpdate(); + } } } @@ -305,53 +259,36 @@ void Sensors::adc_poll() return; } -#ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL +#if defined(ADC_AIRSPEED_VOLTAGE_CHANNEL) - if (_parameters.diff_pres_analog_scale > 0.0f) { + if (_param_sens_dpres_ansc.get() > 0.0f) { + adc_report_s adc; - const hrt_abstime t = hrt_absolute_time(); + if (_adc_report_sub.update(&adc)) { + /* Read add channels we got */ + for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; i++) { + if (ADC_AIRSPEED_VOLTAGE_CHANNEL == adc.channel_id[i]) { + /* calculate airspeed, raw is the difference from */ + const float voltage = (float)(adc.raw_data[i]) * adc.v_ref / adc.resolution * ADC_DP_V_DIV; - /* rate limit to 100 Hz */ - if (t - _last_adc >= 10000) { - adc_report_s adc; - - if (_adc_report_sub.update(&adc)) { - /* Read add channels we got */ - for (unsigned i = 0; i < PX4_MAX_ADC_CHANNELS; i++) { - if (adc.channel_id[i] == -1) { - continue; // skip non-exist channels - } - - if (ADC_AIRSPEED_VOLTAGE_CHANNEL == adc.channel_id[i]) { - - /* calculate airspeed, raw is the difference from */ - const float voltage = (float)(adc.raw_data[i]) * adc.v_ref / adc.resolution * ADC_DP_V_DIV; - - /** - * The voltage divider pulls the signal down, only act on - * a valid voltage from a connected sensor. Also assume a non- - * zero offset from the sensor if its connected. - * - * Notice: This won't work on devices which have PGA controlled - * vref. Those devices require no divider at all. - */ - if (voltage > 0.4f) { - const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale - _parameters.diff_pres_offset_pa; - - _diff_pres.differential_pressure_raw_pa = diff_pres_pa_raw; - _diff_pres.differential_pressure_filtered_pa = (_diff_pres.differential_pressure_filtered_pa * 0.9f) + - (diff_pres_pa_raw * 0.1f); - _diff_pres.temperature = NAN; - - _diff_pres.timestamp = hrt_absolute_time(); - - _diff_pres_pub.publish(_diff_pres); - } + /** + * The voltage divider pulls the signal down, only act on + * a valid voltage from a connected sensor. Also assume a non- + * zero offset from the sensor if its connected. + * + * Notice: This won't work on devices which have PGA controlled + * vref. Those devices require no divider at all. + */ + if (voltage > 0.4f) { + sensor_differential_pressure_s diff_pres{}; + diff_pres.timestamp_sample = adc.timestamp; + diff_pres.differential_pressure_pa = voltage * _param_sens_dpres_ansc.get(); + diff_pres.temperature = NAN; + diff_pres.timestamp = hrt_absolute_time(); + _diff_pres_pub.publish(diff_pres); } } } - - _last_adc = t; } } @@ -361,7 +298,7 @@ void Sensors::adc_poll() void Sensors::InitializeAirspeed() { if (_airspeed == nullptr) { - if (orb_exists(ORB_ID(differential_pressure), 0) == PX4_OK) { + if (orb_exists(ORB_ID(sensor_differential_pressure), 0) == PX4_OK) { _airspeed = new Airspeed(); if (_airspeed) { @@ -483,11 +420,18 @@ void Sensors::Run() ScheduleDelayed(10_ms); // check vehicle status for changes to publication state - if (_vcontrol_mode_sub.updated()) { - vehicle_control_mode_s vcontrol_mode{}; + if (_vehicle_status_sub.updated()) { + vehicle_status_s vehicle_status; - if (_vcontrol_mode_sub.copy(&vcontrol_mode)) { - _armed = vcontrol_mode.flag_armed; + if (_vehicle_status_sub.copy(&vehicle_status)) { + const bool hil_prev = _hil_enabled; + + _armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED); + _hil_enabled = (vehicle_status.hil_state == vehicle_status_s::HIL_STATE_ON); + + if (!hil_prev && _hil_enabled) { + _voted_sensors_update.set_hil_enabled(); + } } } @@ -524,35 +468,7 @@ void Sensors::Run() int Sensors::task_spawn(int argc, char *argv[]) { - bool hil_enabled = false; - bool error_flag = false; - - int myoptind = 1; - int ch; - const char *myoptarg = nullptr; - - while ((ch = px4_getopt(argc, argv, "h", &myoptind, &myoptarg)) != EOF) { - switch (ch) { - case 'h': - hil_enabled = true; - break; - - case '?': - error_flag = true; - break; - - default: - PX4_WARN("unrecognized flag"); - error_flag = true; - break; - } - } - - if (error_flag) { - return PX4_ERROR; - } - - Sensors *instance = new Sensors(hil_enabled); + Sensors *instance = new Sensors(); if (instance) { _object.store(instance); @@ -649,7 +565,6 @@ It runs in its own thread and polls on the currently selected gyro topic. PRINT_MODULE_USAGE_NAME("sensors", "system"); PRINT_MODULE_USAGE_COMMAND("start"); - PRINT_MODULE_USAGE_PARAM_FLAG('h', "Start in HIL mode", true); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); return 0; diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index f1563f02e7..570254758d 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -46,18 +46,11 @@ using namespace sensors; using namespace matrix; -using namespace time_literals; -VotedSensorsUpdate::VotedSensorsUpdate(bool hil_enabled, - uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[MAX_SENSOR_COUNT]) : +VotedSensorsUpdate::VotedSensorsUpdate(uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[MAX_SENSOR_COUNT]) : ModuleParams(nullptr), - _vehicle_imu_sub(vehicle_imu_sub), - _hil_enabled(hil_enabled) + _vehicle_imu_sub(vehicle_imu_sub) { - if (_hil_enabled) { // HIL has less accurate timing so increase the timeouts a bit - _gyro.voter.set_timeout(500000); - _accel.voter.set_timeout(500000); - } } int VotedSensorsUpdate::init(sensor_combined_s &raw) @@ -259,7 +252,7 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) bool VotedSensorsUpdate::checkFailover(SensorData &sensor, const char *sensor_name) { - if (sensor.last_failover_count != sensor.voter.failover_count() && !_hil_enabled) { + if (sensor.last_failover_count != sensor.voter.failover_count()) { uint32_t flags = sensor.voter.failover_state(); int failover_index = sensor.voter.failover_index(); diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 750a3d5a2c..6b853c85c5 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -58,6 +58,8 @@ #include #include +using namespace time_literals; + namespace sensors { @@ -75,7 +77,7 @@ public: * @param parameters parameter values. These do not have to be initialized when constructing this object. * Only when calling init(), they have to be initialized. */ - VotedSensorsUpdate(bool hil_enabled, uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[MAX_SENSOR_COUNT]); + VotedSensorsUpdate(uORB::SubscriptionCallbackWorkItem(&vehicle_imu_sub)[MAX_SENSOR_COUNT]); /** * initialize subscriptions etc. @@ -107,6 +109,13 @@ public: */ void setRelativeTimestamps(sensor_combined_s &raw); + void set_hil_enabled() + { + // HIL has less accurate timing so increase the timeouts a bit + _gyro.voter.set_timeout(1_s); + _accel.voter.set_timeout(1_s); + } + private: static constexpr uint8_t DEFAULT_PRIORITY = 50; @@ -167,8 +176,6 @@ private: sensor_combined_s _last_sensor_data[MAX_SENSOR_COUNT] {}; /**< latest sensor data from all sensors instances */ - const bool _hil_enabled{false}; /**< is hardware-in-the-loop mode enabled? */ - bool _selection_changed{true}; /**< true when a sensor selection has changed and not been published */ matrix::Vector3f _accel_diff[MAX_SENSOR_COUNT] {}; /**< filtered accel differences between IMU units (m/s/s) */ diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index f128fd1e27..2b0c7875aa 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -58,7 +58,7 @@ #include #include #include -#include +#include #include #include #include @@ -189,7 +189,7 @@ private: perf_counter_t _perf_sim_interval{perf_alloc(PC_INTERVAL, MODULE_NAME": network interval")}; // uORB publisher handlers - uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; + uORB::Publication _differential_pressure_pub{ORB_ID(sensor_differential_pressure)}; uORB::PublicationMulti _flow_pub{ORB_ID(optical_flow)}; uORB::Publication _irlock_report_pub{ORB_ID(irlock_report)}; uORB::Publication _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index a1cce7617b..ee133a6fa8 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -265,12 +265,11 @@ void Simulator::update_sensors(const hrt_abstime &time, const mavlink_hil_sensor // differential pressure if ((sensors.fields_updated & SensorSource::DIFF_PRESS) == SensorSource::DIFF_PRESS && !_airspeed_blocked) { - differential_pressure_s report{}; - report.timestamp = time; + sensor_differential_pressure_s report{}; + report.timestamp_sample = time; report.temperature = _sensors_temperature; - report.differential_pressure_filtered_pa = sensors.diff_pressure * 100.0f; // convert from millibar to bar; - report.differential_pressure_raw_pa = sensors.diff_pressure * 100.0f; // convert from millibar to bar; - + report.differential_pressure_pa = sensors.diff_pressure * 100.0f; // convert from millibar to bar; + report.timestamp = hrt_absolute_time(); _differential_pressure_pub.publish(report); } }