From 7b179776e291b039dc1f7111cb46c5fb3b7d44c5 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Tue, 6 Aug 2019 13:28:06 -0400
Subject: [PATCH] vehicle_angular_velocity: sensor_selection callback

 - needed to update the main sensor_gyro subscription if the primary
stops responding
---
 .../VehicleAngularVelocity.cpp                         |  5 +++--
 .../VehicleAngularVelocity.hpp                         | 10 ++++++----
 2 files changed, 9 insertions(+), 6 deletions(-)

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 <uORB/topics/sensor_bias.h>
 #include <uORB/topics/sensor_correction.h>
 #include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/sensor_selection.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 
 #define MAX_GYRO_COUNT 3
@@ -85,11 +86,12 @@ private:
 
 	uORB::Publication<vehicle_angular_velocity_s>	_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}