From 97a75fc388ebc6063ea9b803a90730fe1702c253 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 7 Feb 2022 10:44:51 -0500 Subject: [PATCH] sensors: skip selection and failover checks during parameter update cycles --- .../vehicle_air_data/VehicleAirData.cpp | 16 +++-- .../vehicle_air_data/VehicleAirData.hpp | 4 +- .../VehicleMagnetometer.cpp | 17 +++-- .../VehicleMagnetometer.hpp | 4 +- src/modules/sensors/voted_sensors_update.cpp | 69 +++++++++++-------- src/modules/sensors/voted_sensors_update.h | 4 +- 6 files changed, 68 insertions(+), 46 deletions(-) diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.cpp index 5cf0a42c63..8e4ab8f413 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-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 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 @@ -127,7 +127,7 @@ void VehicleAirData::AirTemperatureUpdate() } } -void VehicleAirData::ParametersUpdate() +bool VehicleAirData::ParametersUpdate() { // Check if parameters have changed if (_parameter_update_sub.updated()) { @@ -136,7 +136,10 @@ void VehicleAirData::ParametersUpdate() _parameter_update_sub.copy(¶m_update); updateParams(); + return true; } + + return false; } void VehicleAirData::Run() @@ -145,7 +148,7 @@ void VehicleAirData::Run() const hrt_abstime time_now_us = hrt_absolute_time(); - ParametersUpdate(); + const bool parameter_update = ParametersUpdate(); SensorCorrectionsUpdate(); @@ -203,7 +206,8 @@ void VehicleAirData::Run() _voter.get_best(time_now_us, &best_index); if (best_index >= 0) { - if (_selected_sensor_sub_index != best_index) { + // handle selection change (don't process on same iteration as parameter update) + if ((_selected_sensor_sub_index != best_index) && !parameter_update) { // clear all registered callbacks for (auto &sub : _sensor_sub) { sub.unregisterCallback(); @@ -281,8 +285,8 @@ void VehicleAirData::Run() } } - // check failover and report - if (_last_failover_count != _voter.failover_count()) { + // check failover and report (save failover report for a cycle where parameters didn't update) + if (_last_failover_count != _voter.failover_count() && !parameter_update) { uint32_t flags = _voter.failover_state(); int failover_index = _voter.failover_index(); diff --git a/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp b/src/modules/sensors/vehicle_air_data/VehicleAirData.hpp index 4fed84dc9d..736fbf6c34 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-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 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 @@ -71,7 +71,7 @@ public: private: void Run() override; - void ParametersUpdate(); + bool ParametersUpdate(); void SensorCorrectionsUpdate(bool force = false); void AirTemperatureUpdate(); diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp index cda243e567..1c609fed58 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 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 @@ -79,7 +79,7 @@ void VehicleMagnetometer::Stop() } } -void VehicleMagnetometer::ParametersUpdate(bool force) +bool VehicleMagnetometer::ParametersUpdate(bool force) { // Check if parameters have changed if (_parameter_update_sub.updated() || force) { @@ -161,7 +161,11 @@ void VehicleMagnetometer::ParametersUpdate(bool force) } } + + return true; } + + return false; } void VehicleMagnetometer::UpdateMagBiasEstimate() @@ -360,7 +364,7 @@ void VehicleMagnetometer::Run() const hrt_abstime time_now_us = hrt_absolute_time(); - ParametersUpdate(); + const bool parameter_update = ParametersUpdate(); // check vehicle status for changes to armed state if (_vehicle_control_mode_sub.updated()) { @@ -450,7 +454,8 @@ void VehicleMagnetometer::Run() _voter.get_best(time_now_us, &best_index); if (best_index >= 0) { - if (_selected_sensor_sub_index != best_index) { + // handle selection change (don't process on same iteration as parameter update) + if ((_selected_sensor_sub_index != best_index) && !parameter_update) { // clear all registered callbacks for (auto &sub : _sensor_sub) { sub.unregisterCallback(); @@ -488,9 +493,9 @@ void VehicleMagnetometer::Run() } - // check failover and report if (_param_sens_mag_mode.get()) { - if (_last_failover_count != _voter.failover_count()) { + // check failover and report (save failover report for a cycle where parameters didn't update) + if (_last_failover_count != _voter.failover_count() && !parameter_update) { uint32_t flags = _voter.failover_state(); int failover_index = _voter.failover_index(); diff --git a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp index aaa5968ba6..bf673e5efa 100644 --- a/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp +++ b/src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2020-2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2020-2022 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 @@ -79,7 +79,7 @@ public: private: void Run() override; - void ParametersUpdate(bool force = false); + bool ParametersUpdate(bool force = false); void Publish(uint8_t instance, bool multi = false); diff --git a/src/modules/sensors/voted_sensors_update.cpp b/src/modules/sensors/voted_sensors_update.cpp index fd14defed3..5deb0106d1 100644 --- a/src/modules/sensors/voted_sensors_update.cpp +++ b/src/modules/sensors/voted_sensors_update.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016, 2021 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2022 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 @@ -80,6 +80,8 @@ void VotedSensorsUpdate::initializeSensors() void VotedSensorsUpdate::parametersUpdate() { + _parameter_update = true; + updateParams(); // run through all IMUs @@ -188,40 +190,44 @@ void VotedSensorsUpdate::imuPoll(struct sensor_combined_s &raw) } // find the best sensor - int accel_best_index = -1; - int gyro_best_index = -1; - _accel.voter.get_best(time_now_us, &accel_best_index); - _gyro.voter.get_best(time_now_us, &gyro_best_index); + int accel_best_index = _accel.last_best_vote; + int gyro_best_index = _gyro.last_best_vote; - if (!_param_sens_imu_mode.get() && ((_selection.timestamp != 0) || (_sensor_selection_sub.updated()))) { - // use sensor_selection to find best - if (_sensor_selection_sub.update(&_selection)) { - // reset inconsistency checks against primary - for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { - _accel_diff[sensor_index].zero(); - _gyro_diff[sensor_index].zero(); + if (!_parameter_update) { + // update current accel/gyro selection, skipped on cycles where parameters update + _accel.voter.get_best(time_now_us, &accel_best_index); + _gyro.voter.get_best(time_now_us, &gyro_best_index); + + if (!_param_sens_imu_mode.get() && ((_selection.timestamp != 0) || (_sensor_selection_sub.updated()))) { + // use sensor_selection to find best + if (_sensor_selection_sub.update(&_selection)) { + // reset inconsistency checks against primary + for (int sensor_index = 0; sensor_index < MAX_SENSOR_COUNT; sensor_index++) { + _accel_diff[sensor_index].zero(); + _gyro_diff[sensor_index].zero(); + } } + + for (int i = 0; i < MAX_SENSOR_COUNT; i++) { + if ((_accel_device_id[i] != 0) && (_accel_device_id[i] == _selection.accel_device_id)) { + accel_best_index = i; + } + + if ((_gyro_device_id[i] != 0) && (_gyro_device_id[i] == _selection.gyro_device_id)) { + gyro_best_index = i; + } + } + + } else { + // use sensor voter to find best if SENS_IMU_MODE is enabled or ORB_ID(sensor_selection) has never published + checkFailover(_accel, "Accel", events::px4::enums::sensor_type_t::accel); + checkFailover(_gyro, "Gyro", events::px4::enums::sensor_type_t::gyro); } - - for (int i = 0; i < MAX_SENSOR_COUNT; i++) { - if ((_accel_device_id[i] != 0) && (_accel_device_id[i] == _selection.accel_device_id)) { - accel_best_index = i; - } - - if ((_gyro_device_id[i] != 0) && (_gyro_device_id[i] == _selection.gyro_device_id)) { - gyro_best_index = i; - } - } - - } else { - // use sensor voter to find best if SENS_IMU_MODE is enabled or ORB_ID(sensor_selection) has never published - checkFailover(_accel, "Accel", events::px4::enums::sensor_type_t::accel); - checkFailover(_gyro, "Gyro", events::px4::enums::sensor_type_t::gyro); } // write data for the best sensor to output variables - if ((accel_best_index >= 0) && (accel_best_index < MAX_SENSOR_COUNT) - && (gyro_best_index >= 0) && (gyro_best_index < MAX_SENSOR_COUNT)) { + if ((accel_best_index >= 0) && (accel_best_index < MAX_SENSOR_COUNT) && (_accel_device_id[accel_best_index] != 0) + && (gyro_best_index >= 0) && (gyro_best_index < MAX_SENSOR_COUNT) && (_gyro_device_id[gyro_best_index] != 0)) { raw.timestamp = _last_sensor_data[gyro_best_index].timestamp; memcpy(&raw.accelerometer_m_s2, &_last_sensor_data[accel_best_index].accelerometer_m_s2, @@ -423,6 +429,11 @@ void VotedSensorsUpdate::sensorsPoll(sensor_combined_s &raw) status.timestamp = hrt_absolute_time(); _sensors_status_imu_pub.publish(status); + + if (_parameter_update) { + // reset + _parameter_update = false; + } } void VotedSensorsUpdate::setRelativeTimestamps(sensor_combined_s &raw) diff --git a/src/modules/sensors/voted_sensors_update.h b/src/modules/sensors/voted_sensors_update.h index 4ae773a867..fab257b182 100644 --- a/src/modules/sensors/voted_sensors_update.h +++ b/src/modules/sensors/voted_sensors_update.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2016-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2016-2022 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 @@ -182,6 +182,8 @@ private: sensor_selection_s _selection {}; /**< struct containing the sensor selection to be published to the uORB */ + bool _parameter_update{false}; + DEFINE_PARAMETERS( (ParamBool) _param_sens_imu_mode )