电池低电量和电池老化以及电池通讯丢失会触发返航
void Copter::south_batt()
{
static uint32_t srtl_delaytime = 0;
float ts = 0.1;
float R = 0.01;
static int south_batt_info_print_cnt =0;
static uint32_t south_batt_info_print_time =0;
static bool south_batt_info_print_flag =false;
// ii++;
// gcs().send_text(MAV_SEVERITY_CRITICAL, "ii=%d",ii);
// if(ii==500)
// gcs().mode_change_reason = 1;
// gcs().sw
// gcs().send_to_active_channels(uint32_t msgid, const char *pkt);
if((motors->armed() && south_batt_info_print_cnt <5) || south_batt_info_print_flag ==false)//20220518,添加开机打印电池版本和循环次数,开机记录一次和起飞记录5次
{
if(AP_HAL::millis() -south_batt_info_print_time >4000){
south_batt_info_print_time = AP_HAL::millis();
south_batt_info_print_cnt ++;
south_batt_info_print_flag =true;
gcs().send_text(MAV_SEVERITY_CRITICAL, "Battery-VER:%d, Battery-CYC:%d", (int)hal.util->battery_version, (int)hal.util->battery_cyc_life);
}
}
else if(!motors->armed()){
south_batt_info_print_cnt =0;
}
if(!motors->armed() || flightmode->mode_number() == Mode::Number::LOITER || flightmode->mode_number() == Mode::Number::ALT_HOLD || flightmode->mode_number() == Mode::Number::STABILIZE)
{
south_rtl_avoid_flag = false;//
south_rtl_avoid_count = 0;
}
if(!motors->armed() || flightmode->mode_number() != Mode::Number::AUTO)
{
is_avoid_flag = false;
is_wp_flag = false;
south_mission_reached_flag = false;
}
if ((flightmode->mode_number() == Mode::Number::AUTO || flightmode->mode_number() == Mode::Number::RTL || flightmode->mode_number() == Mode::Number::LOITER || flightmode->mode_number() == Mode::Number::BRAKE || flightmode->mode_number() == Mode::Number::GUIDED) && motors->armed() && south_baro_alt > 0)
{
if (batt_reckon_flag == false)
{
x_pre.matrix[0][0] = -0.00337;
x_pre.matrix[0][1] = 25;
transposeMatrix(x, x_pre);
p_pre.matrix[0][0] = 0.01;
p_pre.matrix[0][1] = 0;
p_pre.matrix[1][0] = 0;
p_pre.matrix[1][1] = 0.01;
h_pre.matrix[0][0] = 0;
h_pre.matrix[0][1] = 1;
f_pre.matrix[0][0] = 1;
f_pre.matrix[0][1] = 0;
f_pre.matrix[1][0] = ts;
f_pre.matrix[1][1] = 1;
q_pre.matrix[0][0] = 0;
q_pre.matrix[0][1] = 0;
q_pre.matrix[1][0] = 0;
q_pre.matrix[1][1] = 0.01;
q_now = numMul(q_pre, ts);
start_batt_t = AP_HAL::millis();
batt_reckon_flag = true;
gcs().send_text(MAV_SEVERITY_CRITICAL, "bat_type&res:%d_%d", hal.util->battery_type, hal.util->battery_res_capability);
}
else
{
//2021.3.12
if (fabs(copter.battery.voltage() - last_voltage) < 0.0001)//与上次电压相差小于0.0001记录一次
{
voltage_count ++;
}
else
{
voltage_count = 0;
}
if (voltage_count == 300 && copter.south_auto_mode != ((int) ModeAuto::SubMode::RTL ))
{
gcs().send_text(MAV_SEVERITY_CRITICAL, "Battery No Connect..");
copter.set_mode(Mode::Number::RTL, ModeReason::LOSE_BATTERY_COMMUNICATION);//电池数据丢失返航
gcs().mode_change_reason = 7;
}
mul(x_, f_pre, x);
transposeMatrix(f_t, f_pre);
mul(f_p, f_pre, p_pre);
mul(f_p_f_t, f_p, f_t);
p_ = add(f_p_f_t, q_now);
transposeMatrix(h_t, h_pre);
mul(p_h_t, p_, h_t);
k = numMul(p_h_t, 1 / (p_pre.matrix[1][1] + R));
x = add(x_, numMul(k, (copter.battery.voltage() - (h_pre.matrix[0][0] * x_.matrix[0][0] + h_pre.matrix[0][1] * x_.matrix[1][0]))));
mul(k_h, k, h_pre);
mul(k_h_p,k_h, p_);
p_pre = sub(p_, k_h_p);
uint32_t now_batt_t = AP_HAL::millis();
if(get_wp_flag == false)
{
if (hal.util->battery_type == BATTERYTYPE::GREP12000 || hal.util->battery_type == BATTERYTYPE::GREP154000){
float mid_voltage = (origin_voltage + 19.8) / 2;
start_remain_time = (hal.util->battery_res_capability * mid_voltage * 3.6) / (hal.util->enrgtot);
// start_remain_time = 24.106 * powf(copter.battery.voltage(), 3) - 1810.2 * powf(copter.battery.voltage(), 2) + 45680 * copter.battery.voltage() - 385036;
}
else if (hal.util->battery_type == BATTERYTYPE::WL16000){
float mid_voltage = (origin_voltage + 19.0) / 2;
start_remain_time = (hal.util->battery_res_capability * mid_voltage * 3.6) / (hal.util->enrgtot);
// start_remain_time = -6.8063 * powf(copter.battery.voltage(), 3) + 482.53 * powf(copter.battery.voltage(), 2) - 10688 * copter.battery.voltage() + 75506;
}
// else if (hal.util->battery_type == BATTERYTYPE::GREP154000){
// start_remain_time = -0.076 * powf(copter.battery.voltage(), 3) - 69.638 * powf(copter.battery.voltage(), 2) + 3951.5 * copter.battery.voltage() - 51817;
// }
else{
start_remain_time = (-95.522 * copter.battery.voltage() * copter.battery.voltage() + 5108.9 * copter.battery.voltage() - 65700);
}
get_wp_flag = true;
}
Location cur{};
ahrs.get_position(cur);
float to_home_dist = cur.get_distance(ahrs.get_home());
float travel_xy_time = (to_home_dist * 100) / copter.wp_nav->get_default_speed_xy();
float climb_time = 0.0;
float land_time = 0.0;
float total_time = 0.0;
if(south_baro_alt < g.rtl_altitude)
{
climb_time = 1.6 * (g.rtl_altitude - south_baro_alt) / copter.wp_nav->get_default_speed_up();
land_time = (g.rtl_altitude / copter.wp_nav->get_default_speed_down()) / 60;
}
else
{
climb_time = 1.6 * (south_baro_alt - g.rtl_altitude) / copter.wp_nav->get_default_speed_down();
land_time = (south_baro_alt / copter.wp_nav->get_default_speed_down()) / 60;
}
total_time = g2.forced_land_min + land_time + (climb_time + travel_xy_time) / 60;
if(total_time <= 0)
{
total_time = 0;
}
if(land_time <= 0)
{
land_time = 0;
}
gcs().total_time = (int)(total_time * 60);
gcs().land_time = (int)(land_time * 60);
if ((now_batt_t - start_batt_t)> 30 * 1000) //600 * 1000
{
if(x.matrix[0][0] > -0.00005)
{
x.matrix[0][0] = -0.00005;
}
if(x.matrix[0][0] < -0.01)
{
x.matrix[0][0] = -0.01;
}
// remain_time1 = (21.3 - x.matrix[1][0]) / (x.matrix[0][0] * 36);
float fly_t = (now_batt_t - start_batt_t) / 1000;
if (hal.util->battery_type == BATTERYTYPE::GREP12000 || hal.util->battery_type == BATTERYTYPE::GREP154000){
float mid_voltage = (origin_voltage + 19.8) / 2;
temp_remain_time = (hal.util->battery_res_capability * mid_voltage * 3.6) / (hal.util->enrgtot);
// temp_remain_time = 24.106 * powf(x.matrix[1][0], 3) - 1810.2 * powf(x.matrix[1][0], 2) + 45680 * x.matrix[1][0] - 385036;
}
else if (hal.util->battery_type == BATTERYTYPE::WL16000){
float mid_voltage = (origin_voltage + 19.0) / 2;
temp_remain_time = (hal.util->battery_res_capability * mid_voltage * 3.6) / (hal.util->enrgtot);
// temp_remain_time = -6.8063 * powf(x.matrix[1][0], 3) + 482.53 * powf(x.matrix[1][0], 2) - 10688 * x.matrix[1][0] + 75506;
}
// else if (hal.util->battery_type == BATTERYTYPE::GREP154000){
// temp_remain_time = -0.076 * powf(x.matrix[1][0], 3) - 69.638 * powf(x.matrix[1][0], 2) + 3951.5 * x.matrix[1][0] - 51817;
// }
else{
temp_remain_time = (-95.522 * x.matrix[1][0] * x.matrix[1][0] + 5108.9 * x.matrix[1][0] - 65700);
}
if (m_count == 600)
{
if (fabsf(start_remain_time - temp_remain_time) >= 0.000001)
{
slope = (1 - ((start_remain_time - temp_remain_time) - fly_t) / (start_remain_time - temp_remain_time));
if(slope < 0.001)
{
slope_count++;
}
gcs().send_text(MAV_SEVERITY_CRITICAL, "slope:%.1f", slope);
}
m_count = 0;
}
m_count ++;
if (slope_count >= 2 && copter.south_auto_mode != ((int) ModeAuto::SubMode::RTL ) && is_rtl_flag == false)
{
copter.set_mode(Mode::Number::RTL, ModeReason::BATTERY_COMPARATIVELY_OLD);//电池严重老化返航
gcs().mode_change_reason = 8;
is_rtl_flag = true;
gcs().send_text(MAV_SEVERITY_CRITICAL, "The battery are comparatively old.");
}
if (slope < 0.5 || slope > 1.5)
{
remain_time = temp_remain_time;
}
else
{
remain_time = temp_remain_time;// * slope;
}
remain_time = remain_time / 60;
if(remain_time <= 0)
{
remain_time = 0;
}
gcs().remain_time = (int)(remain_time * 60);
if (remain_time < total_time && is_rtl_flag == false)
{
rtl_count ++;
}
else
{
rtl_count = 0;
}
if( flightmode->mode_number() == Mode::Number::AUTO ){
if ((rtl_count == 700 && copter.south_auto_mode !=((int) ModeAuto::SubMode::RTL )) || (gcs().south_srtl ==1))
{
if(gcs().south_srtl ==0){
gcs().send_text(MAV_SEVERITY_CRITICAL, "Notice: SRTL trigger...");
srtl_delaytime = AP_HAL::millis() + 15000;
gcs().south_srtl =1;
}
else if( copter.g3.south_srtl_disable!=1 && gcs().south_srtl==1 && AP_HAL::millis()>srtl_delaytime && is_rtl_flag==false ){
copter.set_mode(Mode::Number::RTL, ModeReason::BATTERY_RTL);//电池续航不足返航
gcs().mode_change_reason = 1;
is_rtl_flag = true;
}
}
}
else{
if (rtl_count == 700 && copter.south_auto_mode !=((int) ModeAuto::SubMode::RTL ))
{
copter.set_mode(Mode::Number::RTL, ModeReason::BATTERY_RTL);//电池续航不足返航
gcs().mode_change_reason = 1;
is_rtl_flag = true;
}
}
if (rtl_count == 700 && copter.south_auto_mode !=((int) ModeAuto::SubMode::RTL ))
{
copter.set_mode(Mode::Number::RTL, ModeReason::BATTERY_RTL);//电池续航不足返航
gcs().mode_change_reason = 1;
is_rtl_flag = true;
}
}
if(remain_time < land_time && is_land_flag == false)
{
land_cout ++;
}
else
{
land_cout = 0;
}
if( flightmode->mode_number() == Mode::Number::AUTO ){
if ((land_cout == 700 && copter.south_rtl_state != ((int)ModeRTL::SubMode::LAND)) || (gcs().south_srtl ==2))
{
if(gcs().south_srtl ==0){
gcs().send_text(MAV_SEVERITY_CRITICAL, "Notice: SRTL2 trigger...");
srtl_delaytime = AP_HAL::millis() + 15000;
gcs().south_srtl =2;
}
else if( copter.g3.south_srtl_disable!=1 && gcs().south_srtl==2 && AP_HAL::millis()>srtl_delaytime && is_land_flag==false ){
copter.set_mode(Mode::Number::LAND, ModeReason::BATTERY_LAND);//续航不足降落
gcs().mode_change_reason = 2;
is_land_flag = true;
}
}
}
else{
if (land_cout == 700 && copter.south_rtl_state != ((int)ModeRTL::SubMode::LAND))
{
copter.set_mode(Mode::Number::LAND, ModeReason::BATTERY_LAND);//续航不足降落
gcs().mode_change_reason = 2;
is_land_flag = true;
}
}
}
else// if (now_batt_t - start_batt_t > 30 * 1000)
{
float remain_time1 = 0.0;
if (hal.util->battery_type == BATTERYTYPE::GREP12000 || hal.util->battery_type == BATTERYTYPE::GREP154000){
float mid_voltage = (origin_voltage + 19.8) / 2;
remain_time1 = ((hal.util->battery_res_capability * mid_voltage * 3.6) / (hal.util->enrgtot)) /60;
// remain_time1 = (24.106 * powf(x.matrix[1][0], 3) - 1810.2 * powf(x.matrix[1][0], 2) + 45680 * x.matrix[1][0] - 385036) / 60;
}
else if (hal.util->battery_type == BATTERYTYPE::WL16000){
float mid_voltage = (origin_voltage + 19.0) / 2;
remain_time1 = ((hal.util->battery_res_capability * mid_voltage * 3.6) / (hal.util->enrgtot)) / 60;
// remain_time1 = (-6.8063 * powf(x.matrix[1][0], 3) + 482.53 * powf(x.matrix[1][0], 2) - 10688 * x.matrix[1][0] + 75506) / 60;
}
// else if (hal.util->battery_type == BATTERYTYPE::GREP154000){
// remain_time1 = (-0.076 * powf(x.matrix[1][0], 3) - 69.638 * powf(x.matrix[1][0], 2) + 3951.5 * x.matrix[1][0] - 51817) / 60;
// }
else{
remain_time1 = (-95.522 * x.matrix[1][0] * x.matrix[1][0] + 5108.9 * x.matrix[1][0] - 65700) / 60;
}
if(remain_time1 <= 0)
{
remain_time1 = 0;
}
remain_time1 = 0;
}
gcs().remain_time = (int)(remain_time1 * 60);
if (remain_time1 < total_time && is_rtl_flag == false)
{
rtl_count ++;
}
else
{
rtl_count = 0;
}
if( flightmode->mode_number() == Mode::Number::AUTO ){
if ((rtl_count == 700 && copter.south_auto_mode !=((int) ModeAuto::SubMode::RTL )) || (gcs().south_srtl ==1))
{
if(gcs().south_srtl ==0){
gcs().send_text(MAV_SEVERITY_CRITICAL, "Notice: SRTL trigger...");
srtl_delaytime = AP_HAL::millis() + 15000;
gcs().south_srtl =1;
}
else if( copter.g3.south_srtl_disable!=1 && gcs().south_srtl==1 && AP_HAL::millis()>srtl_delaytime && is_rtl_flag==false ){
copter.set_mode(Mode::Number::RTL, ModeReason::BATTERY_RTL);//续航不足返航
gcs().mode_change_reason = 1;
is_rtl_flag = true;
}
}
}
else{
if (rtl_count == 700 && copter.south_auto_mode != ((int) ModeAuto::SubMode::RTL ))
{
copter.set_mode(Mode::Number::RTL, ModeReason::BATTERY_RTL);//续航不足返航
gcs().mode_change_reason = 1;
is_rtl_flag = true;
}
}
if(remain_time1 < land_time && is_land_flag == false)
{
land_cout ++;
}
else
{
land_cout = 0;
}
if( flightmode->mode_number() == Mode::Number::AUTO ){
if ((land_cout == 700 && copter.south_rtl_state != ((int)ModeRTL::SubMode::LAND)) || (gcs().south_srtl ==2))
{
if(gcs().south_srtl ==0){
gcs().send_text(MAV_SEVERITY_CRITICAL, "Notice: SRTL2 trigger...");
srtl_delaytime = AP_HAL::millis() + 15000;
gcs().south_srtl =2;
}
else if( copter.g3.south_srtl_disable!=1 && gcs().south_srtl==2 && AP_HAL::millis()>srtl_delaytime && is_land_flag==false ){
copter.set_mode(Mode::Number::LAND, ModeReason::BATTERY_LAND);//续航不足降落
gcs().mode_change_reason = 2;
is_land_flag = true;
}
}
}
else{
if (land_cout == 700 && copter.south_rtl_state != ((int)ModeRTL::SubMode::LAND))
{
copter.set_mode(Mode::Number::LAND, ModeReason::BATTERY_LAND);//续航不足降落
gcs().mode_change_reason = 2;
is_land_flag = true;
}
}
} //20220905 电芯电压差过大判断老化,且低电压强制返航
int batt_cell_volt_min = hal.util->battery_cells[0];
int batt_cell_volt_max = hal.util->battery_cells[0];
int batt_cell_volt_diff = 0;
for(int i=1; i<6; i++){
if(hal.util->battery_cells[i] < batt_cell_volt_min) batt_cell_volt_min = hal.util->battery_cells[i];
if(hal.util->battery_cells[i] > batt_cell_volt_max) batt_cell_volt_max = hal.util->battery_cells[i];
}
batt_cell_volt_diff = batt_cell_volt_max -batt_cell_volt_min;
if (batt_cell_volt_diff > 150){//飞行中电芯压差大于150mv判断老化
if ((copter.south_rtl_state != ((int)ModeRTL::SubMode::LAND)) && (copter.south_auto_mode != ((int) ModeAuto::SubMode::RTL )) && (last_voltage < g3.south_srtl_low_vol) && is_force_rtl_flag==false){
force_rtl_count ++;
}
else{
force_rtl_count =0;
}
if(force_rtl_count >= 50){
copter.set_mode(Mode::Number::RTL, ModeReason::BATTERY_COMPARATIVELY_OLD);//电池老化严重返航
gcs().mode_change_reason = 8;
is_force_rtl_flag = true;
gcs().send_text(MAV_SEVERITY_CRITICAL, "BATTERY_OLD_RTL..");
}
if ((battery_old_print_cnt ++) %100 == 0){
gcs().send_text(MAV_SEVERITY_CRITICAL, "Notice: BATTERY_OLD...");
}
}
// else{
// // force_rtl_count =0;
// // battery_old_print_cnt =0;
// }
// 2021.3.12
last_voltage = copter.battery.voltage();
}
}
}