diff --git a/ArduCopter/AP_State.pde b/ArduCopter/AP_State.pde index b5f75bf440..8deb25a101 100644 --- a/ArduCopter/AP_State.pde +++ b/ArduCopter/AP_State.pde @@ -96,15 +96,6 @@ void set_land_complete(bool b) ap.land_complete = b; } -// --------------------------------------------- -void set_rtl_reached_alt(bool b) -{ - if(b){ - Log_Write_Event(DATA_RTL_REACHED_ALT); - } - ap.rtl_reached_alt = b; -} - // --------------------------------------------- void set_alt_change(uint8_t flag){ diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 9be069e0e4..7333038cd4 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -400,11 +400,10 @@ static union { uint8_t failsafe : 1; // 9 // A status flag for the failsafe state uint8_t do_flip : 1; // 10 // Used to enable flip code uint8_t takeoff_complete : 1; // 11 - uint8_t rtl_reached_alt : 1; // 12 - uint8_t land_complete : 1; // 13 - uint8_t compass_status : 1; // 14 - uint8_t gps_status : 1; // 15 - uint8_t fast_corner : 1; // 16 // should we take the waypoint quickly or slow down? + uint8_t land_complete : 1; // 12 + uint8_t compass_status : 1; // 13 + uint8_t gps_status : 1; // 14 + uint8_t fast_corner : 1; // 15 // should we take the waypoint quickly or slow down? }; uint16_t value; } ap; @@ -574,6 +573,7 @@ static int16_t max_speed_old; static int32_t long_error, lat_error; static int16_t control_roll; static int16_t control_pitch; +static uint8_t rtl_state; //////////////////////////////////////////////////////////////////////////////// // Orientation @@ -812,7 +812,7 @@ static int32_t nav_yaw; static int32_t auto_yaw; static uint8_t yaw_timer; // Options include: no tracking, point at next wp, or at a target -static byte yaw_tracking = MAV_ROI_WPNEXT; +static byte auto_yaw_tracking = MAV_ROI_WPNEXT; // In AP Mission scripting we have a fine level of control for Yaw // This is our initial angle for relative Yaw movements static int32_t command_yaw_start; @@ -828,7 +828,7 @@ static int16_t command_yaw_speed; static byte command_yaw_dir; // Direction we will turn – 1 = relative, 0 = Absolute static byte command_yaw_relative; -// Yaw will point at this location if yaw_tracking is set to MAV_ROI_LOCATION +// Yaw will point at this location if auto_yaw_tracking is set to MAV_ROI_LOCATION static struct Location target_WP; @@ -897,7 +897,7 @@ static float dTnav; // Counters for branching from 4 minute control loop used to save Compass offsets static int16_t superslow_loopCounter; // Loiter timer - Records how long we have been in loiter -static uint32_t loiter_timer; +static uint32_t rtl_loiter_start_time; // disarms the copter while in Acro or Stabilize mode after 30 seconds of no flight static uint8_t auto_disarming_counter; // prevents duplicate GPS messages from entering system @@ -2242,17 +2242,24 @@ static void update_nav_wp() static void update_auto_yaw() { - if(wp_control == CIRCLE_MODE) { - auto_yaw = get_bearing_cd(¤t_loc, &circle_WP); - - }else if(wp_control == LOITER_MODE) { - // just hold nav_yaw - - }else if(yaw_tracking == MAV_ROI_LOCATION) { - auto_yaw = get_bearing_cd(¤t_loc, &target_WP); - - }else if(yaw_tracking == MAV_ROI_WPNEXT) { - // Point towards next WP - auto_yaw = original_target_bearing; + switch( wp_control ) { + case LOITER_MODE: + // just hold nav_yaw + break; + case WP_MODE: + if(auto_yaw_tracking == MAV_ROI_LOCATION) { + auto_yaw = get_bearing_cd(¤t_loc, &target_WP); + }else if(auto_yaw_tracking == MAV_ROI_WPNEXT) { + // Point towards next WP + // we don't use target_bearing because we don't want the copter to turn too much during flight + auto_yaw = original_target_bearing; + } + break; + case CIRCLE_MODE: + auto_yaw = get_bearing_cd(¤t_loc, &circle_WP); + break; + case NO_NAV_MODE: + // just hold nav_yaw + break; } } diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index e5d7d7591d..a5f363931a 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -764,6 +764,8 @@ void set_throttle_out( int16_t throttle_out, bool apply_angle_boost ) // clear angle_boost for logging purposes angle_boost = 0; } + + // TO-DO: fix line below because this function is also called by accel based throttle controller throttle_accel_controller_active = false; } @@ -1032,7 +1034,7 @@ get_throttle_althold(int32_t target_alt, int16_t max_climb_rate) // sends the desired acceleration in the accel based throttle controller // called at 50hz #define LAND_START_ALT 1000 // altitude in cm where land controller switches to slow rate of descent -#define LAND_DETECTOR_TRIGGER 250 // number of 50hz iterations with near zero climb rate that triggers landing complete. +#define LAND_DETECTOR_TRIGGER 50 // number of 50hz iterations with near zero climb rate and low throttle that triggers landing complete. static void get_throttle_land() { @@ -1042,17 +1044,17 @@ get_throttle_land() }else{ get_throttle_rate_stabilized(-abs(g.land_speed)); - // detect whether we have landed + // detect whether we have landed by watching for minimum throttle and now movement #if INERTIAL_NAV_Z == ENABLED - if (abs(inertial_nav._velocity.z) < 20) { + if (abs(inertial_nav._velocity.z) < 20 && (g.rc_3.servo_out <= get_angle_boost(g.throttle_min) || g.pid_throttle_accel.get_integrator() <= -150)) { #else - if (abs(climb_rate) < 20) { + if (abs(climb_rate) < 20 && (g.rc_3.servo_out <= get_angle_boost(g.throttle_min) || g.pid_throttle_accel.get_integrator() <= -150)) { #endif if( land_detector < LAND_DETECTOR_TRIGGER ) { land_detector++; }else{ set_land_complete(true); - if( g.rc_3.control_in == 0 ) { + if( g.rc_3.control_in == 0 || ap.failsafe ) { init_disarm_motors(); } } diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index 5c77c42510..1121aab49c 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -134,10 +134,10 @@ public: // // 160: Navigation parameters // - k_param_RTL_altitude = 160, + k_param_rtl_altitude = 160, k_param_crosstrack_gain, - k_param_auto_land_timeout, - k_param_rtl_approach_alt, + k_param_rtl_loiter_time, + k_param_rtl_alt_final, k_param_tilt_comp, //164 @@ -243,7 +243,7 @@ public: AP_Int8 serial3_baud; AP_Int8 telem_delay; - AP_Int16 RTL_altitude; + AP_Int16 rtl_altitude; AP_Int8 sonar_enabled; AP_Int8 sonar_type; // 0 = XL, 1 = LV, // 2 = XLL (XL with 10m range) @@ -260,7 +260,7 @@ public: AP_Int8 optflow_enabled; AP_Float low_voltage; AP_Int8 super_simple; - AP_Int16 rtl_approach_alt; + AP_Int16 rtl_alt_final; AP_Int8 tilt_comp; AP_Int8 axis_enabled; AP_Int8 copter_leds_mode; // Operating mode of LED @@ -281,7 +281,7 @@ public: AP_Int16 waypoint_speed_max; AP_Float crosstrack_gain; AP_Int16 crosstrack_min_distance; - AP_Int32 auto_land_timeout; + AP_Int32 rtl_loiter_time; AP_Int16 land_speed; diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index d0fad6a314..0b8bd14f19 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -36,14 +36,14 @@ const AP_Param::Info var_info[] PROGMEM = { // @Increment: 1 GSCALAR(telem_delay, "TELEM_DELAY", 0), - // @Param: ALT_HOLD_RTL + // @Param: ALT_RTL // @DisplayName: RTL Altitude - // @Description: This is the altitude the model will move to before Returning to Launch. Set to zero to return at current altitude. + // @Description: The minimum altitude the model will move to before Returning to Launch. Set to zero to return at current altitude. // @Units: centimeters // @Range: 0 4000 // @Increment: 1 // @User: Standard - GSCALAR(RTL_altitude, "ALT_HOLD_RTL", RTL_HOLD_ALT), + GSCALAR(rtl_altitude, "RTL_ALT", RTL_ALT), // @Param: SONAR_ENABLE // @DisplayName: Enable Sonar @@ -106,14 +106,14 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(super_simple, "SUPER_SIMPLE", SUPER_SIMPLE), - // @Param: APPROACH_ALT - // @DisplayName: RTL Approach Altitude - // @Description: This is the altitude the vehicle will move to as the final stage of Returning to Launch. Set to zero to land. + // @Param: RTL_ALT_FINAL + // @DisplayName: RTL Final Altitude + // @Description: This is the altitude the vehicle will move to as the final stage of Returning to Launch or after completing a mission. Set to -1 to disable, zero to land. // @Units: centimeters - // @Range: 0 1000 + // @Range: -1 1000 // @Increment: 1 // @User: Standard - GSCALAR(rtl_approach_alt, "APPROACH_ALT", RTL_APPROACH_ALT), + GSCALAR(rtl_alt_final, "RTL_ALT_FINAL", RTL_ALT_FINAL), // @Param: TILT // @DisplayName: Auto Tilt Compensation @@ -191,7 +191,14 @@ const AP_Param::Info var_info[] PROGMEM = { // @User: Standard GSCALAR(crosstrack_min_distance, "XTRK_MIN_DIST", CROSSTRACK_MIN_DISTANCE), - GSCALAR(auto_land_timeout, "AUTO_LAND", AUTO_LAND_TIME*1000), + // @Param: RTL_LOITER_TIME + // @DisplayName: RTL loiter time + // @Description: Time (in milliseconds) to loiter above home before begining final descent + // @Units: ms + // @Range: 0 60000 + // @Increment: 1000 + // @User: Standard + GSCALAR(rtl_loiter_time, "RTL_LOIT_TIME", RTL_LOITER_TIME), // @Param: LAND_SPEED // @DisplayName: Land speed diff --git a/ArduCopter/commands.pde b/ArduCopter/commands.pde index 8f420af54f..8cb5bc4e1b 100644 --- a/ArduCopter/commands.pde +++ b/ArduCopter/commands.pde @@ -12,8 +12,8 @@ static void init_commands() ap.fast_corner = false; reset_desired_speed(); - // default Yaw tracking - yaw_tracking = MAV_ROI_WPNEXT; + // default auto mode yaw tracking + auto_yaw_tracking = MAV_ROI_WPNEXT; } // Getters @@ -110,12 +110,12 @@ static void set_cmd_with_index(struct Location temp, int i) static int32_t get_RTL_alt() { - if(g.RTL_altitude <= 0) { - return min(current_loc.alt, MAXIMUM_RTL_ALT); - }else if (g.RTL_altitude < current_loc.alt) { - return min(current_loc.alt, MAXIMUM_RTL_ALT); + if(g.rtl_altitude <= 0) { + return min(current_loc.alt, RTL_ALT_MAX); + }else if (g.rtl_altitude < current_loc.alt) { + return min(current_loc.alt, RTL_ALT_MAX); }else{ - return g.RTL_altitude; + return g.rtl_altitude; } } @@ -202,6 +202,8 @@ static void init_home() #if INERTIAL_NAV_XY == ENABLED // set inertial nav's home position inertial_nav.set_current_position(g_gps->longitude, g_gps->latitude); + inertial_nav.set_altitude(home.alt); + inertial_nav.set_velocity_z(0); #endif if (g.log_bitmask & MASK_LOG_CMD) @@ -213,7 +215,7 @@ static void init_home() // Load home for a default guided_WP // ------------- guided_WP = home; - guided_WP.alt += g.RTL_altitude; + guided_WP.alt += g.rtl_altitude; } diff --git a/ArduCopter/commands_logic.pde b/ArduCopter/commands_logic.pde index 32e17dab1e..9528437339 100644 --- a/ArduCopter/commands_logic.pde +++ b/ArduCopter/commands_logic.pde @@ -209,19 +209,29 @@ static bool verify_may() // /********************************************************************************/ +// do_RTL - start Return-to-Launch static void do_RTL(void) { - // TODO: Altitude option from mission planner - Location temp = home; - temp.alt = get_RTL_alt(); + // set rtl state + rtl_state = RTL_STATE_RETURNING_HOME; - //so we know where we are navigating from - // -------------------------------------- - next_WP = current_loc; + // set roll, pitch and yaw modes + roll_pitch_mode = RTL_RP; + yaw_mode = YAW_AUTO; + auto_yaw_tracking = MAV_ROI_WPNEXT; + set_throttle_mode(RTL_THR); - // Loads WP from Memory - // -------------------- - set_next_WP(&temp); + // set navigation mode + wp_control = WP_MODE; + + // so we know where we are navigating from + next_WP = current_loc; + + // Set navigation target to home + set_next_WP(&home); + + // override altitude to RTL altitude + set_new_altitude(get_RTL_alt()); // output control mode to the ground station // ----------------------------------------- @@ -354,6 +364,7 @@ static bool verify_land() } // turn off loiter below 1m + // To-Do: instead of turning off loiter we should make loiter less aggressive if(current_loc.alt < 100 ) { wp_control = NO_NAV_MODE; } @@ -452,19 +463,79 @@ static bool verify_loiter_turns() return false; } +// verify_RTL - handles any state changes required to implement RTL +// do_RTL should have been called once first to initialise all variables +// returns true with RTL has completed successfully static bool verify_RTL() { - wp_control = WP_MODE; + bool retval = false; - // Did we pass the WP? // Distance checking - if((wp_distance <= (g.waypoint_radius * 100)) || check_missed_wp()) { - wp_control = LOITER_MODE; + switch( rtl_state ) { - //gcs_send_text_P(SEVERITY_LOW,PSTR("Reached home")); - return true; - }else{ - return false; + case RTL_STATE_RETURNING_HOME: + // if we've reached home initiate loiter + if (wp_distance <= g.waypoint_radius * 100 || check_missed_wp()) { + rtl_state = RTL_STATE_LOITERING_AT_HOME; + wp_control = LOITER_MODE; + + // set loiter timer + rtl_loiter_start_time = millis(); + + // give pilot back control of yaw + yaw_mode = YAW_HOLD; + } + break; + + case RTL_STATE_LOITERING_AT_HOME: + // check if we've loitered long enough + if( millis() - rtl_loiter_start_time > (uint32_t)g.rtl_loiter_time.get() ) { + // initiate landing or descent + if(g.rtl_alt_final == 0) { + // land + do_land(); + // override landing location (do_land defaults to current location) + next_WP.lat = home.lat; + next_WP.lng = home.lng; + // update RTL state + rtl_state = RTL_STATE_LAND; + }else{ + // descend + if(current_loc.alt > g.rtl_alt_final) { + set_new_altitude(g.rtl_alt_final); + } + // update RTL state + rtl_state = RTL_STATE_FINAL_DESCENT; + } + } + break; + + case RTL_STATE_FINAL_DESCENT: + // rely on altitude check to confirm we have reached final altitude + if(current_loc.alt <= g.rtl_alt_final || alt_change_flag == REACHED_ALT) { + // switch to regular loiter mode + set_mode(LOITER); + // override location and altitude + set_next_WP(&home); + // override altitude to RTL altitude + set_new_altitude(g.rtl_alt_final); + retval = true; + } + break; + + case RTL_STATE_LAND: + // rely on verify_land to return correct status + retval = verify_land(); + break; + + default: + // this should never happen + // TO-DO: log an error + retval = true; + break; } + + // true is returned if we've successfully completed RTL + return retval; } /********************************************************************************/ @@ -496,7 +567,7 @@ static void do_within_distance() static void do_yaw() { //cliSerial->println("dyaw "); - yaw_tracking = MAV_ROI_NONE; + auto_yaw_tracking = MAV_ROI_NONE; // target angle in degrees command_yaw_start = nav_yaw; // current position @@ -669,9 +740,9 @@ static void do_change_speed() static void do_target_yaw() { - yaw_tracking = command_cond_queue.p1; + auto_yaw_tracking = command_cond_queue.p1; - if(yaw_tracking == MAV_ROI_LOCATION) { + if(auto_yaw_tracking == MAV_ROI_LOCATION) { target_WP = command_cond_queue; } } @@ -793,7 +864,7 @@ static void do_nav_roi() // check if mount type requires us to rotate the quad if( camera_mount.get_mount_type() != AP_Mount::k_pan_tilt && camera_mount.get_mount_type() != AP_Mount::k_pan_tilt_roll ) { - yaw_tracking = MAV_ROI_LOCATION; + auto_yaw_tracking = MAV_ROI_LOCATION; target_WP = command_nav_queue; auto_yaw = get_bearing_cd(¤t_loc, &target_WP); } @@ -808,7 +879,7 @@ static void do_nav_roi() // 4: point at a target given a target id (can't be implmented) #else // if we have no camera mount simply rotate the quad - yaw_tracking = MAV_ROI_LOCATION; + auto_yaw_tracking = MAV_ROI_LOCATION; target_WP = command_nav_queue; auto_yaw = get_bearing_cd(¤t_loc, &target_WP); #endif diff --git a/ArduCopter/commands_process.pde b/ArduCopter/commands_process.pde index edc463e7d8..8b8bf92c34 100644 --- a/ArduCopter/commands_process.pde +++ b/ArduCopter/commands_process.pde @@ -217,11 +217,11 @@ static void exit_mission() // we will disarm the motors after landing. }else{ // If the approach altitude is valid (above 1m), do approach, else land - if(g.rtl_approach_alt == 0) { + if(g.rtl_alt_final == 0) { set_mode(LAND); }else{ set_mode(LOITER); - set_new_altitude(g.rtl_approach_alt); + set_new_altitude(g.rtl_alt_final); } } diff --git a/ArduCopter/config.h b/ArduCopter/config.h index dcba3bb327..82fa494161 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -433,8 +433,8 @@ # define MAXIMUM_THROTTLE 1000 #endif -#ifndef AUTO_LAND_TIME - # define AUTO_LAND_TIME 5 +#ifndef RTL_LOITER_TIME + # define RTL_LOITER_TIME 5000 // Time (in milliseconds) to loiter above home before begining final descent #endif #ifndef LAND_SPEED @@ -584,20 +584,12 @@ // RTL Mode -#ifndef RTL_YAW - #if FRAME_CONFIG == HELI_FRAME - # define RTL_YAW YAW_LOOK_AT_HOME - #else - # define RTL_YAW YAW_HOLD - #endif -#endif - #ifndef RTL_RP # define RTL_RP ROLL_PITCH_AUTO #endif #ifndef RTL_THR - # define RTL_THR THROTTLE_HOLD + # define RTL_THR THROTTLE_AUTO #endif #ifndef SUPER_SIMPLE @@ -609,16 +601,16 @@ #endif // RTL Mode -#ifndef RTL_APPROACH_ALT - # define RTL_APPROACH_ALT 200 // cm!!! +#ifndef RTL_ALT_FINAL + # define RTL_ALT_FINAL 200 // the altitude the vehicle will move to as the final stage of Returning to Launch. Set to zero to land. #endif -#ifndef RTL_HOLD_ALT - # define RTL_HOLD_ALT 1500 // height to return to Home in CM, 0 = Maintain current altitude +#ifndef RTL_ALT + # define RTL_ALT 1500 // default alt to return to home in cm, 0 = Maintain current altitude #endif -#ifndef MAXIMUM_RTL_ALT - # define MAXIMUM_RTL_ALT 8000 // Max height to return to Home in CM +#ifndef RTL_ALT_MAX + # define RTL_ALT_MAX 8000 // Max height to return to home in cm #endif diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 6849dfd796..e444ae9294 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -220,12 +220,11 @@ #define NO_COMMAND 0 +// Navigation modes held in wp_control variable #define LOITER_MODE 1 #define WP_MODE 2 #define CIRCLE_MODE 3 #define NO_NAV_MODE 4 -#define TOY_MODE 5 // THOR This mode defines the Virtual - // WP following mode // TOY mixing options #define TOY_LOOKUP_TABLE 0 @@ -243,6 +242,12 @@ //#define WP_OPTION_ 64 #define WP_OPTION_NEXT_CMD 128 +// RTL state +#define RTL_STATE_RETURNING_HOME 0 +#define RTL_STATE_LOITERING_AT_HOME 1 +#define RTL_STATE_FINAL_DESCENT 2 +#define RTL_STATE_LAND 3 + //repeating events #define NO_REPEAT 0 #define CH_5_TOGGLE 1 diff --git a/ArduCopter/limits.pde b/ArduCopter/limits.pde index bba90570c6..8c2adc45a7 100644 --- a/ArduCopter/limits.pde +++ b/ArduCopter/limits.pde @@ -23,7 +23,7 @@ void set_recovery_home_alt() { return_altitude_cm_ahl = (uint32_t) (home.alt + (100 * (uint16_t) ((amax_meters_ahl - amin_meters_ahl) / 2))); } } else { - return_altitude_cm_ahl = (uint32_t) (home.alt + g.RTL_altitude*100); + return_altitude_cm_ahl = (uint32_t) (home.alt + g.rtl_altitude); } // final sanity check // if our return is less than 4 meters from ground, set it to 4m, to clear "people" height. diff --git a/ArduCopter/navigation.pde b/ArduCopter/navigation.pde index 4acaac44b9..20ef827ff7 100644 --- a/ArduCopter/navigation.pde +++ b/ArduCopter/navigation.pde @@ -201,25 +201,11 @@ static void run_navigation_contollers() break; case RTL: - // have we reached the desired Altitude? - if(alt_change_flag == REACHED_ALT || alt_change_flag == DESCENDING) { - // we are at or above the target alt - if(false == ap.rtl_reached_alt) { - set_rtl_reached_alt(true); - do_RTL(); - } - wp_control = WP_MODE; - // checks if we have made it to home - update_nav_RTL(); - } else{ - // we need to loiter until we are ready to come home - wp_control = LOITER_MODE; - } + // execute the RTL state machine + verify_RTL(); // calculates desired Yaw -#if FRAME_CONFIG == HELI_FRAME update_auto_yaw(); -#endif // calculates the desired Roll and Pitch update_nav_wp(); @@ -254,25 +240,6 @@ static void run_navigation_contollers() wp_control = LOITER_MODE; } - if(loiter_timer != 0) { - // If we have a safe approach alt set and we have been loitering for 20 seconds(default), begin approach - if((millis() - loiter_timer) > (uint32_t)g.auto_land_timeout.get()) { - // just to make sure we clear the timer - loiter_timer = 0; - if(g.rtl_approach_alt == 0) { - set_mode(LAND); - if(home_distance < 300) { - next_WP.lat = home.lat; - next_WP.lng = home.lng; - } - }else{ - if(g.rtl_approach_alt < current_loc.alt) { - set_new_altitude(g.rtl_approach_alt); - } - } - } - } - // calculates the desired Roll and Pitch update_nav_wp(); break; @@ -318,30 +285,6 @@ static void run_navigation_contollers() } } -static void update_nav_RTL() -{ - // Have we have reached Home? - if(wp_distance <= 200 || check_missed_wp()) { - // if loiter_timer value > 0, we are set to trigger auto_land or approach - set_mode(LOITER); - - // just in case we arrive and we aren't at the lower RTL alt yet. - set_new_altitude(get_RTL_alt()); - - // force loitering above home - next_WP.lat = home.lat; - next_WP.lng = home.lng; - - // If failsafe OR auto approach altitude is set - // we will go into automatic land, (g.rtl_approach_alt) is the lowest point - // -1 means disable feature - if(ap.failsafe || g.rtl_approach_alt >= 0) - loiter_timer = millis(); - else - loiter_timer = 0; - } -} - static bool check_missed_wp() { int32_t temp; diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index d6f0986277..af1bc6af88 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -410,14 +410,9 @@ static void set_mode(byte mode) motors.auto_armed(g.rc_3.control_in > 0); set_auto_armed(g.rc_3.control_in > 0); - // do not auto_land if we are leaving RTL - loiter_timer = 0; - // if we change modes, we must clear landed flag set_land_complete(false); - // have we achieved the proper altitude before RTL is enabled - set_rtl_reached_alt(false); // debug to Serial terminal //cliSerial->println(flight_mode_strings[control_mode]); @@ -528,12 +523,7 @@ static void set_mode(byte mode) case RTL: ap.manual_throttle = false; ap.manual_attitude = false; - yaw_mode = RTL_YAW; - roll_pitch_mode = RTL_RP; - set_throttle_mode(RTL_THR); - set_rtl_reached_alt(false); - set_next_WP(¤t_loc); - set_new_altitude(get_RTL_alt()); + do_RTL(); break; case OF_LOITER: diff --git a/ArduCopter/test.pde b/ArduCopter/test.pde index 1c633bb047..e63d736154 100644 --- a/ArduCopter/test.pde +++ b/ArduCopter/test.pde @@ -739,10 +739,10 @@ test_wp(uint8_t argc, const Menu::arg *argv) // save the alitude above home option cliSerial->printf_P(PSTR("Hold alt ")); - if(g.RTL_altitude < 0) { + if(g.rtl_altitude < 0) { cliSerial->printf_P(PSTR("\n")); }else{ - cliSerial->printf_P(PSTR("of %dm\n"), (int)g.RTL_altitude / 100); + cliSerial->printf_P(PSTR("of %dm\n"), (int)g.rtl_altitude / 100); } cliSerial->printf_P(PSTR("%d wp\n"), (int)g.command_total); @@ -1068,7 +1068,7 @@ test_logging(uint8_t argc, const Menu::arg *argv) * * } * - * g.RTL_altitude.set_and_save(300); + * g.rtl_altitude.set_and_save(300); * g.command_total.set_and_save(4); * g.waypoint_radius.set_and_save(3); *