fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 18:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

ObstacleAvoidance: fix bug in requesting update of the mission item.

Browse Source 
During takeoff you're always in the condition within xy acceptance radius
and more than altitude radius away from the takeoff waypoint
This commit is contained in:
Martina Rivizzigno
2019-05-29 15:16:33 +02:00
committed by Matthias Grob
parent 9e8575b71b
commit 0963dc9af1
3 changed files with 9 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
+3 -2
View File
@@ -167,7 +167,7 @@ void ObstacleAvoidance::updateAvoidanceDesiredSetpoints(const Vector3f &pos_sp,
}
void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector3f &prev_wp,
float target_acceptance_radius, const Vector2f &closest_pt)
float target_acceptance_radius, const Vector2f &closest_pt, const int wp_type)
{
_position = pos;
position_controller_status_s pos_control_status = {};
@@ -190,7 +190,8 @@ void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector
const float pos_to_target_z = fabsf(_curr_wp(2) - _position(2));
if (pos_to_target.length() < target_acceptance_radius && pos_to_target_z > NAV_MC_ALT_RAD.get()) {
if (pos_to_target.length() < target_acceptance_radius && pos_to_target_z > NAV_MC_ALT_RAD.get()
&& wp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
// vehicle above or below the target waypoint
pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f;
}