dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

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

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-22 10:46:53 +02:00 committed by Matthias Grob
parent 9e8575b71b
commit 0963dc9af1
3 changed files with 9 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
View File

@ -280,7 +280,8 @@ bool FlightTaskAuto::_evaluateTriplets()
_sub_triplet_setpoint->get().next.yaw,
_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);
_obstacle_avoidance.checkAvoidanceProgress(_position, _triplet_prev_wp, _target_acceptance_radius, _closest_pt,
_sub_triplet_setpoint->get().current.type);
}
return true;

5
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
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;
}

5
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp
View File

@ -50,6 +50,8 @@
#include <uORB/topics/vehicle_command.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/vehicle_trajectory_waypoint.h>
#include <uORB/topics/position_setpoint.h>
#include <matrix/matrix/math.hpp>
@ -96,9 +98,10 @@ public:
* @param prev_wp, previous position triplet
* @param target_acceptance_radius, current position triplet xy acceptance radius
* @param closest_pt, closest point to the vehicle on the line previous-current position triplet
* @param
*/
void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp,
float target_acceptance_radius, const matrix::Vector2f &closest_pt);
float target_acceptance_radius, const matrix::Vector2f &closest_pt, const int wp_type);
private: