From 19059a80bd7f969082f6231748595f8ab9800751 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sun, 12 Jul 2020 10:46:25 -0400 Subject: [PATCH] sensors: throttle vehicle_air_data publication (SENS_BARO_RATE) - don't bother running baro aggregator if SYS_HAS_BARO disabled --- src/modules/sensors/sensors.cpp | 47 ++++++--- .../vehicle_air_data/VehicleAirData.cpp | 98 +++++++++++-------- .../vehicle_air_data/VehicleAirData.hpp | 8 +- src/modules/sensors/vehicle_air_data/params.c | 16 +++ 4 files changed, 117 insertions(+), 52 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 738c755b35..42f50a7d56 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -168,7 +168,7 @@ private: VehicleAcceleration _vehicle_acceleration; VehicleAngularVelocity _vehicle_angular_velocity; - VehicleAirData _vehicle_air_data; + VehicleAirData *_vehicle_air_data{nullptr}; static constexpr int MAX_SENSOR_COUNT = 3; VehicleIMU *_vehicle_imu_list[MAX_SENSOR_COUNT] {}; @@ -200,8 +200,12 @@ private: */ void adc_poll(); + void InitializeVehicleAirData(); void InitializeVehicleIMU(); + DEFINE_PARAMETERS( + (ParamBool) _param_sys_has_baro + ) }; Sensors::Sensors(bool hil_enabled) : @@ -218,9 +222,6 @@ Sensors::Sensors(bool hil_enabled) : _vehicle_acceleration.Start(); _vehicle_angular_velocity.Start(); - _vehicle_air_data.Start(); - - InitializeVehicleIMU(); } Sensors::~Sensors() @@ -232,11 +233,16 @@ Sensors::~Sensors() _vehicle_acceleration.Stop(); _vehicle_angular_velocity.Stop(); - _vehicle_air_data.Stop(); - for (auto &i : _vehicle_imu_list) { - if (i != nullptr) { - i->Stop(); + if (_vehicle_air_data) { + _vehicle_air_data->Stop(); + delete _vehicle_air_data; + } + + for (auto &vehicle_imu : _vehicle_imu_list) { + if (vehicle_imu) { + vehicle_imu->Stop(); + delete vehicle_imu; } } @@ -418,6 +424,21 @@ void Sensors::adc_poll() #endif /* ADC_AIRSPEED_VOLTAGE_CHANNEL */ } +void Sensors::InitializeVehicleAirData() +{ + if (_param_sys_has_baro.get()) { + if (_vehicle_air_data == nullptr) { + if (orb_exists(ORB_ID(sensor_baro), 0) == PX4_OK) { + _vehicle_air_data = new VehicleAirData(); + + if (_vehicle_air_data) { + _vehicle_air_data->Start(); + } + } + } + } +} + void Sensors::InitializeVehicleIMU() { // create a VehicleIMU instance for each accel/gyro pair @@ -439,7 +460,6 @@ void Sensors::InitializeVehicleIMU() // Start VehicleIMU instance and store if (imu->Start()) { _vehicle_imu_list[i] = imu; - ScheduleNow(); } else { delete imu; @@ -468,6 +488,8 @@ void Sensors::Run() // run once if (_last_config_update == 0) { + InitializeVehicleAirData(); + InitializeVehicleIMU(); _voted_sensors_update.init(_sensor_combined); parameter_update_poll(true); } @@ -555,6 +577,7 @@ void Sensors::Run() // when not adding sensors poll for param updates if (!_armed && hrt_elapsed_time(&_last_config_update) > 500_ms) { _voted_sensors_update.initializeSensors(); + InitializeVehicleAirData(); InitializeVehicleIMU(); _last_config_update = hrt_absolute_time(); @@ -621,8 +644,10 @@ int Sensors::print_status() { _voted_sensors_update.printStatus(); - PX4_INFO_RAW("\n"); - _vehicle_air_data.PrintStatus(); + if (_vehicle_air_data) { + PX4_INFO_RAW("\n"); + _vehicle_air_data->PrintStatus(); + } PX4_INFO_RAW("\n"); PX4_INFO("Airspeed status:"); diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 63663c09b7..f8e02e03f3 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp @@ -200,46 +200,64 @@ void VehicleAirData::Run() const sensor_baro_s &baro = _last_data[_selected_sensor_sub_index]; - // populate vehicle_air_data with primary baro and publish - vehicle_air_data_s out{}; - out.timestamp_sample = baro.timestamp; // TODO: baro.timestamp_sample; - out.baro_device_id = baro.device_id; - out.baro_temp_celcius = baro.temperature; - - // Convert from millibar to Pa and apply temperature compensation - out.baro_pressure_pa = 100.0f * baro.pressure - _thermal_offset[_selected_sensor_sub_index]; - - // calculate altitude using the hypsometric equation - static constexpr float T1 = 15.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin - static constexpr float a = -6.5f / 1000.0f; // temperature gradient in degrees per metre - - // current pressure at MSL in kPa (QNH in hPa) - const float p1 = _param_sens_baro_qnh.get() * 0.1f; - - // measured pressure in kPa - const float p = out.baro_pressure_pa * 0.001f; - - /* - * Solve: - * - * / -(aR / g) \ - * | (p / p1) . T1 | - T1 - * \ / - * h = ------------------------------- + h1 - * a - */ - 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.timestamp = hrt_absolute_time(); - _vehicle_air_data_pub.publish(out); + _baro_timestamp_sum += baro.timestamp; + _baro_sum += baro.pressure; + _baro_sum_count++; + + if ((_param_sens_baro_rate.get() > 0) + && hrt_elapsed_time(&_last_publication_timestamp) >= (1e6f / _param_sens_baro_rate.get())) { + + const float pressure = _baro_sum / _baro_sum_count; + const hrt_abstime timestamp_sample = _baro_timestamp_sum / _baro_sum_count; + + // reset + _baro_timestamp_sum = 0; + _baro_sum = 0.f; + _baro_sum_count = 0; + + // populate vehicle_air_data with primary baro and publish + vehicle_air_data_s out{}; + out.timestamp_sample = timestamp_sample; // TODO: baro.timestamp_sample; + out.baro_device_id = baro.device_id; + out.baro_temp_celcius = baro.temperature; + + // Convert from millibar to Pa and apply temperature compensation + out.baro_pressure_pa = 100.0f * pressure - _thermal_offset[_selected_sensor_sub_index]; + + // calculate altitude using the hypsometric equation + static constexpr float T1 = 15.0f - CONSTANTS_ABSOLUTE_NULL_CELSIUS; // temperature at base height in Kelvin + static constexpr float a = -6.5f / 1000.0f; // temperature gradient in degrees per metre + + // current pressure at MSL in kPa (QNH in hPa) + const float p1 = _param_sens_baro_qnh.get() * 0.1f; + + // measured pressure in kPa + const float p = out.baro_pressure_pa * 0.001f; + + /* + * Solve: + * + * / -(aR / g) \ + * | (p / p1) . T1 | - T1 + * \ / + * h = ------------------------------- + h1 + * a + */ + 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.timestamp = hrt_absolute_time(); + _vehicle_air_data_pub.publish(out); + + _last_publication_timestamp = out.timestamp; + } } // check failover and report diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index 7748b189bc..731c850174 100644 --- a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp +++ b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp @@ -86,12 +86,17 @@ private: perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + hrt_abstime _last_publication_timestamp{0}; hrt_abstime _last_error_message{0}; orb_advert_t _mavlink_log_pub{nullptr}; DataValidatorGroup _voter{1}; unsigned _last_failover_count{0}; + uint64_t _baro_timestamp_sum{0}; + float _baro_sum{0.f}; + int _baro_sum_count{0}; + sensor_baro_s _last_data[MAX_SENSOR_COUNT] {}; bool _advertised[MAX_SENSOR_COUNT] {}; @@ -102,7 +107,8 @@ private: int8_t _selected_sensor_sub_index{-1}; DEFINE_PARAMETERS( - (ParamFloat) _param_sens_baro_qnh + (ParamFloat) _param_sens_baro_qnh, + (ParamFloat) _param_sens_baro_rate ) }; }; // namespace sensors diff --git a/src/modules/sensors/vehicle_air_data/params.c b/src/modules/sensors/vehicle_air_data/params.c index af00b31c91..f86bc43969 100644 --- a/src/modules/sensors/vehicle_air_data/params.c +++ b/src/modules/sensors/vehicle_air_data/params.c @@ -43,3 +43,19 @@ * */ PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f); + +/** + * Baro max rate. + * + * Barometric air data maximum publication rate. This is an upper bound, + * actual barometric data rate is still dependant on the sensor. + * + * @min 1 + * @max 200 + * @group Sensors + * @unit Hz + * + * @reboot_required true + * + */ +PARAM_DEFINE_FLOAT(SENS_BARO_RATE, 20.0f);