AC_ZR_App: 增加电池初始电量计算

zr_app: 增加开关锁定信号，电量计算调用

ArduCopter/zr_app.cpp

@@ -3,6 +3,21 @@
 
 void Copter::zr_app_1hz()
 {
+    static bool before_fly = true;
+    if(motors->armed()){
+         if(before_fly){
+            before_fly = false;
+        }
+        relay.on(3);    // 电子开关，保持通路
+        // camera.set_in_arm_mode(true);
+    }else{
+        relay.off(3);
+        if(before_fly){
+            uint8_t bat_cnt = zr_app.get_battery_capacity(1,battery.voltage());
+            battery.reset_remaining(0,(float)bat_cnt );
+            // gcs().send_text(MAV_SEVERITY_INFO, "bat:%.2f,pst%d,%.2f",battery.voltage(),bat_cnt,(float)bat_cnt);
+        }
+    }
 
 }
 

bd-pix4.sh

@@ -0,0 +1,3 @@
+./waf configure --board Pixhawk4
+./waf copter
+cp ./build/Pixhawk4/bin/arducopter.apj  /mnt/f/_01-work/100=data/固件/编译/pixhakw4-master.px4

cubeblack.sh

@@ -0,0 +1,3 @@
+./waf configure --board cubeblack
+./waf copter
+cp ./build/CubeBlack/bin/arducopter.apj /mnt/e/_00-inbox/_temp/cubeblack.px4

libraries/AC_ZR_APP/AC_ZR_App.cpp

@@ -180,4 +180,76 @@ uint16_t AC_ZR_App::get_land_deceleration(int32_t alt_cm,int16_t sys_speed_dn)
     }
 
     return abs(speed_dn_now);
+}
+
+
+
+uint8_t AC_ZR_App::BinarySearch2f(float a[], float value, int low, int high)
+{
+    static uint8_t cacl_volt_pst = 0;
+    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);
+}
+
+
+uint8_t AC_ZR_App::get_battery_capacity(uint8_t type,float volt)
+{
+    uint8_t bat_cnt = 100;
+    // float cycle_pst = (1 - bat_cycled * 0.0002);
+    switch (type)
+    {
+        case 0:
+            /* code */
+            break;
+        case 1:
+        
+            static float batt_mah_teb[] =
+            {
+                16.80, 17.40, 18.00, 18.51, 18.80, 19.00, 19.19, 19.34, 19.53, 19.73,
+                19.88, 20.04, 20.16, 20.29, 20.47, 20.56, 20.63, 20.69, 20.75, 20.80,
+                20.85, 20.90, 20.96, 21.02, 21.07, 21.13, 21.18, 21.24, 21.29, 21.34,
+                21.40, 21.45, 21.49, 21.54, 21.59, 21.64, 21.69, 21.72, 21.76, 21.79,
+                21.84, 21.88, 21.92, 21.96, 22.01, 22.06, 22.11, 22.16, 22.21, 22.26,
+                22.31, 22.37, 22.43, 22.48, 22.54, 22.59, 22.65, 22.70, 22.76, 22.81,
+                22.86, 22.91, 22.95, 23.01, 23.07, 23.13, 23.19, 23.24, 23.30, 23.36,
+                23.41, 23.46, 23.51, 23.58, 23.65, 23.71, 23.79, 23.88, 23.94, 24.00,
+                24.07, 24.14, 24.21, 24.26, 24.32, 24.36, 24.40, 24.43, 24.45, 24.48,
+                24.50, 24.52, 24.54, 24.57, 24.60, 24.64, 24.69, 24.76, 24.84, 24.90,
+                25.20
+            };
+            bat_cnt =  BinarySearch2f(batt_mah_teb,volt,0,100);
+            // gcs().send_text(MAV_SEVERITY_INFO,"cnt:%d,mid:%d",bat_cnt,cacl_volt_pst);
+            break;
+        case 2:
+            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
+            };
+            bat_cnt = BinarySearch2f(batt_mah_teb_7s,volt,0,100);
+            // gcs().send_text(MAV_SEVERITY_INFO,"cnt:%d,mid:%d",bat_cnt,cacl_volt_pst);
+            break;
+        
+        default:
+            break;
+    }   
+    return bat_cnt;
 }

libraries/AC_ZR_APP/AC_ZR_App.h

@@ -42,6 +42,7 @@ public:
     uint32_t get_zr_sysid();
 
     uint16_t get_land_deceleration(int32_t alt_cm,int16_t sys_speed_dn);
+    uint8_t get_battery_capacity(uint8_t type,float volt);
 
     static const struct AP_Param::GroupInfo var_info[];
 
@@ -64,5 +65,6 @@ protected:
 
 private:
     static AC_ZR_App *_singleton;
+    uint8_t BinarySearch2f(float a[], float value, int low, int high);
 
 };

modules/DroneCAN/dronecan_dsdlc

@@ -0,0 +1 @@
+Subproject commit c00fc457e22c0b71e3a385f092b519830ef58ea2

modules/DroneCAN/libcanard

@@ -0,0 +1 @@
+Subproject commit 8e66f733981c8acb5bed67d57b0079238e611cfa

modules/DroneCAN/pydronecan

@@ -0,0 +1 @@
+Subproject commit 8fb1b18ab3250695d85fe2943dfc68e8b45024b7

periph_copy.sh

@@ -0,0 +1,3 @@
+./waf AP_Periph
+cp ./build/f103-GPS/bin/AP_Periph.hex /mnt/e/_00-inbox/periph-1.3.1.hex
+cp ./build/f103-GPS/bin/AP_Periph_with_bl.hex /mnt/e/_00-inbox/periph-1.3_with_bl.hex