From 7b9cfac7675c21a2e70481cf55a7b77e461e427d Mon Sep 17 00:00:00 2001 From: Matthias Grob 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