diff --git a/boards/agam/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld b/boards/agam/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld index 31d150558a2..c0a5fd419db 100644 --- a/boards/agam/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld +++ b/boards/agam/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld @@ -302,7 +302,7 @@ *(.text.imxrt_progressep) *(.text.imxrt_gpio_configinput) *(.text._ZN17FlightModeManager26generateTrajectorySetpointEfRK24vehicle_local_position_s) -*(.text._ZN7Sensors14diff_pres_pollEv) +*(.text._ZN7sensors15VehicleAirspeed3RunEv) *(.text._ZN21MavlinkStreamAttitude4sendEv) *(.text._ZN4EKF220UpdateMagCalibrationERKy) *(.text._ZN22MavlinkStreamEfiStatus8get_sizeEv) diff --git a/boards/ark/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld b/boards/ark/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld index dda597af5e7..0fd83a5558d 100644 --- a/boards/ark/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld +++ b/boards/ark/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld @@ -294,7 +294,7 @@ *(.text.imxrt_progressep) *(.text.imxrt_gpio_configinput) *(.text._ZN17FlightModeManager26generateTrajectorySetpointEfRK24vehicle_local_position_s) -*(.text._ZN7Sensors14diff_pres_pollEv) +*(.text._ZN7sensors15VehicleAirspeed3RunEv) *(.text._ZN21MavlinkStreamAttitude4sendEv) *(.text._ZN4EKF220UpdateMagCalibrationERKy) *(.text._ZN22MavlinkStreamEfiStatus8get_sizeEv) diff --git a/boards/nxp/mr-tropic/nuttx-config/scripts/itcm_functions_includes.ld b/boards/nxp/mr-tropic/nuttx-config/scripts/itcm_functions_includes.ld index 30277734c38..73631ca8be7 100644 --- a/boards/nxp/mr-tropic/nuttx-config/scripts/itcm_functions_includes.ld +++ b/boards/nxp/mr-tropic/nuttx-config/scripts/itcm_functions_includes.ld @@ -298,7 +298,7 @@ *(.text.imxrt_progressep) *(.text.imxrt_gpio_configinput) *(.text._ZN17FlightModeManager26generateTrajectorySetpointEfRK24vehicle_local_position_s) -*(.text._ZN7Sensors14diff_pres_pollEv) +*(.text._ZN7sensors15VehicleAirspeed3RunEv) *(.text._ZN21MavlinkStreamAttitude4sendEv) *(.text._ZN4EKF220UpdateMagCalibrationERKy) *(.text._ZN22MavlinkStreamEfiStatus8get_sizeEv) diff --git a/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld b/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld index 80d81bb197c..497830f9aa5 100644 --- a/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld +++ b/boards/nxp/tropic-community/nuttx-config/scripts/itcm_functions_includes.ld @@ -298,7 +298,7 @@ *(.text.imxrt_progressep) *(.text.imxrt_gpio_configinput) *(.text._ZN17FlightModeManager26generateTrajectorySetpointEfRK24vehicle_local_position_s) -*(.text._ZN7Sensors14diff_pres_pollEv) +*(.text._ZN7sensors15VehicleAirspeed3RunEv) *(.text._ZN21MavlinkStreamAttitude4sendEv) *(.text._ZN4EKF220UpdateMagCalibrationERKy) *(.text._ZN22MavlinkStreamEfiStatus8get_sizeEv) diff --git a/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld b/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld index 31d150558a2..c0a5fd419db 100644 --- a/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld +++ b/boards/px4/fmu-v6xrt/nuttx-config/scripts/itcm_functions_includes.ld @@ -302,7 +302,7 @@ *(.text.imxrt_progressep) *(.text.imxrt_gpio_configinput) *(.text._ZN17FlightModeManager26generateTrajectorySetpointEfRK24vehicle_local_position_s) -*(.text._ZN7Sensors14diff_pres_pollEv) +*(.text._ZN7sensors15VehicleAirspeed3RunEv) *(.text._ZN21MavlinkStreamAttitude4sendEv) *(.text._ZN4EKF220UpdateMagCalibrationERKy) *(.text._ZN22MavlinkStreamEfiStatus8get_sizeEv) diff --git a/msg/Airspeed.msg b/msg/Airspeed.msg index 3dd62d9c6f1..f7b4bffacbf 100644 --- a/msg/Airspeed.msg +++ b/msg/Airspeed.msg @@ -5,6 +5,7 @@ uint64 timestamp # [us] Time since system start uint64 timestamp_sample # [us] Timestamp of the raw data +uint32 device_id # [-] Unique device ID of the sensor this measurement came from, 0 if unknown float32 indicated_airspeed_m_s # [m/s] Indicated airspeed float32 true_airspeed_m_s # [m/s] True airspeed float32 confidence # [@range 0,1] Confidence value for this sensor diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 66281d6d571..5f888a1b30a 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -136,6 +136,7 @@ #define DRV_BARO_DEVTYPE_MPL3115A2 0x51 #define DRV_ACC_DEVTYPE_FXOS8701C 0x52 #define DRV_DIFF_PRESS_DEVTYPE_DLVR 0x53 +#define DRV_DIFF_PRESS_DEVTYPE_ANALOG 0x55 #define DRV_GYR_DEVTYPE_FXAS2100C 0x54 #define DRV_IMU_DEVTYPE_ADIS16448 0x57 diff --git a/src/drivers/uavcan/sensors/airspeed.cpp b/src/drivers/uavcan/sensors/airspeed.cpp index 32da7d3f1a3..1a16abb1482 100644 --- a/src/drivers/uavcan/sensors/airspeed.cpp +++ b/src/drivers/uavcan/sensors/airspeed.cpp @@ -102,6 +102,7 @@ UavcanAirspeedBridge::ias_sub_cb(const * The proper solution is to be developed. */ report.timestamp = hrt_absolute_time(); + report.device_id = make_uavcan_device_id(msg); report.indicated_airspeed_m_s = msg.indicated_airspeed; report.true_airspeed_m_s = _last_tas_m_s; diff --git a/src/lib/sensor_calibration/CMakeLists.txt b/src/lib/sensor_calibration/CMakeLists.txt index c85bde638e7..1227f5884f8 100644 --- a/src/lib/sensor_calibration/CMakeLists.txt +++ b/src/lib/sensor_calibration/CMakeLists.txt @@ -36,6 +36,8 @@ px4_add_library(sensor_calibration Accelerometer.hpp Barometer.cpp Barometer.hpp + DifferentialPressure.cpp + DifferentialPressure.hpp Gyroscope.cpp Gyroscope.hpp Magnetometer.cpp diff --git a/src/lib/sensor_calibration/DifferentialPressure.cpp b/src/lib/sensor_calibration/DifferentialPressure.cpp new file mode 100644 index 00000000000..2d0d8d48e4b --- /dev/null +++ b/src/lib/sensor_calibration/DifferentialPressure.cpp @@ -0,0 +1,221 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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. + * + ****************************************************************************/ + +#include "DifferentialPressure.hpp" + +#include "Utilities.hpp" + +#include +#include + +#include + +namespace calibration +{ + +DifferentialPressure::DifferentialPressure() +{ + Reset(); +} + +DifferentialPressure::DifferentialPressure(uint32_t device_id) +{ + set_device_id(device_id); +} + +void DifferentialPressure::set_device_id(uint32_t device_id) +{ + const bool external = device::device_is_external(device_id); + + if (_device_id != device_id || _external != external) { + + _device_id = device_id; + _external = external; + + Reset(); + + ParametersUpdate(); + } +} + +bool DifferentialPressure::set_offset(const float &offset) +{ + if (fabsf(_offset - offset) > 0.01f) { + if (PX4_ISFINITE(offset)) { + _offset = offset; + _calibration_count++; + return true; + } + } + + return false; +} + +bool DifferentialPressure::set_calibration_index(int calibration_index) +{ + if ((calibration_index >= 0) && (calibration_index < MAX_SENSOR_COUNT)) { + _calibration_index = calibration_index; + return true; + } + + return false; +} + +void DifferentialPressure::ParametersUpdate() +{ + if (_device_id == 0) { + return; + } + + _calibration_index = FindCurrentCalibrationIndex(SensorString(), _device_id); + + if (_calibration_index == -1) { + // no saved calibration available + Reset(); + + } else { + ParametersLoad(); + } +} + +bool DifferentialPressure::ParametersLoad() +{ + if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) { + // CAL_DPRESx_OFF + set_offset(GetCalibrationParamFloat(SensorString(), "OFF", _calibration_index)); + + return true; + } + + return false; +} + +bool DifferentialPressure::AdoptLegacyOffset() +{ + if ((_device_id == 0) || calibrated()) { + return false; + } + + // slot 0 must still be free, otherwise another sensor already owns the legacy offset + if (GetCalibrationParamInt32(SensorString(), "ID", kLegacyIndex) != 0) { + return false; + } + + float legacy_offset = 0.f; + + if (param_get(param_find(kLegacyOffsetParam), &legacy_offset) != PX4_OK) { + return false; + } + + if (!PX4_ISFINITE(legacy_offset) || (fabsf(legacy_offset) < FLT_EPSILON)) { + return false; + } + + _offset = legacy_offset; + _calibration_index = kLegacyIndex; + + PX4_INFO("%s %" PRIu32 " adopted %s %.3f Pa", SensorString(), _device_id, kLegacyOffsetParam, (double)_offset); + + return true; +} + +void DifferentialPressure::UpdateLegacyOffset() const +{ + // keep SENS_DPRES_OFF in step with slot 0 so that ground stations reading it can still tell + // whether airspeed has been calibrated + if (_calibration_index != kLegacyIndex) { + return; + } + + const param_t handle = param_find(kLegacyOffsetParam); + + if (handle == PARAM_INVALID) { + return; + } + + float offset = _offset; + param_set_no_notification(handle, &offset); +} + +void DifferentialPressure::Reset() +{ + _offset = 0; + + _calibration_index = -1; + + _calibration_count = 0; +} + +bool DifferentialPressure::ParametersSave(int desired_calibration_index, bool force) +{ + if (force && desired_calibration_index >= 0 && desired_calibration_index < MAX_SENSOR_COUNT) { + _calibration_index = desired_calibration_index; + + } else if (!force || (_calibration_index < 0) + || (desired_calibration_index != -1 && desired_calibration_index != _calibration_index)) { + + // ensure we have a valid calibration slot (matching existing or first available slot) + int8_t calibration_index_prev = _calibration_index; + _calibration_index = FindAvailableCalibrationIndex(SensorString(), _device_id, desired_calibration_index); + + if (calibration_index_prev >= 0 && (calibration_index_prev != _calibration_index)) { + PX4_WARN("%s %" PRIu32 " calibration index changed %" PRIi8 " -> %" PRIi8, SensorString(), _device_id, + calibration_index_prev, _calibration_index); + } + } + + if (_calibration_index >= 0 && _calibration_index < MAX_SENSOR_COUNT) { + // save calibration + bool success = true; + success &= SetCalibrationParam(SensorString(), "ID", _calibration_index, _device_id); + success &= SetCalibrationParam(SensorString(), "OFF", _calibration_index, _offset); + + UpdateLegacyOffset(); + + return success; + } + + // index 3 is possible: FindAvailableCalibrationIndex() searches four slots, we define three + PX4_ERR("%s %" PRIu32 " no free calibration slot (got %" PRIi8 ")", SensorString(), _device_id, _calibration_index); + + return false; +} + +void DifferentialPressure::PrintStatus() +{ + PX4_INFO_RAW("%s %" PRIu32 " offset: %05.3f Pa, %s\n", + SensorString(), device_id(), (double)_offset, + external() ? "Ext" : "Internal"); +} + +} // namespace calibration diff --git a/src/lib/sensor_calibration/DifferentialPressure.hpp b/src/lib/sensor_calibration/DifferentialPressure.hpp new file mode 100644 index 00000000000..0d187bc635e --- /dev/null +++ b/src/lib/sensor_calibration/DifferentialPressure.hpp @@ -0,0 +1,104 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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 + +namespace calibration +{ +class DifferentialPressure +{ +public: + static constexpr int MAX_SENSOR_COUNT = 3; ///< also the airspeed selector's sensor limit + + static constexpr const char *SensorString() { return "DPRES"; } + + DifferentialPressure(); + explicit DifferentialPressure(uint32_t device_id); + + ~DifferentialPressure() = default; + + void PrintStatus(); + + bool set_calibration_index(int calibration_index); + void set_device_id(uint32_t device_id); + bool set_offset(const float &offset); + + bool calibrated() const { return (_device_id != 0) && (_calibration_index >= 0); } + uint8_t calibration_count() const { return _calibration_count; } + int8_t calibration_index() const { return _calibration_index; } + uint32_t device_id() const { return _device_id; } + bool external() const { return _external; } + const float &offset() const { return _offset; } + + /// remove the zero offset from a raw differential pressure reading [Pa] + inline float Correct(const float &data) const { return data - _offset; } + + inline float Uncorrect(const float &corrected_data) const { return corrected_data + _offset; } + + bool ParametersLoad(); + bool ParametersSave(int desired_calibration_index = -1, bool force = false); + void ParametersUpdate(); + + /** + * Take over the offset left in SENS_DPRES_OFF, using slot 0. + * Does nothing if slot 0 is already claimed. + * + * @return true if the offset was adopted (the caller should save) + */ + bool AdoptLegacyOffset(); + + /** mirror slot 0's offset into SENS_DPRES_OFF for ground stations that still read it */ + void UpdateLegacyOffset() const; + + void Reset(); + +private: + static constexpr int kLegacyIndex{0}; + static constexpr const char *kLegacyOffsetParam{"SENS_DPRES_OFF"}; + + float _offset{0}; + + int8_t _calibration_index{-1}; + uint32_t _device_id{0}; + uint8_t _calibration_count{0}; + + bool _external{false}; +}; +} // namespace calibration diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index b9ad184f9d2..63b93ea6720 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -43,6 +43,7 @@ #include #include #include +#include #include #include @@ -104,7 +105,7 @@ private: void Run() override; - static constexpr int MAX_NUM_AIRSPEED_SENSORS = 3; /**< Support max 3 airspeed sensors */ + static constexpr int MAX_NUM_AIRSPEED_SENSORS = calibration::DifferentialPressure::MAX_SENSOR_COUNT; enum class AirspeedSource : int { DISABLED = -1, GROUND_MINUS_WIND, diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp index f68e9c807bf..a63598cc89c 100644 --- a/src/modules/commander/airspeed_calibration.cpp +++ b/src/modules/commander/airspeed_calibration.cpp @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -65,25 +66,27 @@ static void feedback_calibration_failed(orb_advert_t *mavlink_log_pub) calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); } -int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) +static constexpr int MAX_SENSORS = calibration::DifferentialPressure::MAX_SENSOR_COUNT; + +struct AirspeedSensor { + uORB::SubscriptionData sub{ORB_ID(differential_pressure)}; + uint32_t device_id{0}; + float offset{0.f}; + int instance{0}; + bool connected{false}; +}; + +/** + * Collect the zero reading of every connected sensor simultaneously. + */ +static int collect_zero_offsets(orb_advert_t *mavlink_log_pub, const hrt_abstime &calibration_started, + AirspeedSensor sensors[MAX_SENSORS], int num_sensors) { - const hrt_abstime calibration_started = hrt_absolute_time(); + static constexpr unsigned calibration_count = (500 * 2) / 3; - int result = PX4_OK; unsigned calibration_counter = 0; - const unsigned maxcount = 500; - - /* give directions */ - calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); - - const unsigned calibration_count = (maxcount * 2) / 3; - - float diff_pres_offset = 0.0f; - - calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind"); - px4_usleep(500 * 1000); - - uORB::SubscriptionData diff_pres_sub{ORB_ID(differential_pressure)}; + float sums[MAX_SENSORS] {}; + unsigned counts[MAX_SENSORS] {}; while (calibration_counter < calibration_count) { @@ -91,11 +94,17 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) return PX4_ERROR; } - if (diff_pres_sub.update()) { + bool any_update = false; - const differential_pressure_s &diff_pres = diff_pres_sub.get(); + for (int i = 0; i < num_sensors; i++) { + if (sensors[i].sub.update()) { + sums[i] += sensors[i].sub.get().differential_pressure_pa; + counts[i]++; + any_update = true; + } + } - diff_pres_offset += diff_pres.differential_pressure_pa; + if (any_update) { calibration_counter++; if (calibration_counter % (calibration_count / 20) == 0) { @@ -111,77 +120,97 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) px4_usleep(10000); } - diff_pres_offset = diff_pres_offset / calibration_count; - - if (PX4_ISFINITE(diff_pres_offset)) { - // Prevent a completely zero param - // since this is used to detect a missing calibration - // This value is numerically down in the noise and has - // no effect on the sensor performance. - if (fabsf(diff_pres_offset) < 0.00000001f) { - diff_pres_offset = 0.00000001f; - } - - if (param_set(param_find("SENS_DPRES_OFF"), &diff_pres_offset)) { - calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG); + for (int i = 0; i < num_sensors; i++) { + if (counts[i] == 0) { + calibration_log_critical(mavlink_log_pub, "[cal] Airspeed sensor %d stopped reporting", i + 1); + feedback_calibration_failed(mavlink_log_pub); return PX4_ERROR; } - } else { - feedback_calibration_failed(mavlink_log_pub); - return PX4_ERROR; + sensors[i].offset = sums[i] / counts[i]; + + if (!PX4_ISFINITE(sensors[i].offset)) { + feedback_calibration_failed(mavlink_log_pub); + return PX4_ERROR; + } } - calibration_log_info(mavlink_log_pub, "[cal] Offset of %d Pascal", (int)diff_pres_offset); + return PX4_OK; +} - /* wait 500 ms to ensure parameter propagated through the system */ - px4_usleep(500 * 1000); +/** Store (or clear, with an offset of 0) the zero offset of one sensor. */ +static bool save_offset(uint32_t device_id, float offset, int instance) +{ + calibration::DifferentialPressure calibration{device_id}; + calibration.set_offset(offset); - calibration_log_critical(mavlink_log_pub, "[cal] Blow into front of pitot without touching"); + // claim the slot matching the sensor instance, as the other sensor calibrations do, so that + // recalibrating reuses a slot instead of consuming a new one every time + return calibration.ParametersSave(instance, true); +} + +/** + * Only the sensor that failed is cleared + */ +static void clear_offset(const AirspeedSensor &sensor) +{ + save_offset(sensor.device_id, 0.f, sensor.instance); + + param_notify_changes(); +} + +/** + * Verify that positive pressure reaches the sensor, which catches swapped static and dynamic + * ports. Each sensor has its own pitot, so this has to be repeated for each of them. + */ +static int check_pitot_direction(orb_advert_t *mavlink_log_pub, const hrt_abstime &calibration_started, + AirspeedSensor sensors[MAX_SENSORS], int num_sensors, int index) +{ + static constexpr unsigned maxcount = 500; + + AirspeedSensor &sensor = sensors[index]; + + if (num_sensors > 1) { + calibration_log_critical(mavlink_log_pub, "[cal] Blow into front of pitot %d without touching", index + 1); + + } else { + calibration_log_critical(mavlink_log_pub, "[cal] Blow into front of pitot without touching"); + } + + const hrt_abstime sensor_started = hrt_absolute_time(); float differential_pressure_sum = 0.f; int differential_pressure_sum_count = 0; + unsigned calibration_counter = 0; - calibration_counter = 0; - - /* just take a few samples and make sure pitot tubes are not reversed, timeout after ~30 seconds */ while (calibration_counter < maxcount) { if (calibrate_cancel_check(mavlink_log_pub, calibration_started)) { return PX4_ERROR; } - if (diff_pres_sub.update()) { - const differential_pressure_s &diff_pres = diff_pres_sub.get(); - - differential_pressure_sum += diff_pres.differential_pressure_pa; + if (sensor.sub.update()) { + differential_pressure_sum += sensor.sub.get().differential_pressure_pa; differential_pressure_sum_count++; - const float differential_pressure_pa = (differential_pressure_sum / differential_pressure_sum_count) - diff_pres_offset; + const float differential_pressure_pa = (differential_pressure_sum / differential_pressure_sum_count) - + sensor.offset; if ((differential_pressure_sum_count > 10) && (fabsf(differential_pressure_pa) > 50.f)) { if (differential_pressure_pa > 0) { calibration_log_info(mavlink_log_pub, "[cal] Positive pressure: OK (%d Pa)", (int)differential_pressure_pa); - break; - - } else { - /* do not allow negative values */ - calibration_log_critical(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)", - (int)differential_pressure_pa); - calibration_log_critical(mavlink_log_pub, "[cal] Swap static and dynamic ports or set SENS_DPRES_REV"); - - /* 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))) { - calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG); - return PX4_ERROR; - } - - calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0); - feedback_calibration_failed(mavlink_log_pub); - return PX4_ERROR; + return PX4_OK; } + + /* do not allow negative values */ + calibration_log_critical(mavlink_log_pub, "[cal] Negative pressure difference detected (%d Pa)", + (int)differential_pressure_pa); + calibration_log_critical(mavlink_log_pub, "[cal] Swap static and dynamic ports or set SENS_DPRES_REV"); + + clear_offset(sensor); + calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0); + feedback_calibration_failed(mavlink_log_pub); + return PX4_ERROR; } if (calibration_counter % 300 == 0) { @@ -197,14 +226,8 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) calibration_counter++; } - if (hrt_elapsed_time(&calibration_started) > 90_s) { - diff_pres_offset = 0.0f; - - if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG); - return PX4_ERROR; - } - + if (hrt_elapsed_time(&sensor_started) > 90_s) { + clear_offset(sensor); calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0); feedback_calibration_failed(mavlink_log_pub); return PX4_ERROR; @@ -213,17 +236,70 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) px4_usleep(10000); } - if (calibration_counter == maxcount) { - diff_pres_offset = 0.0f; + clear_offset(sensor); + calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0); + feedback_calibration_failed(mavlink_log_pub); + return PX4_ERROR; +} - if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { +int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) +{ + const hrt_abstime calibration_started = hrt_absolute_time(); + + /* give directions */ + calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); + + // find every connected differential pressure sensor. + AirspeedSensor sensors[MAX_SENSORS] {}; + int num_sensors = 0; + + for (int i = 0; i < MAX_SENSORS; i++) { + uORB::SubscriptionData sub{ORB_ID(differential_pressure), (uint8_t)i}; + + if (sub.advertised() && sub.get().timestamp != 0) { + sensors[num_sensors].sub.ChangeInstance(i); + sensors[num_sensors].instance = i; + sensors[num_sensors].device_id = sub.get().device_id; + sensors[num_sensors].connected = true; + num_sensors++; + } + } + + if (num_sensors == 0) { + calibration_log_critical(mavlink_log_pub, "[cal] No airspeed sensor found"); + feedback_calibration_failed(mavlink_log_pub); + return PX4_ERROR; + } + + if (num_sensors > 1) { + calibration_log_info(mavlink_log_pub, "[cal] Calibrating %d airspeed sensors", num_sensors); + } + + calibration_log_critical(mavlink_log_pub, "[cal] Ensure sensor is not measuring wind"); + px4_usleep(500 * 1000); + + if (collect_zero_offsets(mavlink_log_pub, calibration_started, sensors, num_sensors) != PX4_OK) { + return PX4_ERROR; + } + + for (int i = 0; i < num_sensors; i++) { + if (!save_offset(sensors[i].device_id, sensors[i].offset, sensors[i].instance)) { calibration_log_critical(mavlink_log_pub, CAL_ERROR_SET_PARAMS_MSG); return PX4_ERROR; } - calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 0); - feedback_calibration_failed(mavlink_log_pub); - return PX4_ERROR; + calibration_log_info(mavlink_log_pub, "[cal] Sensor %d offset of %d Pascal", i + 1, (int)sensors[i].offset); + } + + param_notify_changes(); + + /* wait 500 ms to ensure parameter propagated through the system */ + px4_usleep(500 * 1000); + + for (int i = 0; i < num_sensors; i++) { + if (check_pitot_direction(mavlink_log_pub, calibration_started, sensors, num_sensors, i) != PX4_OK) { + return PX4_ERROR; + } } calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100); @@ -234,5 +310,5 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub) // This give a chance for the log messages to go out of the queue before someone else stomps on then px4_usleep(200000); - return result; + return PX4_OK; } diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 73e611241a6..b1c748ee532 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -48,7 +48,6 @@ void LoggedTopics::add_default_topics() add_topic("action_request"); add_topic("actuator_armed"); add_optional_topic("actuator_controls_status_0", 300); - add_topic("airspeed", 1000); add_optional_topic("airspeed_validated", 200); add_optional_topic("autotune_attitude_control_status", 100); add_topic_multi("battery_info", 5000, 3); @@ -170,6 +169,7 @@ void LoggedTopics::add_default_topics() // multi topics add_optional_topic_multi("actuator_outputs", 100, 3); + add_topic_multi("airspeed", 1000, 3); add_optional_topic_multi("airspeed_wind", 1000, 4); add_optional_topic_multi("control_allocator_status", 200, 2); add_topic_multi("fuel_tank_status", 200, 3); @@ -238,7 +238,7 @@ void LoggedTopics::add_default_topics() // log all raw sensors at minimal rate (at least 1 Hz) add_topic_multi("battery_status", 200, 3); - add_topic_multi("differential_pressure", 1000, 2); + add_topic_multi("differential_pressure", 1000, 3); add_topic_multi("distance_sensor", 1000, 2); add_optional_topic_multi("sensor_accel", 1000, 4); add_topic_multi("sensor_baro", 1000, 4); diff --git a/src/modules/sensors/CMakeLists.txt b/src/modules/sensors/CMakeLists.txt index 12d56683cbe..6b01a117e53 100644 --- a/src/modules/sensors/CMakeLists.txt +++ b/src/modules/sensors/CMakeLists.txt @@ -45,6 +45,10 @@ if(CONFIG_SENSORS_VEHICLE_AIR_DATA) add_subdirectory(vehicle_air_data) endif() +if(CONFIG_SENSORS_VEHICLE_AIRSPEED) + add_subdirectory(vehicle_airspeed) +endif() + if(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY) add_subdirectory(vehicle_angular_velocity) endif() @@ -99,7 +103,7 @@ if(CONFIG_SENSORS_VEHICLE_AIR_DATA) endif() if(CONFIG_SENSORS_VEHICLE_AIRSPEED) - target_link_libraries(modules__sensors PRIVATE airspeed) + target_link_libraries(modules__sensors PRIVATE vehicle_airspeed) endif() if(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY) diff --git a/src/modules/sensors/module.yaml b/src/modules/sensors/module.yaml index 901d2dab1aa..43f76fe13a6 100644 --- a/src/modules/sensors/module.yaml +++ b/src/modules/sensors/module.yaml @@ -210,6 +210,29 @@ parameters: num_instances: *max_num_sensor_instances instance_start: 0 + # Differential pressure (airspeed) calibration + CAL_DPRES${i}_ID: + description: + short: Differential pressure sensor ${i} calibration device ID + long: Device ID of the differential pressure sensor this calibration applies to. + category: System + type: int32 + default: 0 + num_instances: 3 + instance_start: 0 + + CAL_DPRES${i}_OFF: + description: + short: Differential pressure sensor ${i} offset + long: The zero-reading of this differential pressure sensor, in Pascal. + category: System + type: float + default: 0.0 + decimal: 3 + volatile: true + num_instances: 3 + instance_start: 0 + # Gyroscope calibration CAL_GYRO${i}_ID: description: diff --git a/src/modules/sensors/sensor_params.yaml b/src/modules/sensors/sensor_params.yaml index 083ae8918c7..ce731b413a0 100644 --- a/src/modules/sensors/sensor_params.yaml +++ b/src/modules/sensors/sensor_params.yaml @@ -4,8 +4,12 @@ parameters: definitions: SENS_DPRES_OFF: description: - short: Differential pressure sensor offset - long: The offset (zero-reading) in Pascal + short: Differential pressure sensor offset of the first airspeed sensor + long: |- + The offset (zero-reading) in Pascal of the sensor holding calibration slot 0. + + Kept as a mirror of CAL_DPRES0_OFF so that ground stations can still detect whether + airspeed has been calibrated. The firmware applies CAL_DPRESx_OFF, not this value. category: System type: float default: 0.0 diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 276ec416bf0..4d109412993 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -59,18 +59,9 @@ Sensors::Sensors(bool hil_enabled) : #endif // CONFIG_SENSORS_VEHICLE_ACCELERATION #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) - /* 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 */ - - _parameter_handles.air_cmodel = param_find("CAL_AIR_CMODEL"); - _parameter_handles.air_tube_length = param_find("CAL_AIR_TUBELEN"); - _parameter_handles.air_tube_diameter_mm = param_find("CAL_AIR_TUBED_MM"); - - _airspeed_validator.set_timeout(300000); - _airspeed_validator.set_equal_value_threshold(100); #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED #if defined(CONFIG_SENSORS_VEHICLE_ANGULAR_VELOCITY) @@ -115,6 +106,17 @@ Sensors::~Sensors() #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) + + for (auto &airspeed : _vehicle_airspeed_list) { + if (airspeed) { + airspeed->Stop(); + delete airspeed; + } + } + +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED + #if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) if (_vehicle_gps_position) { @@ -169,15 +171,9 @@ int Sensors::parameters_update() } #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) - /* 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 */ - - param_get(_parameter_handles.air_cmodel, &_parameters.air_cmodel); - param_get(_parameter_handles.air_tube_length, &_parameters.air_tube_length); - param_get(_parameter_handles.air_tube_diameter_mm, &_parameters.air_tube_diameter_mm); #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED _voted_sensors_update.parametersUpdate(); @@ -250,6 +246,10 @@ int Sensors::parameters_update() InitializeVehicleAirData(); #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) + InitializeVehicleAirspeed(); +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED + #if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) InitializeVehicleGPSPosition(); #endif // CONFIG_SENSORS_VEHICLE_GPS_POSITION @@ -266,93 +266,6 @@ int Sensors::parameters_update() } #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) -void Sensors::diff_pres_poll() -{ - differential_pressure_s diff_pres{}; - - if (_diff_pres_sub.update(&diff_pres)) { - - if (!PX4_ISFINITE(diff_pres.differential_pressure_pa)) { - // ignore invalid data and reset accumulated - - // reset - _diff_pres_timestamp_sum = 0; - _diff_pres_pressure_sum = 0; - _diff_pres_temperature_sum = 0; - _baro_pressure_sum = 0; - _diff_pres_count = 0; - return; - } - - vehicle_air_data_s air_data{}; - _vehicle_air_data_sub.copy(&air_data); - const float temperature = air_data.ambient_temperature; - - // push raw data into validator - float airspeed_input[3] { diff_pres.differential_pressure_pa, 0.0f, 0.0f }; - _airspeed_validator.put(diff_pres.timestamp_sample, airspeed_input, diff_pres.error_count, 100); // TODO: real priority? - - // accumulate average for publication - _diff_pres_timestamp_sum += diff_pres.timestamp_sample; - _diff_pres_pressure_sum += diff_pres.differential_pressure_pa; - _baro_pressure_sum += air_data.baro_pressure_pa; - _diff_pres_count++; - - if ((_diff_pres_count > 0) && hrt_elapsed_time(&_airspeed_last_publish) >= 50_ms) { - - // average data and apply calibration offset (SENS_DPRES_OFF) - const uint64_t timestamp_sample = _diff_pres_timestamp_sum / _diff_pres_count; - const float differential_pressure_pa = _diff_pres_pressure_sum / _diff_pres_count - _parameters.diff_pres_offset_pa; - const float baro_pressure_pa = _baro_pressure_sum / _diff_pres_count; - - // reset - _diff_pres_timestamp_sum = 0; - _diff_pres_pressure_sum = 0; - _baro_pressure_sum = 0; - _diff_pres_count = 0; - - - enum AIRSPEED_SENSOR_MODEL smodel; - - switch ((diff_pres.device_id >> 16) & 0xFF) { - case DRV_DIFF_PRESS_DEVTYPE_SDP31: - - // fallthrough - case DRV_DIFF_PRESS_DEVTYPE_SDP32: - - // fallthrough - case DRV_DIFF_PRESS_DEVTYPE_SDP33: - smodel = AIRSPEED_SENSOR_MODEL_SDP3X; - break; - - default: - smodel = AIRSPEED_SENSOR_MODEL_MEMBRANE; - break; - } - - float indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)_parameters.air_cmodel, - smodel, _parameters.air_tube_length, _parameters.air_tube_diameter_mm, - differential_pressure_pa, baro_pressure_pa, temperature); - - // assume that CAS = IAS as we don't have an CAS-scale here - float true_airspeed_m_s = calc_TAS_from_CAS(indicated_airspeed_m_s, baro_pressure_pa, temperature); - - if (PX4_ISFINITE(indicated_airspeed_m_s) && PX4_ISFINITE(true_airspeed_m_s)) { - - airspeed_s airspeed; - airspeed.timestamp_sample = timestamp_sample; - airspeed.indicated_airspeed_m_s = indicated_airspeed_m_s; - airspeed.true_airspeed_m_s = true_airspeed_m_s; - airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time()); - airspeed.timestamp = hrt_absolute_time(); - _airspeed_pub.publish(airspeed); - - _airspeed_last_publish = airspeed.timestamp; - } - } - } -} - void Sensors::adc_poll() { /* only read if not in HIL mode */ @@ -388,8 +301,14 @@ void Sensors::adc_poll() if (voltage > 0.4f) { const float diff_pres_pa_raw = voltage * _parameters.diff_pres_analog_scale; + // the analogue sensor is not on a bus, but it still needs an identity so that it + // can hold a calibration slot like any other airspeed sensor + device::Device::DeviceId device_id{}; + device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_ANALOG; + differential_pressure_s diff_pres{}; diff_pres.timestamp_sample = adc.timestamp; + diff_pres.device_id = device_id.devid; diff_pres.differential_pressure_pa = diff_pres_pa_raw; diff_pres.temperature = NAN; diff_pres.timestamp = hrt_absolute_time(); @@ -420,6 +339,32 @@ void Sensors::InitializeVehicleAirData() } #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) +void Sensors::InitializeVehicleAirspeed() +{ + // create a VehicleAirspeed instance for each advertised differential pressure sensor. + for (uint8_t i = 0; i < calibration::DifferentialPressure::MAX_SENSOR_COUNT; i++) { + if (_vehicle_airspeed_list[i] == nullptr) { + + uORB::Subscription differential_pressure_sub{ORB_ID(differential_pressure), i}; + + if (differential_pressure_sub.advertised()) { + VehicleAirspeed *airspeed = new VehicleAirspeed(i); + + if (airspeed != nullptr) { + if (airspeed->Start()) { + _vehicle_airspeed_list[i] = airspeed; + + } else { + delete airspeed; + } + } + } + } + } +} +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED + #if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) void Sensors::InitializeVehicleGPSPosition() { @@ -547,6 +492,16 @@ void Sensors::Run() #endif // CONFIG_SENSORS_VEHICLE_AIR_DATA +#if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) + const int n_diff_pres = orb_group_count(ORB_ID(differential_pressure)); + + if (n_diff_pres != _n_diff_pres) { + _n_diff_pres = n_diff_pres; + updated = true; + } + +#endif // CONFIG_SENSORS_VEHICLE_AIRSPEED + #if defined(CONFIG_SENSORS_VEHICLE_GPS_POSITION) const int n_gnss = orb_group_count(ORB_ID(sensor_gnss)); @@ -619,7 +574,6 @@ void Sensors::Run() #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) // check analog airspeed adc_poll(); - diff_pres_poll(); #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED // backup schedule as a watchdog timeout @@ -704,7 +658,13 @@ int Sensors::print_status() #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) PX4_INFO_RAW("\n"); PX4_INFO_RAW("Airspeed status:\n"); - _airspeed_validator.print(); + + for (auto &airspeed : _vehicle_airspeed_list) { + if (airspeed) { + airspeed->PrintStatus(); + } + } + #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED #if defined(CONFIG_SENSORS_VEHICLE_OPTICAL_FLOW) diff --git a/src/modules/sensors/sensors.hpp b/src/modules/sensors/sensors.hpp index a14d8b421b0..a588bdee317 100644 --- a/src/modules/sensors/sensors.hpp +++ b/src/modules/sensors/sensors.hpp @@ -58,11 +58,10 @@ #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) # include +# include # include -# include -# include # include -# include +# include "vehicle_airspeed/VehicleAirspeed.hpp" #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED #if defined(CONFIG_SENSORS_VEHICLE_AIR_DATA) @@ -162,14 +161,6 @@ private: uORB::Publication _sensor_pub{ORB_ID(sensor_combined)}; #if defined(CONFIG_SENSORS_VEHICLE_AIRSPEED) - /** - * Poll the differential pressure sensor for updated data. - * - * @param raw Combined sensor data structure into which - * data should be returned. - */ - void diff_pres_poll(); - /** * Poll the ADC and update readings to suit. * @@ -178,48 +169,23 @@ private: */ void adc_poll(); - uORB::Subscription _diff_pres_sub {ORB_ID(differential_pressure)}; - uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; + void InitializeVehicleAirspeed(); - uORB::Publication _airspeed_pub{ORB_ID(airspeed)}; - - DataValidator _airspeed_validator; /**< data validator to monitor airspeed */ - - float _diff_pres_pressure_sum{0.f}; - float _diff_pres_temperature_sum{0.f}; - float _baro_pressure_sum{0.f}; - - int _diff_pres_count{0}; - - uint64_t _airspeed_last_publish{0}; - uint64_t _diff_pres_timestamp_sum{0}; + VehicleAirspeed *_vehicle_airspeed_list[calibration::DifferentialPressure::MAX_SENSOR_COUNT] {}; + uint8_t _n_diff_pres{0}; # ifdef ADC_AIRSPEED_VOLTAGE_CHANNEL uORB::Subscription _adc_report_sub {ORB_ID(adc_report)}; uORB::PublicationMulti _diff_pres_pub{ORB_ID(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 */ - - int32_t air_cmodel; - float air_tube_length; - float air_tube_diameter_mm; - } _parameters{}; /**< local copies of interesting parameters */ + } _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 */ - - param_t air_cmodel; - param_t air_tube_length; - param_t air_tube_diameter_mm; - } _parameter_handles{}; /**< handles for interesting parameters */ + } _parameter_handles {}; /**< handles for interesting parameters */ +# endif // ADC_AIRSPEED_VOLTAGE_CHANNEL #endif // CONFIG_SENSORS_VEHICLE_AIRSPEED #if defined(CONFIG_SENSORS_VEHICLE_ACCELERATION) diff --git a/src/modules/sensors/vehicle_airspeed/CMakeLists.txt b/src/modules/sensors/vehicle_airspeed/CMakeLists.txt new file mode 100644 index 00000000000..788a6c5c47c --- /dev/null +++ b/src/modules/sensors/vehicle_airspeed/CMakeLists.txt @@ -0,0 +1,44 @@ +############################################################################ +# +# Copyright (c) 2026 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(vehicle_airspeed + VehicleAirspeed.cpp + VehicleAirspeed.hpp +) +target_link_libraries(vehicle_airspeed + PRIVATE + airspeed + data_validator + px4_work_queue + sensor_calibration +) diff --git a/src/modules/sensors/vehicle_airspeed/VehicleAirspeed.cpp b/src/modules/sensors/vehicle_airspeed/VehicleAirspeed.cpp new file mode 100644 index 00000000000..dfbf14e7757 --- /dev/null +++ b/src/modules/sensors/vehicle_airspeed/VehicleAirspeed.cpp @@ -0,0 +1,202 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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. + * + ****************************************************************************/ + +#include "VehicleAirspeed.hpp" + +#include +#include +#include + +namespace sensors +{ + +VehicleAirspeed::VehicleAirspeed(uint8_t instance) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), + _instance(instance), + _sensor_sub{this, ORB_ID(differential_pressure), instance} +{ + _airspeed_validator.set_timeout(300000); + _airspeed_validator.set_equal_value_threshold(100); + + _airspeed_pub.advertise(); +} + +VehicleAirspeed::~VehicleAirspeed() +{ + Stop(); + perf_free(_cycle_perf); +} + +bool VehicleAirspeed::Start() +{ + ParametersUpdate(true); + + return _sensor_sub.registerCallback(); +} + +void VehicleAirspeed::Stop() +{ + Deinit(); + + _sensor_sub.unregisterCallback(); +} + +void VehicleAirspeed::ResetAccumulation() +{ + _timestamp_sample_sum = 0; + _differential_pressure_sum = 0.f; + _baro_pressure_sum = 0.f; + _temperature_sum = 0.f; + _sample_count = 0; +} + +void VehicleAirspeed::ParametersUpdate(bool force) +{ + if (_parameter_update_sub.updated() || force) { + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + + updateParams(); + + _calibration.ParametersUpdate(); + } +} + +void VehicleAirspeed::Run() +{ + perf_begin(_cycle_perf); + + ParametersUpdate(); + + differential_pressure_s diff_pres; + + while (_sensor_sub.update(&diff_pres)) { + + if (_calibration.device_id() != diff_pres.device_id) { + _calibration.set_device_id(diff_pres.device_id); + + // the global SENS_DPRES_OFF only ever applied to the first sensor, so only that one + // may take it over + if ((_instance == 0) && _calibration.AdoptLegacyOffset()) { + _calibration.ParametersSave(); + param_notify_changes(); + } + } + + if (!PX4_ISFINITE(diff_pres.differential_pressure_pa)) { + // ignore invalid data and reset accumulated + ResetAccumulation(); + continue; + } + + vehicle_air_data_s air_data{}; + _vehicle_air_data_sub.copy(&air_data); + + // push raw data into validator + float airspeed_input[3] { diff_pres.differential_pressure_pa, 0.0f, 0.0f }; + _airspeed_validator.put(diff_pres.timestamp_sample, airspeed_input, diff_pres.error_count, 100); + + // accumulate average for publication + _timestamp_sample_sum += diff_pres.timestamp_sample; + _differential_pressure_sum += diff_pres.differential_pressure_pa; + _baro_pressure_sum += air_data.baro_pressure_pa; + _temperature_sum += air_data.ambient_temperature; + _sample_count++; + } + + if ((_sample_count > 0) && (hrt_elapsed_time(&_last_publish) >= kPublishInterval)) { + + // average the accumulated data and remove this sensor's zero offset (CAL_DPRESx_OFF) + const uint64_t timestamp_sample = _timestamp_sample_sum / _sample_count; + const float differential_pressure_pa = _calibration.Correct(_differential_pressure_sum / _sample_count); + const float baro_pressure_pa = _baro_pressure_sum / _sample_count; + const float temperature = _temperature_sum / _sample_count; + + ResetAccumulation(); + + enum AIRSPEED_SENSOR_MODEL smodel; + + switch ((_calibration.device_id() >> 16) & 0xFF) { + case DRV_DIFF_PRESS_DEVTYPE_SDP31: + + // fallthrough + case DRV_DIFF_PRESS_DEVTYPE_SDP32: + + // fallthrough + case DRV_DIFF_PRESS_DEVTYPE_SDP33: + smodel = AIRSPEED_SENSOR_MODEL_SDP3X; + break; + + default: + smodel = AIRSPEED_SENSOR_MODEL_MEMBRANE; + break; + } + + const float indicated_airspeed_m_s = calc_IAS_corrected((enum AIRSPEED_COMPENSATION_MODEL)_param_cal_air_cmodel.get(), + smodel, _param_cal_air_tubelen.get(), _param_cal_air_tubed_mm.get(), + differential_pressure_pa, baro_pressure_pa, temperature); + + // assume that CAS = IAS as we don't have an CAS-scale here + const float true_airspeed_m_s = calc_TAS_from_CAS(indicated_airspeed_m_s, baro_pressure_pa, temperature); + + if (PX4_ISFINITE(indicated_airspeed_m_s) && PX4_ISFINITE(true_airspeed_m_s)) { + airspeed_s airspeed{}; + airspeed.timestamp_sample = timestamp_sample; + airspeed.device_id = _calibration.device_id(); + airspeed.indicated_airspeed_m_s = indicated_airspeed_m_s; + airspeed.true_airspeed_m_s = true_airspeed_m_s; + airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time()); + airspeed.timestamp = hrt_absolute_time(); + _airspeed_pub.publish(airspeed); + + _last_publish = airspeed.timestamp; + } + } + + perf_end(_cycle_perf); +} + +void VehicleAirspeed::PrintStatus() +{ + PX4_INFO_RAW("[vehicle_airspeed] instance %" PRIu8 ", airspeed instance %d\n", _instance, + _airspeed_pub.get_instance()); + + _calibration.PrintStatus(); + _airspeed_validator.print(); + PX4_INFO_RAW("\n"); + + perf_print_counter(_cycle_perf); +} + +} // namespace sensors diff --git a/src/modules/sensors/vehicle_airspeed/VehicleAirspeed.hpp b/src/modules/sensors/vehicle_airspeed/VehicleAirspeed.hpp new file mode 100644 index 00000000000..c7510050404 --- /dev/null +++ b/src/modules/sensors/vehicle_airspeed/VehicleAirspeed.hpp @@ -0,0 +1,116 @@ +/**************************************************************************** + * + * Copyright (c) 2026 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 "data_validator/DataValidator.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace time_literals; + +namespace sensors +{ + +/** + * Converts the differential pressure of a single sensor into an airspeed measurement. + * + * One instance is created per advertised differential_pressure instance. + * The airspeed selector is responsible for validating and choosing between them. + */ +class VehicleAirspeed : public ModuleParams, public px4::ScheduledWorkItem +{ +public: + explicit VehicleAirspeed(uint8_t instance); + ~VehicleAirspeed() override; + + bool Start(); + void Stop(); + + void PrintStatus(); + + uint32_t device_id() const { return _calibration.device_id(); } + +private: + void Run() override; + + void ParametersUpdate(bool force = false); + void ResetAccumulation(); + + static constexpr hrt_abstime kPublishInterval{50_ms}; + + const uint8_t _instance; + + uORB::SubscriptionCallbackWorkItem _sensor_sub; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; + + uORB::PublicationMulti _airspeed_pub{ORB_ID(airspeed)}; + + calibration::DifferentialPressure _calibration; + + DataValidator _airspeed_validator; + + // accumulated between publications so that the published airspeed is an average + uint64_t _timestamp_sample_sum{0}; + float _differential_pressure_sum{0.f}; + float _baro_pressure_sum{0.f}; + float _temperature_sum{0.f}; + int _sample_count{0}; + + hrt_abstime _last_publish{0}; + + perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + + DEFINE_PARAMETERS( + (ParamInt) _param_cal_air_cmodel, + (ParamFloat) _param_cal_air_tubelen, + (ParamFloat) _param_cal_air_tubed_mm + ) +}; + +} // namespace sensors diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index d9806eb9dda..6d91d78286a 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -772,8 +772,13 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us) } // TODO: send differential pressure instead? + device::Device::DeviceId device_id{}; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SIM; + airspeed_s airspeed{}; airspeed.timestamp_sample = time_now_us; + airspeed.device_id = device_id.devid; const Vector3f v_apparent_B = _q.rotateVectorInverse(_v_apparent_N); const float v_pitot = (_vehicle == VehicleType::TailsitterVTOL) ? -v_apparent_B(2) : v_apparent_B(0);