diff --git a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp index f3dfc59ab2..adfd681109 100644 --- a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp @@ -36,9 +36,9 @@ */ #include "FlightTaskOrbit.hpp" + #include #include -#include using namespace matrix; @@ -47,11 +47,6 @@ FlightTaskOrbit::FlightTaskOrbit() : _circle_approach_line(_position) _sticks_data_required = false; } -FlightTaskOrbit::~FlightTaskOrbit() -{ - orb_unadvertise(_orbit_status_pub); -} - bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command) { bool ret = true; @@ -100,22 +95,17 @@ bool FlightTaskOrbit::applyCommandParameters(const vehicle_command_s &command) bool FlightTaskOrbit::sendTelemetry() { - orbit_status_s _orbit_status = {}; - _orbit_status.timestamp = hrt_absolute_time(); - _orbit_status.radius = math::signNoZero(_v) * _r; - _orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL + orbit_status_s orbit_status{}; + orbit_status.timestamp = hrt_absolute_time(); + orbit_status.radius = math::signNoZero(_v) * _r; + orbit_status.frame = 0; // MAV_FRAME::MAV_FRAME_GLOBAL - if (globallocalconverter_toglobal(_center(0), _center(1), _position_setpoint(2), &_orbit_status.x, &_orbit_status.y, - &_orbit_status.z)) { + if (globallocalconverter_toglobal(_center(0), _center(1), _position_setpoint(2), &orbit_status.x, &orbit_status.y, + &orbit_status.z)) { return false; // don't send the message if the transformation failed } - if (_orbit_status_pub == nullptr) { - _orbit_status_pub = orb_advertise(ORB_ID(orbit_status), &_orbit_status); - - } else { - orb_publish(ORB_ID(orbit_status), _orbit_status_pub, &_orbit_status); - } + _orbit_status_pub.publish(orbit_status); return true; } diff --git a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp index 08df6a453f..1c0e970f78 100644 --- a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp @@ -42,14 +42,15 @@ #pragma once #include "FlightTaskManualAltitudeSmooth.hpp" -#include +#include +#include #include class FlightTaskOrbit : public FlightTaskManualAltitudeSmooth { public: FlightTaskOrbit(); - virtual ~FlightTaskOrbit(); + virtual ~FlightTaskOrbit() = default; bool applyCommandParameters(const vehicle_command_s &command) override; bool activate(vehicle_local_position_setpoint_s last_setpoint) override; @@ -103,7 +104,7 @@ private: const float _velocity_max = 10.f; const float _acceleration_max = 2.f; - orb_advert_t _orbit_status_pub = nullptr; + uORB::Publication _orbit_status_pub{ORB_ID(orbit_status)}; DEFINE_PARAMETERS( (ParamFloat) _param_mpc_xy_cruise /**< cruise speed for circle approach */ diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidanceTest.cpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidanceTest.cpp index 2389ac7233..67cfbaf9ac 100644 --- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidanceTest.cpp +++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidanceTest.cpp @@ -81,13 +81,13 @@ TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy) message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true; // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle status message - orb_advert_t vehicle_trajectory_waypoint_pub = orb_advertise(ORB_ID(vehicle_trajectory_waypoint), &message); - orb_publish(ORB_ID(vehicle_trajectory_waypoint), vehicle_trajectory_waypoint_pub, &message); + uORB::Publication vehicle_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; + vehicle_trajectory_waypoint_pub.publish(message); vehicle_status_s vehicle_status{}; vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - orb_advert_t vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &vehicle_status); - orb_publish(ORB_ID(vehicle_status), vehicle_status_pub, &vehicle_status); + uORB::Publication vehicle_status_pub{ORB_ID(vehicle_status)}; + vehicle_status_pub.publish(vehicle_status); // WHEN: we inject the vehicle_trajectory_waypoint in the interface oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp); @@ -111,13 +111,13 @@ TEST_F(ObstacleAvoidanceTest, oa_enabled_not_healthy) oa.test_setPosition(pos); // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle_status - orb_advert_t vehicle_trajectory_waypoint_pub = orb_advertise(ORB_ID(vehicle_trajectory_waypoint), &message); - orb_publish(ORB_ID(vehicle_trajectory_waypoint), vehicle_trajectory_waypoint_pub, &message); + uORB::Publication vehicle_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; + vehicle_trajectory_waypoint_pub.publish(message); vehicle_status_s vehicle_status{}; vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - orb_advert_t vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &vehicle_status); - orb_publish(ORB_ID(vehicle_status), vehicle_status_pub, &vehicle_status); + uORB::Publication vehicle_status_pub{ORB_ID(vehicle_status)}; + vehicle_status_pub.publish(vehicle_status); // WHEN: we inject the vehicle_trajectory_waypoint in the interface oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp);