mc_att_control move most orb subscriptions to uORB::Subscription

src/modules/mc_att_control/mc_att_control.hpp

@@ -41,6 +41,7 @@
 #include <px4_module_params.h>
 #include <px4_posix.h>
 #include <px4_tasks.h>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
@@ -101,19 +102,10 @@ private:
 	/**
 	 * Check for parameter update and handle it.
 	 */
-	void		battery_status_poll();
 	void		parameter_update_poll();
-	void		sensor_bias_poll();
-	void		vehicle_land_detected_poll();
-	void		sensor_correction_poll();
 	bool		vehicle_attitude_poll();
-	void		vehicle_attitude_setpoint_poll();
-	void		vehicle_control_mode_poll();
-	bool		vehicle_manual_poll();
 	void		vehicle_motor_limits_poll();
-	bool		vehicle_rates_setpoint_poll();
 	void		vehicle_status_poll();
-	void 		landing_gear_state_poll();
 
 	void		publish_actuator_controls();
 	void		publish_rates_setpoint();
@@ -150,20 +142,21 @@ private:
 
 	AttitudeControl _attitude_control; /**< class for attitude control calculations */
 
-	int		_v_att_sub{-1};			/**< vehicle attitude subscription */
-	int		_v_att_sp_sub{-1};		/**< vehicle attitude setpoint subscription */
-	int		_v_rates_sp_sub{-1};		/**< vehicle rates setpoint subscription */
-	int		_v_control_mode_sub{-1};	/**< vehicle control mode subscription */
-	int		_params_sub{-1};		/**< parameter updates subscription */
-	int		_manual_control_sp_sub{-1};	/**< manual control setpoint subscription */
-	int		_vehicle_status_sub{-1};	/**< vehicle status subscription */
-	int		_motor_limits_sub{-1};		/**< motor limits subscription */
-	int		_battery_status_sub{-1};	/**< battery status subscription */
+	uORB::Subscription _v_att_sub{ORB_ID(vehicle_attitude)};			/**< vehicle attitude subscription */
+	uORB::Subscription _v_att_sp_sub{ORB_ID(vehicle_attitude_setpoint)};		/**< vehicle attitude setpoint subscription */
+	uORB::Subscription _v_rates_sp_sub{ORB_ID(vehicle_rates_setpoint)};		/**< vehicle rates setpoint subscription */
+	uORB::Subscription _v_control_mode_sub{ORB_ID(vehicle_control_mode)};		/**< vehicle control mode subscription */
+	uORB::Subscription _params_sub{ORB_ID(parameter_update)};			/**< parameter updates subscription */
+	uORB::Subscription _manual_control_sp_sub{ORB_ID(manual_control_setpoint)};	/**< manual control setpoint subscription */
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};			/**< vehicle status subscription */
+	uORB::Subscription _motor_limits_sub{ORB_ID(multirotor_motor_limits)};		/**< motor limits subscription */
+	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};			/**< battery status subscription */
+	uORB::Subscription _sensor_correction_sub{ORB_ID(sensor_correction)};		/**< sensor thermal correction subscription */
+	uORB::Subscription _sensor_bias_sub{ORB_ID(sensor_bias)};			/**< sensor in-run bias correction subscription */
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};	/**< vehicle land detected subscription */
+	uORB::Subscription _landing_gear_sub{ORB_ID(landing_gear)};
+
 	int		_sensor_gyro_sub[MAX_GYRO_COUNT];	/**< gyro data subscription */
-	int		_sensor_correction_sub{-1};	/**< sensor thermal correction subscription */
-	int		_sensor_bias_sub{-1};		/**< sensor in-run bias correction subscription */
-	int		_vehicle_land_detected_sub{-1};	/**< vehicle land detected subscription */
-	int		_landing_gear_sub{-1};
 
 	unsigned _gyro_count{1};
 	int _selected_gyro{0};

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -171,83 +171,20 @@ MulticopterAttitudeControl::parameters_updated()
 void
 MulticopterAttitudeControl::parameter_update_poll()
 {
-	bool updated;
-
 	/* Check if parameters have changed */
-	orb_check(_params_sub, &updated);
+	parameter_update_s param_update;
 
-	if (updated) {
-		struct parameter_update_s param_update;
-		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
+	if (_params_sub.update(&param_update)) {
 		updateParams();
 		parameters_updated();
 	}
 }
 
-void
-MulticopterAttitudeControl::vehicle_control_mode_poll()
-{
-	bool updated;
-
-	/* Check if vehicle control mode has changed */
-	orb_check(_v_control_mode_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
-	}
-}
-
-bool
-MulticopterAttitudeControl::vehicle_manual_poll()
-{
-	bool updated;
-
-	/* get pilots inputs */
-	orb_check(_manual_control_sp_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
-		return true;
-	}
-	return false;
-}
-
-void
-MulticopterAttitudeControl::vehicle_attitude_setpoint_poll()
-{
-	/* check if there is a new setpoint */
-	bool updated;
-	orb_check(_v_att_sp_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(vehicle_attitude_setpoint), _v_att_sp_sub, &_v_att_sp);
-	}
-}
-
-bool
-MulticopterAttitudeControl::vehicle_rates_setpoint_poll()
-{
-	/* check if there is a new setpoint */
-	bool updated;
-	orb_check(_v_rates_sp_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_sub, &_v_rates_sp);
-		return true;
-	}
-	return false;
-}
-
 void
 MulticopterAttitudeControl::vehicle_status_poll()
 {
 	/* check if there is new status information */
-	bool vehicle_status_updated;
-	orb_check(_vehicle_status_sub, &vehicle_status_updated);
-
-	if (vehicle_status_updated) {
-		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
-
+	if (_vehicle_status_sub.update(&_vehicle_status)) {
 		/* set correct uORB ID, depending on if vehicle is VTOL or not */
 		if (_actuators_id == nullptr) {
 			if (_vehicle_status.is_vtol) {
@@ -271,41 +208,20 @@ void
 MulticopterAttitudeControl::vehicle_motor_limits_poll()
 {
 	/* check if there is a new message */
-	bool updated;
-	orb_check(_motor_limits_sub, &updated);
-
-	if (updated) {
-		multirotor_motor_limits_s motor_limits = {};
-		orb_copy(ORB_ID(multirotor_motor_limits), _motor_limits_sub, &motor_limits);
+	multirotor_motor_limits_s motor_limits{};
 
+	if (_motor_limits_sub.update(&motor_limits)) {
 		_saturation_status.value = motor_limits.saturation_status;
 	}
 }
 
-void
-MulticopterAttitudeControl::battery_status_poll()
-{
-	/* check if there is a new message */
-	bool updated;
-	orb_check(_battery_status_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(battery_status), _battery_status_sub, &_battery_status);
-	}
-}
-
 bool
 MulticopterAttitudeControl::vehicle_attitude_poll()
 {
 	/* check if there is a new message */
-	bool updated;
-	orb_check(_v_att_sub, &updated);
-
-	if (updated) {
-		uint8_t prev_quat_reset_counter = _v_att.quat_reset_counter;
-
-		orb_copy(ORB_ID(vehicle_attitude), _v_att_sub, &_v_att);
+	const uint8_t prev_quat_reset_counter = _v_att.quat_reset_counter;
 
+	if (_v_att_sub.update(&_v_att)) {
 		// Check for a heading reset
 		if (prev_quat_reset_counter != _v_att.quat_reset_counter) {
 			// we only extract the heading change from the delta quaternion
@@ -316,60 +232,6 @@ MulticopterAttitudeControl::vehicle_attitude_poll()
 	return false;
 }
 
-void
-MulticopterAttitudeControl::sensor_correction_poll()
-{
-	/* check if there is a new message */
-	bool updated;
-	orb_check(_sensor_correction_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(sensor_correction), _sensor_correction_sub, &_sensor_correction);
-	}
-
-	/* update the latest gyro selection */
-	if (_sensor_correction.selected_gyro_instance < _gyro_count) {
-		_selected_gyro = _sensor_correction.selected_gyro_instance;
-	}
-}
-
-void
-MulticopterAttitudeControl::sensor_bias_poll()
-{
-	/* check if there is a new message */
-	bool updated;
-	orb_check(_sensor_bias_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(sensor_bias), _sensor_bias_sub, &_sensor_bias);
-	}
-
-}
-
-void
-MulticopterAttitudeControl::vehicle_land_detected_poll()
-{
-	/* check if there is a new message */
-	bool updated;
-	orb_check(_vehicle_land_detected_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(vehicle_land_detected), _vehicle_land_detected_sub, &_vehicle_land_detected);
-	}
-
-}
-
-void
-MulticopterAttitudeControl::landing_gear_state_poll()
-{
-	bool updated;
-	orb_check(_landing_gear_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(landing_gear), _landing_gear_sub, &_landing_gear);
-	}
-}
-
 float
 MulticopterAttitudeControl::throttle_curve(float throttle_stick_input)
 {
@@ -519,7 +381,7 @@ MulticopterAttitudeControl::generate_attitude_setpoint(float dt, bool reset_yaw_
 void
 MulticopterAttitudeControl::control_attitude()
 {
-	vehicle_attitude_setpoint_poll();
+	_v_att_sp_sub.update(&_v_att_sp);
 
 	// reinitialize the setpoint while not armed to make sure no value from the last flight is still kept
 	if (!_v_control_mode.flag_armed) {
@@ -714,31 +576,12 @@ MulticopterAttitudeControl::publish_actuator_controls()
 void
 MulticopterAttitudeControl::run()
 {
-
-	/*
-	 * do subscriptions
-	 */
-	_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
-	_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
-	_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	_params_sub = orb_subscribe(ORB_ID(parameter_update));
-	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
-	_motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));
-	_battery_status_sub = orb_subscribe(ORB_ID(battery_status));
-
 	_gyro_count = math::constrain(orb_group_count(ORB_ID(sensor_gyro)), 1, MAX_GYRO_COUNT);
 
 	for (unsigned s = 0; s < _gyro_count; s++) {
 		_sensor_gyro_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
 	}
 
-	_sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
-	_sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias));
-	_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
-	_landing_gear_sub = orb_subscribe(ORB_ID(landing_gear));
-
 	/* wakeup source: gyro data from sensor selected by the sensor app */
 	px4_pollfd_struct_t poll_fds = {};
 	poll_fds.events = POLLIN;
@@ -754,7 +597,13 @@ MulticopterAttitudeControl::run()
 	while (!should_exit()) {
 
 		// check if the selected gyro has updated first
-		sensor_correction_poll();
+		_sensor_correction_sub.update(&_sensor_correction);
+
+		/* update the latest gyro selection */
+		if (_sensor_correction.selected_gyro_instance < _gyro_count) {
+			_selected_gyro = _sensor_correction.selected_gyro_instance;
+		}
+
 		poll_fds.fd = _sensor_gyro_sub[_selected_gyro];
 
 		/* wait for up to 100ms for data */
@@ -795,15 +644,16 @@ MulticopterAttitudeControl::run()
 			}
 
 			/* check for updates in other topics */
-			vehicle_control_mode_poll();
+			_v_control_mode_sub.update(&_v_control_mode);
+			_battery_status_sub.update(&_battery_status);
+			_sensor_bias_sub.update(&_sensor_bias);
+			_vehicle_land_detected_sub.update(&_vehicle_land_detected);
+			_landing_gear_sub.update(&_landing_gear);
 			vehicle_status_poll();
 			vehicle_motor_limits_poll();
-			battery_status_poll();
-			sensor_bias_poll();
-			vehicle_land_detected_poll();
-			landing_gear_state_poll();
-			const bool manual_control_updated = vehicle_manual_poll();
+			const bool manual_control_updated = _manual_control_sp_sub.update(&_manual_control_sp);
 			const bool attitude_updated = vehicle_attitude_poll();
+
 			attitude_dt += dt;
 
 			/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
@@ -864,7 +714,7 @@ MulticopterAttitudeControl::run()
 
 				} else {
 					/* attitude controller disabled, poll rates setpoint topic */
-					if (vehicle_rates_setpoint_poll()) {
+					if (_v_rates_sp_sub.update(&_v_rates_sp)) {
 						_rates_sp(0) = _v_rates_sp.roll;
 						_rates_sp(1) = _v_rates_sp.pitch;
 						_rates_sp(2) = _v_rates_sp.yaw;
@@ -913,24 +763,9 @@ MulticopterAttitudeControl::run()
 		perf_end(_loop_perf);
 	}
 
-	orb_unsubscribe(_v_att_sub);
-	orb_unsubscribe(_v_att_sp_sub);
-	orb_unsubscribe(_v_rates_sp_sub);
-	orb_unsubscribe(_v_control_mode_sub);
-	orb_unsubscribe(_params_sub);
-	orb_unsubscribe(_manual_control_sp_sub);
-	orb_unsubscribe(_vehicle_status_sub);
-	orb_unsubscribe(_motor_limits_sub);
-	orb_unsubscribe(_battery_status_sub);
-
 	for (unsigned s = 0; s < _gyro_count; s++) {
 		orb_unsubscribe(_sensor_gyro_sub[s]);
 	}
-
-	orb_unsubscribe(_sensor_correction_sub);
-	orb_unsubscribe(_sensor_bias_sub);
-	orb_unsubscribe(_vehicle_land_detected_sub);
-	orb_unsubscribe(_landing_gear_sub);
 }
 
 int MulticopterAttitudeControl::task_spawn(int argc, char *argv[])