px4io: move to uORB::Publication<>

src/drivers/px4io/px4io.cpp

@@ -76,6 +76,8 @@
 #include <circuit_breaker/circuit_breaker.h>
 #include <systemlib/mavlink_log.h>
 
+#include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/PublicationQueued.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/actuator_controls.h>
@@ -262,11 +264,11 @@ private:
 	bool			_param_update_force;	///< force a parameter update
 
 	/* advertised topics */
-	orb_advert_t 		_to_input_rc;		///< rc inputs from io
-	orb_advert_t		_to_outputs;		///< mixed servo outputs topic
-	orb_advert_t		_to_servorail;		///< servorail status
-	orb_advert_t		_to_safety;		///< status of safety
-	orb_advert_t 		_to_mixer_status; 	///< mixer status flags
+	uORB::PublicationMulti<input_rc_s>			_to_input_rc{ORB_ID(input_rc)};
+	uORB::PublicationMulti<actuator_outputs_s>		_to_outputs{ORB_ID(actuator_outputs)};
+	uORB::PublicationMulti<multirotor_motor_limits_s>	_to_mixer_status{ORB_ID(multirotor_motor_limits)};
+	uORB::Publication<servorail_status_s>			_to_servorail{ORB_ID(servorail_status)};
+	uORB::Publication<safety_s>				_to_safety{ORB_ID(safety)};
 
 	bool			_primary_pwm_device;	///< true if we are the default PWM output
 	bool			_lockdown_override;	///< allow to override the safety lockdown
@@ -474,11 +476,6 @@ PX4IO::PX4IO(device::Device *interface) :
 	_last_written_arming_c(0),
 	_t_actuator_controls_0(-1),
 	_param_update_force(false),
-	_to_input_rc(nullptr),
-	_to_outputs(nullptr),
-	_to_servorail(nullptr),
-	_to_safety(nullptr),
-	_to_mixer_status(nullptr),
 	_primary_pwm_device(false),
 	_lockdown_override(false),
 	_armed(false),
@@ -1221,27 +1218,6 @@ out:
 		unregister_driver(PWM_OUTPUT0_DEVICE_PATH);
 	}
 
-	if (_to_input_rc) {
-		orb_unadvertise(_to_input_rc);
-	}
-
-	if (_to_outputs) {
-		orb_unadvertise(_to_outputs);
-	}
-
-	if (_to_servorail) {
-		orb_unadvertise(_to_servorail);
-	}
-
-	if (_to_safety) {
-		orb_unadvertise(_to_safety);
-	}
-
-	if (_to_mixer_status) {
-		orb_unadvertise(_to_mixer_status);
-	}
-
-
 	/* tell the dtor that we are exiting */
 	_task = -1;
 	_exit(0);
@@ -1618,21 +1594,14 @@ PX4IO::io_handle_status(uint16_t status)
 	/**
 	 * Get and handle the safety status
 	 */
-	struct safety_s safety;
+	safety_s safety{};
 	safety.timestamp = hrt_absolute_time();
 	safety.safety_switch_available = true;
 	safety.safety_off = (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? true : false;
 	safety.override_available = _override_available;
 	safety.override_enabled = (status & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? true : false;
 
-	/* lazily publish the safety status */
-	if (_to_safety != nullptr) {
-		orb_publish(ORB_ID(safety), _to_safety, &safety);
-
-	} else {
-		int instance;
-		_to_safety = orb_advertise_multi(ORB_ID(safety), &safety, &instance, ORB_PRIO_DEFAULT);
-	}
+	_to_safety.publish(safety);
 
 	return ret;
 }
@@ -1671,7 +1640,7 @@ PX4IO::io_handle_alarms(uint16_t alarms)
 void
 PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi)
 {
-	servorail_status_s servorail_status = {};
+	servorail_status_s servorail_status{};
 
 	servorail_status.timestamp = hrt_absolute_time();
 
@@ -1690,12 +1659,7 @@ PX4IO::io_handle_vservo(uint16_t vservo, uint16_t vrssi)
 	}
 
 	/* lazily publish the servorail voltages */
-	if (_to_servorail != nullptr) {
-		orb_publish(ORB_ID(servorail_status), _to_servorail, &servorail_status);
-
-	} else {
-		_to_servorail = orb_advertise(ORB_ID(servorail_status), &servorail_status);
-	}
+	_to_servorail.publish(servorail_status);
 }
 
 int
@@ -1864,8 +1828,8 @@ PX4IO::io_publish_raw_rc()
 		}
 	}
 
-	int instance = 0;
-	orb_publish_auto(ORB_ID(input_rc), &_to_input_rc, &rc_val, &instance, ORB_PRIO_HIGH);
+	_to_input_rc.publish(rc_val);
+
 	return OK;
 }
 
@@ -1889,8 +1853,7 @@ PX4IO::io_publish_pwm_outputs()
 		outputs.output[i] = ctl[i];
 	}
 
-	int instance;
-	orb_publish_auto(ORB_ID(actuator_outputs), &_to_outputs, &outputs, &instance, ORB_PRIO_DEFAULT);
+	_to_outputs.publish(outputs);
 
 	/* get mixer status flags from IO */
 	MultirotorMixer::saturation_status saturation_status;
@@ -1902,11 +1865,11 @@ PX4IO::io_publish_pwm_outputs()
 
 	/* publish mixer status */
 	if (saturation_status.flags.valid) {
-		multirotor_motor_limits_s motor_limits;
+		multirotor_motor_limits_s motor_limits{};
 		motor_limits.timestamp = hrt_absolute_time();
 		motor_limits.saturation_status = saturation_status.value;
 
-		orb_publish_auto(ORB_ID(multirotor_motor_limits), &_to_mixer_status, &motor_limits, &instance, ORB_PRIO_DEFAULT);
+		_to_mixer_status.publish(motor_limits);
 	}
 
 	return OK;