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

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;
         }
-
+        if (i == 0)
-        const struct log_RFND pkt = {
+        {
+            const struct log_RFND pkt = {
                 LOG_PACKET_HEADER_INIT(LOG_RFND_MSG),
-                time_us      : AP_HAL::micros64(),
+                time_us : AP_HAL::micros64(),
-                instance     : i,
+                instance : i,
-                dist         : s->distance_cm(),
+                dist : s->distance_cm(),
-                status       : (uint8_t)s->status(),
+                status : (uint8_t)s->status(),
-                orient       : s->orientation(),
+                orient : s->orientation(),
-        };
+            };
-        AP::logger().WriteBlock(&pkt, sizeof(pkt));
+            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));
+        }
     }
 }