diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp index b91f90ce0b..910965fb93 100644 --- a/ArduCopter/mode_auto.cpp +++ b/ArduCopter/mode_auto.cpp @@ -764,11 +764,9 @@ bool ModeAuto::verify_command(const AP_Mission::Mission_Command& cmd) void ModeAuto::takeoff_run() { if (motors->armed() || copter.ap.auto_armed) - { - int32_t alt_rev = 0; - if (copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_rev)) - { - if (g.zr_tk_req_alt > 0 && alt_rev >= g.zr_tk_req_alt + { + if (g.zr_tk_req_alt > 0 && copter.current_loc.alt >= g.zr_tk_req_alt + && copter.updownStatus < UpDown_RequestClimb) { copter.updownStatus = UpDown_RequestClimb; @@ -785,7 +783,8 @@ void ModeAuto::takeoff_run() } set_mode(Mode::Number::BRAKE, ModeReason::REQUEST_CMD); } - else if (g.zr_tk_req_alt > 0 && alt_rev < g.zr_tk_req_alt && alt_rev > 0 + else if (g.zr_tk_req_alt > 0 && copter.current_loc.alt < g.zr_tk_req_alt + && copter.current_loc.alt > 0 &&(AP_HAL::millis()-last_time_send_updownload>500)) { last_time_send_updownload = AP_HAL::millis(); @@ -795,8 +794,8 @@ void ModeAuto::takeoff_run() zr_flying_status_t.countdown = 255; gcs().update_zr_fly_status(&zr_flying_status_t); } - } } + auto_takeoff_run(); }