电池状态是飞控安全运行的关键指标。FMT通过ADC采样获取电池电压和电流使用OCV-SOC查找表和递推最小二乘法RLS估算电池剩余电量SOC并通过多级阈值触发低电压保护。本文从硬件采样到算法实现完整讲解电源管理系统。1. 系统架构┌──────────────┐ ADC采样 ┌──────────────┐ uMCN发布 ┌──────────────┐ │ 电池 │──────────────►│ PMU模块 │──────────────►│ 其他模块 │ │ (LiPo) │ 电压/电流 │ (power_manage)│ bat_status │ (FMS/GCS) │ └──────────────┘ └──────────────┘ └─────────────┘ │ │ mlog记录 ▼ ┌──────────────┐ │ 日志系统 │ └──────────────┘2. 硬件接口2.1 ADC采样电池电压和电流通过ADC通道采集// power_manage.cstaticrt_device_tadc_dev;fmt_err_tpmu_init(void){adc_devrt_device_find(adc0);if(adc_dev!NULL){rt_device_open(adc_dev,RT_DEVICE_FLAG_RDONLY);returnFMT_EOK;}returnFMT_ERROR;}2.2 参数配置// power_manage.cstaticparam_t__param_list[]{PARAM_FLOAT(BAT_V_DIV,1.0,false),// 电压分压比PARAM_FLOAT(BAT_A_PER_V,1.0,false),// 电流灵敏度 (A/V)PARAM_UINT8(BAT_N_CELLS,0,false),// 电池串数PARAM_FLOAT(CAPACITY_OF_BAT,3500.0,false),// 电池容量 (mAh)PARAM_FLOAT(VOLTAGE_EMPTY,3.0,false),// 放空电压 (V/cell)PARAM_FLOAT(VOLTAGE_CHARGED,4.2,false),// 充满电压 (V/cell)PARAM_FLOAT(R_INTERNAL_CONST,-0.005,false),// 内阻估计常数PARAM_FLOAT(R_INTERNAL,0.020,false),// 初始内阻 (Ω)PARAM_FLOAT(LOW_THR,3.5,false),// 低电压阈值 (V/cell)PARAM_FLOAT(CRIT_THR,3.4,false),// 严重低电压阈值PARAM_FLOAT(EMERGEN_THR,3.3,false),// 紧急低电压阈值};PARAM_GROUP_DEFINE(POWER,__param_list);关键参数BAT_V_DIV电压分压比将电池高压衰减到ADC量程内BAT_A_PER_V电流传感器灵敏度将电压转换为电流BAT_N_CELLS电池串数用于计算单体电压3. 数据采集3.1 电压电流读取// power_manage.cfmt_err_tpmu_poll_battery_status(void){structbattery_statusbat_status;uint32_tvalue;// 读取电压ADCif(rt_device_read(adc_dev,BAT1_V_CHANNEL,value,sizeof(value))sizeof(value)){bat_status.battery_voltagevalue*PARAM_GET_FLOAT(POWER,BAT_V_DIV);// mVbattery_update_voltage(battery,bat_status.battery_voltage/1000.0f);}// 读取电流ADCif(rt_device_read(adc_dev,BAT1_I_CHANNEL,value,sizeof(value))sizeof(value)){bat_status.battery_currentvalue*PARAM_GET_FLOAT(POWER,BAT_A_PER_V);// mAbattery_update_current(battery,bat_status.battery_current/1000.0f);}// 更新SOCbattery_update_state_of_charge(battery,timestamps);// 发布到uMCNmcn_publish(MCN_HUB(bat_status),bat_status);// 记录到mlogmlog_push_msg((uint8_t*)bat_status,BATTERY_ID,sizeof(bat_status));returnFMT_EOK;}3.2 数据滤波电压和电流使用一阶低通滤波Alpha Filter// battery.c#defineAlpha_filter(alpha,new,old)((alpha)*(new)(1.0f-(alpha))*(old))voidbattery_update_voltage(Battery*battery,floatvoltage_v){if(battery-voltage_v0.0f){battery-voltage_vvoltage_v;// 首次赋值}else{battery-voltage_vAlpha_filter(0.01f,voltage_v,battery-voltage_v);// 低通滤波}}voidbattery_update_current(Battery*battery,floatcurrent_a){if(battery-current_a0.0f){battery-current_acurrent_a;}else{battery-current_aAlpha_filter(0.1f,current_a,battery-current_a);}}电压滤波系数0.01更平滑电流滤波系数0.1响应更快。4. SOC估算算法4.1 OCV-SOC查找表锂聚合物电池的开路电压OCV与SOC有确定的对应关系// battery.cconstOCV_SOC_Map ocv_soc_table[]{{4.200,100.0},// 满电{4.150,95.0},{4.100,90.0},{4.050,85.0},{4.000,80.0},{3.950,70.0},// 进入平坦区{3.900,60.0},{3.850,50.0},// 中间点{3.800,40.0},{3.750,30.0},// 离开平坦区{3.700,20.0},{3.650,15.0},{3.600,10.0},{3.500,5.0},{3.000,0.0}// 放空};4.2 线性插值// battery.cfloatestimate_soc_from_ocv(Battery*battery){floatvoltagebattery-cell_voltage;// 边界检查if(voltageocv_soc_table[0].ocv)return100.0;if(voltageocv_soc_table[SOC_TABLE_SIZE-1].ocv)return0.0;// 查找区间并线性插值for(inti0;iSOC_TABLE_SIZE-1;i){if(voltageocv_soc_table[i].ocvvoltageocv_soc_table[i1].ocv){floatv1ocv_soc_table[i].ocv;floatv2ocv_soc_table[i1].ocv;floatsoc1ocv_soc_table[i].soc;floatsoc2ocv_soc_table[i1].soc;returnsoc1(soc2-soc1)*(voltage-v1)/(v2-v1);}}return0.0;}4.3 单体电压计算// battery.cvoidbattery_update_state_of_charge(Battery*battery,uint32_ttimestamp){// 计算单体电压if(battery-params.n_cells0){battery-cell_voltage_originbattery-voltage_v/battery-params.n_cells;// 补偿内阻压降battery-cell_voltagebattery-cell_voltage_originbattery-internal_resistance*battery-current_a;}// OCV法估算SOCbattery-state_of_charge_volt_basedestimate_soc_from_ocv(battery);// 电流积分法估算SOC库仑计数floatdt(timestamp-battery-last_timestamp)/1000.0f;// 秒floatconsumed_mahbattery-current_a*dt*1000.0f/3600.0f;battery-discharged_mahconsumed_mah;if(battery-params.capacity0){battery-state_of_charge_current_based100.0f-(battery-discharged_mah/battery-params.capacity*100.0f);}// 融合两种估算方法battery-state_of_chargebattery-state_of_charge_volt_based;battery-last_timestamptimestamp;}4.4 递推最小二乘法RLS内阻估算FMT使用RLS算法在线估算电池内阻// battery.cstaticfloat_RLS_est[2]{0.0f,0.0f};// [OCV, 内阻×串数]staticfloat_estimation_covariance[2][2];voidbattery_update_state_of_charge_RLS(Battery*battery){floatvoltagebattery-voltage_v;floatcurrentbattery-current_a;// RLS更新// 模型V_measured OCV - I * R_internal// 观测向量h [1, -I]// 参数向量theta [OCV, R_internal * n_cells]floath[2]{1.0f,-current};floatyvoltage;// 计算增益floatPh[2];Ph[0]_estimation_covariance[0][0]*h[0]_estimation_covariance[0][1]*h[1];Ph[1]_estimation_covariance[1][0]*h[0]_estimation_covariance[1][1]*h[1];floatdenom1.0fh[0]*Ph[0]h[1]*Ph[1];floatK[2]{Ph[0]/denom,Ph[1]/denom};// 更新参数floatinnovationy-(h[0]*_RLS_est[0]h[1]*_RLS_est[1]);_RLS_est[0]K[0]*innovation;_RLS_est[1]K[1]*innovation;// 更新协方差矩阵// ... (标准RLS更新公式)// 提取内阻if(battery-params.n_cells0){battery-internal_resistancefabsf(_RLS_est[1])/battery-params.n_cells;}}5. 低电压保护5.1 多级警告// battery.h#defineBATTERY_WARNING_NONE0#defineBATTERY_WARNING_LOW_VOLTAGE1// 低电压#defineBATTERY_WARNING_CRITICAL_VOLTAGE2// 严重低电压#defineBATTERY_WARNING_EMERGENCY_VOLTAGE3// 紧急5.2 阈值判断// battery.cvoidbattery_check_warning(Battery*battery){floatcell_vbattery-cell_voltage;if(cell_vbattery-params.emergen_thr){battery-warningBATTERY_WARNING_EMERGENCY;}elseif(cell_vbattery-params.crit_thr){battery-warningBATTERY_WARNING_CRITICAL;}elseif(cell_vbattery-params.low_thr){battery-warningBATTERY_WARNING_LOW;}else{battery-warningBATTERY_WARNING_NONE;}}5.3 响应动作警告级别阈值 (默认)FMS响应LOW3.5V/cell发送警告到地面站CRITICAL3.4V/cell触发返航EMERGENCY3.3V/cell强制降落6. mlog记录电池状态通过mlog记录用于事后分析// power_manage.cstaticmlog_elem_tBattery_Elems[]{MLOG_ELEMENT(timestamp,MLOG_UINT32),MLOG_ELEMENT(voltage,MLOG_FLOAT),MLOG_ELEMENT(current,MLOG_FLOAT),MLOG_ELEMENT(remaining,MLOG_FLOAT),// SOC (%)MLOG_ELEMENT(remaining_c,MLOG_FLOAT),// 电流积分法SOCMLOG_ELEMENT(remaining_v,MLOG_FLOAT),// OCV法SOCMLOG_ELEMENT(SOH,MLOG_FLOAT),// 健康度MLOG_ELEMENT(connected,MLOG_UINT8),MLOG_ELEMENT(init_SOC,MLOG_FLOAT),MLOG_ELEMENT(resistance,MLOG_FLOAT),// 内阻MLOG_ELEMENT(cvoltage,MLOG_FLOAT),// 单体电压MLOG_ELEMENT(cvoltage_orin,MLOG_FLOAT),// 原始单体电压};MLOG_BUS_DEFINE(BATTERY,Battery_Elems);7. 电池状态结构// power_manager.hstructbattery_status{uint32_ttimestamp;floatbattery_voltage;// 总电压 (mV)floatbattery_current;// 总电流 (mA)floatbattery_remaining;// SOC (%)floatremaining_c;// 电流积分法SOCfloatremaining_v;// OCV法SOCfloatstate_of_health;// 健康度 (%)uint8_tbattery_init;// 是否初始化uint8_tconnected;// 是否连接floatinit_SOC;// 初始SOCfloatinternal_resistance;// 内阻 (Ω)floatcell_voltage;// 单体电压 (V)floatcell_voltage_origin;// 原始单体电压未补偿内阻};总结FMT电源管理的核心设计要点设计原因实现ADC采样获取电压/电流原始数据rt_device_readAlpha滤波抑制ADC噪声Alpha_filter()OCV-SOC查找表锂电池特性建模15点线性插值RLS内阻估算在线估计内阻补偿电压降递推最小二乘法多级阈值分级响应避免误触发LOW/CRITICAL/EMERGENCYuMCN发布其他模块实时获取电池状态mcn_publishmlog记录事后分析电池行为MLOG_BUS_DEFINE