From 43d14defd45de043944e68fc512c1e9b264e1ed4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 22 Apr 2016 20:44:57 +0900 Subject: [PATCH] Copter: RTL handles terrain data failure --- ArduCopter/Copter.h | 6 +++-- ArduCopter/control_rtl.cpp | 52 ++++++++++++++++++++------------------ ArduCopter/defines.h | 2 +- 3 files changed, 33 insertions(+), 27 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index a5b6a23f4d..a16d779d47 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -353,6 +353,7 @@ private: Location_Class return_target; Location_Class descent_target; bool land; + bool terrain_used; } rtl_path; // Circle @@ -823,6 +824,7 @@ private: bool throw_height_good(); bool rtl_init(bool ignore_checks); + void rtl_restart_without_terrain(); void rtl_run(); void rtl_climb_start(); void rtl_return_start(); @@ -833,8 +835,8 @@ private: void rtl_descent_run(); void rtl_land_start(); void rtl_land_run(); - void rtl_build_path(); - void rtl_compute_return_alt(const Location_Class &rtl_origin_point, Location_Class &rtl_return_target); + void rtl_build_path(bool terrain_following_allowed); + void rtl_compute_return_alt(const Location_Class &rtl_origin_point, Location_Class &rtl_return_target, bool terrain_following_allowed); bool sport_init(bool ignore_checks); void sport_run(); bool stabilize_init(bool ignore_checks); diff --git a/ArduCopter/control_rtl.cpp b/ArduCopter/control_rtl.cpp index 1cd43f7f06..6c8ed70c40 100644 --- a/ArduCopter/control_rtl.cpp +++ b/ArduCopter/control_rtl.cpp @@ -13,7 +13,7 @@ bool Copter::rtl_init(bool ignore_checks) { if (position_ok() || ignore_checks) { - rtl_build_path(); + rtl_build_path(true); rtl_climb_start(); return true; }else{ @@ -21,6 +21,18 @@ bool Copter::rtl_init(bool ignore_checks) } } +// re-start RTL with terrain following disabled +void Copter::rtl_restart_without_terrain() +{ + // log an error + Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_RESTARTED_RTL); + if (rtl_path.terrain_used) { + rtl_build_path(false); + rtl_climb_start(); + gcs_send_text(MAV_SEVERITY_CRITICAL,"Restarting RTL - Terrain data missing"); + } +} + // rtl_run - runs the return-to-launch controller // should be called at 100hz or more void Copter::rtl_run() @@ -91,9 +103,10 @@ void Copter::rtl_climb_start() // set the destination if (!wp_nav.set_wp_destination(rtl_path.climb_target)) { - // failure to set destination (likely because of missing terrain data) + // this should not happen because rtl_build_path will have checked terrain data was available Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION); - // To-Do: handle failure + set_mode(LAND, MODE_REASON_TERRAIN_FAILSAFE); + return; } wp_nav.set_fast_waypoint(true); @@ -108,9 +121,8 @@ void Copter::rtl_return_start() rtl_state_complete = false; if (!wp_nav.set_wp_destination(rtl_path.return_target)) { - // failure to set destination (likely because of missing terrain data) - Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_SET_DESTINATION); - // To-Do: handle failure + // failure must be caused by missing terrain data, restart RTL + rtl_restart_without_terrain(); } // initialise yaw to point home (maybe) @@ -151,11 +163,7 @@ void Copter::rtl_climb_return_run() motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller - if (!wp_nav.update_wpnav()) { - // failures to update probably caused by missing terrain data - Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_UPDATE_TARGET); - // To-Do: handle failure to update probably caused by lack of terrain data by removing need for terrain data? - } + failsafe_terrain_set_status(wp_nav.update_wpnav()); // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); @@ -173,7 +181,7 @@ void Copter::rtl_climb_return_run() rtl_state_complete = wp_nav.reached_wp_destination(); } -// rtl_return_start - initialise return to home +// rtl_loiterathome_start - initialise return to home void Copter::rtl_loiterathome_start() { rtl_state = RTL_LoiterAtHome; @@ -222,11 +230,7 @@ void Copter::rtl_loiterathome_run() motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller - if (!wp_nav.update_wpnav()) { - // failures to update probably caused by missing terrain data - Log_Write_Error(ERROR_SUBSYSTEM_NAVIGATION, ERROR_CODE_FAILED_TO_UPDATE_TARGET); - // To-Do: handle failure to update probably caused by lack of terrain data by removing need for terrain data? - } + failsafe_terrain_set_status(wp_nav.update_wpnav()); // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); @@ -441,7 +445,7 @@ void Copter::rtl_land_run() rtl_state_complete = ap.land_complete; } -void Copter::rtl_build_path() +void Copter::rtl_build_path(bool terrain_following_allowed) { // origin point is our stopping point Vector3f stopping_point; @@ -458,7 +462,7 @@ void Copter::rtl_build_path() #endif // compute return altitude - rtl_compute_return_alt(rtl_path.origin_point, rtl_path.return_target); + rtl_compute_return_alt(rtl_path.origin_point, rtl_path.return_target, terrain_following_allowed); // climb target is above our origin point at the return altitude rtl_path.climb_target = Location_Class(rtl_path.origin_point.lat, rtl_path.origin_point.lng, rtl_path.return_target.alt, rtl_path.return_target.get_alt_frame()); @@ -474,7 +478,7 @@ void Copter::rtl_build_path() // rtl_origin_point is the stopping point of the vehicle when rtl is initiated // rtl_return_target is the home or rally point that the vehicle is returning to. It's lat, lng and alt values must already have been filled in before this function is called // rtl_return_target's altitude is updated to a higher altitude that the vehicle can safely return at (frame may also be set) -void Copter::rtl_compute_return_alt(const Location_Class &rtl_origin_point, Location_Class &rtl_return_target) +void Copter::rtl_compute_return_alt(const Location_Class &rtl_origin_point, Location_Class &rtl_return_target, bool terrain_following_allowed) { float rtl_return_dist_cm = rtl_return_target.get_distance(rtl_origin_point) * 100.0f; @@ -482,14 +486,14 @@ void Copter::rtl_compute_return_alt(const Location_Class &rtl_origin_point, Loca int32_t curr_alt = current_loc.alt; // decide if we should use terrain altitudes - bool rtl_terrain_use = terrain_use(); - if (rtl_terrain_use) { + rtl_path.terrain_used = terrain_use() && terrain_following_allowed; + if (rtl_path.terrain_used) { // attempt to retrieve terrain alt for current location, stopping point and origin int32_t origin_terr_alt, return_target_terr_alt; if (!rtl_origin_point.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, origin_terr_alt) || !rtl_origin_point.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, return_target_terr_alt) || !current_loc.get_alt_cm(Location_Class::ALT_FRAME_ABOVE_TERRAIN, curr_alt)) { - rtl_terrain_use = false; + rtl_path.terrain_used = false; Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA); } } @@ -514,7 +518,7 @@ void Copter::rtl_compute_return_alt(const Location_Class &rtl_origin_point, Loca ret = MAX(ret, curr_alt); // convert return-target to alt-above-home or alt-above-terrain - if (!rtl_terrain_use || !rtl_return_target.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_TERRAIN)) { + if (!rtl_path.terrain_used || !rtl_return_target.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_TERRAIN)) { if (!rtl_return_target.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME)) { // this should never happen but just in case rtl_return_target.set_alt(0, Location_Class::ALT_FRAME_ABOVE_HOME); diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 6a35524766..b26fda7776 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -406,7 +406,7 @@ enum ThrowModeState { #define ERROR_CODE_MISSING_TERRAIN_DATA 2 // subsystem specific error codes -- navigation #define ERROR_CODE_FAILED_TO_SET_DESTINATION 2 -#define ERROR_CODE_FAILED_TO_UPDATE_TARGET 3 +#define ERROR_CODE_RESTARTED_RTL 3 // parachute failed to deploy because of low altitude or landed #define ERROR_CODE_PARACHUTE_TOO_LOW 2