From d2cbe10243551b860ba06e745bf6dd9fb1d22dd7 Mon Sep 17 00:00:00 2001 From: Marco Hauswirth <58551738+haumarco@users.noreply.github.com> Date: Tue, 18 Feb 2025 13:23:10 +0100 Subject: [PATCH] Clean up temperature msg fields (#24272) * remove temp field from airspeed.msg, adjust temp selection * temp-sensor hirarchy: airspeed, ext. baro, default value * directly use diff-press or baro temp in true-airspeed calc * improve clarity * add enum for temperature source in VehicleAirData.msg --- msg/Airspeed.msg | 2 - msg/VehicleAirData.msg | 4 +- src/drivers/telemetry/hott/messages.cpp | 2 +- src/drivers/uavcan/sensors/airspeed.cpp | 1 - .../airspeed_selector_main.cpp | 2 +- src/modules/mavlink/mavlink_receiver.cpp | 1 - src/modules/mavlink/streams/HIGHRES_IMU.hpp | 2 +- src/modules/mavlink/streams/HIGH_LATENCY2.hpp | 13 +++++- src/modules/sensors/sensors.cpp | 24 +---------- src/modules/sensors/sensors.hpp | 6 +-- .../vehicle_air_data/VehicleAirData.cpp | 40 +++++++++++-------- .../vehicle_air_data/VehicleAirData.hpp | 10 +++-- src/modules/simulation/simulator_sih/sih.cpp | 1 - 13 files changed, 50 insertions(+), 58 deletions(-) diff --git a/msg/Airspeed.msg b/msg/Airspeed.msg index aaed7b72ca..bc673cf0aa 100644 --- a/msg/Airspeed.msg +++ b/msg/Airspeed.msg @@ -5,6 +5,4 @@ float32 indicated_airspeed_m_s # indicated airspeed in m/s float32 true_airspeed_m_s # true filtered airspeed in m/s -float32 air_temperature_celsius # air temperature in degrees Celsius, -1000 if unknown - float32 confidence # confidence value from 0 to 1 for this sensor diff --git a/msg/VehicleAirData.msg b/msg/VehicleAirData.msg index 59ca5e5c8d..41e5358ae8 100644 --- a/msg/VehicleAirData.msg +++ b/msg/VehicleAirData.msg @@ -6,10 +6,10 @@ uint64 timestamp_sample # the timestamp of the raw data (microseconds) uint32 baro_device_id # unique device ID for the selected barometer float32 baro_alt_meter # Altitude above MSL calculated from temperature compensated baro sensor data using an ISA corrected for sea level pressure SENS_BARO_QNH. -float32 baro_temp_celcius # Temperature in degrees Celsius float32 baro_pressure_pa # Absolute pressure in Pascals +float32 ambient_temperature # Abient temperature in degrees Celsius +uint8 temperature_source # Source of temperature data: 0: Default Temperature (15°C), 1: External Baro, 2: Airspeed float32 rho # air density -float32 eas2tas # equivalent airspeed to true airspeed conversion factor uint8 calibration_count # Calibration changed counter. Monotonically increases whenever calibration changes. diff --git a/src/drivers/telemetry/hott/messages.cpp b/src/drivers/telemetry/hott/messages.cpp index 3be05d87a3..364d5005a7 100644 --- a/src/drivers/telemetry/hott/messages.cpp +++ b/src/drivers/telemetry/hott/messages.cpp @@ -150,7 +150,7 @@ build_eam_response(uint8_t *buffer, size_t *size) msg.eam_sensor_id = EAM_SENSOR_ID; msg.sensor_text_id = EAM_SENSOR_TEXT_ID; - msg.temperature1 = (uint8_t)(airdata.baro_temp_celcius + 20); + msg.temperature1 = (uint8_t)(airdata.ambient_temperature + 20); msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG; msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10); diff --git a/src/drivers/uavcan/sensors/airspeed.cpp b/src/drivers/uavcan/sensors/airspeed.cpp index e62b29d5ea..2127817fd0 100644 --- a/src/drivers/uavcan/sensors/airspeed.cpp +++ b/src/drivers/uavcan/sensors/airspeed.cpp @@ -104,7 +104,6 @@ UavcanAirspeedBridge::ias_sub_cb(const report.timestamp = 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 + atmosphere::kAbsoluteNullCelsius; publish(msg.getSrcNodeID().get(), &report); } diff --git a/src/modules/airspeed_selector/airspeed_selector_main.cpp b/src/modules/airspeed_selector/airspeed_selector_main.cpp index f8012529e1..03b9d48574 100644 --- a/src/modules/airspeed_selector/airspeed_selector_main.cpp +++ b/src/modules/airspeed_selector/airspeed_selector_main.cpp @@ -388,7 +388,7 @@ AirspeedModule::Run() input_data.airspeed_indicated_raw = airspeed_raw.indicated_airspeed_m_s; input_data.airspeed_true_raw = airspeed_raw.true_airspeed_m_s; input_data.airspeed_timestamp = airspeed_raw.timestamp; - input_data.air_temperature_celsius = airspeed_raw.air_temperature_celsius; + input_data.air_temperature_celsius = _vehicle_air_data.ambient_temperature; if (_in_takeoff_situation) { // set flag to false if either speed is above stall speed, diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index aa8b50d01a..00c0b9ec93 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -2669,7 +2669,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) airspeed.timestamp_sample = timestamp_sample; airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f; airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f; - airspeed.air_temperature_celsius = 15.f; airspeed.timestamp = hrt_absolute_time(); _airspeed_pub.publish(airspeed); } diff --git a/src/modules/mavlink/streams/HIGHRES_IMU.hpp b/src/modules/mavlink/streams/HIGHRES_IMU.hpp index a6682213bf..7a3d96cb38 100644 --- a/src/modules/mavlink/streams/HIGHRES_IMU.hpp +++ b/src/modules/mavlink/streams/HIGHRES_IMU.hpp @@ -193,7 +193,7 @@ private: msg.abs_pressure = air_data.baro_pressure_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.temperature = air_data.ambient_temperature; msg.fields_updated = fields_updated; mavlink_msg_highres_imu_send_struct(_mavlink->get_channel(), &msg); diff --git a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp index 6aca9f7d98..8623950e02 100644 --- a/src/modules/mavlink/streams/HIGH_LATENCY2.hpp +++ b/src/modules/mavlink/streams/HIGH_LATENCY2.hpp @@ -57,6 +57,7 @@ #include #include #include +#include #include @@ -521,6 +522,7 @@ private: update_gps(); update_vehicle_status(); update_wind(); + update_vehicle_air_data(); } void update_airspeed() @@ -529,7 +531,6 @@ private: if (_airspeed_sub.update(&airspeed)) { _airspeed.add_value(airspeed.indicated_airspeed_m_s, _update_rate_filtered); - _temperature.add_value(airspeed.air_temperature_celsius, _update_rate_filtered); } } @@ -610,6 +611,15 @@ private: } } + void update_vehicle_air_data() + { + vehicle_air_data_s air_data; + + if (_vehicle_air_data_sub.update(&air_data)) { + _temperature.add_value(air_data.ambient_temperature, _update_rate_filtered); + } + } + void set_default_values(mavlink_high_latency2_t &msg) const { msg.airspeed = 0; @@ -659,6 +669,7 @@ private: uORB::Subscription _tecs_status_sub{ORB_ID(tecs_status)}; uORB::Subscription _wind_sub{ORB_ID(wind)}; uORB::Subscription _health_report_sub{ORB_ID(health_report)}; + uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; SimpleAnalyzer _airspeed; SimpleAnalyzer _airspeed_sp; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 26cc3329da..97ceb6db21 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -278,32 +278,15 @@ void Sensors::diff_pres_poll() vehicle_air_data_s air_data{}; _vehicle_air_data_sub.copy(&air_data); - - float air_temperature_celsius = NAN; - - // assume anything outside of a (generous) operating range of -40C to 125C is invalid - if (PX4_ISFINITE(diff_pres.temperature) && (diff_pres.temperature >= -40.f) && (diff_pres.temperature <= 125.f)) { - - air_temperature_celsius = diff_pres.temperature; - - } else { - // differential pressure temperature invalid, check barometer - if ((air_data.timestamp != 0) && PX4_ISFINITE(air_data.baro_temp_celcius) - && (air_data.baro_temp_celcius >= -40.f) && (air_data.baro_temp_celcius <= 125.f)) { - - // TODO: review PCB_TEMP_ESTIMATE_DEG, ignore for external baro - air_temperature_celsius = air_data.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG; - } - } + const float temperature = air_data.ambient_temperature; // push raw data into validator - float airspeed_input[3] { diff_pres.differential_pressure_pa, air_temperature_celsius, 0.0f }; + 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; - _diff_pres_temperature_sum += air_temperature_celsius; _baro_pressure_sum += air_data.baro_pressure_pa; _diff_pres_count++; @@ -313,12 +296,10 @@ void Sensors::diff_pres_poll() 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; - const float temperature = _diff_pres_temperature_sum / _diff_pres_count; // reset _diff_pres_timestamp_sum = 0; _diff_pres_pressure_sum = 0; - _diff_pres_temperature_sum = 0; _baro_pressure_sum = 0; _diff_pres_count = 0; @@ -354,7 +335,6 @@ void Sensors::diff_pres_poll() airspeed.timestamp_sample = timestamp_sample; airspeed.indicated_airspeed_m_s = indicated_airspeed_m_s; airspeed.true_airspeed_m_s = true_airspeed_m_s; - airspeed.air_temperature_celsius = temperature; airspeed.confidence = _airspeed_validator.confidence(hrt_absolute_time()); airspeed.timestamp = hrt_absolute_time(); _airspeed_pub.publish(airspeed); diff --git a/src/modules/sensors/sensors.hpp b/src/modules/sensors/sensors.hpp index 911005249c..3a8bcd50bc 100644 --- a/src/modules/sensors/sensors.hpp +++ b/src/modules/sensors/sensors.hpp @@ -90,11 +90,7 @@ using namespace sensors; using namespace time_literals; -/** - * HACK - true temperature is much less than indicated temperature in baro, - * subtract 5 degrees in an attempt to account for the electrical upheating of the PCB - */ -#define PCB_TEMP_ESTIMATE_DEG 5.0f + class Sensors : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { public: diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 263a56090f..a12634055f 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -45,6 +45,9 @@ using namespace matrix; using namespace atmosphere; static constexpr uint32_t SENSOR_TIMEOUT{300_ms}; +static constexpr float DEFAULT_TEMPERATURE_CELSIUS = 15.f; +static constexpr float TEMPERATURE_MIN_CELSIUS = -60.f; +static constexpr float TEMPERATURE_MAX_CELSIUS = 60.f; VehicleAirData::VehicleAirData() : ModuleParams(nullptr), @@ -77,21 +80,23 @@ void VehicleAirData::Stop() } } -void VehicleAirData::AirTemperatureUpdate() +float VehicleAirData::AirTemperatureUpdate(const float temperature_baro, TemperatureSource &source, + const hrt_abstime time_now_us) { + // use the temperature from the differential pressure sensor if available + // otherwise use the temperature from the external barometer + // Temperature measurements from internal baros are not used as typically not representative for ambient temperature + float temperature = source == TemperatureSource::EXTERNAL_BARO ? temperature_baro : DEFAULT_TEMPERATURE_CELSIUS; differential_pressure_s differential_pressure; - static constexpr float temperature_min_celsius = -20.f; - static constexpr float temperature_max_celsius = 35.f; - - // update air temperature if data from differential pressure sensor is finite and not exactly 0 - // limit the range to max 35°C to limt the error due to heated up airspeed sensors prior flight - if (_differential_pressure_sub.update(&differential_pressure) && PX4_ISFINITE(differential_pressure.temperature) - && fabsf(differential_pressure.temperature) > FLT_EPSILON) { - - _air_temperature_celsius = math::constrain(differential_pressure.temperature, temperature_min_celsius, - temperature_max_celsius); + if (_differential_pressure_sub.copy(&differential_pressure) + && time_now_us - differential_pressure.timestamp_sample < 1_s + && PX4_ISFINITE(differential_pressure.temperature)) { + temperature = differential_pressure.temperature; + source = TemperatureSource::AIRSPEED; } + + return math::constrain(temperature, TEMPERATURE_MIN_CELSIUS, TEMPERATURE_MAX_CELSIUS); } bool VehicleAirData::ParametersUpdate(bool force) @@ -139,8 +144,6 @@ void VehicleAirData::Run() const bool parameter_update = ParametersUpdate(); - AirTemperatureUpdate(); - estimator_status_flags_s estimator_status_flags; const bool estimator_status_flags_updated = _estimator_status_flags_sub.update(&estimator_status_flags); @@ -272,23 +275,26 @@ void VehicleAirData::Run() if (publish) { const float pressure_pa = _data_sum[instance] / _data_sum_count[instance]; - const float temperature = _temperature_sum[instance] / _data_sum_count[instance]; + const float temperature_baro = _temperature_sum[instance] / _data_sum_count[instance]; + TemperatureSource temperature_source = _calibration[instance].external() ? TemperatureSource::EXTERNAL_BARO : + TemperatureSource::DEFAULT_TEMP; + const float ambient_temperature = AirTemperatureUpdate(temperature_baro, temperature_source, time_now_us); const float pressure_sealevel_pa = _param_sens_baro_qnh.get() * 100.f; const float altitude = getAltitudeFromPressure(pressure_pa, pressure_sealevel_pa); // calculate air density - const float air_density = getDensityFromPressureAndTemp(pressure_pa, temperature); + const float air_density = getDensityFromPressureAndTemp(pressure_pa, ambient_temperature); // populate vehicle_air_data with and publish vehicle_air_data_s out{}; out.timestamp_sample = timestamp_sample; out.baro_device_id = _calibration[instance].device_id(); out.baro_alt_meter = altitude; - out.baro_temp_celcius = temperature; + out.ambient_temperature = ambient_temperature; + out.temperature_source = static_cast(temperature_source); out.baro_pressure_pa = pressure_pa; out.rho = air_density; - out.eas2tas = sqrtf(kAirDensitySeaLevelStandardAtmos / math::max(air_density, FLT_EPSILON)); out.calibration_count = _calibration[instance].calibration_count(); out.timestamp = hrt_absolute_time(); diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index a0bdb4007d..1e6555fcdd 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -73,9 +73,15 @@ public: void PrintStatus(); private: + enum TemperatureSource { + DEFAULT_TEMP = 0, + EXTERNAL_BARO = 1, + AIRSPEED = 2, + }; + void Run() override; - void AirTemperatureUpdate(); + float AirTemperatureUpdate(const float temperature_baro, TemperatureSource &source, const hrt_abstime time_now_us); void CheckFailover(const hrt_abstime &time_now_us); bool ParametersUpdate(bool force = false); void UpdateStatus(); @@ -124,8 +130,6 @@ private: int8_t _selected_sensor_sub_index{-1}; - float _air_temperature_celsius{20.f}; // initialize with typical 20degC ambient temperature - bool _last_status_baro_fault{false}; DEFINE_PARAMETERS( diff --git a/src/modules/simulation/simulator_sih/sih.cpp b/src/modules/simulation/simulator_sih/sih.cpp index 07c24fe642..2ebf936823 100644 --- a/src/modules/simulation/simulator_sih/sih.cpp +++ b/src/modules/simulation/simulator_sih/sih.cpp @@ -556,7 +556,6 @@ void Sih::send_airspeed(const hrt_abstime &time_now_us) // regardless of vehicle type, body frame, etc this holds as long as wind=0 airspeed.true_airspeed_m_s = fmaxf(0.1f, _v_E.norm() + generate_wgn() * 0.2f); airspeed.indicated_airspeed_m_s = airspeed.true_airspeed_m_s * sqrtf(_wing_l.get_rho() / RHO); - airspeed.air_temperature_celsius = NAN; airspeed.confidence = 0.7f; airspeed.timestamp = hrt_absolute_time(); _airspeed_pub.publish(airspeed);