无人机上锁更难状态码，起飞降落倒计时分开

ArduCopter/Parameters.cpp

@@ -124,16 +124,33 @@ const AP_Param::Info Copter::var_info[] = {
     // @Bitmask: 0:Roll,1:Pitch,2:Yaw,3:AccelZ
     GSCALAR(gcs_pid_mask,           "GCS_PID_MASK",     0),
 
-    // @Param: TAKEOFF_ALT_REQ
+    // @Param: ZR_TK_REQ_ALT
     // @DisplayName: take off  Altitude
     // @Description:  
     // @Units: cm
-    // @Range: -1 5000
+    // @Range:0 5000
     // @Increment: 1
     // @User: Standard
-    GSCALAR(takeoff_alt_req,  "TAKEOFF_ALT_REQ", TAKEOFF_ALT_REQ),
+    GSCALAR(zr_tk_req_alt,  "ZR_TK_REQ_ALT", TAKEOFF_ALT_REQ),
+
+    // @Param: ZR_TK_DELAY
+    // @DisplayName: take off  delay at  ZR_TK_REQ_ALT
+    // @Description:  
+    // @Units: second
+    // @Range: 0 360
+    // @Increment: 1
+    // @User: Standard
+    GSCALAR(zr_tk_delay,  "ZR_TK_DELAY", ZR_TK_DELAY),
+
+    // @Param: ZR_RTL_DELAY
+    // @DisplayName: rtl  Altitude  when at final decent alt
+    // @Description:  
+    // @Units: second
+    // @Range: 0 360
+    // @Increment: 1
+    // @User: Standard
+    GSCALAR(zr_rtl_delay,  "ZR_RTL_DELAY", ZR_RTL_DELAY),
 
-   GSCALAR(updown_countdown,  "UPDOWN_COUNTDOWN", UPDOWN_COUNTDOWN),
 #if MODE_RTL_ENABLED == ENABLED
     // @Param: RTL_ALT
     // @DisplayName: RTL Altitude

ArduCopter/Parameters.h

@@ -386,8 +386,9 @@ public:
         k_param_land_lock_alt,
         k_param_land_lock_rpy,
 
-        k_param_takeoff_alt_req,
+        k_param_zr_tk_req_alt,
-        k_param_updown_countdown,
+        k_param_zr_tk_delay,
+        k_param_zr_rtl_delay,
         // the k_param_* space is 9-bits in size
         // 511: reserved
     };
@@ -496,8 +497,9 @@ public:
     AP_Float                acro_balance_pitch;
     AP_Int8                  acro_trainer;
     AP_Float                acro_rp_expo;
-    AP_Int16                takeoff_alt_req;
+    AP_Int16                 zr_tk_req_alt;
-    AP_Int16                updown_countdown;
+    AP_Int16                 zr_tk_delay;
+    AP_Int16                 zr_rtl_delay;
     // Note: keep initializers here in the same order as they are declared
     // above.
     Parameters()

ArduCopter/UserCode.cpp

@@ -82,8 +82,15 @@ void Copter::userhook_SuperSlowLoop()
 {
     // put your 1Hz code here
     static bool before_fly = true;
-    if(motors->armed())
+    if(motors->armed()){
         before_fly = false;
+    }else{
+        updownStatus =UpDown_TakeOffStart;
+        mavlink_zr_flying_status_t  zr_flying_status_t; 
+        zr_flying_status_t.updown_status = updownStatus;
+        //TODO UPDATE COUNTDOWN
+        gcs().update_zr_fly_status(&zr_flying_status_t);
+    }
     if(before_fly){
         uint8_t cnt,cacl_volt_pst;
         float delt_volt;

ArduCopter/config.h

@@ -537,7 +537,15 @@
 #endif
 
 #ifndef UPDOWN_COUNTDOWN
-# define UPDOWN_COUNTDOWN           20       // the altitude the vehicle will move to as the final stage of Returning to Launch.  Set to zero to land.
+#define UPDOWN_COUNTDOWN           20       // the altitude the vehicle will move to as the final stage of Returning to Launch.  Set to zero to land.
+#endif
+
+#ifndef ZR_TK_DELAY
+#define ZR_TK_DELAY                               5
+#endif
+
+#ifndef  ZR_RTL_DELAY
+#define ZR_RTL_DELAY                          15
 #endif
 
 // RTL Mode

ArduCopter/mode.h

@@ -133,7 +133,8 @@ protected:
     float &G_Dt;
     int16_t countdown ;
     uint32_t last_takeoff_request_time;
-    bool is_countdown_enable = true;
+    bool is_takeoff_delay_enable = true;
+    bool is_rtl_delay_enable = true;
     // note that we support two entirely different automatic takeoffs:
 
     // "user-takeoff", which is available in modes such as ALT_HOLD

ArduCopter/mode_auto.cpp

@@ -768,24 +768,24 @@ void ModeAuto::takeoff_run()
         int32_t alt_rev = 0;
         if (copter.current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_rev))
         {
-            if (g.takeoff_alt_req > 0 && alt_rev >= g.takeoff_alt_req 
+            if (g.zr_tk_req_alt > 0 && alt_rev >= g.zr_tk_req_alt 
                         && copter.updownStatus < UpDown_RequestClimb)
             {
                 copter.updownStatus = UpDown_RequestClimb;  
-                countdown = g.updown_countdown;               
+                countdown = g.zr_tk_delay;               
                 mavlink_zr_flying_status_t  zr_flying_status_t;
                 zr_flying_status_t.updown_status = copter.updownStatus;
                 zr_flying_status_t.countdown = countdown;
                 gcs().update_zr_fly_status(&zr_flying_status_t);          
                 last_takeoff_request_time = AP_HAL::millis();             
-                if(g.updown_countdown>0){
+                if(g.zr_tk_delay>0){
-                    is_countdown_enable =true;
+                    is_takeoff_delay_enable =true;
                 }else{
-                      is_countdown_enable =false;
+                      is_takeoff_delay_enable =false;
                 }
              set_mode(Mode::Number::BRAKE, ModeReason::REQUEST_CMD);
             }            
-            else if (g.takeoff_alt_req > 0 && alt_rev < g.takeoff_alt_req && alt_rev > 0 
+            else if (g.zr_tk_req_alt > 0 && alt_rev < g.zr_tk_req_alt && alt_rev > 0 
                     &&(AP_HAL::millis()-last_time_send_updownload>500))
             {
                 last_time_send_updownload = AP_HAL::millis();

ArduCopter/mode_brake.cpp

@@ -24,13 +24,13 @@ bool ModeBrake::init(bool ignore_checks)
 
     _timeout_ms = 0;
   if(copter.updownStatus == UpDown_RequestClimb){
-      countdown = g.updown_countdown;
+      countdown = g.zr_tk_delay;
   }
    
-   if(g.updown_countdown>0){
+   if(g.zr_tk_delay>0){
-       is_countdown_enable =true;
+       is_takeoff_delay_enable =true;
    }else{     
-       is_countdown_enable =false;
+       is_takeoff_delay_enable =false;
    }
     return true;
 }
@@ -76,12 +76,12 @@ void ModeBrake::run()
         }
     }
 
-   if(copter.updownStatus == UpDown_RequestClimb&&is_countdown_enable)
+   if(copter.updownStatus == UpDown_RequestClimb&&is_takeoff_delay_enable)
             {         
                 if((AP_HAL::millis()-last_takeoff_request_time)>1000)
                 {
                     last_takeoff_request_time =  AP_HAL::millis();    
-                     countdown --;
+                     
                     // mavlink_zr_flying_status_t  zr_flying_status_t;
                     // zr_flying_status_t.updown_status = copter.updownStatus;
                     // zr_flying_status_t.countdown = countdown;  
@@ -94,6 +94,7 @@ void ModeBrake::run()
                        copter.updownStatus = UpDown_ContinueClimb;                    
                        set_mode(Mode::Number::AUTO, ModeReason::REQUEST_CMD);
                     }
+                    countdown --;  //TODO  Actually 1   second longer
                 }
             }
 

ArduCopter/mode_rtl.cpp

@@ -72,21 +72,20 @@ void ModeRTL::run(bool disarm_on_land)
                 copter.updownStatus = UpDown_RequestDescent;
                 mavlink_zr_flying_status_t  zr_flying_status_t;
                 zr_flying_status_t.updown_status = copter.updownStatus;
-                countdown = g.updown_countdown;  
+                countdown = g.zr_rtl_delay;  
                 zr_flying_status_t.countdown = countdown;  
                 gcs().update_zr_fly_status(&zr_flying_status_t);
                 last_decent_time =  AP_HAL::millis();       
-                if(g.updown_countdown>0){
+                if(g.zr_rtl_delay>0){
-                    is_countdown_enable = true;
+                    is_rtl_delay_enable = true;
                 }else{
-                        is_countdown_enable = false;
+                    is_rtl_delay_enable = false;
                 }         
             }
-            else if(copter.updownStatus ==UpDown_RequestDescent&&is_countdown_enable)
+            else if(copter.updownStatus ==UpDown_RequestDescent&&is_rtl_delay_enable)
             {
                  if((AP_HAL::millis()- last_decent_time)>=1000){
                     last_decent_time =  AP_HAL::millis();                     
-                    countdown --;
                     // mavlink_zr_flying_status_t  zr_flying_status_t;
                     // zr_flying_status_t.updown_status = copter.updownStatus;
                     // zr_flying_status_t.countdown = countdown;  
@@ -99,6 +98,7 @@ void ModeRTL::run(bool disarm_on_land)
                         copter.updownStatus = UpDown_ContinueDescent;
                         _state = RTL_Land;
                     }
+                       countdown --; //TODO  shiji countdown+1 s
                  }
             }
          else  if (copter.updownStatus == UpDown_ContinueDescent)