From faae5feecc80639f9ce2f89d0b7733e9a15bf1d2 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 30 Nov 2019 12:39:40 -0500 Subject: [PATCH] vmount update orb usage --- src/modules/vmount/output.cpp | 57 ++++++++------------------------ src/modules/vmount/output.h | 18 +++++----- src/modules/vmount/output_rc.cpp | 12 ++----- src/modules/vmount/output_rc.h | 9 ++--- 4 files changed, 30 insertions(+), 66 deletions(-) diff --git a/src/modules/vmount/output.cpp b/src/modules/vmount/output.cpp index f5ec0bd117..5ad4bf8a25 100644 --- a/src/modules/vmount/output.cpp +++ b/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) { - orb_check(_vehicle_global_position_sub, &need_update); + vehicle_global_position_s vehicle_global_position{}; + + 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); } diff --git a/src/modules/vmount/output.h b/src/modules/vmount/output.h index b54b58a63d..a937b02d07 100644 --- a/src/modules/vmount/output.h +++ b/src/modules/vmount/output.h @@ -42,10 +42,12 @@ #include "common.h" #include #include -#include +#include +#include +#include +#include #include - 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; - int _vehicle_global_position_sub = -1; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)}; - orb_advert_t _mount_orientation_pub = nullptr; + uORB::Publication _mount_orientation_pub{ORB_ID(mount_orientation)}; }; diff --git a/src/modules/vmount/output_rc.cpp b/src/modules/vmount/output_rc.cpp index 8f25a4cb52..196772a6fc 100644 --- a/src/modules/vmount/output_rc.cpp +++ b/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; - orb_publish_auto(ORB_ID(actuator_controls_2), &_actuator_controls_pub, &actuator_controls, - &instance, ORB_PRIO_DEFAULT); + _actuator_controls_pub.publish(actuator_controls); _last_update = t; diff --git a/src/modules/vmount/output_rc.h b/src/modules/vmount/output_rc.h index 7e9220af4c..340e913cc7 100644 --- a/src/modules/vmount/output_rc.h +++ b/src/modules/vmount/output_rc.h @@ -41,8 +41,8 @@ #include "output.h" -#include - +#include +#include 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_pub{ORB_ID(actuator_controls_2)}; + bool _retract_gimbal = true; };