测流修改，增加7s，CMD接收

ArduCopter/Copter.h

@@ -1039,6 +1039,10 @@ public:
     void zr_SlowLoop();
     bool flow_start_sw;
     bool flow_index_sw;
+
+    uint8_t BinarySearch2f(float a[], float value, int low, int high);
+
+    uint8_t cacl_volt_pst;
 };
 
 extern Copter copter;

ArduCopter/Parameters.cpp

@@ -1667,16 +1667,16 @@ const char* Copter::get_sysid_board_id(void)
     switch (type)
     {
         case 0:
-            snprintf(buf, sizeof(buf),  "Version: zr-v4.1.0 ,ID: RS100%04d%05d",(int)nameValue1,(int)nameValue2);
+            snprintf(buf, sizeof(buf),  "Version: zr-v4.1.1 ,ID: RS100%04d%05d",(int)nameValue1,(int)nameValue2);
             break;
         case 1:
-            snprintf(buf, sizeof(buf),  "Version: zr-v4.1.0 ,ID: M660%04d%05d",(int)nameValue1,(int)nameValue2);
+            snprintf(buf, sizeof(buf),  "Version: zr-v4.1.1 ,ID: M660%04d%05d",(int)nameValue1,(int)nameValue2);
             break;
         case 2:
-            snprintf(buf, sizeof(buf),  "Version: zr-v4.1.0 ,ID: RF610%04d%05d",(int)nameValue1,(int)nameValue2);
+            snprintf(buf, sizeof(buf),  "Version: zr-v4.1.1 ,ID: RF610%04d%05d",(int)nameValue1,(int)nameValue2);
             break;
         case 3:
-            snprintf(buf, sizeof(buf),  "Version: zr-v4.1.0 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2);
+            snprintf(buf, sizeof(buf),  "Version: zr-v4.1.1 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2);
             break;
         
         default:

ArduCopter/version.h

@@ -6,12 +6,12 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "ZRUAV v4.1.0"  //"ArduCopter V4.0.0"
+#define THISFIRMWARE "ZRUAV v4.1.1"  //"ArduCopter V4.0.0"
 
 // the following line is parsed by the autotest scripts
-#define FIRMWARE_VERSION 4,1,0,FIRMWARE_VERSION_TYPE_OFFICIAL
+#define FIRMWARE_VERSION 4,1,1,FIRMWARE_VERSION_TYPE_OFFICIAL
 
 #define FW_MAJOR 4
 #define FW_MINOR 1
-#define FW_PATCH 0
+#define FW_PATCH 1
 #define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL

ArduCopter/zr_flight.cpp

@@ -52,54 +52,92 @@ void Copter::zr_SlowLoop(){
 
 }
 
+uint8_t Copter::BinarySearch2f(float a[], float value, int low, int high)
+{
+
+    if (low > high)
+        return cacl_volt_pst;
+    cacl_volt_pst = (low + high) / 2;
+
+    if (fabs(a[cacl_volt_pst] - value)<0.01)
+        //if (abs(a[mid] - value) < 30)
+        return cacl_volt_pst;
+    else if (a[cacl_volt_pst] > value)
+        return BinarySearch2f(a, value, low, cacl_volt_pst - 1);
+    else
+        return BinarySearch2f(a, value, cacl_volt_pst + 1, high);
+}
+
 void Copter::zr_SuperSlowLoop(){
     static bool before_fly = true;
     static float batt_mah_teb[] =
     {
-        25.20,24.90,24.84,24.76,24.69,24.64,24.60,24.57,24.54,24.52,
+        16.80, 17.40, 18.00, 18.51, 18.80, 19.00, 19.19, 19.34, 19.53, 19.73,
-        24.50,24.48,24.45,24.43,24.40,24.36,24.32,24.26,24.21,24.14,
+        19.88, 20.04, 20.16, 20.29, 20.47, 20.56, 20.63, 20.69, 20.75, 20.80,
-        24.07,24.00,23.94,23.88,23.79,23.71,23.65,23.58,23.51,23.46,
+        20.85, 20.90, 20.96, 21.02, 21.07, 21.13, 21.18, 21.24, 21.29, 21.34,
-        23.41,23.36,23.30,23.24,23.19,23.13,23.07,23.01,22.95,22.91,
+        21.40, 21.45, 21.49, 21.54, 21.59, 21.64, 21.69, 21.72, 21.76, 21.79,
-        22.86,22.81,22.76,22.70,22.65,22.59,22.54,22.48,22.43,22.37,
+        21.84, 21.88, 21.92, 21.96, 22.01, 22.06, 22.11, 22.16, 22.21, 22.26,
-        22.31,22.26,22.21,22.16,22.11,22.06,22.01,21.96,21.92,21.88,
+        22.31, 22.37, 22.43, 22.48, 22.54, 22.59, 22.65, 22.70, 22.76, 22.81,
-        21.84,21.79,21.76,21.72,21.69,21.64,21.59,21.54,21.49,21.45,
+        22.86, 22.91, 22.95, 23.01, 23.07, 23.13, 23.19, 23.24, 23.30, 23.36,
-        21.40,21.34,21.29,21.24,21.18,21.13,21.07,21.02,20.96,20.90,
+        23.41, 23.46, 23.51, 23.58, 23.65, 23.71, 23.79, 23.88, 23.94, 24.00,
-        20.85,20.80,20.75,20.69,20.63,20.56,20.47,20.29,20.16,20.04,
+        24.07, 24.14, 24.21, 24.26, 24.32, 24.36, 24.40, 24.43, 24.45, 24.48,
-        19.88,19.73,19.53,19.34,19.19,19.00,18.80,18.51,18.00,17.40,
+        24.50, 24.52, 24.54, 24.57, 24.60, 24.64, 24.69, 24.76, 24.84, 24.90,
-        16.80
+        25.20
+    };
+    static float batt_mah_teb_7s[] =
+    {
+        21.60, 21.80, 22.04, 22.28, 22.52, 22.74, 22.95, 23.19, 23.32, 23.52,
+        23.65, 23.81, 23.94, 24.02, 24.10, 24.18, 24.22, 24.25, 24.30, 24.39,
+        24.46, 24.52, 24.60, 24.67, 24.74, 24.80, 24.86, 24.93, 24.97, 25.03,
+        25.08, 25.13, 25.18, 25.23, 25.27, 25.32, 25.36, 25.41, 25.45, 25.50,
+        25.54, 25.58, 25.63, 25.69, 25.74, 25.79, 25.85, 25.91, 25.98, 26.05,
+        26.10, 26.16, 26.21, 26.28, 26.35, 26.40, 26.47, 26.54, 26.61, 26.67,
+        26.74, 26.79, 26.85, 26.93, 27.00, 27.07, 27.13, 27.19, 27.26, 27.32,
+        27.38, 27.46, 27.52, 27.59, 27.66, 27.75, 27.80, 27.87, 27.94, 28.03,
+        28.10, 28.16, 28.21, 28.28, 28.33, 28.38, 28.43, 28.46, 28.50, 28.53,
+        28.56, 28.59, 28.61, 28.64, 28.67, 28.71, 28.77, 28.86, 28.97, 29.13,
+        29.30
     };
     
     if(motors->armed()){
-        before_fly = false;
+         if(before_fly){
+            before_fly = false;
+            gcs().send_text(MAV_SEVERITY_INFO,"%s",get_sysid_board_id());
+        }
         relay.on(3);
+        gcs().send_message(MSG_ZR_FLYING_STATUS);
+        // camera.set_in_arm_mode(true);
 
     }else{
         relay.off(3);
+        // camera.set_in_arm_mode(false);
         // rc().set_enable_ch();
-        updownStatus =UpDown_TakeOffStart;
+        updownStatus =UpDown_TakeOffStart;        
-        
     }
     if(before_fly){
-        uint8_t cnt,cacl_volt_pst;
+        cacl_volt_pst = 0;
-        float delt_volt;
+        uint8_t bat_cnt;
-        for(cnt = 0; cnt<102; cnt++ ){
+        float now_volt = battery.voltage();
-            delt_volt = batt_mah_teb[cnt] - battery.voltage();
+        switch (battery.get_batt_type(0))
-            // delt_volt = batt_mah_teb[cnt] - test_volt;
+        {
-            if(delt_volt <= 0)
+            case 0:
+                /* code */
+                break;
+            case 1:
+                bat_cnt =  BinarySearch2f(batt_mah_teb,now_volt,0,100);
+                battery.reset_remaining(1,bat_cnt);
+                // gcs().send_text(MAV_SEVERITY_INFO,"cnt:%d,mid:%d",bat_cnt,cacl_volt_pst);
+                break;
+            case 2:
+                bat_cnt = BinarySearch2f(batt_mah_teb_7s,now_volt,0,100);
+                battery.reset_remaining(1,bat_cnt);
+                // gcs().send_text(MAV_SEVERITY_INFO,"cnt:%d,mid:%d",bat_cnt,cacl_volt_pst);
+                break;
+            
+            default:
                 break;
         }
-        cacl_volt_pst = 100 - cnt;
-        battery.reset_remaining(1, cacl_volt_pst);
     }
 
-    // gcs().send_message(MSG_ZR_FLYING_STATUS);
-
-    // // RC6 测流开关，RC9 计数开关
-    // uint16_t flow_start_sw = RC_Channels::rc_channel(CH_6)->get_radio_in();
-    // uint16_t flow_index_sw = RC_Channels::rc_channel(CH_9)->get_radio_in();
-    // uint8_t measure_time = wp_nav->get_measure_time();
-    // int32_t real_alt = gps.location().alt;  
-    // flow_measure.flow_auto_measure(flow_start_sw,flow_index_sw, real_alt, measure_time);
-
 }
 #endif

copy.sh

@@ -1,2 +0,0 @@
-sudo cp ./build/Pixhawk4/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/zr-flow-v2-test.px4
-

just_build.sh

@@ -1,2 +1,4 @@
 ./waf configure --board Pixhawk4
 ./waf  copter
+cp ./build/Pixhawk4/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/zr-flow-v2-rc3.px4
+

libraries/AC_Flow_Measure/AC_Flow_Measure.cpp

@@ -223,6 +223,7 @@ bool Flow_Measure::parse(uint8_t temp)
                 flow_value.c[2] = data_to_mp.radar_value[23];
                 flow_value.c[3] = data_to_mp.radar_value[24];
                 data_to_mp.flow_velocity = (int32_t)(flow_value.f*100);
+
                 if(is_data_flag)
                     data_to_mp.cmd = 2; // ready to send
                 else
@@ -263,12 +264,12 @@ void Flow_Measure::mav_send_flow_measure(mavlink_channel_t chan)
  * 
  * @param msg 
  */
-void Flow_Measure::mav_handle_flow_measure_data(mavlink_message_t *msg)
+void Flow_Measure::mav_handle_flow_measure_data(const mavlink_message_t &msg)
 {
         static uint8_t last_cmd = 3;
         // decode message
         mavlink_flow_measure_t packet;
-        mavlink_msg_flow_measure_decode(msg, &packet);
+        mavlink_msg_flow_measure_decode(&msg, &packet);
         // memset(data_to_mp.raw_array,0,22);
         // memcpy(data_to_mp.raw_array, packet.from_mp_array, 22);
         data_to_mp.cmd = packet.cmd;
@@ -427,7 +428,7 @@ void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uin
                     send_cmd(START_MEASURE);
                     repeat = 0;
                     // gcs().send_text(MAV_SEVERITY_NOTICE, "[INFO]设置测流历时: %3ds",measure_time);
-                    gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]Send START_MEASURE command ");
+                    // gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]Send START_MEASURE command ");
                 }
                 // else if(index_sw > 1400)
                 // {
@@ -471,7 +472,7 @@ void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uin
                     set_is_data_flag(true);
                     send_cmd(GET_DATA);
                     repeat = 0;
-                    gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]Send GET_DATA command ");
+                    // gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]Send GET_DATA command ");
                 }
                 break;
         }
@@ -577,3 +578,16 @@ void Flow_Measure::update(void)
 #endif
 }
 
+
+
+// singleton instance
+Flow_Measure *Flow_Measure::_singleton;
+
+namespace AP {
+
+    Flow_Measure *flowmeasure()
+    {
+        return Flow_Measure::get_singleton();
+    }
+
+}

libraries/AC_Flow_Measure/AC_Flow_Measure.h

@@ -78,13 +78,16 @@ class Flow_Measure
 public:
     Flow_Measure();
 
+    // get singleton instance
+    static Flow_Measure *get_singleton() { return _singleton; }
+
 	bool init(const AP_SerialManager& serial_manager);
 
     bool read(void);
     void update(void);
 
     void mav_send_flow_measure(mavlink_channel_t chan);
-    void mav_handle_flow_measure_data(mavlink_message_t *msg);
+    void mav_handle_flow_measure_data(const mavlink_message_t &msg);
     void MBCRC16( uint8_t c);
 	uint16_t MBCRC16_u( uint8_t * pucFrame, uint16_t usLen );
 	void crc_reset(void);
@@ -113,6 +116,8 @@ public:
     void set_flow_date_flag(bool flag){get_flow_data = flag;};
 
 private:
+    static Flow_Measure *_singleton;
+
     bool parse(uint8_t temp);
     AP_HAL::UARTDriver  *_uart;
     uint8_t     ucCRCHi;
@@ -198,4 +203,9 @@ private:
     
 };
 
+namespace AP {
+    Flow_Measure *flowmeasure();
+};
+
+
 #endif /* __AC_FLOW_MEASURE_H__ */

libraries/AP_BattMonitor/AP_BattMonitor.cpp

@@ -499,6 +499,15 @@ void AP_BattMonitor::request_history(void)
     }
 }
 
+uint8_t AP_BattMonitor::get_batt_type(uint8_t i)
+{
+
+    if (drivers[i] != nullptr && _params[i].type() == AP_BattMonitor_Params::BattMonitor_TYPE_ANALOG_VOLTAGE_AND_CURRENT) {
+        return _params[i].batt_type();        
+    }
+    return 0;
+}
+
 
 /*
   reset battery remaining percentage for batteries that integrate to

libraries/AP_BattMonitor/AP_BattMonitor.h

@@ -166,6 +166,7 @@ public:
 
     void  request_info(void) ;        //battgo
     void  request_history(void) ;  //battgo
+    uint8_t  get_batt_type(uint8_t i);
 
 protected:
 

libraries/AP_BattMonitor/AP_BattMonitor_Params.cpp

@@ -171,6 +171,8 @@ const AP_Param::GroupInfo AP_BattMonitor_Params::var_info[] = {
     AP_GROUPINFO("DROPOUT_V", 20, AP_BattMonitor_Params, _arming_dropout_voltage, 0),
 
     AP_GROUPINFO("CELL_TOTAL", 21, AP_BattMonitor_Params, _batt_cells_amount, 0),
+  
+    AP_GROUPINFO("BAT_TYPE", 22, AP_BattMonitor_Params, _batt_type, 1),
     
     AP_GROUPEND
 

libraries/AP_BattMonitor/AP_BattMonitor_Params.h

@@ -36,6 +36,7 @@ public:
 
     BattMonitor_Type type(void) const { return (enum BattMonitor_Type)_type.get(); }
     BattMonitor_LowVoltage_Source failsafe_voltage_source(void) { return (enum BattMonitor_LowVoltage_Source)_failsafe_voltage_source.get(); }
+    int8_t batt_type( ) const { return _batt_type;} ;
 
     AP_Int8  _type;                     /// 0=disabled, 3=voltage only, 4=voltage and current
     AP_Int8  _volt_pin;                 /// board pin used to measure battery voltage
@@ -58,4 +59,5 @@ public:
     AP_Float _arming_minimum_voltage;   /// voltage level required to arm
    AP_Float _arming_dropout_voltage;   /// voltage level required to arm
    AP_Int8 _batt_cells_amount;   /// voltage level required to arm
+   AP_Int8 _batt_type;   /// voltage level required to arm
 };

libraries/GCS_MAVLink/GCS_Common.cpp

@@ -41,6 +41,9 @@
 #include<AP_BattMonitor/AP_BattMonitor_Serial_BattGo.h>
 #include <stdio.h>
 
+#include <AC_Flow_Measure/AC_Flow_Measure.h>
+
+
 #if HAL_RCINPUT_WITH_AP_RADIO
 #include <AP_Radio/AP_Radio.h>
 #include <AP_BoardConfig/AP_BoardConfig.h>
@@ -3154,6 +3157,16 @@ void GCS_MAVLINK::handle_common_message(const mavlink_message_t &msg)
         handle_param_value(msg);
         break;
 
+    case MAVLINK_MSG_ID_FLOW_MEASURE:  //@Brown , ID 19
+    {
+            Flow_Measure *flowmeasure = AP::flowmeasure();
+            if (flowmeasure == nullptr) {
+                return;
+            }
+        flowmeasure->mav_handle_flow_measure_data(msg);
+        break;
+    }
+
     case MAVLINK_MSG_ID_SERIAL_CONTROL:
         handle_serial_control(msg);
         break;