backup commit 0801

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_updown_countdown,
+        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
     };
@@ -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                updown_countdown;
+    AP_Int16                 zr_tk_req_alt;
+    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

@@ -84,10 +84,19 @@ void Copter::userhook_SuperSlowLoop()
     static bool before_fly = true;
     if(motors->armed()){
         before_fly = false;
+<<<<<<< HEAD
         relay.on(3);
 
     }else{
         relay.off(3);
+=======
+    }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);
+>>>>>>> b3fbb47935ac0226cb0daf620677df256ab603c4
     }
     if(before_fly){
         uint8_t cnt,cacl_volt_pst;

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){
-                    is_countdown_enable =true;
+                if(g.zr_tk_delay>0){
+                    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){
-       is_countdown_enable =true;
+   if(g.zr_tk_delay>0){
+       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){
-                    is_countdown_enable = true;
+                if(g.zr_rtl_delay>0){
+                    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 --;
+                    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;  
@@ -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)

libraries/AP_BattMonitor/AP_BattMonitor_Backend.cpp

@@ -17,7 +17,9 @@
 #include <AP_HAL/AP_HAL.h>
 #include "AP_BattMonitor.h"
 #include "AP_BattMonitor_Backend.h"
-
+#include <GCS_MAVLink/GCS.h>
+#include <GCS_MAVLink/GCS_MAVLink.h>
+#include <stdio.h>
 /*
   base class constructor.
   This incorporates initialisation as well.
@@ -167,6 +169,10 @@ bool AP_BattMonitor_Backend::arming_checks(char * buffer, size_t buflen) const
     bool fs_voltage_inversion = is_positive(_params._critical_voltage) &&
                                 is_positive(_params._low_voltage) &&
                                 (_params._low_voltage < _params._critical_voltage);
+   uint8_t cell_min_index;
+   uint8_t  cell_max_index;
+   uint16_t  dropout_voltage;
+    bool fs_cells_dropout_voltage = cells_dropout_voltage_checks(cell_min_index,cell_max_index,dropout_voltage);
 
     bool result = update_check(buflen, buffer, low_voltage,  "low voltage failsafe");
     result = result && update_check(buflen, buffer, low_capacity, "low capacity failsafe");
@@ -176,7 +182,9 @@ bool AP_BattMonitor_Backend::arming_checks(char * buffer, size_t buflen) const
     result = result && update_check(buflen, buffer, below_arming_capacity, "below minimum arming capacity");
     result = result && update_check(buflen, buffer, fs_capacity_inversion, "capacity failsafe critical > low");
     result = result && update_check(buflen, buffer, fs_voltage_inversion, "voltage failsafe critical > low");
-
+   // char bat_message[60];
+  //   snprintf(bat_message,60,"电池压差过大,%d号与%d号电芯，压差 %f V",cell_max_index,cell_min_index,dropout_voltage/1000.0);
+    result = result && update_check(buflen, buffer, fs_cells_dropout_voltage,"电池压差过大");
     return result;
 }
 
@@ -245,3 +253,35 @@ bool AP_BattMonitor_Backend::reset_remaining(float percentage)
 
     return true;
 }
+
+bool AP_BattMonitor_Backend::cells_dropout_voltage_checks(uint8_t &cell_min_index, uint8_t &cell_max_index, uint16_t &dropout_voltage) const
+{ 
+    if (has_cell_voltages()&&(_params._arming_dropout_voltage>0))
+    {
+        uint16_t cell_min = 65535;
+        uint16_t cell_max = 0;
+        for (int k = 0; k < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN; k++)
+        {           
+            if ((_state.cell_voltages.cells[k]!=65535) && _state.cell_voltages.cells[k] != 0)  //TODO  if valid cell valtage  =0
+            {
+                if (_state.cell_voltages.cells[k] < cell_min)
+                {
+                    cell_min = _state.cell_voltages.cells[k];
+                    cell_min_index = k + 1;
+                }
+
+                if (_state.cell_voltages.cells[k] > cell_max)
+                {
+                    cell_max = _state.cell_voltages.cells[k];
+                    cell_max_index = k + 1;
+                }
+            }
+        }
+        dropout_voltage = abs(cell_max - cell_min);
+        if ((dropout_voltage/1000.0) >= _params._arming_dropout_voltage) //0.1v
+        {
+            return true;
+        }
+    }
+    return false;
+}

libraries/AP_BattMonitor/AP_BattMonitor_Backend.h

@@ -61,6 +61,7 @@ public:
 
     // returns false if we fail arming checks, in which case the buffer will be populated with a failure message
     bool arming_checks(char * buffer, size_t buflen) const;
+    bool  cells_dropout_voltage_checks(uint8_t &cell_min_index, uint8_t &cell_max_index, uint16_t&  dropout_voltage)const;
 
     // reset remaining percentage to given value
     virtual bool reset_remaining(float percentage);

libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp

@@ -162,6 +162,14 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
     // @User: Advanced
     AP_GROUPINFO("ARM_MAH", 19, AP_BattMonitor_Params, _arming_minimum_capacity, 0),
 
+    // @Param: DROPOUT_V
+    // @DisplayName: Required dropout valtage
+    // @Description: if the  each cells  max   dropout valtage great than or equal to this value,cant`t arm the UAV, Set to 0 to disable this function.
+    // @Units: V
+    // @Increment: 0.01
+    // @User: Standard
+    AP_GROUPINFO("DROPOUT_V", 20, AP_BattMonitor_Params, _arming_dropout_voltage, 0),
+    
     AP_GROUPEND
 
 };

libraries/AP_BattMonitor/AP_BattMonitor_Params.h

@@ -56,5 +56,5 @@ public:
     AP_Int8  _failsafe_critical_action; /// action to preform on a critical battery failsafe
     AP_Int32 _arming_minimum_capacity;  /// capacity level required to arm
     AP_Float _arming_minimum_voltage;   /// voltage level required to arm
-
+   AP_Float _arming_dropout_voltage;   /// voltage level required to arm
 };

libraries/AP_Logger/LogStructure.h

@@ -1384,6 +1384,8 @@ struct PACKED log_Arm_Disarm {
       "MODE", "QMBB",         "TimeUS,Mode,ModeNum,Rsn", "s---", "F---" }, \
     { LOG_RFND_MSG, sizeof(log_RFND), \
       "RFND", "QBCBB", "TimeUS,Instance,Dist,Stat,Orient", "s#m--", "F-B--" }, \
+     { LOG_RFND2_MSG, sizeof(log_RFND), \
+      "RFN2", "QBCBB", "TimeUS,Instance,Dist,Stat,Orient", "s#m--", "F-B--" }, \
     { LOG_MAV_STATS, sizeof(log_MAV_Stats), \
       "DMS", "IIIIIBBBBBBBBB",         "TimeMS,N,Dp,RT,RS,Fa,Fmn,Fmx,Pa,Pmn,Pmx,Sa,Smn,Smx", "s-------------", "C-------------" }, \
     { LOG_BEACON_MSG, sizeof(log_Beacon), \
@@ -1772,7 +1774,7 @@ enum LogMessages : uint8_t {
     LOG_ARM_DISARM_MSG,
     LOG_OA_BENDYRULER_MSG,
     LOG_OA_DIJKSTRA_MSG,
-
+    LOG_RFND2_MSG,
     _LOG_LAST_MSG_
 };
 

libraries/AP_RangeFinder/RangeFinder.cpp

@@ -676,16 +676,30 @@ void RangeFinder::Log_RFND()
         if (s == nullptr) {
             continue;
         }
-
-        const struct log_RFND pkt = {
+        if (i == 0)
+        {
+            const struct log_RFND pkt = {
                 LOG_PACKET_HEADER_INIT(LOG_RFND_MSG),
-                time_us      : AP_HAL::micros64(),
-                instance     : i,
-                dist         : s->distance_cm(),
-                status       : (uint8_t)s->status(),
-                orient       : s->orientation(),
-        };
-        AP::logger().WriteBlock(&pkt, sizeof(pkt));
+                time_us : AP_HAL::micros64(),
+                instance : i,
+                dist : s->distance_cm(),
+                status : (uint8_t)s->status(),
+                orient : s->orientation(),
+            };
+            AP::logger().WriteBlock(&pkt, sizeof(pkt));
+        }
+        else
+        {
+            const struct log_RFND pkt2 = {
+                LOG_PACKET_HEADER_INIT(LOG_RFND2_MSG),
+                time_us : AP_HAL::micros64(),
+                instance : i,
+                dist : s->distance_cm(),
+                status : (uint8_t)s->status(),
+                orient : s->orientation(),
+            };
+            AP::logger().WriteBlock(&pkt2, sizeof(pkt2));
+        }
     }
 }