From ad656c7e8adc380c3b105739e9a2f20bef44880a Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 10 May 2013 22:37:15 +0900 Subject: [PATCH] Copter: allow lat/lon to be specified for land --- ArduCopter/ArduCopter.pde | 3 +- ArduCopter/commands_logic.pde | 119 +++++++++++++++++++++++++++------- ArduCopter/defines.h | 4 ++ 3 files changed, 103 insertions(+), 23 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index ac7a70b1a0..b9545408ee 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -509,7 +509,8 @@ static uint8_t command_cond_index; static float lon_error, lat_error; // Used to report how many cm we are from the next waypoint or loiter target position static int16_t control_roll; static int16_t control_pitch; -static uint8_t rtl_state; +static uint8_t rtl_state; // records state of rtl (initial climb, returning home, etc) +static uint8_t land_state; // records state of land (flying to location, descending) //////////////////////////////////////////////////////////////////////////////// // Orientation diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 2a71e3f6d0..b0d76f63a5 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -293,22 +293,46 @@ static void do_nav_wp() // caller should set yaw_mode static void do_land() { - if( ap.home_is_set ) { - // switch to loiter if we have gps - set_roll_pitch_mode(ROLL_PITCH_LOITER); + // To-Do: check if we have already landed + + // if location provided we fly to that location at current altitude + if (command_nav_queue.lat != 0 || command_nav_queue.lng != 0) { + // set state to fly to location + land_state = LAND_STATE_FLY_TO_LOCATION; + + // set roll-pitch mode + set_roll_pitch_mode(AUTO_RP); + + // set yaw_mode depending upon contents of WP_YAW_BEHAVIOR parameter + set_yaw_mode(get_wp_yaw_mode(false)); + + // set throttle mode + set_throttle_mode(THROTTLE_AUTO); + + // set nav mode + set_nav_mode(NAV_WP); + + // calculate and set desired location above landing target + Vector3f pos = pv_location_to_vector(command_nav_queue); + pos.z = min(current_loc.alt, RTL_ALT_MAX); + wp_nav.set_destination(pos); }else{ - // otherwise remain with stabilize roll and pitch - set_roll_pitch_mode(ROLL_PITCH_STABLE); + // set landing state + land_state = LAND_STATE_DESCENDING; + + // switch to loiter which restores horizontal control to pilot + // To-Do: check that we are not in failsafe to ensure we don't process bad roll-pitch commands + set_roll_pitch_mode(ROLL_PITCH_LOITER); + + // hold yaw while landing + set_yaw_mode(YAW_HOLD); + + // set throttle mode to land + set_throttle_mode(THROTTLE_LAND); + + // switch into loiter nav mode + set_nav_mode(NAV_LOITER); } - - // hold yaw while landing - set_yaw_mode(YAW_HOLD); - - // set throttle mode to land - set_throttle_mode(THROTTLE_LAND); - - // switch into loiter nav mode - set_nav_mode(NAV_LOITER); } // do_loiter_unlimited - start loitering with no end conditions @@ -439,8 +463,50 @@ static bool verify_takeoff() // verify_land - returns true if landing has been completed static bool verify_land() { - // rely on THROTTLE_LAND mode to correctly update landing status - return ap.land_complete; + bool retval = false; + + switch( land_state ) { + case LAND_STATE_FLY_TO_LOCATION: + // check if we've reached the location + if (wp_nav.reached_destination()) { + // get destination so we can use it for loiter target + Vector3f dest = wp_nav.get_destination(); + + // switch into loiter nav mode + set_nav_mode(NAV_LOITER); + + // override loiter target + wp_nav.set_loiter_target(dest); + + // switch to loiter which restores horizontal control to pilot + // To-Do: check that we are not in failsafe to ensure we don't process bad roll-pitch commands + set_roll_pitch_mode(ROLL_PITCH_LOITER); + + // give pilot control of yaw + set_yaw_mode(YAW_HOLD); + + // set throttle mode to land + set_throttle_mode(THROTTLE_LAND); + + // advance to next state + land_state = LAND_STATE_DESCENDING; + } + break; + + case LAND_STATE_DESCENDING: + // rely on THROTTLE_LAND mode to correctly update landing status + retval = ap.land_complete; + break; + + default: + // this should never happen + // TO-DO: log an error + retval = true; + break; + } + + // true is returned if we've successfully landed + return retval; } // verify_nav_wp - check if we have reached the next way point @@ -573,11 +639,20 @@ static bool verify_RTL() if( millis() - rtl_loiter_start_time > (uint32_t)g.rtl_loiter_time.get() ) { // initiate landing or descent if(g.rtl_alt_final == 0 || ap.failsafe_radio) { - // land - this will switch us into land throttle mode and loiter nav mode and give horizontal control back to pilot - do_land(); - // override landing location (do_land defaults to current location) - // Note: loiter controller ignores target altitude + // switch to loiter which restores horizontal control to pilot + // To-Do: check that we are not in failsafe to ensure we don't process bad roll-pitch commands + set_roll_pitch_mode(ROLL_PITCH_LOITER); + // switch into loiter nav mode + set_nav_mode(NAV_LOITER); + // override landing location (loiter defaults to a projection from current location) wp_nav.set_loiter_target(Vector3f(0,0,0)); + + // hold yaw while landing + set_yaw_mode(YAW_HOLD); + + // set throttle mode to land + set_throttle_mode(THROTTLE_LAND); + // update RTL state rtl_state = RTL_STATE_LAND; }else{ @@ -603,8 +678,8 @@ static bool verify_RTL() break; case RTL_STATE_LAND: - // rely on verify_land to return correct status - retval = verify_land(); + // rely on land_complete flag to indicate if we have landed + retval = ap.land_complete; break; default: diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 7a16f6ec12..5da8d3e265 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -219,6 +219,10 @@ #define RTL_STATE_FINAL_DESCENT 4 #define RTL_STATE_LAND 5 +// LAND state +#define LAND_STATE_FLY_TO_LOCATION 0 +#define LAND_STATE_DESCENDING 1 + //repeating events #define RELAY_TOGGLE 5