fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-22 01:27:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

run avoidance also in Loiter

Browse Source
This commit is contained in:
Martina Rivizzigno
2020-02-19 09:19:12 +01:00
committed by Julian Kent
parent aee1e70642
commit a9491dfaf7
1 changed files with 8 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/flight_tasks/tasks/Utility/ObstacleAvoidance.cpp
+8 -6
View File
@@ -62,11 +62,6 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
_sub_vehicle_status.update();
_sub_vehicle_trajectory_waypoint.update();
if (_sub_vehicle_status.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) {
// if in failsafe nav_state LOITER, don't inject setpoints from avoidance system
return;
}
const bool avoidance_data_timeout = hrt_elapsed_time((hrt_abstime *)&_sub_vehicle_trajectory_waypoint.get().timestamp)
> TRAJECTORY_STREAM_TIMEOUT_US;
const bool avoidance_point_valid =
@@ -74,7 +69,14 @@ void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel
_avoidance_point_not_valid_hysteresis.set_state_and_update(!avoidance_point_valid, hrt_absolute_time());
if (avoidance_data_timeout || _avoidance_point_not_valid_hysteresis.get_state()) {
const bool avoidance_invalid = (avoidance_data_timeout || _avoidance_point_not_valid_hysteresis.get_state());
if ((_sub_vehicle_status.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) && avoidance_invalid) {
// if in nav_state LOITER and avoidance isn't healthy, don't inject setpoints from avoidance system
return;
}
if (avoidance_invalid) {
PX4_WARN("Obstacle Avoidance system failed, loitering");
_publishVehicleCmdDoLoiter();