排序电量测试OK

ArduCopter/zr_flight.cpp

@@ -59,7 +59,7 @@ uint8_t Copter::BinarySearch2f(float a[], float value, int low, int high)
         return cacl_volt_pst;
     cacl_volt_pst = (low + high) / 2;
 
-    if (a[cacl_volt_pst] == value)
+    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)
@@ -115,37 +115,27 @@ void Copter::zr_SuperSlowLoop(){
     }
     if(before_fly){
         cacl_volt_pst = 0;
+        uint8_t bat_cnt;
         float now_volt = battery.voltage();
-        switch (battery.get_batt_type(1))
+        switch (battery.get_batt_type(0))
         {
             case 0:
                 /* code */
                 break;
             case 1:
-                // for(bat_pst = 0; bat_pst<102; bat_pst++ ){
-                //     delt_volt = batt_mah_teb[bat_pst] - battery.voltage();
-                //     // delt_volt = batt_mah_teb[bat_pst] - test_volt;
-                //     if(delt_volt <= 0)
-                //         break;
-                // }
-                battery.reset_remaining(1, BinarySearch2f(batt_mah_teb,now_volt,0,100));
+                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:
-                // for(bat_pst = 0; bat_pst<102; bat_pst++ ){
-                //     delt_volt = batt_mah_teb_7s[bat_pst] - battery.voltage();
-                //     // delt_volt = batt_mah_teb[bat_pst] - test_volt;
-                //     if(delt_volt <= 0)
-                //         break;
-                // }
-                battery.reset_remaining(1, BinarySearch2f(batt_mah_teb_7s,now_volt,0,100));
+                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 - bat_pst;
-        // cacl_volt_pst = bat_pst;
-        // battery.reset_remaining(1, cacl_volt_pst);
     }
 
     gcs().send_message(MSG_ZR_FLYING_STATUS);