From 352b6ae82a451847153e742dca8638301f46a1cb Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 18 Mar 2016 19:44:09 +0900 Subject: [PATCH] Copter: RTL supports terrain altitudes --- ArduCopter/Copter.h | 12 ++-- ArduCopter/control_rtl.cpp | 109 +++++++++++++++++++++++++------------ 2 files changed, 79 insertions(+), 42 deletions(-) diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 4dc0f4bb7a..33abfbc126 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -344,11 +344,11 @@ private: bool rtl_state_complete; // set to true if the current state is completed struct { - // NEU w/ origin-relative altitude - Vector3f origin_point; - Vector3f climb_target; - Vector3f return_target; - Vector3f descent_target; + // NEU w/ Z element alt-above-ekf-origin unless use_terrain is true in which case Z element is alt-above-terrain + Location_Class origin_point; + Location_Class climb_target; + Location_Class return_target; + Location_Class descent_target; bool land; } rtl_path; @@ -830,7 +830,7 @@ private: void rtl_land_start(); void rtl_land_run(); void rtl_build_path(); - float rtl_compute_return_alt_above_origin(float rtl_return_dist); + void rtl_compute_return_alt(const Location_Class &rtl_origin_point, Location_Class &rtl_return_target); 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 f4d10c56cb..1cd43f7f06 100644 --- a/ArduCopter/control_rtl.cpp +++ b/ArduCopter/control_rtl.cpp @@ -90,7 +90,11 @@ void Copter::rtl_climb_start() } // set the destination - wp_nav.set_wp_destination(rtl_path.climb_target); + if (!wp_nav.set_wp_destination(rtl_path.climb_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 + } wp_nav.set_fast_waypoint(true); // hold current yaw during initial climb @@ -103,7 +107,11 @@ void Copter::rtl_return_start() rtl_state = RTL_ReturnHome; rtl_state_complete = false; - wp_nav.set_wp_destination(rtl_path.return_target); + 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 + } // initialise yaw to point home (maybe) set_auto_yaw_mode(get_default_auto_yaw_mode(true)); @@ -143,7 +151,11 @@ void Copter::rtl_climb_return_run() motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller - wp_nav.update_wpnav(); + 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? + } // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); @@ -210,7 +222,11 @@ void Copter::rtl_loiterathome_run() motors.set_desired_spool_state(AP_Motors::DESIRED_THROTTLE_UNLIMITED); // run waypoint controller - wp_nav.update_wpnav(); + 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? + } // call z-axis position controller (wpnav should have already updated it's alt target) pos_control.update_z_controller(); @@ -310,14 +326,14 @@ void Copter::rtl_descent_run() wp_nav.update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler); // call z-axis position controller - pos_control.set_alt_target_with_slew(rtl_path.descent_target.z, G_Dt); + pos_control.set_alt_target_with_slew(rtl_path.descent_target.alt, G_Dt); pos_control.update_z_controller(); // roll & pitch from waypoint controller, yaw rate from pilot attitude_control.input_euler_angle_roll_pitch_euler_rate_yaw(wp_nav.get_roll(), wp_nav.get_pitch(), target_yaw_rate); // check if we've reached within 20cm of final altitude - rtl_state_complete = fabsf(rtl_path.descent_target.z - inertial_nav.get_altitude()) < 20.0f; + rtl_state_complete = fabsf(rtl_path.descent_target.alt - current_loc.alt) < 20.0f; } // rtl_loiterathome_start - initialise controllers to loiter over home @@ -428,62 +444,83 @@ void Copter::rtl_land_run() void Copter::rtl_build_path() { // origin point is our stopping point - pos_control.get_stopping_point_xy(rtl_path.origin_point); - pos_control.get_stopping_point_z(rtl_path.origin_point); + Vector3f stopping_point; + pos_control.get_stopping_point_xy(stopping_point); + pos_control.get_stopping_point_z(stopping_point); + rtl_path.origin_point = Location_Class(stopping_point); + rtl_path.origin_point.change_alt_frame(Location_Class::ALT_FRAME_ABOVE_HOME); // set return target to nearest rally point or home position #if AC_RALLY == ENABLED - Location rally_point = rally.calc_best_rally_or_home_location(current_loc, 0); - rtl_path.return_target = pv_location_to_vector(rally_point); + rtl_path.return_target = rally.calc_best_rally_or_home_location(current_loc, ahrs.get_home().alt); #else - rtl_path.return_target = pv_location_to_vector(ahrs.get_home()); + rtl_path.return_target = ahrs.get_home(); #endif - Vector3f return_vector = rtl_path.return_target-rtl_path.origin_point; - - float rtl_return_dist = pythagorous2(return_vector.x, return_vector.y); - // compute return altitude - rtl_path.return_target.z = rtl_compute_return_alt_above_origin(rtl_return_dist); + rtl_compute_return_alt(rtl_path.origin_point, rtl_path.return_target); // climb target is above our origin point at the return altitude - rtl_path.climb_target.x = rtl_path.origin_point.x; - rtl_path.climb_target.y = rtl_path.origin_point.y; - rtl_path.climb_target.z = rtl_path.return_target.z; + 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()); // descent target is below return target at rtl_alt_final - rtl_path.descent_target.x = rtl_path.return_target.x; - rtl_path.descent_target.y = rtl_path.return_target.y; - rtl_path.descent_target.z = pv_alt_above_origin(g.rtl_alt_final); + rtl_path.descent_target = Location_Class(rtl_path.return_target.lat, rtl_path.return_target.lng, g.rtl_alt_final, Location_Class::ALT_FRAME_ABOVE_HOME); // set land flag rtl_path.land = g.rtl_alt_final <= 0; } -// return altitude in cm above origin at which vehicle should return home -float Copter::rtl_compute_return_alt_above_origin(float rtl_return_dist) +// return altitude in cm above home at which vehicle should return home +// 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) { - // maximum of current altitude + climb_min and rtl altitude - float ret = MAX(current_loc.alt + MAX(0, g.rtl_climb_min), MAX(g.rtl_altitude, RTL_ALT_MIN)); + float rtl_return_dist_cm = rtl_return_target.get_distance(rtl_origin_point) * 100.0f; - if (g.rtl_cone_slope >= RTL_MIN_CONE_SLOPE) { // don't allow really shallow slopes - ret = MAX(current_loc.alt, MIN(ret, MAX(rtl_return_dist*g.rtl_cone_slope, current_loc.alt+RTL_ABS_MIN_CLIMB))); + // curr_alt is current altitude above home or above terrain depending upon use_terrain + int32_t curr_alt = current_loc.alt; + + // decide if we should use terrain altitudes + bool rtl_terrain_use = terrain_use(); + if (rtl_terrain_use) { + // 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; + Log_Write_Error(ERROR_SUBSYSTEM_TERRAIN, ERROR_CODE_MISSING_TERRAIN_DATA); + } + } + + // maximum of current altitude + climb_min and rtl altitude + float ret = MAX(curr_alt + MAX(0, g.rtl_climb_min), MAX(g.rtl_altitude, RTL_ALT_MIN)); + + // don't allow really shallow slopes + if (g.rtl_cone_slope >= RTL_MIN_CONE_SLOPE) { + ret = MAX(curr_alt, MIN(ret, MAX(rtl_return_dist_cm*g.rtl_cone_slope, curr_alt+RTL_ABS_MIN_CLIMB))); } #if AC_FENCE == ENABLED // ensure not above fence altitude if alt fence is enabled + // Note: we are assuming the fence alt is the same frame as ret if ((fence.get_enabled_fences() & AC_FENCE_TYPE_ALT_MAX) != 0) { ret = MIN(ret, fence.get_safe_alt()*100.0f); } #endif -#if AC_RALLY == ENABLED - // rally_point.alt will be the altitude of the nearest rally point or the RTL_ALT. uses absolute altitudes - Location rally_point = rally.calc_best_rally_or_home_location(current_loc, ret+ahrs.get_home().alt); - rally_point.alt -= ahrs.get_home().alt; // convert to altitude above home - rally_point.alt = MAX(rally_point.alt, current_loc.alt); // ensure we do not descend before reaching home - ret = rally_point.alt; -#endif + // ensure we do not descend + ret = MAX(ret, curr_alt); - return pv_alt_above_origin(ret); + // 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_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); + } + } + + // add ret to altitude + rtl_return_target.alt += ret; }