fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-23 07:37:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

do not overwrite with obstacle avoidance yaw setpoints if external yaw

Browse Source 
handling is enabled
This commit is contained in:
Martina Rivizzigno
2019-05-16 09:12:18 +02:00
committed by Matthias Grob
parent 44b8b4f79f
commit 9e8575b71b
3 changed files with 13 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
+2 -1
View File
@@ -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);
}
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
+8 -3
View File
@@ -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);
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
+3 -1
View File
@@ -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.
*/