fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 17:48:53 +08:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
Daniel Agar
2019-09-30 18:26:01 -04:00
committed by GitHub
parent 045f6233d4
commit 6941077218
3 changed files with 20 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.cpp
+8 -18
View File
@@ -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;
}
src/lib/FlightTasks/tasks/Orbit/FlightTaskOrbit.hpp
+4 -3
View File
@@ -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 @@ private:
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 */
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidanceTest.cpp
+8 -8
View File
@@ -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);