Browse Source

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
sbg
Martina Rivizzigno 6 years ago committed by Matthias Grob
parent
commit
0963dc9af1
  1. 3
      src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp
  2. 5
      src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
  3. 5
      src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp

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

@ -280,7 +280,8 @@ bool FlightTaskAuto::_evaluateTriplets() @@ -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

@ -167,7 +167,7 @@ void ObstacleAvoidance::updateAvoidanceDesiredSetpoints(const Vector3f &pos_sp, @@ -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 @@ -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

@ -50,6 +50,8 @@ @@ -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: @@ -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:

Loading…
Cancel
Save