motor_ctl-2/User/Src/remain_distance.c

/**
  ******************************************************************************
  * @file    remain_distance.c
  * @author  Damon
  * @version V1.5
  * @date    20190105
  * @brief   电池剩余续航里程统计
  ******************************************************************************
  * @attention
  *本功能模块输出计算结果为，剩余续航里程RemainDis.remainDistance
  * 
  ******************************************************************************
  */

#include "remain_distance.h"
#include "battery_info.h"
#include "eeprom_24c02.h"
#include "math_tools.h"

/*电流积分，最小值，单位mA*/
#define CURRENT_MIN    130

/*电流积分周期*/
#define Q_ADD_TIME_MS  5 

extern const MC_AvgPower_Struct_t MC_AvgPower_Default;

//续航计算相关参数
Remain_Dis_t RemainDis = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0xFFFF, 0, 0, FALSE, 0};

//电池电量计算相关参数
Battery_Info_t Battery_Info = {0, 0, 0, 0, 0, FALSE, 0, 0, 0};

/******************************************************************************
* @Function:  void RD_getQRemainRecord( void )
* @Discrible: 读取电池剩余电量  
* @Param:  
* @Return:   
* @Others:    此函数在系统上电时调用一次，读取电池剩余电量
							已添加到RD_RemainDis_init(void)，无需单独调用
******************************************************************************
* @Recode    date       version	        author             modify 
*          ------------------------------------------------------------------
*          20190815      V1.3          Damon             modify
* 
*******************************************************************************/
static void RD_getQRemainRecord(uint16_t Volgate)
{
	uint16_t BusVoltage;
	uint16_t i;
	uint16_t num,batteryTotalQ,batteryRemainQ;
	
  //获取母线初始电压
	BusVoltage = Volgate;
	
	//查表读取电池电量
	num = sizeof(battery_VoltageQuantity) / 4;
	batteryTotalQ = battery_VoltageQuantity[num-1][1];
	batteryRemainQ = batteryTotalQ;
	for(i=0; i<num; i++)
	{
		if(battery_VoltageQuantity[i][0] <=  BusVoltage)
		{
			batteryRemainQ = batteryTotalQ - battery_VoltageQuantity[i][1] ;    //A*s
			Battery_Info.Q_remain_record = batteryRemainQ * 10 / 36;    //mAh
			break;
		}
		else//电池电压低于最小值
		{
			batteryRemainQ = 0;
		}
	}
	
	MC_RunInfo.SOC = batteryRemainQ * 100 / batteryTotalQ;
	
	Battery_Info.Q_remain_real = Battery_Info.Q_remain_record;
}

/******************************************************************************
* @Function:  void RD_RemainDis_init(void)
* @Discrible: 模块功能初始化  
* @Param:  uwWheelLength:轮胎周长,BusVoltage:母线电压
* @Return:   
* @Others:    此函数在进入主循环前调用，调用后，标志位置位，开启模块的统计功能
******************************************************************************
* @Recode    date       version	        author             modify 
*          ------------------------------------------------------------------
*          20181221      V1.2          Damon             modify
* 
*******************************************************************************/
void RD_RemainDis_Init(uint16_t uwWheelLength, TrueOrFalse_Flag_Struct_t BMS_COM_OK, uint16_t BusVoltage, uint8_t DesignVol, uint16_t BMS_RC)
{
	Battery_Info.Q_discharged = 0;
	Battery_Info.Q_remain_real = 0;
	Battery_Info.Q_remain_record = BATTERY_TATAL_Q_MAH;
	Battery_Info.Q_remain_real_last = 0;
	
	if(BMS_COM_OK == TRUE) //电池通讯正常时，直接获取电池容量
	{
	  Battery_Info.Q_remain_real = BMS_RC;
	}
	else //电池不含通讯时，根据母线电压估算剩余容量
	{
//	  RD_getQRemainRecord(BusVoltage * 36 / DesignVol); //放电曲线是按照36V,针对48V或24V做等效转换
	}
	
	RemainDis.start = 0;
	RemainDis.I_motor = 0;
	RemainDis.acc_period = Q_ADD_TIME_MS;
	RemainDis.Q_add = 0;
	RemainDis.wheelLength = uwWheelLength;
	RemainDis.wheel_Count = 0;
	RemainDis.Power_per_km = 0;
	RemainDis.Power_per_km_average = 0;
	RemainDis.remainDistance = 0xFFFF;//码表显示---km
	RemainDis.flag_saveInfo = 0;
	RemainDis.flag_helpModeChange = 0;
	RemainDis.IsBMS_ComOK_Flag = FALSE;
	RemainDis.BMS_Com_DelayTimeCnt = HAL_GetTick();	
	
}

/******************************************************************************
* @Function:  void RD_CalculateRemainDis(void)
* @Discrible: 计算电池剩余续航里程
* @Param:  
* @Return:   
* @Others:    计算平均功耗（中间变量），和剩余续航里程
******************************************************************************
* @Recode    date       version	        author             modify 
*          ------------------------------------------------------------------
*          20181210      V1.4          Damon             modify
* 
*******************************************************************************/
void RD_CalculateRemainDis(uint32_t WheelTurnCnt, uint16_t RemainCap, uint8_t SOC, uint16_t BusCurrent)  
{
	/*以下宏定义会影响平均功耗计算结果的变化快慢
	WHEEL_COUNT_REFRESH_VALUE 电量统计周期，每10圈统计一次电量
	Q_FIFO_LENGTH 动态平均功耗，取平均值的长度。Power_per_km（10圈x25=250圈，即动态功耗是车轮骑行250圈的平均值）
	P_FIFO_LENGTH 对Power_per_km求平均值的长度，Power_per_km_average（250圈x10=-2500圈）
	修改宏定义的大小，可以改变Power_per_km_average的更新快慢，最终影响剩余续航里程的更新快慢。
	*/
	
	/*车轮圈数，最小的电量统计周期*/
	#define WHEEL_COUNT_REFRESH_VALUE 10
	/*动态平均功耗，缓存长度*/
	#define Q_FIFO_LENGTH 25   //25   250圈  约500m
	/*平均功耗，缓存长度*/
	#define P_FIFO_LENGTH   30  //约15KM
	
	static uint16_t Q_FIFO[Q_FIFO_LENGTH]={0};
	static uint16_t  Q_Index=0;
	static uint32_t Q_sum=0;
	static uint8_t Q_fifo_full_falg=0;
		
	static int16_t Q_add_tmp=0;
	
	static int32_t PowerPerKmFltSum=0;
	
	uint8_t i;
	int32_t ptmp;
	
	static uint32_t RunTimeCnt = 0;
	static uint32_t WheelTurnCntOld = 0;
	
	if((HAL_GetTick() - RunTimeCnt) < Q_ADD_TIME_MS)//计算周期为Q_ADD_TIME_MS
	{
	  return;
	}
	RunTimeCnt = HAL_GetTick();
	
	//更新母线电流值
	RemainDis.I_motor = BusCurrent;
	//更新计算周期内轮子圈数
	RemainDis.wheel_Count = WheelTurnCnt - WheelTurnCntOld;
	
	/*开始计算*/
	if(RemainDis.start == 1)
	{
		/*电池通讯中断超时2s判断*/
		if((HAL_GetTick() - RemainDis.BMS_Com_DelayTimeCnt) > 2000)
		{
		  RemainDis.BMS_Com_DelayTimeCnt = HAL_GetTick();
			RemainDis.IsBMS_ComOK_Flag = FALSE;
		}
		else
		{
				//更新电池剩余容量
				Battery_Info.Q_remain_real = RemainCap;			
		}
		
		/*助力档位切换后*/
		if( RemainDis.flag_helpModeChange == 1 )
		{
			RemainDis.flag_helpModeChange = 0;
			
			/*清空一级缓冲区数据*/
			for(i=0;i<Q_FIFO_LENGTH;i++)
			{
				Q_FIFO[i] = 0;
			}
			Q_Index = 0;
			Q_sum = 0;
			Q_fifo_full_falg=0;
			
			if(RemainDis.Power_per_km_average != 0)
			{
				/*赋值求平均值的总和*/
				PowerPerKmFltSum = RemainDis.Power_per_km_average  << 8;
			}
		}
		
		/*轮圈数大于设定值，计算一次*/
		if( RemainDis.wheel_Count >=  WHEEL_COUNT_REFRESH_VALUE)
		{
			WheelTurnCntOld = WheelTurnCnt;
						
			/*消耗电量信息来自电池*/
			if(RemainDis.IsBMS_ComOK_Flag == TRUE )
			{
				//更新电池剩余容量
				Battery_Info.Q_remain_real = RemainCap;
				//更新电池剩余电量
				Battery_Info.SOC = SOC;
				if(Battery_Info.Q_remain_real_last != 0)
				{
					Q_add_tmp = (int16_t)(Battery_Info.Q_remain_real_last - Battery_Info.Q_remain_real);	
				
					/*充电状态，或由母线电流切换到电量信息计算时，此值会出现负数*/
					Q_add_tmp = (Q_add_tmp > 0) ? Q_add_tmp : 0;
					Q_add_tmp = (Q_add_tmp < 30) ? Q_add_tmp : 30;
					/*判断电池是否满充*/
	        if( ( Q_add_tmp == 0 ) && ( Battery_Info.SOC == 100 ) )
					{
						/*当电池满充时，发出的电量信息会是标称值，
						在Battery.SOC==100时不会随电量消耗尔减小*/
						/*置标志位，不计算更新*/
					}
					else
					{
						/*将一定轮圈数消耗的电量，写入FIFO,FIFO中的数据滑动求平均值*/
						Q_sum -= Q_FIFO[Q_Index];
						Q_sum += Q_add_tmp;
						Q_FIFO[Q_Index++] = Q_add_tmp;
						
						if(Q_Index>=Q_FIFO_LENGTH)
						{
							Q_Index = 0;
							Q_fifo_full_falg = 1;
							
							/*250圈,动态功耗刷新一次，置一次eeprom保存标志*/
							RemainDis.flag_saveInfo = 1;
						}
						/*
						wheelCount x WHEEL_LENGTH(cm) 
						------------------------------- (km)  wheelCount=(Q_FIFO_LENGTH * WHEEL_COUNT_REFRESH_VALUE)
											 1000 x 100
						
							Q_sum(mA*h) x 1000 x 100
						------------------------------- (mA*h/km)
						 wheelCount x WHEEL_LENGTH  
						*/						
						if( Q_sum > 0 )
						{
							/*缓存满后，求平均值*/
							if(Q_fifo_full_falg==1)
							{
								RemainDis.Power_per_km = (Q_sum * 1000 * 100 ) / ( Q_FIFO_LENGTH * WHEEL_COUNT_REFRESH_VALUE * RemainDis.wheelLength );
							}
							else
							{
								/*缓存数据未满，以当前有效数据个数求平均值*/
								RemainDis.Power_per_km = (Q_sum * 1000 * 100 ) / ( Q_Index * WHEEL_COUNT_REFRESH_VALUE * RemainDis.wheelLength );
							}
						}
						else
						{
							RemainDis.Power_per_km = 0;
						}
						
						/*电量缓存Q_FIFO计满一次，滑动滤波求平均值*/
						if(Q_Index==0)
						{		
							ptmp = RemainDis.Power_per_km;
							PowerPerKmFltSum += ((ptmp << 8) - PowerPerKmFltSum) / P_FIFO_LENGTH;
							RemainDis.Power_per_km_average = (uint16_t)(PowerPerKmFltSum >> 8);
						}

						/*计算剩余续航里程*/
						if(RemainDis.Power_per_km_average > 0)
						{
							RemainDis.remainDistance = Battery_Info.Q_remain_real / RemainDis.Power_per_km_result;//单位km
						}
						else
						{
							RemainDis.remainDistance = 0xFFFF;//码表显示---km
						}
					}
				}
			}

			/*电量信息来自电池的计算，需要用到此数据*/
			Battery_Info.Q_remain_real_last = Battery_Info.Q_remain_real;
		}
	}
}

/******************************************************************************
* @Function:  void RD_saveAndUpdateInfo(uint8_t uwHelpMode)
* @Discrible: 保存功耗数据到EEPROM
* @Param:  
* @Return:   
* @Others:    每一公里，保存一次数据到eeprom;助力档位切换时，刷新计算的功耗数据。此函数在主循环中调用
******************************************************************************
* @Recode    date       version	        author             modify 
*          ------------------------------------------------------------------
*          20190105      V1.4          Damon             modify
* 
*******************************************************************************/
void RD_SaveAndUpdateInfo(MC_GearSt_Struct_t GearSt, MC_AssistRunMode_Struct_t MC_AssistRunMode)
{
	static MC_GearSt_Struct_t MC_GearSt_Old = MC_GearSt_OFF;
	static MC_AssistRunMode_Struct_t MC_AssistRunMode_Old = MC_AssistRunMode_INVALID;
	static uint16_t usPowerPerKmAverageLast =0;
	static uint16_t powerDefaultCurrentMode=0;
	int16_t sPercent;
	int32_t ltmp;
	
	if( ( (GearSt & 0x0F) != 0 ) && ( MC_AssistRunMode != MC_AssistRunMode_INVALID ))
	{
		RemainDis.start = 1;
		
		/*保存当前助力档位的数据到eeprom*/
		if( RemainDis.flag_saveInfo == 1 )
		{
			RemainDis.flag_saveInfo = 0;
			
			/*平均功耗值有更新时,执行保存数据相关操作*/
			if( RemainDis.Power_per_km_average != usPowerPerKmAverageLast )
			{	
				if(RemainDis.Power_per_km_average != 0)
				{
					/*计算平均功耗，增减量的百分比，放大100倍*/
					ltmp = (int32_t)(RemainDis.Power_per_km_average - powerDefaultCurrentMode) * 100;
					sPercent = ltmp / powerDefaultCurrentMode;
					
           /*各档位关联*/
					/*不低于默认值30%*/
					if( (sPercent >= -30)&&(sPercent <= 455) )
					{
						/*更新平均功耗*/
						RemainDis.Power_per_km_result = powerDefaultCurrentMode * ( 100 +  sPercent ) / 100;
						
						/*计算各档位增减后的值*/
						ltmp = MC_AvgPower_Default.GearSt_ECO;
						MC_AvgPower.GearSt_ECO = ltmp * ( 100 +  sPercent ) / 100;				
						ltmp = MC_AvgPower_Default.GearSt_NORM;
						MC_AvgPower.GearSt_NORM = ltmp * ( 100 +  sPercent ) / 100;
						ltmp = MC_AvgPower_Default.GearSt_SPORT;
						MC_AvgPower.GearSt_SPORT = ltmp * ( 100 +  sPercent ) / 100;
						ltmp = MC_AvgPower_Default.GearSt_TURBO;
						MC_AvgPower.GearSt_TURBO = ltmp * ( 100 +  sPercent ) / 100;
						ltmp = MC_AvgPower_Default.GearSt_SMART;
						MC_AvgPower.GearSt_SMART = ltmp * ( 100 +  sPercent ) / 100;
						
						/*保存所有档位的更新数据*/
						SaveParamToEEprom_24C02(&I2C_Handle_EEPROM, EEPROM_24C02_ADDR_AVG_POWER, sizeof(MC_AvgPower_Struct_t), (uint8_t*)&MC_AvgPower.GearSt_ECO);
					}     
				}	
				
				/*备份平均功耗值*/
				usPowerPerKmAverageLast = RemainDis.Power_per_km_average;
			}
		}
		
		/*助力档位切换后，根据当前档位更新功耗数据*/
		if((GearSt != MC_GearSt_Old) || (MC_AssistRunMode != MC_AssistRunMode_Old))
		{
			switch(GearSt) 
			{
				case MC_GearSt_Torque_ECO:
					RemainDis.Power_per_km_average = MC_AvgPower.GearSt_ECO;				
					powerDefaultCurrentMode = MC_AvgPower_Default.GearSt_ECO;
					break;			
				case MC_GearSt_Torque_NORM:
					RemainDis.Power_per_km_average = MC_AvgPower.GearSt_NORM;
					powerDefaultCurrentMode = MC_AvgPower_Default.GearSt_NORM;
					break;		
				case MC_GearSt_Torque_SPORT:
					RemainDis.Power_per_km_average = MC_AvgPower.GearSt_SPORT;
					powerDefaultCurrentMode = MC_AvgPower_Default.GearSt_SPORT;
					break;			
				case MC_GearSt_Torque_TURBO:
					RemainDis.Power_per_km_average = MC_AvgPower.GearSt_TURBO;
					powerDefaultCurrentMode = MC_AvgPower_Default.GearSt_TURBO;
					break;				
				case MC_GearSt_SMART:
					RemainDis.Power_per_km_average = MC_AvgPower.GearSt_SMART;
					powerDefaultCurrentMode = MC_AvgPower_Default.GearSt_SMART;
					break;			
				default:
					RemainDis.Power_per_km_average = 0;
					powerDefaultCurrentMode = 0;
					break;
			}
			
			/*备份平均功耗值*/
			usPowerPerKmAverageLast = RemainDis.Power_per_km_average;
			
			RemainDis.Power_per_km_result = RemainDis.Power_per_km_average;
			
			/*计算剩余续航里程*/
			if(RemainDis.Power_per_km_average > 0)
			{
				RemainDis.remainDistance = Battery_Info.Q_remain_real / RemainDis.Power_per_km_result;//单位km
			}
			else
			{
				RemainDis.remainDistance = 0xFFFF;//码表界面显示：---km
			}
			
			/*置标志位，在后续计算中，刷新相关数据*/
			RemainDis.flag_helpModeChange = 1;
		}
	}
	else
	{
		RemainDis.start = 0;
		RemainDis.Power_per_km_average = 0;
		RemainDis.Power_per_km_result = 0;
		RemainDis.remainDistance = 0xFFFF;//码表界面显示：---km
	}
	
	/*备份助力档位*/
	MC_GearSt_Old = GearSt;
	MC_AssistRunMode_Old = MC_AssistRunMode;
}


/*
根据电池电压计算剩余电量，运行周期200ms
*/
uint8_t Battery_SocCal(Battery_Info_t* p_Battery_Info, uint8_t SOC_Old, uint8_t Vol_Design)
{
  static uint16_t TimeDelayCnt = 0;
	static uint8_t Result = 0, Result_Pre=0;
	static TrueOrFalse_Flag_Struct_t InitFlag = FALSE;
	uint16_t i;
	uint16_t TableNum;
	uint32_t batteryTotalQ, batteryRemainQ;
	static uint8_t u8LoopCnt = 0;
	static uint16_t u16MaxVoltage = 0;
  static uint32_t u32VolSum = 0;
	static TrueOrFalse_Flag_Struct_t FlagFristCal = TRUE;
	
	TableNum = sizeof(battery_VoltageQuantity) / 4;
	batteryTotalQ = battery_VoltageQuantity[TableNum - 1][1];
	batteryRemainQ = batteryTotalQ;
	
	if(InitFlag == FALSE) //初始化动态显示百分比
	{
	  if(HAL_GetTick() < 2500)
			return 0;
		else
		{
			TimeDelayCnt++;
			u32VolSum += p_Battery_Info->u16CurVoltage;
			if(TimeDelayCnt <= 1) return 25;
			else if(TimeDelayCnt <= 3) return 50;
			else if(TimeDelayCnt <= 5) return 75;
			else if(TimeDelayCnt <= 7) return 100;
			else
			{
				InitFlag = TRUE;
				u16MaxVoltage = (Vol_Design == 24) ? (uint16_t)(u32VolSum >> 3) * 10 / 7
																						 : ((Vol_Design == 48) ? (uint16_t)(u32VolSum >> 3) * 10 / 13
																																	 : (uint16_t)(u32VolSum >> 3));	
				//查表计算SOC
				for(i=0; i<TableNum; i++)
				{
					if(battery_VoltageQuantity[i][0] <= u16MaxVoltage)
					{
						batteryRemainQ = batteryTotalQ - battery_VoltageQuantity[i][1]; //A*s
						break;
					}
					else//电池电压低于最小值
					{
						batteryRemainQ = 0;
					}
				}				 
				return batteryRemainQ * 100 / batteryTotalQ;
			}
		}		
	}
	else	//初始化后根据电压平均值计算百分比
	{
	  Result_Pre = SOC_Old;
		Result = Result_Pre;
		
		if(p_Battery_Info->BFlagCalSoc == TRUE)
		{
			//后面取12个值的最大值		
			p_Battery_Info->u16BusAverageVol[u8LoopCnt++] = (uint16_t)(p_Battery_Info->u32TotalBusVoltage / 15000);
			if(u8LoopCnt >= 12)
			{
				u16MaxVoltage = (Vol_Design == 24) ? GetMaxData(p_Battery_Info->u16BusAverageVol, 12) * 10 / 7
																					 : ((Vol_Design == 48) ? GetMaxData(p_Battery_Info->u16BusAverageVol, 12) * 10 / 13
																																 : GetMaxData(p_Battery_Info->u16BusAverageVol, 12));				
				u8LoopCnt = 0;
			}			
			
			//查表计算SOC
			for(i=0; i<TableNum; i++)
			{
				if(battery_VoltageQuantity[i][0] <= u16MaxVoltage)
				{
					batteryRemainQ = batteryTotalQ - battery_VoltageQuantity[i][1]; //A*s
					break;
				}
				else//电池电压低于最小值
				{
					batteryRemainQ = 0;
				}
			}
			Result = batteryRemainQ * 100 / batteryTotalQ;
			
			//电量上升时，突变较小，取上次计算值
			if(Result > Result_Pre)
			{
				if((Result < (Result_Pre + 30)) && (Result < 90))
				{
					Result = Result_Pre;
				}
			}
			
			p_Battery_Info->u32TotalBusVoltage = 0;
			p_Battery_Info->BFlagCalSoc = FALSE;
		}
		
		return Result;
	}
}