diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 438d9e6f08..f3aeb46699 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 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 @@ -110,6 +110,22 @@ void VehicleAirData::SensorCorrectionsUpdate(bool force) } } +void VehicleAirData::AirTemperatureUpdate() +{ + 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); + } +} + void VehicleAirData::ParametersUpdate() { // Check if parameters have changed @@ -130,6 +146,8 @@ void VehicleAirData::Run() SensorCorrectionsUpdate(); + AirTemperatureUpdate(); + bool updated[MAX_SENSOR_COUNT] {}; for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) { @@ -250,12 +268,8 @@ void VehicleAirData::Run() out.baro_alt_meter = (((powf((p / p1), (-(a * CONSTANTS_AIR_GAS_CONST) / CONSTANTS_ONE_G))) * T1) - T1) / a; // calculate air density - // estimate air density assuming typical 20degC ambient temperature - // TODO: use air temperature if available (differential pressure sensors) - static constexpr float pressure_to_density = 1.0f / (CONSTANTS_AIR_GAS_CONST * (20.0f - - CONSTANTS_ABSOLUTE_NULL_CELSIUS)); - - out.rho = pressure_to_density * out.baro_pressure_pa; + out.rho = out.baro_pressure_pa / (CONSTANTS_AIR_GAS_CONST * (_air_temperature_celsius - + CONSTANTS_ABSOLUTE_NULL_CELSIUS)); out.timestamp = hrt_absolute_time(); _vehicle_air_data_pub.publish(out); diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index 3b6c10b2a0..4fed84dc9d 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2021 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 @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include @@ -72,6 +73,7 @@ private: void ParametersUpdate(); void SensorCorrectionsUpdate(bool force = false); + void AirTemperatureUpdate(); static constexpr int MAX_SENSOR_COUNT = 4; @@ -80,6 +82,7 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; + uORB::Subscription _differential_pressure_sub{ORB_ID(differential_pressure)}; uORB::SubscriptionCallbackWorkItem _sensor_sub[MAX_SENSOR_COUNT] { {this, ORB_ID(sensor_baro), 0}, @@ -110,6 +113,8 @@ private: int8_t _selected_sensor_sub_index{-1}; + float _air_temperature_celsius{20.f}; // initialize with typical 20degC ambient temperature + DEFINE_PARAMETERS( (ParamFloat) _param_sens_baro_qnh, (ParamFloat) _param_sens_baro_rate