diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 321b8b9b54..5a16ada4db 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -25,8 +25,6 @@ Mode::Mode(void) : G_Dt(copter.G_Dt) { }; -float Mode::auto_takeoff_no_nav_alt_cm = 0; - // return the static controller object corresponding to supplied mode Mode *Copter::mode_from_mode_num(const Mode::Number mode) { diff --git a/ArduCopter/mode_guided.cpp b/ArduCopter/mode_guided.cpp index 1b5fc8a0c7..25313eb7dd 100644 --- a/ArduCopter/mode_guided.cpp +++ b/ArduCopter/mode_guided.cpp @@ -375,42 +375,6 @@ void ModeGuided::takeoff_run() } } -void Mode::auto_takeoff_run() -{ - // if not armed set throttle to zero and exit immediately - if (!motors->armed() || !copter.ap.auto_armed) { - make_safe_spool_down(); - wp_nav->shift_wp_origin_to_current_pos(); - return; - } - - // process pilot's yaw input - float target_yaw_rate = 0; - if (!copter.failsafe.radio) { - // get pilot's desired yaw rate - target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); - } - - // aircraft stays in landed state until rotor speed runup has finished - if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) { - set_land_complete(false); - } else { - wp_nav->shift_wp_origin_to_current_pos(); - } - - // set motors to full range - motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); - - // run waypoint controller - copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); - - // call z-axis position controller (wpnav should have already updated it's alt target) - copter.pos_control->update_z_controller(); - - // call attitude controller - auto_takeoff_attitude_run(target_yaw_rate); -} - // guided_pos_control_run - runs the guided position controller // called from guided_run void ModeGuided::pos_control_run() diff --git a/ArduCopter/takeoff.cpp b/ArduCopter/takeoff.cpp index 69c5994378..700fece85a 100644 --- a/ArduCopter/takeoff.cpp +++ b/ArduCopter/takeoff.cpp @@ -2,6 +2,8 @@ Mode::_TakeOff Mode::takeoff; +float Mode::auto_takeoff_no_nav_alt_cm = 0; + // This file contains the high-level takeoff logic for Loiter, PosHold, AltHold, Sport modes. // The take-off can be initiated from a GCS NAV_TAKEOFF command which includes a takeoff altitude // A safe takeoff speed is calculated and used to calculate a time_ms @@ -138,6 +140,42 @@ void Mode::_TakeOff::get_climb_rates(float& pilot_climb_rate, } } +void Mode::auto_takeoff_run() +{ + // if not armed set throttle to zero and exit immediately + if (!motors->armed() || !copter.ap.auto_armed) { + make_safe_spool_down(); + wp_nav->shift_wp_origin_to_current_pos(); + return; + } + + // process pilot's yaw input + float target_yaw_rate = 0; + if (!copter.failsafe.radio) { + // get pilot's desired yaw rate + target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); + } + + // aircraft stays in landed state until rotor speed runup has finished + if (motors->get_spool_state() == AP_Motors::SpoolState::THROTTLE_UNLIMITED) { + set_land_complete(false); + } else { + wp_nav->shift_wp_origin_to_current_pos(); + } + + // set motors to full range + motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED); + + // run waypoint controller + copter.failsafe_terrain_set_status(wp_nav->update_wpnav()); + + // call z-axis position controller (wpnav should have already updated it's alt target) + copter.pos_control->update_z_controller(); + + // call attitude controller + auto_takeoff_attitude_run(target_yaw_rate); +} + void Mode::auto_takeoff_set_start_alt(void) { // start with our current altitude