From 9e8575b71b8e9b99aff8e8275515eca6686bbfcd Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Thu, 16 May 2019 09:12:18 +0200 Subject: [PATCH] do not overwrite with obstacle avoidance yaw setpoints if external yaw handling is enabled --- src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp | 3 ++- .../FlightTasks/tasks/Utility/ObstacleAvoidance.cpp | 11 ++++++++--- .../FlightTasks/tasks/Utility/ObstacleAvoidance.hpp | 4 +++- 3 files changed, 13 insertions(+), 5 deletions(-) diff --git a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp index 4b4a41cf65..236d0b2c2e 100644 --- a/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp +++ b/src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp @@ -278,7 +278,8 @@ bool FlightTaskAuto::_evaluateTriplets() _obstacle_avoidance.updateAvoidanceDesiredWaypoints(_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); + _sub_triplet_setpoint->get().next.yawspeed_valid ? _sub_triplet_setpoint->get().next.yawspeed : NAN, + _ext_yaw_handler != nullptr && _ext_yaw_handler->is_active()); _obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt); } diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp index 080abaa6e0..e98c6d10b5 100644 --- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp +++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp @@ -121,17 +121,22 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel pos_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position; vel_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity; - yaw_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw; - yaw_speed_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed; + + if (!_ext_yaw_active) { + // inject yaw setpoints only if weathervane isn't active + yaw_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw; + yaw_speed_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed; + } } void ObstacleAvoidance::updateAvoidanceDesiredWaypoints(const Vector3f &curr_wp, const float curr_yaw, const float curr_yawspeed, - const Vector3f &next_wp, const float next_yaw, const float next_yawspeed) + const Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active) { _desired_waypoint.timestamp = hrt_absolute_time(); _desired_waypoint.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS; _curr_wp = curr_wp; + _ext_yaw_active = ext_yaw_active; 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); diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp index 0472882a6d..9a295f2f21 100644 --- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp +++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp @@ -82,7 +82,7 @@ public: * @param next_yawspeed, next yaw speed triplet */ void updateAvoidanceDesiredWaypoints(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); + const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active); /** * Updates the desired setpoints to send to the obstacle avoidance system. * @param pos_sp, desired position setpoint computed by the active FlightTask @@ -119,6 +119,8 @@ private: matrix::Vector3f _position = {}; /**< current vehicle position */ matrix::Vector3f _failsafe_position = {}; /**< vehicle position when entered in failsafe */ + bool _ext_yaw_active = false; /**< true, if external yaw handling is active */ + /** * Publishes vehicle trajectory waypoint desired. */