vmount update orb usage

src/modules/vmount/output.cpp

@@ -62,44 +62,16 @@ OutputBase::OutputBase(const OutputConfig &output_config)
 	_last_update = hrt_absolute_time();
 }
 
-OutputBase::~OutputBase()
-{
-	if (_vehicle_attitude_sub >= 0) {
-		orb_unsubscribe(_vehicle_attitude_sub);
-	}
-
-	if (_vehicle_global_position_sub >= 0) {
-		orb_unsubscribe(_vehicle_global_position_sub);
-	}
-
-	if (_mount_orientation_pub) {
-		orb_unadvertise(_mount_orientation_pub);
-	}
-}
-
-int OutputBase::initialize()
-{
-	if ((_vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude))) < 0) {
-		return -errno;
-	}
-
-	if ((_vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position))) < 0) {
-		return -errno;
-	}
-
-	return 0;
-}
-
 void OutputBase::publish()
 {
-	int instance;
+	mount_orientation_s mount_orientation{};
-	mount_orientation_s mount_orientation;
 
 	for (unsigned i = 0; i < 3; ++i) {
 		mount_orientation.attitude_euler_angle[i] = _angle_outputs[i];
 	}
 
-	orb_publish_auto(ORB_ID(mount_orientation), &_mount_orientation_pub, &mount_orientation, &instance, ORB_PRIO_DEFAULT);
+	mount_orientation.timestamp = hrt_absolute_time();
+	_mount_orientation_pub.publish(mount_orientation);
 }
 
 float OutputBase::_calculate_pitch(double lon, double lat, float altitude,
@@ -155,22 +127,21 @@ void OutputBase::_set_angle_setpoints(const ControlData *control_data)
 
 void OutputBase::_handle_position_update(bool force_update)
 {
-	bool need_update = force_update;
-
 	if (!_cur_control_data || _cur_control_data->type != ControlData::Type::LonLat) {
 		return;
 	}
 
-	if (!force_update) {
+	vehicle_global_position_s vehicle_global_position{};
-		orb_check(_vehicle_global_position_sub, &need_update);
+
+	if (force_update) {
+		_vehicle_global_position_sub.copy(&vehicle_global_position);
+
+	} else {
+		if (!_vehicle_global_position_sub.update(&vehicle_global_position)) {
+			return;
+		}
 	}
 
-	if (!need_update) {
-		return;
-	}
-
-	vehicle_global_position_s vehicle_global_position;
-	orb_copy(ORB_ID(vehicle_global_position), _vehicle_global_position_sub, &vehicle_global_position);
 	const double &vlat = vehicle_global_position.lat;
 	const double &vlon = vehicle_global_position.lon;
 
@@ -208,11 +179,11 @@ void OutputBase::_calculate_output_angles(const hrt_abstime &t)
 	}
 
 	//get the output angles and stabilize if necessary
-	vehicle_attitude_s vehicle_attitude;
+	vehicle_attitude_s vehicle_attitude{};
 	matrix::Eulerf euler;
 
 	if (_stabilize[0] || _stabilize[1] || _stabilize[2]) {
-		orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &vehicle_attitude);
+		_vehicle_attitude_sub.copy(&vehicle_attitude);
 		euler = matrix::Quatf(vehicle_attitude.q);
 	}
 

src/modules/vmount/output.h

@@ -42,10 +42,12 @@
 #include "common.h"
 #include <drivers/drv_hrt.h>
 #include <lib/ecl/geo/geo.h>
-#include <uORB/uORB.h>
+#include <uORB/Publication.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/mount_orientation.h>
+#include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
 
-
 namespace vmount
 {
 
@@ -77,9 +79,9 @@ class OutputBase
 {
 public:
 	OutputBase(const OutputConfig &output_config);
-	virtual ~OutputBase();
+	virtual ~OutputBase() = default;
 
-	virtual int initialize();
+	virtual int initialize() { return 0; }
 
 	/**
 	 * Update the output.
@@ -119,13 +121,11 @@ protected:
 	float _angle_outputs[3] = { 0.f, 0.f, 0.f }; ///< calculated output angles (roll, pitch, yaw) [rad]
 	hrt_abstime _last_update;
 
-	int _get_vehicle_attitude_sub() const { return _vehicle_attitude_sub; }
-
 private:
-	int _vehicle_attitude_sub = -1;
+	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
-	int _vehicle_global_position_sub = -1;
+	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
 
-	orb_advert_t _mount_orientation_pub = nullptr;
+	uORB::Publication<mount_orientation_s> _mount_orientation_pub{ORB_ID(mount_orientation)};
 };
 
 

src/modules/vmount/output_rc.cpp

@@ -51,12 +51,6 @@ OutputRC::OutputRC(const OutputConfig &output_config)
 	: OutputBase(output_config)
 {
 }
-OutputRC::~OutputRC()
-{
-	if (_actuator_controls_pub) {
-		orb_unadvertise(_actuator_controls_pub);
-	}
-}
 
 int OutputRC::update(const ControlData *control_data)
 {
@@ -71,7 +65,7 @@ int OutputRC::update(const ControlData *control_data)
 	hrt_abstime t = hrt_absolute_time();
 	_calculate_output_angles(t);
 
-	actuator_controls_s actuator_controls;
+	actuator_controls_s actuator_controls{};
 	actuator_controls.timestamp = hrt_absolute_time();
 	// _angle_outputs are in radians, actuator_controls are in [-1, 1]
 	actuator_controls.control[0] = (_angle_outputs[0] + _config.roll_offset) * _config.roll_scale;
@@ -79,9 +73,7 @@ int OutputRC::update(const ControlData *control_data)
 	actuator_controls.control[2] = (_angle_outputs[2] + _config.yaw_offset) * _config.yaw_scale;
 	actuator_controls.control[3] = _retract_gimbal ? _config.gimbal_retracted_mode_value : _config.gimbal_normal_mode_value;
 
-	int instance;
+	_actuator_controls_pub.publish(actuator_controls);
-	orb_publish_auto(ORB_ID(actuator_controls_2), &_actuator_controls_pub, &actuator_controls,
-			 &instance, ORB_PRIO_DEFAULT);
 
 	_last_update = t;
 

src/modules/vmount/output_rc.h

@@ -41,8 +41,8 @@
 
 #include "output.h"
 
-#include <uORB/uORB.h>
+#include <uORB/Publication.hpp>
-
+#include <uORB/topics/actuator_controls.h>
 
 namespace vmount
 {
@@ -56,14 +56,15 @@ class OutputRC : public OutputBase
 {
 public:
 	OutputRC(const OutputConfig &output_config);
-	virtual ~OutputRC();
+	virtual ~OutputRC() = default;
 
 	virtual int update(const ControlData *control_data);
 
 	virtual void print_status();
 
 private:
-	orb_advert_t _actuator_controls_pub = nullptr;
+	uORB::Publication <actuator_controls_s>	_actuator_controls_pub{ORB_ID(actuator_controls_2)};
+
 	bool _retract_gimbal = true;
 };