From 7b9cfac7675c21a2e70481cf55a7b77e461e427d Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Sun, 13 Jan 2019 13:36:03 +0100
Subject: [PATCH] mc_pos_control: change use avoidance to handle data loss
during mission
There were accidents because when missions lead through an obstacle
and it should be avoided but the setpoints from the external obstacle
avoidance module are suddenly missing the mission is continued into the
obstacle which results in a crash.
---
.../mc_pos_control/mc_pos_control_main.cpp | 22 ++++++++++++++-----
1 file changed, 16 insertions(+), 6 deletions(-)
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index a7aa08b64f..74a848ba80 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -1211,12 +1211,22 @@ MulticopterPositionControl::reset_setpoint_to_nan(vehicle_local_position_setpoin
bool
MulticopterPositionControl::use_obstacle_avoidance()
{
- /* check that external obstacle avoidance is sending data and that the first point is valid */
- return (MPC_OBS_AVOID.get()
- && (hrt_elapsed_time((hrt_abstime *)&_traj_wp_avoidance.timestamp) < TRAJECTORY_STREAM_TIMEOUT_US)
- && (_traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid == true)
- && ((_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION) ||
- (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL)));
+ if (MPC_OBS_AVOID.get()) {
+ const bool avoidance_data_timeout = hrt_elapsed_time((hrt_abstime *)&_traj_wp_avoidance.timestamp) > TRAJECTORY_STREAM_TIMEOUT_US;
+ const bool avoidance_point_valid = _traj_wp_avoidance.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid == true;
+ const bool in_mission = _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION;
+ const bool in_rtl = _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL;
+
+ // switch to hold mode to stop when we loose external avoidance data during a mission
+ if (avoidance_data_timeout && in_mission) {
+ send_vehicle_cmd_do(vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER);
+ }
+
+ if ((in_mission || in_rtl) && !avoidance_data_timeout && avoidance_point_valid) {
+ return true;
+ }
+ }
+ return false;
}
void
--
GitLab