fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

ObstacleAvoidance: use hysteresis on z to check progress towards the goal

Browse Source
This commit is contained in:
Martina Rivizzigno 2019-06-28 15:22:29 +02:00 committed by Beat Küng
parent 39e59d6cc4
commit d29f2ff60c
2 changed files with 12 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp
View File

@ -44,6 +44,7 @@ using namespace time_literals;
static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms;
/** If Flighttask fails, keep 0.5 seconds the current setpoint before going into failsafe land */
static constexpr uint64_t TIME_BEFORE_FAILSAFE = 500_ms;
static constexpr uint64_t Z_PROGRESS_TIMEOUT_US = 2_s;
const vehicle_trajectory_waypoint_s empty_trajectory_waypoint = {0, 0, {0, 0, 0, 0, 0, 0, 0},
{ {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, UINT8_MAX, false, {0}},
@ -60,6 +61,7 @@ ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) :
_desired_waypoint = empty_trajectory_waypoint;
_failsafe_position.setNaN();
_avoidance_point_not_valid_hysteresis.set_hysteresis_time_from(false, TIME_BEFORE_FAILSAFE);
_no_progress_z_hysteresis.set_hysteresis_time_from(false, Z_PROGRESS_TIMEOUT_US);
}
@ -133,7 +135,6 @@ void ObstacleAvoidance::updateAvoidanceDesiredWaypoints(const Vector3f &curr_wp,
_desired_waypoint.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS;
_curr_wp = curr_wp;
_ext_yaw_active = ext_yaw_active;
_curr_wp_type = wp_type;
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);
@ -142,7 +143,7 @@ void ObstacleAvoidance::updateAvoidanceDesiredWaypoints(const Vector3f &curr_wp,
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw = curr_yaw;
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed = curr_yawspeed;
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid = true;
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].type = _curr_wp_type;
_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].type = wp_type;
next_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].position);
Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].velocity);
@ -189,12 +190,17 @@ void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector
const float pos_to_target_z = fabsf(_curr_wp(2) - _position(2));
bool no_progress_z = (pos_to_target_z > _prev_pos_to_target_z);
_no_progress_z_hysteresis.set_state_and_update(no_progress_z, hrt_absolute_time());
if (pos_to_target.length() < target_acceptance_radius && pos_to_target_z > _param_nav_mc_alt_rad.get()
&& _curr_wp_type != position_setpoint_s::SETPOINT_TYPE_TAKEOFF) {
&& _no_progress_z_hysteresis.get_state()) {
// vehicle above or below the target waypoint
pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f;
}
_prev_pos_to_target_z = pos_to_target_z;
// do not check for waypoints yaw acceptance in navigator
pos_control_status.yaw_acceptance = NAN;

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

@ -128,9 +128,11 @@ private:
matrix::Vector3f _failsafe_position = {}; /**< vehicle position when entered in failsafe */
systemlib::Hysteresis _avoidance_point_not_valid_hysteresis{false}; /**< becomes true if the companion doesn't start sending valid setpoints */
systemlib::Hysteresis _no_progress_z_hysteresis{false}; /**< becomes true if the vehicle is not making progress towards the z component of the goal */
float _prev_pos_to_target_z = -1.f; /**< z distance to the goal */
bool _ext_yaw_active = false; /**< true, if external yaw handling is active */
int _curr_wp_type = 0;
/**
* Publishes vehicle command.