Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Clean up temperature msg fields #24272

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Conversation

haumarco
Copy link
Contributor

Solved Problem

Currently, multiple temperature measurements are used across different functionalities to represent ambient temperature. This creates ambiguity regarding which temperature measurement should be designated as vehicle_temperature. We lack dedicated temperature sensors or topics to consistently rely on a single source.

Solution

Implement a prioritized selection of temperature sensors. When an external airspeed sensor is available, its temperature is used as the primary source. If an external barometer is present, its temperature serves as the secondary option. The temperature from the internal barometer is disregarded due to its unreliability, which is caused by inconsistent PCB heat dissipation across different flight controllers.

Changelog Entry

Create a distinct ambient_temperature field in the VehicleAirData message.

Alternatives

An alternative approach would be to create a dedicated temperature uORB topic to centralize ambient temperature data.

Test Coverage

Tested with various setups and hardcoded measurements in simulation environments to ensure reliability and accuracy.

@haumarco haumarco requested a review from sfuhrer January 30, 2025 10:51
@haumarco haumarco force-pushed the pr_clean_temperature_usage branch from e60a339 to 695bfb9 Compare January 30, 2025 12:58
Copy link

🔎 FLASH Analysis

px4_fmu-v5x [Total VM Diff: -120 byte (-0.01 %)]
    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +56  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
-0.0%     -16  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
  -3.1%      -8  [ = ]       0    ../../src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
-0.0%     -32  [ = ]       0    .debug_frame
+0.0%     +39  [ = ]       0    .debug_info
  -0.1%     -13  [ = ]       0    ../../src/drivers/telemetry/frsky_telemetry/frsky_data.cpp
  -0.1%     -13  [ = ]       0    ../../src/drivers/telemetry/frsky_telemetry/frsky_telemetry.cpp
  -0.1%     -13  [ = ]       0    ../../src/drivers/telemetry/frsky_telemetry/sPort_data.cpp
  -0.1%     -13  [ = ]       0    ../../src/drivers/telemetry/hott/messages.cpp
  +0.0%      +3  [ = ]       0    ../../src/drivers/uavcan/remoteid.cpp
  +0.0%     +16  [ = ]       0    ../../src/drivers/uavcan/sensors/airspeed.cpp
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
  -0.0%     -13  [ = ]       0    ../../src/modules/airspeed_selector/airspeed_selector_main.cpp
  -0.0%     -13  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  -0.1%    -130  [ = ]       0    ../../src/modules/sensors/sensors.cpp
  +0.1%     +55  [ = ]       0    ../../src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
  -0.0%     -13  [ = ]       0    ../../src/modules/vtol_att_control/vtol_att_control_main.cpp
  +0.2%     +16  [ = ]       0    msg/topics_sources/airspeed.cpp
  -0.1%     -18  [ = ]       0    msg/topics_sources/uORBMessageFieldsGenerated.cpp
  -0.1%     -13  [ = ]       0    msg/topics_sources/vehicle_air_data.cpp
  -0.0%     -13  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  +0.0%    +218  [ = ]       0    src/modules/mavlink/modules__mavlink_unity.cpp
+0.0%     +17  [ = ]       0    .debug_line
  +0.3%      +7  [ = ]       0    ../../src/drivers/telemetry/hott/messages.cpp
  -0.2%     -16  [ = ]       0    ../../src/drivers/uavcan/sensors/airspeed.cpp
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  -0.9%    -114  [ = ]       0    ../../src/modules/sensors/sensors.cpp
  +1.6%    +121  [ = ]       0    ../../src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
  -0.0%     -10  [ = ]       0    ../../src/modules/simulation/simulator_sih/sih.cpp
  +0.0%     +56  [ = ]       0    src/modules/mavlink/modules__mavlink_unity.cpp
  -0.2%      -2  [ = ]       0    task/task_cancelpt.c
+0.0%     +81  [ = ]       0    .debug_loc
  -0.0%      -2  [ = ]       0    ../../src/drivers/uavcan/uavcan_main.cpp
  +0.0%     +20  [ = ]       0    ../../src/drivers/uavcan/uavcan_servers.cpp
  -0.1%     -64  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  -1.1%    -115  [ = ]       0    ../../src/modules/sensors/sensors.cpp
  +2.0%    +143  [ = ]       0    ../../src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
  +0.0%      +2  [ = ]       0    [section .debug_loc]
  +0.0%     +23  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  +0.0%     +74  [ = ]       0    src/modules/mavlink/modules__mavlink_unity.cpp
+0.0%     +54  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  +2.6%     +64  [ = ]       0    ../../src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
  -3.0%      -2  [ = ]       0    task/task_cancelpt.c
+0.0%    +129  [ = ]       0    .debug_str
  -0.3%      -6  [ = ]       0    ../../src/drivers/telemetry/frsky_telemetry/frsky_telemetry.cpp
  -0.1%     -24  [ = ]       0    ../../src/drivers/uavcan/sensors/airspeed.cpp
  +0.1%     +24  [ = ]       0    ../../src/modules/airspeed_selector/airspeed_selector_main.cpp
  +2.3%     +45  [ = ]       0    ../../src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
  +0.0%      +8  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  +0.0%     +82  [ = ]       0    src/modules/mavlink/modules__mavlink_unity.cpp
+0.0%      +4  [ = ]       0    .strtab
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  +0.6%      +4  [ = ]       0    ../../src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
  +0.0%     +32  [ = ]       0    [section .strtab]
-0.0%     -16  [ = ]       0    .symtab
  -1.2%     -16  [ = ]       0    ../../src/drivers/uavcan/sensors/airspeed.cpp
  -7.0%     -64  [ = ]       0    ../../src/lib/version/version.c
  +0.3%     +16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +0.1%     +48  [ = ]       0    [section .symtab]
+1.0%    +120  [ = ]       0    [Unmapped]
-0.0%    -120  -0.0%    -120    .text
  +2.0%     +60  +2.0%     +60    ../../src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
  +0.0%     +32  +0.0%     +32    src/modules/mavlink/modules__mavlink_unity.cpp
  +0.4%      +4  +0.4%      +4    ../../src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp
  -0.0%      -8  -0.0%      -8    ../../src/modules/simulation/simulator_sih/sih.cpp
  -0.0%     -16  -0.0%     -16    [section .text]
  -0.9%     -24  -0.9%     -24    ../../src/drivers/uavcan/sensors/airspeed.cpp
  -3.4%    -168  -3.4%    -168    ../../src/modules/sensors/sensors.cpp
+0.0%    +316  -0.0%    -120    TOTAL

px4_fmu-v6x [Total VM Diff: -112 byte (-0.01 %)]
    FILE SIZE        VM SIZE    
--------------  -------------- 
+0.0%     +56  [ = ]       0    .debug_abbrev
   +11%     +56  [ = ]       0    ../../src/lib/version/version.c
-0.0%     -16  [ = ]       0    .debug_aranges
  -5.0%      -8  [ = ]       0    ../../src/lib/version/version.c
  -3.1%      -8  [ = ]       0    ../../src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
-0.0%     -32  [ = ]       0    .debug_frame
+0.0%     +91  [ = ]       0    .debug_info
  +0.0%      +3  [ = ]       0    ../../src/drivers/uavcan/remoteid.cpp
  +0.0%     +16  [ = ]       0    ../../src/drivers/uavcan/sensors/airspeed.cpp
  -0.2%      -4  [ = ]       0    ../../src/lib/version/version.c
  -0.0%     -13  [ = ]       0    ../../src/modules/airspeed_selector/airspeed_selector_main.cpp
  -0.0%     -13  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  -0.1%    -130  [ = ]       0    ../../src/modules/sensors/sensors.cpp
  +0.1%     +55  [ = ]       0    ../../src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
  -0.0%     -13  [ = ]       0    ../../src/modules/vtol_att_control/vtol_att_control_main.cpp
  +0.2%     +16  [ = ]       0    msg/topics_sources/airspeed.cpp
  -0.1%     -18  [ = ]       0    msg/topics_sources/uORBMessageFieldsGenerated.cpp
  -0.1%     -13  [ = ]       0    msg/topics_sources/vehicle_air_data.cpp
  -0.0%     -13  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  +0.0%    +218  [ = ]       0    src/modules/mavlink/modules__mavlink_unity.cpp
+0.0%     +21  [ = ]       0    .debug_line
  -0.2%     -16  [ = ]       0    ../../src/drivers/uavcan/sensors/airspeed.cpp
  -1.3%     -25  [ = ]       0    ../../src/lib/version/version.c
  -0.9%    -114  [ = ]       0    ../../src/modules/sensors/sensors.cpp
  +1.6%    +121  [ = ]       0    ../../src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
  +0.0%     +56  [ = ]       0    src/modules/mavlink/modules__mavlink_unity.cpp
  -0.1%      -1  [ = ]       0    task/task_cancelpt.c
+0.0%     +27  [ = ]       0    .debug_loc
  -0.0%      -2  [ = ]       0    ../../src/drivers/uavcan/uavcan_main.cpp
  -0.0%     -13  [ = ]       0    ../../src/drivers/uavcan/uavcan_servers.cpp
  -0.1%     -80  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  -1.1%    -115  [ = ]       0    ../../src/modules/sensors/sensors.cpp
  +1.8%    +128  [ = ]       0    ../../src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
  +0.0%     +35  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  +0.0%     +74  [ = ]       0    src/modules/mavlink/modules__mavlink_unity.cpp
+0.0%     +56  [ = ]       0    .debug_ranges
  -2.6%      -8  [ = ]       0    ../../src/lib/version/version.c
  +2.6%     +64  [ = ]       0    ../../src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
+0.0%    +129  [ = ]       0    .debug_str
  -0.1%      -6  [ = ]       0    ../../src/drivers/uavcan/remoteid.cpp
  -0.1%     -24  [ = ]       0    ../../src/drivers/uavcan/sensors/airspeed.cpp
  +0.1%     +24  [ = ]       0    ../../src/modules/airspeed_selector/airspeed_selector_main.cpp
  +2.3%     +45  [ = ]       0    ../../src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
  +0.0%      +8  [ = ]       0    src/modules/ekf2/modules__ekf2_unity.cpp
  +0.0%     +82  [ = ]       0    src/modules/mavlink/modules__mavlink_unity.cpp
+0.0%      +4  [ = ]       0    .strtab
  -8.1%     -32  [ = ]       0    ../../src/lib/version/version.c
  +0.6%      +4  [ = ]       0    ../../src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
  +0.0%     +32  [ = ]       0    [section .strtab]
-0.0%     -16  [ = ]       0    .symtab
  -1.2%     -16  [ = ]       0    ../../src/drivers/uavcan/sensors/airspeed.cpp
  -7.0%     -64  [ = ]       0    ../../src/lib/version/version.c
  +0.3%     +16  [ = ]       0    ../../src/modules/fw_pos_control/FixedwingPositionControl.cpp
  +0.1%     +48  [ = ]       0    [section .symtab]
+0.2%    +112  [ = ]       0    [Unmapped]
-0.0%    -112  -0.0%    -112    .text
  +2.0%     +60  +2.0%     +60    ../../src/modules/sensors/vehicle_air_data/VehicleAirData.cpp
  +0.0%     +32  +0.0%     +32    src/modules/mavlink/modules__mavlink_unity.cpp
  +0.4%      +4  +0.4%      +4    ../../src/modules/sensors/vehicle_gps_position/VehicleGPSPosition.cpp
  -0.0%     -16  -0.0%     -16    [section .text]
  -0.9%     -24  -0.9%     -24    ../../src/drivers/uavcan/sensors/airspeed.cpp
  -3.4%    -168  -3.4%    -168    ../../src/modules/sensors/sensors.cpp
+0.0%    +320  -0.0%    -112    TOTAL

Updated: 2025-01-30T13:05:37

{
vehicle_air_data_s air_data;

if (_vehicle_air_data_sub.update(&air_data)) {
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This will need a small edit of the comment in the MAVLink message.
"Air temperature from airspeed sensor" is no longer valid.

@@ -77,21 +77,24 @@ void VehicleAirData::Stop()
}
}

void VehicleAirData::AirTemperatureUpdate()
void VehicleAirData::AirTemperatureUpdate(float &temperature, const bool &external_baro)
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would avoid this pass by reference here. Especially for bools that adds more weight to the calculation and is less readable.

Suggested change
void VehicleAirData::AirTemperatureUpdate(float &temperature, const bool &external_baro)
void VehicleAirData::AirTemperatureUpdate(float temperature_baro, const bool baro_is_external)

// internal baros are not precise enough to be used for temperature
static constexpr float default_temperature_celsius = 15.f;
temperature = external_baro ? temperature : default_temperature_celsius;
static constexpr float temperature_min_celsius = -60.f;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Move constants to the top, see SENSOR_TIMEOUT.

_air_temperature_celsius = math::constrain(differential_pressure.temperature, temperature_min_celsius,
temperature_max_celsius);
if (_differential_pressure_sub.copy(&differential_pressure)
&& hrt_absolute_time() - differential_pressure.timestamp_sample < 1_s
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would pass now as an argument and only call hrt_absolute_time() once per run.

{
// use the temperature from the differential pressure sensor if available
// otherwise use the temperature from the external barometer
// internal baros are not precise enough to be used for temperature
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
// internal baros are not precise enough to be used for temperature
// Temperature measurements from internal baros are not used as typically not representative for ambient temperature

@@ -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;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

How do we get the temperature measurement from UAVCAN airspeed sensors to PX4? Do we need to publish differential_pressure here as well, just for the sake of passing along temperature?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants