diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp index 9f7d312bcc..01885e86b2 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp @@ -68,7 +68,8 @@ VehicleAngularVelocity::Start() ParametersUpdate(true); SensorBiasUpdate(true); - _sensor_correction_sub.register_callback(); + // needed to change the active sensor if the primary stops updating + _sensor_selection_sub.register_callback(); return SensorCorrectionsUpdate(true); } @@ -83,7 +84,7 @@ VehicleAngularVelocity::Stop() sub.unregister_callback(); } - _sensor_correction_sub.unregister_callback(); + _sensor_selection_sub.unregister_callback(); } void diff --git a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp index b3831fe146..cedcb733e0 100644 --- a/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp +++ b/src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #define MAX_GYRO_COUNT 3 @@ -85,11 +86,12 @@ private: uORB::Publication _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)}; - uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ - uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; /**< sensor in-run bias correction subscription */ + uORB::Subscription _params_sub{ORB_ID(parameter_update)}; /**< parameter updates subscription */ + uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)}; /**< sensor in-run bias correction subscription */ + uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)}; /**< sensor thermal correction subscription */ - uORB::SubscriptionCallbackWorkItem _sensor_correction_sub{this, ORB_ID(sensor_correction)}; /**< sensor thermal correction subscription */ - uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub[MAX_GYRO_COUNT] { /**< gyro data subscription */ + uORB::SubscriptionCallbackWorkItem _sensor_selection_sub{this, ORB_ID(sensor_selection)}; /**< selected primary sensor subscription */ + uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub[MAX_GYRO_COUNT] { /**< gyro data subscription */ {this, ORB_ID(sensor_gyro), 0}, {this, ORB_ID(sensor_gyro), 1}, {this, ORB_ID(sensor_gyro), 2}