diff --git a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp index 979b378a49..043019b4a5 100644 --- a/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp +++ b/src/modules/sensors/vehicle_acceleration/VehicleAcceleration.cpp @@ -171,11 +171,12 @@ void VehicleAcceleration::Run() { // update corrections first to set _selected_sensor - SensorCorrectionsUpdate(); + bool sensor_select_update = SensorCorrectionsUpdate(); - sensor_accel_s sensor_data; + if (_sensor_sub[_selected_sensor].updated() || sensor_select_update) { + sensor_accel_s sensor_data; + _sensor_sub[_selected_sensor].copy(&sensor_data); - if (_sensor_sub[_selected_sensor].update(&sensor_data)) { ParametersUpdate(); SensorBiasUpdate(); diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 0f762f0d1d..8bf0ded953 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -210,13 +210,14 @@ void VehicleAngularVelocity::Run() { // update corrections first to set _selected_sensor - SensorCorrectionsUpdate(); + bool sensor_select_update = SensorCorrectionsUpdate(); if (_sensor_control_available) { // using sensor_gyro_control is preferred, but currently not all drivers (eg df_*) provide sensor_gyro_control - sensor_gyro_control_s sensor_data; + if (_sensor_control_sub[_selected_sensor].updated() || sensor_select_update) { + sensor_gyro_control_s sensor_data; + _sensor_control_sub[_selected_sensor].copy(&sensor_data); - if (_sensor_control_sub[_selected_sensor].update(&sensor_data)) { ParametersUpdate(); SensorBiasUpdate(); @@ -239,9 +240,10 @@ VehicleAngularVelocity::Run() } else { // otherwise fallback to using sensor_gyro (legacy that will be removed) - sensor_gyro_s sensor_data; + if (_sensor_sub[_selected_sensor].updated() || sensor_select_update) { + sensor_gyro_s sensor_data; + _sensor_sub[_selected_sensor].copy(&sensor_data); - if (_sensor_sub[_selected_sensor].update(&sensor_data)) { ParametersUpdate(); SensorBiasUpdate();