zrzk
/
zr-v4
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

电量增加排序算法

zr-sdk-4.1.16
zbr 4 years ago
parent
7ff5a5bbfa
commit
dec7dc5bb8
2 changed files with 60 additions and 39 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 3
      ArduCopter/Copter.h
  2. 96
      ArduCopter/zr_flight.cpp

3
ArduCopter/Copter.h
Unescape View File

@ -1020,7 +1020,10 @@ public: @@ -1020,7 +1020,10 @@ public:
void zr_SuperSlowLoop();
void zr_SlowLoop();
uint8_t BinarySearch2f(float a[], float value, int low, int high);
int8_t in_debug_mode;
uint8_t cacl_volt_pst;
};
extern Copter copter;

96
ArduCopter/zr_flight.cpp
Unescape View File

@ -52,35 +52,51 @@ void Copter::zr_SlowLoop(){ @@ -52,35 +52,51 @@ 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 (a[cacl_volt_pst] == value)
//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,
24.50,24.48,24.45,24.43,24.40,24.36,24.32,24.26,24.21,24.14,
24.07,24.00,23.94,23.88,23.79,23.71,23.65,23.58,23.51,23.46,
23.41,23.36,23.30,23.24,23.19,23.13,23.07,23.01,22.95,22.91,
22.86,22.81,22.76,22.70,22.65,22.59,22.54,22.48,22.43,22.37,
22.31,22.26,22.21,22.16,22.11,22.06,22.01,21.96,21.92,21.88,
21.84,21.79,21.76,21.72,21.69,21.64,21.59,21.54,21.49,21.45,
21.40,21.34,21.29,21.24,21.18,21.13,21.07,21.02,20.96,20.90,
20.85,20.80,20.75,20.69,20.63,20.56,20.47,20.29,20.16,20.04,
19.88,19.73,19.53,19.34,19.19,19.00,18.80,18.51,18.00,17.40,
16.80
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
};
static float batt_mah_teb_7s[] =
{
29.30,29.13,28.97,28.86,28.77,28.71,28.67,28.64,28.61,28.59,
28.56,28.53,28.50,28.46,28.43,28.38,28.33,28.28,28.21,28.16,
28.10,28.03,27.94,27.87,27.80,27.75,27.66,27.59,27.52,27.46,
27.38,27.32,27.26,27.19,27.13,27.07,27.00,26.93,26.85,26.79,
26.74,26.67,26.61,26.54,26.47,26.4,26.35,26.28,26.21,26.16,
26.1,26.05,25.98,25.91,25.85,25.79,25.74,25.69,25.63,25.58,
25.54,25.5,25.45,25.41,25.36,25.32,25.27,25.23,25.18,25.13,
25.08,25.03,24.97,24.93,24.86,24.8,24.74,24.67,24.6,24.52,
24.46,24.39,24.3,24.25,24.22,24.18,24.1,24.02,23.94,23.81,
23.65,23.52,23.32,23.19,22.95,22.74,22.52,22.28,22.04,21.8,
21.6,
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()){
@ -98,36 +114,38 @@ void Copter::zr_SuperSlowLoop(){ @@ -98,36 +114,38 @@ void Copter::zr_SuperSlowLoop(){
updownStatus =UpDown_TakeOffStart;
}
if(before_fly){
uint8_t cnt,cacl_volt_pst;
float delt_volt;
cnt = 0;
cacl_volt_pst = 0;
float now_volt = battery.voltage();
switch (battery.get_batt_type(1))
{
case 0:
/* code */
break;
case 1:
for(cnt = 0; cnt<102; cnt++ ){
delt_volt = batt_mah_teb[cnt] - battery.voltage();
// delt_volt = batt_mah_teb[cnt] - test_volt;
if(delt_volt <= 0)
break;
}
// 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));
break;
case 2:
for(cnt = 0; cnt<102; cnt++ ){
delt_volt = batt_mah_teb_7s[cnt] - battery.voltage();
// delt_volt = batt_mah_teb[cnt] - test_volt;
if(delt_volt <= 0)
break;
}
// 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));
break;
default:
break;
}
cacl_volt_pst = 100 - cnt;
battery.reset_remaining(1, cacl_volt_pst);
// 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);

Write Preview
Loading…
Cancel
Save