Merge branch 'landtest' into zr-v4.0.4-dev

ArduCopter/Parameters.cpp

@@ -124,14 +124,32 @@ 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),
 
 #if MODE_RTL_ENABLED == ENABLED
     // @Param: RTL_ALT

ArduCopter/Parameters.h

@@ -386,7 +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_zr_tk_delay,
+        k_param_zr_rtl_delay,
         // the k_param_* space is 9-bits in size
         // 511: reserved
     };
@@ -493,11 +495,11 @@ public:
     AP_Float                acro_yaw_p;
     AP_Float                acro_balance_roll;
     AP_Float                acro_balance_pitch;
-    AP_Int8                 acro_trainer;
+    AP_Int8                  acro_trainer;
     AP_Float                acro_rp_expo;
-
+    AP_Int16                 zr_tk_req_alt;
-    AP_Int16        takeoff_alt_req;
+    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

@@ -536,6 +536,18 @@
  # define TAKEOFF_ALT_REQ           0       // the altitude the vehicle will move to as the final stage of Returning to Launch.  Set to zero to land.
 #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.
+#endif
+
+#ifndef ZR_TK_DELAY
+#define ZR_TK_DELAY                               5
+#endif
+
+#ifndef  ZR_RTL_DELAY
+#define ZR_RTL_DELAY                          15
+#endif
+
 // RTL Mode
 #ifndef RTL_ALT_FINAL
  # define RTL_ALT_FINAL             0       // the altitude the vehicle will move to as the final stage of Returning to Launch.  Set to zero to land.

ArduCopter/mode.cpp

@@ -824,7 +824,10 @@ void Copter::update_updownStatus2(uint8_t status)
     if (status == 3)
     {
         set_mode(Mode::Number::AUTO, ModeReason::REQUEST_CMD);
-        gcs().update_zr_fly_status(status);
+         mavlink_zr_flying_status_t  zr_flying_status_t;
+        zr_flying_status_t.updown_status = copter.updownStatus;
+        zr_flying_status_t.countdown = 255;
+        gcs().update_zr_fly_status(&zr_flying_status_t);
     }
 }
 

ArduCopter/mode.h

@@ -7,9 +7,7 @@ class ParametersG2;
 class GCS_Copter;
 
 class Mode {
-
 public:
-
     // Auto Pilot Modes enumeration
     enum class Number : uint8_t {
         STABILIZE =     0,  // manual airframe angle with manual throttle
@@ -37,7 +35,6 @@ public:
         SYSTEMID  =    25,  // System ID mode produces automated system identification signals in the controllers
         AUTOROTATE =   26,  // Autonomous autorotation
     };
-
     // constructor
     Mode(void);
 
@@ -134,7 +131,10 @@ protected:
     RC_Channel *&channel_throttle;
     RC_Channel *&channel_yaw;
     float &G_Dt;
-
+    int16_t countdown ;
+    uint32_t last_takeoff_request_time;
+    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
@@ -331,7 +331,6 @@ public:
 
     bool init(bool ignore_checks) override;
     void run() override;
-
     bool requires_GPS() const override { return true; }
     bool has_manual_throttle() const override { return false; }
     bool allows_arming(bool from_gcs) const override { return false; };
@@ -480,6 +479,7 @@ private:
     int32_t condition_value;  // used in condition commands (eg delay, change alt, etc.)
     uint32_t condition_start;
     uint32_t last_time_send_updownload;
+
     LandStateType land_state = LandStateType_FlyToLocation; // records state of land (flying to location, descending)
 
     struct {
@@ -1038,7 +1038,7 @@ protected:
     void land_run(bool disarm_on_land);
 
     void set_descent_target_alt(uint32_t alt) { rtl_path.descent_target.alt = alt; }
-
+    
 private:
 
     void climb_start();
@@ -1048,10 +1048,11 @@ private:
     void loiterathome_run();
     void build_path();
     void compute_return_target();
-
+     
     RTLState _state = RTL_InitialClimb;  // records state of rtl (initial climb, returning home, etc)
     bool _state_complete = false; // set to true if the current state is completed
 
+    uint32_t last_decent_time =0;
     struct {
         // NEU w/ Z element alt-above-ekf-origin unless use_terrain is true in which case Z element is alt-above-terrain
         Location origin_point;

ArduCopter/mode_auto.cpp

@@ -238,7 +238,10 @@ void ModeAuto::wp_start(const Location& dest_loc)
     if (copter.updownStatus < UpDown_InMission)
     {
         copter.updownStatus = UpDown_InMission;
-        gcs().update_zr_fly_status(copter.updownStatus);
+        mavlink_zr_flying_status_t  zr_flying_status_t;
+        zr_flying_status_t.updown_status = copter.updownStatus;
+        zr_flying_status_t.countdown = 255;
+        gcs().update_zr_fly_status(&zr_flying_status_t);
     }
 }
 
@@ -765,20 +768,33 @@ 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;
+                copter.updownStatus = UpDown_RequestClimb;  
-                set_mode(Mode::Number::BRAKE, ModeReason::REQUEST_CMD);
+                countdown = g.zr_tk_delay;               
-                gcs().update_zr_fly_status(copter.updownStatus);
+                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.zr_tk_delay>0){
+                    is_takeoff_delay_enable =true;
+                }else{
+                      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();
                 copter.updownStatus = UpDown_TakeOffClimb;     
-                 gcs().update_zr_fly_status(copter.updownStatus);
+                mavlink_zr_flying_status_t  zr_flying_status_t;
-            }
+                zr_flying_status_t.updown_status = copter.updownStatus;
+                zr_flying_status_t.countdown = 255;
+                gcs().update_zr_fly_status(&zr_flying_status_t);
+            }        
         }
     }
     auto_takeoff_run();

ArduCopter/mode_brake.cpp

@@ -23,7 +23,15 @@ bool ModeBrake::init(bool ignore_checks)
     }
 
     _timeout_ms = 0;
-
+  if(copter.updownStatus == UpDown_RequestClimb){
+      countdown = g.zr_tk_delay;
+  }
+   
+   if(g.zr_tk_delay>0){
+       is_takeoff_delay_enable =true;
+   }else{     
+       is_takeoff_delay_enable =false;
+   }
     return true;
 }
 
@@ -67,6 +75,29 @@ void ModeBrake::run()
             copter.set_mode(Mode::Number::ALT_HOLD, ModeReason::BRAKE_TIMEOUT);
         }
     }
+
+   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();    
+                     
+                    // 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);
+                    if(countdown>0){
+                         gcs().send_text(MAV_SEVERITY_INFO,"继续任务倒计时(秒)：%d",countdown);
+                    }               
+                    else  if(countdown<=0){
+                        gcs().send_text(MAV_SEVERITY_INFO," 执行继续任务");
+                       copter.updownStatus = UpDown_ContinueClimb;                    
+                       set_mode(Mode::Number::AUTO, ModeReason::REQUEST_CMD);
+                    }
+                    countdown --;  //TODO  Actually 1   second longer
+                }
+            }
+
 }
 
 void ModeBrake::timeout_to_loiter_ms(uint32_t timeout_ms)

ArduCopter/mode_rtl.cpp

@@ -70,10 +70,38 @@ void ModeRTL::run(bool disarm_on_land)
             if (copter.updownStatus < UpDown_RequestDescent)
             {
                 copter.updownStatus = UpDown_RequestDescent;
-                gcs().update_zr_fly_status(copter.updownStatus);              
+                mavlink_zr_flying_status_t  zr_flying_status_t;
+                zr_flying_status_t.updown_status = copter.updownStatus;
+                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.zr_rtl_delay>0){
+                    is_rtl_delay_enable = true;
+                }else{
+                    is_rtl_delay_enable = false;
+                }         
             }
-
+            else if(copter.updownStatus ==UpDown_RequestDescent&&is_rtl_delay_enable)
-            if (copter.updownStatus == UpDown_ContinueDescent)
+            {
+                 if((AP_HAL::millis()- last_decent_time)>=1000){
+                    last_decent_time =  AP_HAL::millis();                     
+                    // 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);
+                    if(countdown>0){
+                        gcs().send_text(MAV_SEVERITY_INFO,"着陆倒计时(秒): %d",countdown);
+                    }
+                    else if(countdown<=0){
+                       gcs().send_text(MAV_SEVERITY_INFO,"开始着陆");
+                        copter.updownStatus = UpDown_ContinueDescent;
+                        _state = RTL_Land;
+                    }
+                       countdown --; //TODO  shiji countdown+1 s
+                 }
+            }
+         else  if (copter.updownStatus == UpDown_ContinueDescent)
             {          
                 _state = RTL_Land;
             }
@@ -267,7 +295,10 @@ void ModeRTL::loiterathome_run()
 void ModeRTL::descent_start()
 {
     copter.updownStatus  = UpDown_RTLDescent;
-    gcs().update_zr_fly_status(copter.updownStatus);
+   mavlink_zr_flying_status_t  zr_flying_status_t;
+    zr_flying_status_t.updown_status = copter.updownStatus;
+    zr_flying_status_t.countdown = 255;
+    gcs().update_zr_fly_status(&zr_flying_status_t);
     _state = RTL_FinalDescent;
     _state_complete = false;
 
@@ -409,7 +440,10 @@ void ModeRTL::land_run(bool disarm_on_land)
     land_run_vertical_control();  
     
     if(AP_HAL::millis()-last_send_time>=500){
-       gcs().update_zr_fly_status(copter.updownStatus);
+         mavlink_zr_flying_status_t  zr_flying_status_t;
+        zr_flying_status_t.updown_status = copter.updownStatus;
+        zr_flying_status_t.countdown = 255;
+        gcs().update_zr_fly_status(&zr_flying_status_t);
     }
 }
 

libraries/GCS_MAVLink/GCS.h

@@ -219,7 +219,7 @@ public:
     void send_rpm() const;
     bool send_battgo_info(const mavlink_battgo_info_t *mavlink_battgo_info_t);
     bool send_battgo_history(const mavlink_battgo_history_t *mavlink_battgo_info_t);
-    bool send_zr_flying_status(const uint8_t flying_status);
+    bool send_zr_flying_status(const mavlink_zr_flying_status_t *zr_flying_status_t);
     //void send_serial_battgo(mavlink_battgo_info_t &battgo_info_t );
     bool locked() const;
 
@@ -838,7 +838,7 @@ public:
 
    void   update_serial_battgo_info(mavlink_battgo_info_t *battgo_info_t );
    void   update_serial_battgo_history(mavlink_battgo_history_t *battgo_history_t );
-   void   update_zr_fly_status(uint8_t flying_status );
+   void   update_zr_fly_status(mavlink_zr_flying_status_t *zr_flying_status_t );
     // minimum amount of time (in microseconds) that must remain in
     // the main scheduler loop before we are allowed to send any
     // mavlink messages.  We want to prioritise the main flight

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -1924,12 +1924,12 @@ void GCS::update_serial_battgo_history(mavlink_battgo_history_t *battgo_history_
     }          
 }
 
-void GCS::update_zr_fly_status(uint8_t flying_status)
+void GCS::update_zr_fly_status(mavlink_zr_flying_status_t *zr_flying_status_t )
 {
-    for (uint8_t i = 0; i < num_gcs(); i++)
+    for (uint8_t i=0; i<num_gcs(); i++) {
-    {
+    //  mavlink_msg_battgo_info_send_struct(chan,&battgo_info_t);
-        chan(i)->send_zr_flying_status(flying_status);
+    chan(i)->send_zr_flying_status(zr_flying_status_t);
-    }
+    }          
 }
 
 void GCS::send_mission_item_reached_message(uint16_t mission_index)
@@ -4836,9 +4836,9 @@ void GCS_MAVLINK::handle_zr_fly_status(const mavlink_message_t &msg)
     }
 }
 
-bool GCS_MAVLINK::send_zr_flying_status(const uint8_t flying_status)
+bool GCS_MAVLINK::send_zr_flying_status(const mavlink_zr_flying_status_t *zr_flying_status_t)
 {
-    mavlink_msg_zr_flying_status_send(chan, flying_status,0);
+    mavlink_msg_zr_flying_status_send_struct(chan, zr_flying_status_t);
     return true;
 }
 

modules/mavlink

@@ -1 +1 @@
-Subproject commit ca507c39ff56b88ce78a072a691e4b0c2a95f527
+Subproject commit 18cab6950986cd7b9b7a507ec35f3501d4a7e019