修改起飞悬停高度来源

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 (g.zr_tk_req_alt > 0 && copter.current_loc.alt >= g.zr_tk_req_alt 
-        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 
                         && 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();
 }