commander move most orb subscriptions to uORB::Subscription

src/modules/commander/Commander.cpp

@@ -558,11 +558,6 @@ Commander::Commander() :
 {
 	_auto_disarm_landed.set_hysteresis_time_from(false, 10_s);
 	_auto_disarm_killed.set_hysteresis_time_from(false, 5_s);
-	_battery_sub = orb_subscribe(ORB_ID(battery_status));
-
-
-	_telemetry_status_sub = orb_subscribe(ORB_ID(telemetry_status));
-
 
 	// We want to accept RC inputs as default
 	status.rc_input_mode = vehicle_status_s::RC_IN_MODE_DEFAULT;
@@ -582,20 +577,10 @@ Commander::Commander() :
 	status_flags.rc_calibration_valid = true;
 
 	status_flags.avoidance_system_valid = false;
-
-
 }
 
 Commander::~Commander()
 {
-	orb_unsubscribe(_battery_sub);
-	orb_unsubscribe(_telemetry_status_sub);
-
-
-	if (_iridiumsbd_status_sub >= 0) {
-		orb_unsubscribe(_iridiumsbd_status_sub);
-	}
-
 	delete[] _airspeed_fault_type;
 }
 
@@ -1230,7 +1215,7 @@ Commander::run()
 		PX4_WARN("Buzzer init failed");
 	}
 
-	int power_button_state_sub = orb_subscribe(ORB_ID(power_button_state));
+	uORB::Subscription power_button_state_sub{ORB_ID(power_button_state)};
 	{
 		// we need to do an initial publication to make sure uORB allocates the buffer, which cannot happen
 		// in IRQ context.
@@ -1238,7 +1223,8 @@ Commander::run()
 		button_state.timestamp = 0;
 		button_state.event = 0xff;
 		power_button_state_pub = orb_advertise(ORB_ID(power_button_state), &button_state);
-		orb_copy(ORB_ID(power_button_state), power_button_state_sub, &button_state);
+
+		power_button_state_sub.copy(&button_state);
 	}
 
 	if (board_register_power_state_notification_cb(power_button_state_notification_cb) != 0) {
@@ -1275,18 +1261,18 @@ Commander::run()
 
 	bool updated = false;
 
-	int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
-	int cmd_sub = orb_subscribe(ORB_ID(vehicle_command));
-	int cpuload_sub = orb_subscribe(ORB_ID(cpuload));
-	int geofence_result_sub = orb_subscribe(ORB_ID(geofence_result));
-	int land_detector_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
-	int offboard_control_mode_sub = orb_subscribe(ORB_ID(offboard_control_mode));
-	int param_changed_sub = orb_subscribe(ORB_ID(parameter_update));
-	int safety_sub = orb_subscribe(ORB_ID(safety));
-	int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	int subsys_sub = orb_subscribe(ORB_ID(subsystem_info));
-	int system_power_sub = orb_subscribe(ORB_ID(system_power));
-	int vtol_vehicle_status_sub = orb_subscribe(ORB_ID(vtol_vehicle_status));
+	uORB::Subscription actuator_controls_sub{ORB_ID_VEHICLE_ATTITUDE_CONTROLS};
+	uORB::Subscription cmd_sub{ORB_ID(vehicle_command)};
+	uORB::Subscription cpuload_sub{ORB_ID(cpuload)};
+	uORB::Subscription geofence_result_sub{ORB_ID(geofence_result)};
+	uORB::Subscription land_detector_sub{ORB_ID(vehicle_land_detected)};
+	uORB::Subscription offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
+	uORB::Subscription param_changed_sub{ORB_ID(parameter_update)};
+	uORB::Subscription safety_sub{ORB_ID(safety)};
+	uORB::Subscription sp_man_sub{ORB_ID(manual_control_setpoint)};
+	uORB::Subscription subsys_sub{ORB_ID(subsystem_info)};
+	uORB::Subscription system_power_sub{ORB_ID(system_power)};
+	uORB::Subscription vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
 
 	geofence_result_s geofence_result {};
 
@@ -1388,14 +1374,13 @@ Commander::run()
 		transition_result_t arming_ret = TRANSITION_NOT_CHANGED;
 
 		/* update parameters */
-		bool params_updated = false;
-		orb_check(param_changed_sub, &params_updated);
+		bool params_updated = param_changed_sub.updated();
 
 		if (params_updated || param_init_forced) {
 
 			/* parameters changed */
-			struct parameter_update_s param_changed;
-			orb_copy(ORB_ID(parameter_update), param_changed_sub, &param_changed);
+			parameter_update_s param_changed;
+			param_changed_sub.copy(&param_changed);
 
 			updateParams();
 
@@ -1490,28 +1475,20 @@ Commander::run()
 		status_flags.avoidance_system_required = _param_com_obs_avoid.get();
 
 		/* handle power button state */
-		orb_check(power_button_state_sub, &updated);
-
-		if (updated) {
+		if (power_button_state_sub.updated()) {
 			power_button_state_s button_state;
-			orb_copy(ORB_ID(power_button_state), power_button_state_sub, &button_state);
+			power_button_state_sub.copy(&button_state);
 
 			if (button_state.event == power_button_state_s::PWR_BUTTON_STATE_REQUEST_SHUTDOWN) {
 				px4_shutdown_request(false, false);
 			}
 		}
 
-		orb_check(sp_man_sub, &updated);
-
-		if (updated) {
-			orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man);
-		}
+		sp_man_sub.update(&sp_man);
 
-		orb_check(offboard_control_mode_sub, &updated);
-
-		if (updated) {
+		if (offboard_control_mode_sub.updated()) {
 			offboard_control_mode_s old = offboard_control_mode;
-			orb_copy(ORB_ID(offboard_control_mode), offboard_control_mode_sub, &offboard_control_mode);
+			offboard_control_mode_sub.copy(&offboard_control_mode);
 
 			if (old.ignore_thrust != offboard_control_mode.ignore_thrust ||
 			    old.ignore_attitude != offboard_control_mode.ignore_attitude ||
@@ -1558,11 +1535,10 @@ Commander::run()
 			}
 		}
 
-		orb_check(system_power_sub, &updated);
 
-		if (updated) {
+		if (system_power_sub.updated()) {
 			system_power_s system_power = {};
-			orb_copy(ORB_ID(system_power), system_power_sub, &system_power);
+			system_power_sub.copy(&system_power);
 
 			if (hrt_elapsed_time(&system_power.timestamp) < 200_ms) {
 				if (system_power.servo_valid &&
@@ -1589,12 +1565,10 @@ Commander::run()
 		}
 
 		/* update safety topic */
-		orb_check(safety_sub, &updated);
-
-		if (updated) {
+		if (safety_sub.updated()) {
 			bool previous_safety_off = safety.safety_off;
 
-			if (orb_copy(ORB_ID(safety), safety_sub, &safety) == PX4_OK) {
+			if (safety_sub.copy(&safety)) {
 
 				/* disarm if safety is now on and still armed */
 				if (armed.armed && (status.hil_state == vehicle_status_s::HIL_STATE_OFF)
@@ -1624,11 +1598,9 @@ Commander::run()
 		}
 
 		/* update vtol vehicle status*/
-		orb_check(vtol_vehicle_status_sub, &updated);
-
-		if (updated) {
+		if (vtol_vehicle_status_sub.updated()) {
 			/* vtol status changed */
-			orb_copy(ORB_ID(vtol_vehicle_status), vtol_vehicle_status_sub, &vtol_status);
+			vtol_vehicle_status_sub.copy(&vtol_status);
 			status.vtol_fw_permanent_stab = vtol_status.fw_permanent_stab;
 
 			/* Make sure that this is only adjusted if vehicle really is of type vtol */
@@ -1666,10 +1638,8 @@ Commander::run()
 		airspeed_use_check();
 
 		/* Update land detector */
-		orb_check(land_detector_sub, &updated);
-
-		if (updated) {
-			orb_copy(ORB_ID(vehicle_land_detected), land_detector_sub, &land_detector);
+		if (land_detector_sub.updated()) {
+			land_detector_sub.copy(&land_detector);
 
 			// Only take actions if armed
 			if (armed.armed) {
@@ -1749,21 +1719,15 @@ Commander::run()
 			_geofence_warning_action_on = false;
 		}
 
-		orb_check(cpuload_sub, &updated);
-
-		if (updated) {
-			orb_copy(ORB_ID(cpuload), cpuload_sub, &cpuload);
-		}
+		cpuload_sub.update(&cpuload);
 
 		battery_status_check();
 
 		/* update subsystem info which arrives from outside of commander*/
 		do {
-			orb_check(subsys_sub, &updated);
-
-			if (updated) {
+			if (subsys_sub.updated()) {
 				subsystem_info_s info{};
-				orb_copy(ORB_ID(subsystem_info), subsys_sub, &info);
+				subsys_sub.copy(&info);
 				set_health_flags(info.subsystem_type, info.present, info.enabled, info.ok, status);
 				status_changed = true;
 			}
@@ -1826,11 +1790,7 @@ Commander::run()
 		}
 
 		/* start geofence result check */
-		orb_check(geofence_result_sub, &updated);
-
-		if (updated) {
-			orb_copy(ORB_ID(geofence_result), geofence_result_sub, &geofence_result);
-		}
+		geofence_result_sub.update(&geofence_result);
 
 		// Geofence actions
 		const bool geofence_action_enabled = geofence_result.geofence_action != geofence_result_s::GF_ACTION_NONE;
@@ -2158,9 +2118,7 @@ Commander::run()
 
 		// engine failure detection
 		// TODO: move out of commander
-		orb_check(actuator_controls_sub, &updated);
-
-		if (updated) {
+		if (actuator_controls_sub.updated()) {
 			/* Check engine failure
 			 * only for fixed wing for now
 			 */
@@ -2168,7 +2126,7 @@ Commander::run()
 			    !status.is_rotary_wing && !status.is_vtol && armed.armed) {
 
 				actuator_controls_s actuator_controls = {};
-				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
+				actuator_controls_sub.copy(&actuator_controls);
 
 				const float throttle = actuator_controls.control[actuator_controls_s::INDEX_THROTTLE];
 				const float current2throttle = _battery_current / throttle;
@@ -2235,13 +2193,11 @@ Commander::run()
 		}
 
 		/* handle commands last, as the system needs to be updated to handle them */
-		orb_check(cmd_sub, &updated);
-
-		if (updated) {
-			struct vehicle_command_s cmd;
+		if (cmd_sub.updated()) {
+			vehicle_command_s cmd{};
 
 			/* got command */
-			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
+			cmd_sub.copy(&cmd);
 
 			/* handle it */
 			if (handle_command(&status, cmd, &armed, &command_ack_pub, &status_changed)) {
@@ -2498,13 +2454,6 @@ Commander::run()
 	/* close fds */
 	led_deinit();
 	buzzer_deinit();
-	orb_unsubscribe(sp_man_sub);
-	orb_unsubscribe(offboard_control_mode_sub);
-	orb_unsubscribe(safety_sub);
-	orb_unsubscribe(cmd_sub);
-	orb_unsubscribe(subsys_sub);
-	orb_unsubscribe(param_changed_sub);
-	orb_unsubscribe(land_detector_sub);
 
 	thread_running = false;
 }
@@ -3500,7 +3449,7 @@ void *commander_low_prio_loop(void *arg)
 			continue;
 
 		} else if (pret != 0) {
-			struct vehicle_command_s cmd;
+			struct vehicle_command_s cmd {};
 
 			/* if we reach here, we have a valid command */
 			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
@@ -3826,15 +3775,11 @@ bool Commander::preflight_check(bool report)
 
 void Commander::data_link_check(bool &status_changed)
 {
-	bool updated = false;
-
-	orb_check(_telemetry_status_sub, &updated);
-
-	if (updated) {
+	if (_telemetry_status_sub.updated()) {
 
 		telemetry_status_s telemetry;
 
-		if (orb_copy(ORB_ID(telemetry_status), _telemetry_status_sub, &telemetry) == PX4_OK) {
+		if (_telemetry_status_sub.copy(&telemetry)) {
 
 			// handle different radio types
 			switch (telemetry.type) {
@@ -3843,38 +3788,25 @@ void Commander::data_link_check(bool &status_changed)
 				status_flags.usb_connected = true;
 				break;
 
-			case telemetry_status_s::LINK_TYPE_IRIDIUM:
-
-				// lazily subscribe
-				if (_iridiumsbd_status_sub == -1 && orb_exists(ORB_ID(iridiumsbd_status), 0) == PX4_OK) {
-					_iridiumsbd_status_sub = orb_subscribe(ORB_ID(iridiumsbd_status));
-				}
-
-				if (_iridiumsbd_status_sub >= 0) {
-					bool iridiumsbd_updated = false;
-					orb_check(_iridiumsbd_status_sub, &iridiumsbd_updated);
+			case telemetry_status_s::LINK_TYPE_IRIDIUM: {
 
-					if (iridiumsbd_updated) {
-						iridiumsbd_status_s iridium_status;
+					iridiumsbd_status_s iridium_status;
 
-						if (orb_copy(ORB_ID(iridiumsbd_status), _iridiumsbd_status_sub, &iridium_status) == PX4_OK) {
-							_high_latency_datalink_heartbeat = iridium_status.last_heartbeat;
+					if (_iridiumsbd_status_sub.update(&iridium_status)) {
+						_high_latency_datalink_heartbeat = iridium_status.last_heartbeat;
 
-							if (status.high_latency_data_link_lost) {
-								if (hrt_elapsed_time(&_high_latency_datalink_lost) > (_param_com_hldl_reg_t.get() * 1_s)) {
-									status.high_latency_data_link_lost = false;
-									status_changed = true;
-								}
+						if (status.high_latency_data_link_lost) {
+							if (hrt_elapsed_time(&_high_latency_datalink_lost) > (_param_com_hldl_reg_t.get() * 1_s)) {
+								status.high_latency_data_link_lost = false;
+								status_changed = true;
 							}
-
 						}
 					}
-				}
 
-				break;
+					break;
+				}
 			}
 
-
 			// handle different remote types
 			switch (telemetry.remote_type) {
 			case telemetry_status_s::MAV_TYPE_GCS:
@@ -4021,16 +3953,11 @@ void Commander::data_link_check(bool &status_changed)
 
 void Commander::battery_status_check()
 {
-	bool updated = false;
-
 	/* update battery status */
-	orb_check(_battery_sub, &updated);
-
-	if (updated) {
-
-		battery_status_s battery = {};
+	if (_battery_sub.updated()) {
+		battery_status_s battery{};
 
-		if (orb_copy(ORB_ID(battery_status), _battery_sub, &battery) == PX4_OK) {
+		if (_battery_sub.copy(&battery)) {
 
 			if ((hrt_elapsed_time(&battery.timestamp) < 5_s)
 			    && battery.connected

src/modules/commander/Commander.hpp

@@ -208,7 +208,7 @@ private:
 	 */
 	void		data_link_check(bool &status_changed);
 
-	int		_telemetry_status_sub{-1};
+	uORB::Subscription _telemetry_status_sub{ORB_ID(telemetry_status)};
 
 	hrt_abstime	_datalink_last_heartbeat_gcs{0};
 
@@ -221,12 +221,12 @@ private:
 	bool		_avoidance_system_status_change{false};
 	uint8_t	_datalink_last_status_avoidance_system{telemetry_status_s::MAV_STATE_UNINIT};
 
-	int			_iridiumsbd_status_sub{-1};
+	uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
 
 	hrt_abstime	_high_latency_datalink_heartbeat{0};
 	hrt_abstime	_high_latency_datalink_lost{0};
 
-	int _battery_sub{-1};
+	uORB::Subscription _battery_sub{ORB_ID(battery_status)};
 	uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
 	float _battery_current{0.0f};
 

src/modules/commander/accelerometer_calibration.cpp

@@ -148,6 +148,7 @@
 #include <systemlib/mavlink_log.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/sensor_correction.h>
+#include <uORB/Subscription.hpp>
 
 static const char *sensor_name = "accel";
 
@@ -334,11 +335,9 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
 
 		if (tc_enabled_int == 1) {
 			/* Get struct containing sensor thermal compensation data */
-			struct sensor_correction_s sensor_correction; /**< sensor thermal corrections */
-			memset(&sensor_correction, 0, sizeof(sensor_correction));
-			int sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
-			orb_copy(ORB_ID(sensor_correction), sensor_correction_sub, &sensor_correction);
-			orb_unsubscribe(sensor_correction_sub);
+			sensor_correction_s sensor_correction{}; /**< sensor thermal corrections */
+			uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)};
+			sensor_correction_sub.copy(&sensor_correction);
 
 			/* don't allow a parameter instance to be calibrated more than once by another uORB instance */
 			if (!tc_locked[sensor_correction.accel_mapping[uorb_index]]) {