fw_pos_control_l1 move to new uORB::Subscription

src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp

@@ -297,14 +297,10 @@ FixedwingPositionControl::parameters_update()
 void
 FixedwingPositionControl::vehicle_control_mode_poll()
 {
-	bool updated = false;
-
-	orb_check(_control_mode_sub, &updated);
-
-	if (updated) {
+	if (_control_mode_sub.updated()) {
 		const bool was_armed = _control_mode.flag_armed;
 
-		if (orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode) == PX4_OK) {
+		if (_control_mode_sub.copy(&_control_mode)) {
 
 			// reset state when arming
 			if (!was_armed && _control_mode.flag_armed) {
@@ -318,12 +314,8 @@ FixedwingPositionControl::vehicle_control_mode_poll()
 void
 FixedwingPositionControl::vehicle_command_poll()
 {
-	bool updated;
-
-	orb_check(_vehicle_command_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(vehicle_command), _vehicle_command_sub, &_vehicle_command);
+	if (_vehicle_command_sub.updated()) {
+		_vehicle_command_sub.copy(&_vehicle_command);
 		handle_command();
 	}
 }
@@ -331,13 +323,7 @@ FixedwingPositionControl::vehicle_command_poll()
 void
 FixedwingPositionControl::vehicle_status_poll()
 {
-	bool updated;
-
-	orb_check(_vehicle_status_sub, &updated);
-
-	if (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 (_attitude_setpoint_id == nullptr) {
 			if (_vehicle_status.is_vtol) {
@@ -355,31 +341,6 @@ FixedwingPositionControl::vehicle_status_poll()
 	}
 }
 
-void
-FixedwingPositionControl::vehicle_land_detected_poll()
-{
-	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
-FixedwingPositionControl::manual_control_setpoint_poll()
-{
-	bool manual_updated;
-
-	/* Check if manual setpoint has changed */
-	orb_check(_manual_control_sub, &manual_updated);
-
-	if (manual_updated) {
-		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual);
-	}
-}
-
 void
 FixedwingPositionControl::airspeed_poll()
 {
@@ -419,39 +380,21 @@ FixedwingPositionControl::airspeed_poll()
 void
 FixedwingPositionControl::vehicle_attitude_poll()
 {
-	/* check if there is a new position */
-	bool updated;
-	orb_check(_vehicle_attitude_sub, &updated);
-
-	if (updated) {
-		orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &_att);
-	}
-
-	/* set rotation matrix and euler angles */
-	_R_nb = Quatf(_att.q);
-
-	// if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset
-	// between multirotor and fixed wing flight
-	if (static_cast<vtol_type>(_parameters.vtol_type) == vtol_type::TAILSITTER && _vehicle_status.is_vtol) {
-		Dcmf R_offset = Eulerf(0, M_PI_2_F, 0);
-		_R_nb = _R_nb * R_offset;
-	}
-
-	Eulerf euler_angles(_R_nb);
-	_roll    = euler_angles(0);
-	_pitch   = euler_angles(1);
-	_yaw     = euler_angles(2);
-}
-
-void
-FixedwingPositionControl::position_setpoint_triplet_poll()
-{
-	/* check if there is a new setpoint */
-	bool pos_sp_triplet_updated;
-	orb_check(_pos_sp_triplet_sub, &pos_sp_triplet_updated);
+	if (_vehicle_attitude_sub.update(&_att)) {
+		/* set rotation matrix and euler angles */
+		_R_nb = Quatf(_att.q);
+
+		// if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset
+		// between multirotor and fixed wing flight
+		if (static_cast<vtol_type>(_parameters.vtol_type) == vtol_type::TAILSITTER && _vehicle_status.is_vtol) {
+			Dcmf R_offset = Eulerf(0, M_PI_2_F, 0);
+			_R_nb = _R_nb * R_offset;
+		}
 
-	if (pos_sp_triplet_updated) {
-		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
+		Eulerf euler_angles(_R_nb);
+		_roll    = euler_angles(0);
+		_pitch   = euler_angles(1);
+		_yaw     = euler_angles(2);
 	}
 }
 
@@ -1732,20 +1675,7 @@ FixedwingPositionControl::handle_command()
 void
 FixedwingPositionControl::run()
 {
-	/*
-	 * do subscriptions
-	 */
 	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
-	_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
-	_pos_sp_triplet_sub = orb_subscribe(ORB_ID(position_setpoint_triplet));
-	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
-	_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
-	_vehicle_command_sub = orb_subscribe(ORB_ID(vehicle_command));
-	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
-	_vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
-	_params_sub = orb_subscribe(ORB_ID(parameter_update));
-	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	_sensor_baro_sub = orb_subscribe(ORB_ID(sensor_baro));
 
 	/* rate limit position updates to 50 Hz */
 	orb_set_interval(_global_pos_sub, 20);
@@ -1780,13 +1710,12 @@ FixedwingPositionControl::run()
 		}
 
 		/* only update parameters if they changed */
-		bool params_updated = false;
-		orb_check(_params_sub, &params_updated);
+		bool params_updated = _params_sub.updated();
 
 		if (params_updated) {
 			/* read from param to clear updated flag */
 			parameter_update_s update;
-			orb_copy(ORB_ID(parameter_update), _params_sub, &update);
+			_params_sub.copy(&update);
 
 			/* update parameters from storage */
 			parameters_update();
@@ -1798,7 +1727,7 @@ FixedwingPositionControl::run()
 
 			/* load local copies */
 			orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
-			orb_copy(ORB_ID(vehicle_local_position), _local_pos_sub, &_local_pos);
+			_local_pos_sub.update(&_local_pos);
 
 			// handle estimator reset events. we only adjust setpoins for manual modes
 			if (_control_mode.flag_control_manual_enabled) {
@@ -1823,12 +1752,12 @@ FixedwingPositionControl::run()
 
 			_sub_sensors.update();
 			airspeed_poll();
-			manual_control_setpoint_poll();
-			position_setpoint_triplet_poll();
+			_manual_control_sub.update(&_manual);
+			_pos_sp_triplet_sub.update(&_pos_sp_triplet);
 			vehicle_attitude_poll();
 			vehicle_command_poll();
 			vehicle_control_mode_poll();
-			vehicle_land_detected_poll();
+			_vehicle_land_detected_sub.update(&_vehicle_land_detected);
 			vehicle_status_poll();
 
 			Vector2f curr_pos((float)_global_pos.lat, (float)_global_pos.lon);
@@ -2023,13 +1952,9 @@ FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float airspee
 
 	/* scale throttle cruise by baro pressure */
 	if (_parameters.throttle_alt_scale > FLT_EPSILON) {
+		sensor_baro_s baro{};
 
-		bool baro_updated = false;
-		orb_check(_sensor_baro_sub, &baro_updated);
-
-		sensor_baro_s baro;
-
-		if (orb_copy(ORB_ID(sensor_baro), _sensor_baro_sub, &baro) == PX4_OK) {
+		if (_sensor_baro_sub.update(&baro)) {
 			if (PX4_ISFINITE(baro.pressure) && PX4_ISFINITE(_parameters.throttle_alt_scale)) {
 				// scale throttle as a function of sqrt(p0/p) (~ EAS -> TAS at low speeds and altitudes ignoring temperature)
 				const float eas2tas = sqrtf(CONSTANTS_STD_PRESSURE_MBAR / baro.pressure);

src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp

@@ -152,16 +152,17 @@ private:
 	orb_advert_t	_mavlink_log_pub{nullptr};
 
 	int		_global_pos_sub{-1};
-	int		_local_pos_sub{-1};
-	int		_pos_sp_triplet_sub{-1};
-	int		_control_mode_sub{-1};			///< control mode subscription */
-	int		_vehicle_attitude_sub{-1};		///< vehicle attitude subscription */
-	int		_vehicle_command_sub{-1};		///< vehicle command subscription */
-	int		_vehicle_status_sub{-1};		///< vehicle status subscription */
-	int		_vehicle_land_detected_sub{-1};		///< vehicle land detected subscription */
-	int		_params_sub{-1};			///< notification of parameter updates */
-	int		_manual_control_sub{-1};		///< notification of manual control updates */
-	int		_sensor_baro_sub{-1};
+
+	uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)};		///< control mode subscription */
+	uORB::Subscription _local_pos_sub{ORB_ID(vehicle_local_position)};
+	uORB::Subscription _manual_control_sub{ORB_ID(manual_control_setpoint)};	///< notification of manual control updates */
+	uORB::Subscription _params_sub{ORB_ID(parameter_update)};			///< notification of parameter updates */
+	uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)};
+	uORB::Subscription _sensor_baro_sub{ORB_ID(sensor_baro)};
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};		///< vehicle attitude subscription */
+	uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};		///< vehicle command subscription */
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};	///< vehicle land detected subscription */
+	uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)};			///< vehicle status subscription */
 
 	orb_advert_t	_attitude_sp_pub{nullptr};		///< attitude setpoint */
 	orb_advert_t	_pos_ctrl_status_pub{nullptr};		///< navigation capabilities publication */
@@ -172,7 +173,7 @@ private:
 
 	manual_control_setpoint_s	_manual {};			///< r/c channel data */
 	position_setpoint_triplet_s	_pos_sp_triplet {};		///< triplet of mission items */
-	vehicle_attitude_s	_att {};			///< vehicle attitude setpoint */
+	vehicle_attitude_s		_att {};			///< vehicle attitude setpoint */
 	vehicle_attitude_setpoint_s	_att_sp {};			///< vehicle attitude setpoint */
 	vehicle_command_s		_vehicle_command {};		///< vehicle commands */
 	vehicle_control_mode_s		_control_mode {};		///< control mode */
@@ -377,12 +378,9 @@ private:
 	// Update subscriptions
 	void		airspeed_poll();
 	void		control_update();
-	void		manual_control_setpoint_poll();
-	void		position_setpoint_triplet_poll();
 	void		vehicle_attitude_poll();
 	void		vehicle_command_poll();
 	void		vehicle_control_mode_poll();
-	void		vehicle_land_detected_poll();
 	void		vehicle_status_poll();
 
 	void		status_publish();