修改起飞悬停高度来源

ArduCopter/mode_auto.cpp

@@ -765,10 +765,8 @@ 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();
@@ -796,7 +795,7 @@ void ModeAuto::takeoff_run()
                 gcs().update_zr_fly_status(&zr_flying_status_t);
             }        
     }
-    }
+    
     auto_takeoff_run();
 }