sensors partially move to new uORB::Subscription

src/modules/sensors/rc_update.cpp

@@ -39,14 +39,8 @@
 
 #include "rc_update.h"
 
-#include <string.h>
-#include <float.h>
-#include <errno.h>
-
-#include <uORB/uORB.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/manual_control_setpoint.h>
-#include <uORB/topics/input_rc.h>
 
 using namespace sensors;
 
@@ -57,32 +51,6 @@ RCUpdate::RCUpdate(const Parameters &parameters)
 	  _filter_yaw(50.0f, 10.f),
 	  _filter_throttle(50.0f, 10.f)
 {
-	memset(&_rc, 0, sizeof(_rc));
-	memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map));
-	memset(&_param_rc_values, 0, sizeof(_param_rc_values));
-}
-
-int RCUpdate::init()
-{
-	_rc_sub = orb_subscribe(ORB_ID(input_rc));
-
-	if (_rc_sub < 0) {
-		return -errno;
-	}
-
-	_rc_parameter_map_sub = orb_subscribe(ORB_ID(rc_parameter_map));
-
-	if (_rc_parameter_map_sub < 0) {
-		return -errno;
-	}
-
-	return 0;
-}
-
-void RCUpdate::deinit()
-{
-	orb_unsubscribe(_rc_sub);
-	orb_unsubscribe(_rc_parameter_map_sub);
 }
 
 void RCUpdate::update_rc_functions()
@@ -134,11 +102,8 @@ void RCUpdate::update_rc_functions()
 void
 RCUpdate::rc_parameter_map_poll(ParameterHandles &parameter_handles, bool forced)
 {
-	bool map_updated;
+	if (_rc_parameter_map_sub.updated()) {
-	orb_check(_rc_parameter_map_sub, &map_updated);
+		_rc_parameter_map_sub.copy(&_rc_parameter_map);
-
-	if (map_updated) {
-		orb_copy(ORB_ID(rc_parameter_map), _rc_parameter_map_sub, &_rc_parameter_map);
 
 		/* update parameter handles to which the RC channels are mapped */
 		for (int i = 0; i < rc_parameter_map_s::RC_PARAM_MAP_NCHAN; i++) {
@@ -253,14 +218,10 @@ RCUpdate::set_params_from_rc(const ParameterHandles &parameter_handles)
 void
 RCUpdate::rc_poll(const ParameterHandles &parameter_handles)
 {
-	bool rc_updated;
+	if (_rc_sub.updated()) {
-	orb_check(_rc_sub, &rc_updated);
-
-	if (rc_updated) {
 		/* read low-level values from FMU or IO RC inputs (PPM, Spektrum, S.Bus) */
-		struct input_rc_s rc_input;
+		input_rc_s rc_input{};
-
+		_rc_sub.copy(&rc_input);
-		orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
 
 		/* detect RC signal loss */
 		bool signal_lost;

src/modules/sensors/rc_update.h

@@ -44,7 +44,9 @@
 #include <drivers/drv_hrt.h>
 #include <mathlib/mathlib.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/rc_channels.h>
+#include <uORB/topics/rc_parameter_map.h>
 
 namespace sensors
 {
@@ -63,17 +65,6 @@ public:
 	 */
 	RCUpdate(const Parameters &parameters);
 
-	/**
-	 * initialize subscriptions etc.
-	 * @return 0 on success, <0 otherwise
-	 */
-	int init();
-
-	/**
-	 * deinitialize the object (we cannot use the destructor because it is called on the wrong thread)
-	 */
-	void deinit();
-
 	/**
 	 * Check for changes in rc_parameter_map
 	 */
@@ -111,17 +102,17 @@ private:
 	void set_params_from_rc(const ParameterHandles &parameter_handles);
 
 
-	int		_rc_sub = -1;			/**< raw rc channels data subscription */
+	uORB::Subscription	_rc_sub{ORB_ID(input_rc)};				/**< raw rc channels data subscription */
-	int		_rc_parameter_map_sub = -1;		/**< rc parameter map subscription */
+	uORB::Subscription	_rc_parameter_map_sub{ORB_ID(rc_parameter_map)};	/**< rc parameter map subscription */
 
 	orb_advert_t	_rc_pub = nullptr;		/**< raw r/c control topic */
 	orb_advert_t	_manual_control_pub = nullptr;	/**< manual control signal topic */
 	orb_advert_t	_actuator_group_3_pub = nullptr;/**< manual control as actuator topic */
 
-	struct rc_channels_s _rc;			/**< r/c channel data */
+	rc_channels_s _rc {};			/**< r/c channel data */
 
-	struct rc_parameter_map_s _rc_parameter_map;
+	rc_parameter_map_s _rc_parameter_map {};
-	float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN];	/**< parameter values for RC control */
+	float _param_rc_values[rc_parameter_map_s::RC_PARAM_MAP_NCHAN] {};	/**< parameter values for RC control */
 
 	const Parameters &_parameters;
 

src/modules/sensors/sensors.cpp

@@ -163,10 +163,10 @@ private:
 	const bool	_hil_enabled;			/**< if true, HIL is active */
 	bool		_armed{false};				/**< arming status of the vehicle */
 
-	int		_actuator_ctrl_0_sub{-1};		/**< attitude controls sub */
+	uORB::Subscription	_actuator_ctrl_0_sub{ORB_ID(actuator_controls_0)};		/**< attitude controls sub */
-	int		_diff_pres_sub{-1};			/**< raw differential pressure subscription */
+	uORB::Subscription	_diff_pres_sub{ORB_ID(differential_pressure)};			/**< raw differential pressure subscription */
-	int		_vcontrol_mode_sub{-1};		/**< vehicle control mode subscription */
+	uORB::Subscription	_vcontrol_mode_sub{ORB_ID(vehicle_control_mode)};		/**< vehicle control mode subscription */
-	int 		_params_sub{-1};			/**< notification of parameter updates */
+	uORB::Subscription	_params_sub{ORB_ID(parameter_update)};				/**< notification of parameter updates */
 
 	orb_advert_t	_sensor_pub{nullptr};			/**< combined sensor data topic */
 	orb_advert_t	_airdata_pub{nullptr};			/**< combined sensor data topic */
@@ -220,11 +220,6 @@ private:
 	 */
 	void		diff_pres_poll(const vehicle_air_data_s &airdata);
 
-	/**
-	 * Check for changes in vehicle control mode.
-	 */
-	void		vehicle_control_mode_poll();
-
 	/**
 	 * Check for changes in parameters.
 	 */
@@ -297,16 +292,9 @@ Sensors::adc_init()
 void
 Sensors::diff_pres_poll(const vehicle_air_data_s &raw)
 {
-	bool updated;
+	differential_pressure_s diff_pres{};
-	orb_check(_diff_pres_sub, &updated);
 
-	if (updated) {
+	if (_diff_pres_sub.update(&diff_pres)) {
-		differential_pressure_s diff_pres;
-		int ret = orb_copy(ORB_ID(differential_pressure), _diff_pres_sub, &diff_pres);
-
-		if (ret != PX4_OK) {
-			return;
-		}
 
 		float air_temperature_celsius = (diff_pres.temperature > -300.0f) ? diff_pres.temperature :
 						(raw.baro_temp_celcius - PCB_TEMP_ESTIMATE_DEG);
@@ -360,33 +348,14 @@ Sensors::diff_pres_poll(const vehicle_air_data_s &raw)
 	}
 }
 
-void
-Sensors::vehicle_control_mode_poll()
-{
-	struct vehicle_control_mode_s vcontrol_mode;
-	bool vcontrol_mode_updated;
-
-	orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated);
-
-	if (vcontrol_mode_updated) {
-
-		orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode);
-		_armed = vcontrol_mode.flag_armed;
-	}
-}
-
 void
 Sensors::parameter_update_poll(bool forced)
 {
-	bool param_updated = false;
-
 	/* Check if any parameter has changed */
-	orb_check(_params_sub, &param_updated);
+	parameter_update_s update;
 
-	if (param_updated || forced) {
+	if (_params_sub.update(&update) || forced) {
 		/* read from param to clear updated flag */
-		struct parameter_update_s update;
-		orb_copy(ORB_ID(parameter_update), _params_sub, &update);
 
 		parameters_update();
 		updateParams();
@@ -557,10 +526,10 @@ Sensors::adc_poll()
 						connected &= valid_chan[b];
 					}
 
-					actuator_controls_s ctrl;
+					actuator_controls_s ctrl{};
-					orb_copy(ORB_ID(actuator_controls_0), _actuator_ctrl_0_sub, &ctrl);
+					_actuator_ctrl_0_sub.copy(&ctrl);
 
-					battery_status_s battery_status;
+					battery_status_s battery_status{};
 					_battery[b].updateBatteryStatus(t, bat_voltage_v[b], bat_current_a[b],
 									connected, selected_source == b, b,
 									ctrl.control[actuator_controls_s::INDEX_THROTTLE],
@@ -592,20 +561,11 @@ Sensors::run()
 	vehicle_air_data_s airdata = {};
 	vehicle_magnetometer_s magnetometer = {};
 
-	_rc_update.init();
 	_voted_sensors_update.init(raw);
 
 	/* (re)load params and calibration */
 	parameter_update_poll(true);
 
-	/*
-	 * do subscriptions
-	 */
-	_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
-	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	_params_sub = orb_subscribe(ORB_ID(parameter_update));
-	_actuator_ctrl_0_sub = orb_subscribe(ORB_ID(actuator_controls_0));
-
 	/* get a set of initial values */
 	_voted_sensors_update.sensors_poll(raw, airdata, magnetometer);
 
@@ -649,7 +609,11 @@ Sensors::run()
 		perf_begin(_loop_perf);
 
 		/* check vehicle status for changes to publication state */
-		vehicle_control_mode_poll();
+		if (_vcontrol_mode_sub.updated()) {
+			vehicle_control_mode_s vcontrol_mode{};
+			_vcontrol_mode_sub.copy(&vcontrol_mode);
+			_armed = vcontrol_mode.flag_armed;
+		}
 
 		/* the timestamp of the raw struct is updated by the gyro_poll() method (this makes the gyro
 		 * a mandatory sensor) */
@@ -714,11 +678,6 @@ Sensors::run()
 		perf_end(_loop_perf);
 	}
 
-	orb_unsubscribe(_diff_pres_sub);
-	orb_unsubscribe(_vcontrol_mode_sub);
-	orb_unsubscribe(_params_sub);
-	orb_unsubscribe(_actuator_ctrl_0_sub);
-
 	if (_sensor_pub) {
 		orb_unadvertise(_sensor_pub);
 	}
@@ -731,7 +690,6 @@ Sensors::run()
 		orb_unadvertise(_magnetometer_pub);
 	}
 
-	_rc_update.deinit();
 	_voted_sensors_update.deinit();
 }