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

增加记录电池循环的参数,电池损耗计算

zr-sdk-4.1.16
zbr 3 years ago
parent
0ff6baf83b
commit
9a35132e95
6 changed files with 94 additions and 60 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. 2
      ArduCopter/Copter.h
  2. 26
      ArduCopter/Parameters.cpp
  3. 6
      ArduCopter/Parameters.h
  4. 109
      ArduCopter/zr_flight.cpp
  5. 9
      libraries/GCS_MAVLink/GCS_Common.cpp
  6. 2
      modules/mavlink

2
ArduCopter/Copter.h
Unescape View File

@ -1057,11 +1057,11 @@ public: @@ -1057,11 +1057,11 @@ public:
uint8_t BinarySearch2f(float a[], float value, int low, int high);
int8_t in_debug_mode;
uint8_t cacl_volt_pst;
uint8_t mkdir_step;
Mode::Number get_before_mode() const { return before_mode; };
bool far_from_home;
Proxy_Action avoid_action_step;
void zr_set_battery_capacity();
};
extern Copter copter;

26
ArduCopter/Parameters.cpp
Unescape View File

@ -146,7 +146,7 @@ const AP_Param::Info Copter::var_info[] = { @@ -146,7 +146,7 @@ const AP_Param::Info Copter::var_info[] = {
GSCALAR(zr_use_rc, "ZR_USE_RC", 1),
GSCALAR(zr_reg_date, "ZR_REG_DATE", 1),
GSCALAR(zr_avd_wait, "ZR_AVD_WAIT", 60), // 避障无法绕过，等待人为控制延时,单位s
GSCALAR(zr_bat_cycled, "ZR_BAT_CYCL", 1), // 电池循环数
// @Param: ZR_RTL_DELAY
// @DisplayName: rtl Altitude when at final decent alt
// @Description:
@ -1722,12 +1722,30 @@ const char* Copter::get_sysid_board_id(void) @@ -1722,12 +1722,30 @@ const char* Copter::get_sysid_board_id(void)
int32_t deadline = 0;
get_deadline_params(deadline);
gcs().send_text(MAV_SEVERITY_INFO, "date:%ld",deadline);
if(g.zr_reg_date != deadline){
gcs().send_text(MAV_SEVERITY_INFO, "reload date:%ld -> %ld",(int32_t)g.zr_reg_date,deadline);
gcs().send_text(MAV_SEVERITY_INFO, "Date:%ld",deadline);
if((int32_t)g.zr_reg_date != deadline){
// gcs().send_text(MAV_SEVERITY_INFO, "reload date:%ld->%ld",(int32_t)g.zr_reg_date,deadline);
g.zr_reg_date.set_and_save_ifchanged(deadline);
}
enum ap_var_type ptype;
AP_Int16 *bat_parm = (AP_Int16 *)AP_Param::find("ZR_BAT_CYCL", &ptype);
if (bat_parm != nullptr)
{
if (bat_parm->configured_in_storage())
{
if (ptype == AP_PARAM_INT16)
{
gcs().send_text(MAV_SEVERITY_INFO, "Battery cycle: %d", (int16_t)g.zr_bat_cycled);
}
}
else
{
gcs().send_text(MAV_SEVERITY_INFO, "Battery not configured");
AP_Param::set_and_save_by_name("ZR_BAT_CYCL", 1);
}
}
return buf;
}

6
ArduCopter/Parameters.h
Unescape View File

@ -397,6 +397,8 @@ public: @@ -397,6 +397,8 @@ public:
k_param_zr_reg_date,
k_param_zr_avd_wait,
k_param_zr_8_circle,
k_param_zr_bat_cycled,
// the k_param_* space is 9-bits in size
// 511: reserved
};
@ -505,7 +507,7 @@ public: @@ -505,7 +507,7 @@ public:
AP_Float acro_yaw_p;
AP_Float acro_balance_roll;
AP_Float acro_balance_pitch;
AP_Int8 acro_trainer;
AP_Int8 acro_trainer;
AP_Float acro_rp_expo;
AP_Int32 zr_tk_req_alt;
AP_Int16 zr_tk_delay;
@ -514,6 +516,8 @@ public: @@ -514,6 +516,8 @@ public:
AP_Int32 zr_reg_date;
AP_Int16 zr_avd_wait;
AP_Int8 zr_8_circle;
AP_Int16 zr_bat_cycled;
// Note: keep initializers here in the same order as they are declared
// above.
Parameters()

109
ArduCopter/zr_flight.cpp
Unescape View File

@ -49,7 +49,7 @@ void Copter::zr_SlowLoop(){ @@ -49,7 +49,7 @@ void Copter::zr_SlowLoop(){
uint8_t Copter::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;
@ -65,34 +65,6 @@ uint8_t Copter::BinarySearch2f(float a[], float value, int low, int high) @@ -65,34 +65,6 @@ uint8_t Copter::BinarySearch2f(float a[], float value, int low, int high)
void Copter::zr_SuperSlowLoop(){
static bool before_fly = true;
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
};
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()){
if(before_fly){
@ -111,28 +83,63 @@ void Copter::zr_SuperSlowLoop(){ @@ -111,28 +83,63 @@ void Copter::zr_SuperSlowLoop(){
updownStatus =UpDown_TakeOffStart;
}
if(before_fly){
cacl_volt_pst = 0;
uint8_t bat_cnt;
float now_volt = battery.voltage();
switch (battery.get_batt_type(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;
}
zr_set_battery_capacity();
}
}
void Copter::zr_set_battery_capacity()
{
uint8_t bat_cnt;
float now_volt = battery.voltage();
int16_t bat_cycled = (int16_t)g.zr_bat_cycled;
float cycle_pst = (1 - bat_cycled * 0.0002);
switch (battery.get_batt_type(0))
{
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,now_volt,0,100);
battery.reset_remaining(1,bat_cnt * cycle_pst);
// 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,now_volt,0,100);
battery.reset_remaining(1,bat_cnt * cycle_pst);
// gcs().send_text(MAV_SEVERITY_INFO,"cnt:%d,mid:%d",bat_cnt,cacl_volt_pst);
break;
default:
break;
}
}

9
libraries/GCS_MAVLink/GCS_Common.cpp
Unescape View File

@ -1924,8 +1924,13 @@ void GCS::update_serial_battgo_history(mavlink_battgo_history_t *battgo_history_ @@ -1924,8 +1924,13 @@ void GCS::update_serial_battgo_history(mavlink_battgo_history_t *battgo_history_
{
for (uint8_t i=0; i<num_gcs(); i++) {
// mavlink_msg_battgo_info_send_struct(chan,&battgo_info_t);
chan(i)->send_battgo_history(battgo_history_t);
}
chan(i)->send_battgo_history(battgo_history_t);
}
enum ap_var_type ptype;
AP_Int16 *bat_parm = (AP_Int16 *)AP_Param::find("ZR_BAT_CYCL", &ptype);
if (bat_parm != nullptr && bat_parm->configured_in_storage() && ptype == AP_PARAM_INT16) {
bat_parm->set_and_save_ifchanged(battgo_history_t->batt_cycled_time);
}
}
void GCS::send_mission_item_reached_message(uint16_t mission_index)

2
modules/mavlink

@ -1 +1 @@ @@ -1 +1 @@
Subproject commit 6d62818f975c25ffc507904658d8510d3de53c80
Subproject commit 5fc095d17043557589ecc6a28d62fb234081fc87
Write Preview
Loading…
Cancel
Save