自动返航策略-电池篇

电池低电量和电池老化以及电池通讯丢失会触发返航

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();
        }
    }
}

  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值