From 34b0f3309816ee814b6797f30f352d4d4218c503 Mon Sep 17 00:00:00 2001
From: Martina Rivizzigno <martina@rivizzigno.it>
Date: Fri, 1 Mar 2019 15:09:00 +0100
Subject: [PATCH] move all obstacle avoidance interfaces to the
ObstacleAvoidance library
---
.../FlightTasks/tasks/Auto/FlightTaskAuto.cpp | 74 ++-----------
.../FlightTasks/tasks/Auto/FlightTaskAuto.hpp | 4 -
.../tasks/Utility/ObstacleAvoidance.cpp | 103 ++++++++++++++++++
.../tasks/Utility/ObstacleAvoidance.hpp | 18 ++-
.../mc_pos_control/mc_pos_control_main.cpp | 69 ------------
5 files changed, 127 insertions(+), 141 deletions(-)
diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
index de7125d499..3fe5a63f81 100644
--- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
+++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
@@ -235,14 +235,20 @@ bool FlightTaskAuto::_evaluateTriplets()
State previous_state = _current_state;
_current_state = _getCurrentState();
+
+
if (triplet_update || (_current_state != previous_state)) {
_updateInternalWaypoints();
- _updateAvoidanceWaypoints();
_mission_gear = _sub_triplet_setpoint->get().current.landing_gear;
}
if (_param_com_obs_avoid.get() && _sub_vehicle_status->get().is_rotary_wing) {
- _checkAvoidanceProgress();
+ _obstacle_avoidance.updateAvoidanceWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint, _triplet_next_wp,
+ _sub_triplet_setpoint->get().next.yaw,
+ _sub_triplet_setpoint->get().next.yawspeed_valid ? _sub_triplet_setpoint->get().next.yawspeed : NAN);
+ _obstacle_avoidance.updateAvoidanceSetpoints(_position_setpoint, _velocity_setpoint);
+ _obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt);
+ // _checkAvoidanceProgress();
}
return true;
@@ -301,70 +307,6 @@ void FlightTaskAuto::_set_heading_from_mode()
}
}
-void FlightTaskAuto::_updateAvoidanceWaypoints()
-{
- _desired_waypoint.timestamp = hrt_absolute_time();
-
- _triplet_target.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].position);
- Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].velocity);
- Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].acceleration);
-
- _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw = _yaw_setpoint;
- _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed = _yawspeed_setpoint;
- _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid = true;
-
-
- _triplet_next_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].position);
- Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].velocity);
- Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].acceleration);
-
- _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw = _sub_triplet_setpoint->get().next.yaw;
- _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed =
- _sub_triplet_setpoint->get().next.yawspeed_valid ?
- _sub_triplet_setpoint->get().next.yawspeed : NAN;
- _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid = true;
-}
-
-void FlightTaskAuto::_checkAvoidanceProgress()
-{
- position_controller_status_s pos_control_status = {};
- pos_control_status.timestamp = hrt_absolute_time();
-
- // vector from previous triplet to current target
- Vector2f prev_to_target = Vector2f(_triplet_target - _triplet_prev_wp);
- // vector from previous triplet to the vehicle projected position on the line previous-target triplet
- Vector2f prev_to_closest_pt = _closest_pt - Vector2f(_triplet_prev_wp);
- // fraction of the previous-tagerget line that has been flown
- const float prev_curr_travelled = prev_to_closest_pt.length() / prev_to_target.length();
-
- Vector2f pos_to_target = Vector2f(_triplet_target - _position);
-
- if (prev_curr_travelled > 1.0f) {
- // if the vehicle projected position on the line previous-target is past the target waypoint,
- // increase the target acceptance radius such that navigator will update the triplets
- pos_control_status.acceptance_radius = pos_to_target.length() + 0.5f;
- }
-
- const float pos_to_target_z = fabsf(_triplet_target(2) - _position(2));
-
- if (pos_to_target.length() < _target_acceptance_radius && pos_to_target_z > _param_nav_mc_alt_rad.get()) {
- // vehicle above or below the target waypoint
- pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f;
- }
-
- // do not check for waypoints yaw acceptance in navigator
- pos_control_status.yaw_acceptance = NAN;
-
- if (_pub_pos_control_status == nullptr) {
- _pub_pos_control_status = orb_advertise(ORB_ID(position_controller_status), &pos_control_status);
-
- } else {
- orb_publish(ORB_ID(position_controller_status), _pub_pos_control_status, &pos_control_status);
-
- }
-
-}
-
bool FlightTaskAuto::_isFinite(const position_setpoint_s &sp)
{
return (PX4_ISFINITE(sp.lat) && PX4_ISFINITE(sp.lon) && PX4_ISFINITE(sp.alt));
diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
index bb0b1b6585..46265cf2d1 100644
--- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
+++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp
@@ -43,7 +43,6 @@
#include <uORB/topics/position_setpoint_triplet.h>
#include <uORB/topics/position_setpoint.h>
#include <uORB/topics/home_position.h>
-#include <uORB/topics/position_controller_status.h>
#include <uORB/topics/vehicle_status.h>
#include <lib/ecl/geo/geo.h>
#include "lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp"
@@ -93,7 +92,6 @@ protected:
matrix::Vector2f _getTargetVelocityXY(); /**< only used for follow-me and only here because of legacy reason.*/
void _updateInternalWaypoints(); /**< Depending on state of vehicle, the internal waypoints might differ from target (for instance if offtrack). */
bool _compute_heading_from_2D_vector(float &heading, matrix::Vector2f v); /**< Computes and sets heading a 2D vector */
- void _updateAvoidanceWaypoints(); /**< fill desired_waypoints with the triplets. */
matrix::Vector3f _prev_prev_wp{}; /**< Pre-previous waypoint (local frame). This will be used for smoothing trajectories -> not used yet. */
matrix::Vector3f _prev_wp{}; /**< Previous waypoint (local frame). If no previous triplet is available, the prev_wp is set to current position. */
@@ -139,12 +137,10 @@ private:
WeatherVane *_ext_yaw_handler =
nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */
- orb_advert_t _pub_pos_control_status = nullptr; /**< Publisher for the position controller status */
bool _evaluateTriplets(); /**< Checks and sets triplets. */
bool _isFinite(const position_setpoint_s &sp); /**< Checks if all waypoint triplets are finite. */
bool _evaluateGlobalReference(); /**< Check is global reference is available. */
State _getCurrentState(); /**< Computes the current vehicle state based on the vehicle position and navigator triplets. */
void _set_heading_from_mode(); /**< @see MPC_YAW_MODE */
- void _checkAvoidanceProgress();
};
diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
index c402d03cb8..f979e44b1f 100644
--- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
+++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
@@ -52,6 +52,19 @@ ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) :
}
+ObstacleAvoidance::~ObstacleAvoidance()
+{
+ //unadvertise publishers
+ if (_traj_wp_avoidance_desired_pub != nullptr) {
+ orb_unadvertise(_traj_wp_avoidance_desired_pub);
+ }
+
+ if (_pub_pos_control_status != nullptr) {
+ orb_unadvertise(_pub_pos_control_status);
+ }
+
+}
+
bool ObstacleAvoidance::initializeSubscriptions(SubscriptionArray &subscription_array)
{
if (!subscription_array.get(ORB_ID(vehicle_trajectory_waypoint), _sub_vehicle_trajectory_waypoint)) {
@@ -81,3 +94,93 @@ void ObstacleAvoidance::prepareAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &ve
yaw_speed_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed;
}
}
+
+void ObstacleAvoidance::updateAvoidanceWaypoints(const Vector3f &curr_wp, const float curr_yaw,
+ const float curr_yawspeed,
+ const Vector3f &next_wp, const float next_yaw, const float next_yawspeed)
+{
+ _desired_waypoint.timestamp = hrt_absolute_time();
+ _desired_waypoint.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;
+
+ curr_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].position);
+ Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].velocity);
+ Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].acceleration);
+
+ _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw = curr_yaw;
+ _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed = curr_yawspeed;
+ _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid = true;
+
+ next_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].position);
+ Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].velocity);
+ Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].acceleration);
+
+ _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw = next_yaw;
+ _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed = next_yawspeed;
+ _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid = true;
+}
+
+void ObstacleAvoidance::updateAvoidanceSetpoints(const Vector3f &pos_sp, const Vector3f &vel_sp)
+{
+ pos_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position);
+ vel_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity);
+
+ _publish_avoidance_desired_waypoint();
+
+ // TODO: reset all fields to NaN
+}
+
+void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector3f &prev_wp,
+ float target_acceptance_radius,
+ const Vector2f &closest_pt)
+{
+ position_controller_status_s pos_control_status = {};
+ pos_control_status.timestamp = hrt_absolute_time();
+ Vector3f curr_wp = _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].position;
+
+ // vector from previous triplet to current target
+ Vector2f prev_to_target = Vector2f(curr_wp - prev_wp);
+ // vector from previous triplet to the vehicle projected position on the line previous-target triplet
+ Vector2f prev_to_closest_pt = closest_pt - Vector2f(prev_wp);
+ // fraction of the previous-tagerget line that has been flown
+ const float prev_curr_travelled = prev_to_closest_pt.length() / prev_to_target.length();
+
+ Vector2f pos_to_target = Vector2f(curr_wp - pos);
+
+ if (prev_curr_travelled > 1.0f) {
+ // if the vehicle projected position on the line previous-target is past the target waypoint,
+ // increase the target acceptance radius such that navigator will update the triplets
+ pos_control_status.acceptance_radius = pos_to_target.length() + 0.5f;
+ }
+
+ const float pos_to_target_z = fabsf(curr_wp(2) - pos(2));
+
+ if (pos_to_target.length() < target_acceptance_radius && pos_to_target_z > NAV_MC_ALT_RAD.get()) {
+ // vehicle above or below the target waypoint
+ pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f;
+ }
+
+ // do not check for waypoints yaw acceptance in navigator
+ pos_control_status.yaw_acceptance = NAN;
+
+ if (_pub_pos_control_status == nullptr) {
+ _pub_pos_control_status = orb_advertise(ORB_ID(position_controller_status), &pos_control_status);
+
+ } else {
+ orb_publish(ORB_ID(position_controller_status), _pub_pos_control_status, &pos_control_status);
+
+ }
+
+}
+
+void
+ObstacleAvoidance::_publish_avoidance_desired_waypoint()
+{
+ // publish desired waypoint
+ if (_traj_wp_avoidance_desired_pub != nullptr) {
+ orb_publish(ORB_ID(vehicle_trajectory_waypoint_desired), _traj_wp_avoidance_desired_pub, &_desired_waypoint);
+
+ } else {
+ _traj_wp_avoidance_desired_pub = orb_advertise(ORB_ID(vehicle_trajectory_waypoint_desired),
+ &_desired_waypoint);
+ }
+}
diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
index 3378f59def..4caa45fdce 100644
--- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
+++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
@@ -43,6 +43,7 @@
#include <px4_module_params.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
+#include <uORB/topics/position_controller_status.h>
#include <lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp>
#include <matrix/matrix/math.hpp>
#include <px4_defines.h>
@@ -51,18 +52,31 @@ class ObstacleAvoidance : public ModuleParams
{
public:
ObstacleAvoidance(ModuleParams *parent);
- ~ObstacleAvoidance() = default;
+ ~ObstacleAvoidance();
bool initializeSubscriptions(SubscriptionArray &subscription_array);
void prepareAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, float &yaw_speed_sp);
+ void updateAvoidanceWaypoints(const matrix::Vector3f &curr_wp, const float curr_yaw, const float curr_yawspeed,
+ const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed);
+ void updateAvoidanceSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp);
+ void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp,
+ float target_acceptance_radius,
+ const matrix::Vector2f &closest_pt);
private:
uORB::Subscription<vehicle_trajectory_waypoint_s> *_sub_vehicle_trajectory_waypoint{nullptr};
DEFINE_PARAMETERS(
- (ParamInt<px4::params::COM_OBS_AVOID>) COM_OBS_AVOID /**< obstacle avoidance enabled */
+ (ParamInt<px4::params::COM_OBS_AVOID>) COM_OBS_AVOID, /**< obstacle avoidance enabled */
+ (ParamFloat<px4::params::NAV_MC_ALT_RAD>) NAV_MC_ALT_RAD
);
+ vehicle_trajectory_waypoint_s _desired_waypoint = {};
+ orb_advert_t _traj_wp_avoidance_desired_pub{nullptr}; /**< trajectory waypoint desired publication */
+ orb_advert_t _pub_pos_control_status{nullptr};
+
+ void _publish_avoidance_desired_waypoint();
+
};
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 78690bbb60..53098e6830 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -108,7 +108,6 @@ private:
orb_advert_t _att_sp_pub{nullptr}; /**< attitude setpoint publication */
orb_advert_t _traj_sp_pub{nullptr}; /**< trajectory setpoints publication */
orb_advert_t _local_pos_sp_pub{nullptr}; /**< vehicle local position setpoint publication */
- orb_advert_t _traj_wp_avoidance_desired_pub{nullptr}; /**< trajectory waypoint desired publication */
orb_advert_t _pub_vehicle_command{nullptr}; /**< vehicle command publication */
orb_id_t _attitude_setpoint_id{nullptr};
orb_advert_t _landing_gear_pub{nullptr};
@@ -120,7 +119,6 @@ private:
int _local_pos_sub{-1}; /**< vehicle local position */
int _att_sub{-1}; /**< vehicle attitude */
int _home_pos_sub{-1}; /**< home position */
- int _traj_wp_avoidance_sub{-1}; /**< trajectory waypoint */
int _task_failure_count{0}; /**< counter for task failures */
@@ -144,8 +142,6 @@ private:
vehicle_control_mode_s _control_mode{}; /**< vehicle control mode */
vehicle_local_position_s _local_pos{}; /**< vehicle local position */
home_position_s _home_pos{}; /**< home position */
- vehicle_trajectory_waypoint_s _traj_wp_avoidance{}; /**< trajectory waypoint */
- vehicle_trajectory_waypoint_s _traj_wp_avoidance_desired{}; /**< desired waypoints, inputs to an obstacle avoidance module */
landing_gear_s _landing_gear{};
int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP};
@@ -160,7 +156,6 @@ private:
(ParamInt<px4::params::MPC_AUTO_MODE>) _param_mpc_auto_mode,
(ParamInt<px4::params::MPC_ALT_MODE>) _param_mpc_alt_mode,
(ParamFloat<px4::params::MPC_SPOOLUP_TIME>) _param_mpc_spoolup_time, /**< time to let motors spool up after arming */
- (ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid, /**< enable obstacle avoidance */
(ParamFloat<px4::params::MPC_TILTMAX_LND>) _param_mpc_tiltmax_lnd /**< maximum tilt for landing and smooth takeoff */
);
@@ -275,24 +270,11 @@ private:
void failsafe(vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states, const bool force,
const bool warn);
- /**
- * Fill desired vehicle_trajectory_waypoint:
- * point1: current position, desired velocity
- * point2: current triplet only if in auto mode
- * @param states current vehicle state
- */
- void update_avoidance_waypoint_desired(PositionControlStates &states, vehicle_local_position_setpoint_s &setpoint);
-
/**
* Reset setpoints to NAN
*/
void reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint);
- /**
- * Publish desired vehicle_trajectory_waypoint
- */
- void publish_avoidance_desired_waypoint();
-
/**
* Shim for calling task_main from task_create.
*/
@@ -461,11 +443,6 @@ MulticopterPositionControl::poll_subscriptions()
orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
}
- orb_check(_traj_wp_avoidance_sub, &updated);
-
- if (updated) {
- orb_copy(ORB_ID(vehicle_trajectory_waypoint), _traj_wp_avoidance_sub, &_traj_wp_avoidance);
- }
}
void
@@ -574,7 +551,6 @@ MulticopterPositionControl::run()
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
_home_pos_sub = orb_subscribe(ORB_ID(home_position));
- _traj_wp_avoidance_sub = orb_subscribe(ORB_ID(vehicle_trajectory_waypoint));
_local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position));
orb_set_interval(_local_pos_sub, 20); // 50 Hz updates
@@ -671,12 +647,6 @@ MulticopterPositionControl::run()
publish_trajectory_sp(setpoint);
- /* desired waypoints for obstacle avoidance:
- * point_0 contains the current position with the desired velocity
- * point_1 contains _pos_sp_triplet.current if valid
- */
- update_avoidance_waypoint_desired(_states, setpoint);
-
vehicle_constraints_s constraints = _flight_tasks.getConstraints();
landing_gear_s gear = _flight_tasks.getGear();
@@ -826,7 +796,6 @@ MulticopterPositionControl::run()
orb_unsubscribe(_local_pos_sub);
orb_unsubscribe(_att_sub);
orb_unsubscribe(_home_pos_sub);
- orb_unsubscribe(_traj_wp_avoidance_sub);
}
void
@@ -1128,31 +1097,6 @@ MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint
}
}
-void
-MulticopterPositionControl::update_avoidance_waypoint_desired(PositionControlStates &states,
- vehicle_local_position_setpoint_s &setpoint)
-{
- if (_param_com_obs_avoid.get()) {
- _traj_wp_avoidance_desired = _flight_tasks.getAvoidanceWaypoint();
- _traj_wp_avoidance_desired.timestamp = hrt_absolute_time();
- _traj_wp_avoidance_desired.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;
-
- states.position.copyTo(_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position);
-
- _traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[0] = setpoint.vx;
- _traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[1] = setpoint.vy;
- _traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[2] = setpoint.vz;
-
- states.acceleration.copyTo(_traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].acceleration);
-
- _traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw = states.yaw;
- _traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed = NAN;
- _traj_wp_avoidance_desired.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true;
-
- publish_avoidance_desired_waypoint();
- }
-}
-
void
MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoint_s &setpoint)
{
@@ -1163,19 +1107,6 @@ MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoin
setpoint.thrust[0] = setpoint.thrust[1] = setpoint.thrust[2] = NAN;
}
-void
-MulticopterPositionControl::publish_avoidance_desired_waypoint()
-{
- // publish desired waypoint
- if (_traj_wp_avoidance_desired_pub != nullptr) {
- orb_publish(ORB_ID(vehicle_trajectory_waypoint_desired), _traj_wp_avoidance_desired_pub, &_traj_wp_avoidance_desired);
-
- } else {
- _traj_wp_avoidance_desired_pub = orb_advertise(ORB_ID(vehicle_trajectory_waypoint_desired),
- &_traj_wp_avoidance_desired);
- }
-}
-
void
MulticopterPositionControl::publish_attitude()
{
--
GitLab