From b47d4acd3106469db2cfddde27992185c01f92db Mon Sep 17 00:00:00 2001 From: yaozb Date: Sat, 8 Aug 2020 16:19:07 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E8=B5=B7=E9=A3=9E=E6=82=AC?= =?UTF-8?q?=E5=81=9C=E9=AB=98=E5=BA=A6=E6=9D=A5=E6=BA=90?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- ArduCopter/mode_auto.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) 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(); }