FlightTasks: orbit and obstacle test use new uORB::Publication<>

PX4 · Sep 30, 2019 · 6941077 · 6941077
1 parent 045f623
commit 6941077
Show file tree

Hide file tree

Showing 3 changed files with 20 additions and 29 deletions.
diff --git a/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp b/src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp
@@ -36,9 +36,9 @@
  */
 
 #include "FlightTaskOrbit.hpp"
+
 #include <mathlib/mathlib.h>
 #include <lib/ecl/geo/geo.h>
-#include <uORB/topics/orbit_status.h>
 
 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
@@ -42,14 +42,15 @@
 #pragma once
 
 #include "FlightTaskManualAltitudeSmooth.hpp"
-#include <uORB/uORB.h>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/orbit_status.h>
 #include <StraightLine.hpp>
 
 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 @@ class FlightTaskOrbit : public FlightTaskManualAltitudeSmooth
 	const float _velocity_max = 10.f;
 	const float _acceleration_max = 2.f;
 
-	orb_advert_t _orbit_status_pub = nullptr;
+	uORB::Publication<orbit_status_s> _orbit_status_pub{ORB_ID(orbit_status)};
 
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MPC_XY_CRUISE>) _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
@@ -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_s> 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_s> 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_s> 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_s> 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);