From 429f9004608f2fda80e63fddc54baeb3af4b720c Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Sat, 11 May 2013 16:21:25 +0900 Subject: [PATCH] Copter: bug fix for RTL bearing Also smooth RTL's initial climb stage by projecting stopping point --- ArduCopter/commands_logic.pde | 20 +++++++++++++++----- 1 file changed, 15 insertions(+), 5 deletions(-) diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index b0d76f63a5..cf0f6dfde6 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -582,11 +582,13 @@ static bool verify_RTL() // first stage of RTL is the initial climb so just hold current yaw set_yaw_mode(YAW_HOLD); - // get current position - // To-Do: use projection of safe stopping point based on current location and velocity - Vector3f target_pos = inertial_nav.get_position(); - target_pos.z = get_RTL_alt(); - wp_nav.set_destination(target_pos); + // use projection of safe stopping point based on current location and velocity + Vector3f origin, dest; + wp_nav.get_stopping_point(inertial_nav.get_position(),inertial_nav.get_velocity(),origin); + dest.x = origin.x; + dest.y = origin.y; + dest.z = get_RTL_alt(); + wp_nav.set_origin_and_destination(origin,dest); // advance to next rtl state rtl_state = RTL_STATE_INITIAL_CLIMB; @@ -596,6 +598,10 @@ static bool verify_RTL() // Set wp navigation target to above home wp_nav.set_destination(Vector3f(0,0,get_RTL_alt())); + + // initialise original_wp_bearing which is used to point the nose home + wp_bearing = wp_nav.get_bearing_to_destination(); + original_wp_bearing = wp_bearing; // advance to next rtl state rtl_state = RTL_STATE_RETURNING_HOME; @@ -610,6 +616,10 @@ static bool verify_RTL() // Set wp navigation target to above home wp_nav.set_destination(Vector3f(0,0,get_RTL_alt())); + // initialise original_wp_bearing which is used to point the nose home + wp_bearing = wp_nav.get_bearing_to_destination(); + original_wp_bearing = wp_bearing; + // point nose towards home (maybe) set_yaw_mode(get_wp_yaw_mode(true));