Browse Source

vehicle_angular_velocity: sensor_selection callback

- needed to update the main sensor_gyro subscription if the primary
stops responding
sbg
Daniel Agar 6 years ago
parent
commit
7b179776e2
  1. 5
      src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp
  2. 10
      src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

5
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.cpp

@ -68,7 +68,8 @@ VehicleAngularVelocity::Start()
ParametersUpdate(true); ParametersUpdate(true);
SensorBiasUpdate(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); return SensorCorrectionsUpdate(true);
} }
@ -83,7 +84,7 @@ VehicleAngularVelocity::Stop()
sub.unregister_callback(); sub.unregister_callback();
} }
_sensor_correction_sub.unregister_callback(); _sensor_selection_sub.unregister_callback();
} }
void void

10
src/modules/sensors/vehicle_angular_velocity/VehicleAngularVelocity.hpp

@ -50,6 +50,7 @@
#include <uORB/topics/sensor_bias.h> #include <uORB/topics/sensor_bias.h>
#include <uORB/topics/sensor_correction.h> #include <uORB/topics/sensor_correction.h>
#include <uORB/topics/sensor_gyro.h> #include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_angular_velocity.h> #include <uORB/topics/vehicle_angular_velocity.h>
#define MAX_GYRO_COUNT 3 #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::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 _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_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_selection_sub{this, ORB_ID(sensor_selection)}; /**< selected primary sensor subscription */
uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub[MAX_GYRO_COUNT] { /**< gyro data subscription */ uORB::SubscriptionCallbackWorkItem _sensor_gyro_sub[MAX_GYRO_COUNT] { /**< gyro data subscription */
{this, ORB_ID(sensor_gyro), 0}, {this, ORB_ID(sensor_gyro), 0},
{this, ORB_ID(sensor_gyro), 1}, {this, ORB_ID(sensor_gyro), 1},
{this, ORB_ID(sensor_gyro), 2} {this, ORB_ID(sensor_gyro), 2}

Loading…
Cancel
Save