motor_ctl-3/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"

/*电流积分，最小值，单位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;

/******************************************************************************
* @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;
	uint8_t flagBatteryFull=0;
	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;
		}
		
		/*电流大于最小阈值，累加一次电量*/
		if((RemainDis.I_motor > CURRENT_MIN) && (RemainDis.IsBMS_ComOK_Flag == FALSE) )
		{
			RemainDis.Q_add += RemainDis.I_motor * RemainDis.acc_period;//mA * ms
		}
		
		/*助力档位切换后*/
		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;
					
					/*判断电池是否满充*/
	        if( ( Q_add_tmp == 0 ) && ( Battery_Info.SOC == 100 ) )
					{
						/*当电池满充时，发出的电量信息会是标称值，
						在Battery.SOC==100时不会随电量消耗尔减小*/
						/*置标志位，不计算更新*/
						flagBatteryFull = 1;
					}
				
					/*切换到母线电流积分计算后，使用最新的剩余电量数据*/
					Battery_Info.Q_remain_record = Battery_Info.Q_remain_real;
					Battery_Info.Q_discharged = 0;
					RemainDis.Q_add = 0;
				}
			}
			else/*消耗的电量信息来自母线电流，进行积分累计计算*/
			{
				/*单位mA*h*/
				Q_add_tmp = RemainDis.Q_add / ( 3600 * 1000 );
				if(Q_add_tmp > 0)
				{
					RemainDis.Q_add = RemainDis.Q_add % ( 3600 * 1000 );//保留余数，下次累加
				}
			
				/*累计总的消耗电量*/
				Battery_Info.Q_discharged  += Q_add_tmp ;  //mA*h
				
				/*更新电池剩余电量*/
				Battery_Info.Q_remain_real = Battery_Info.Q_remain_record - Battery_Info.Q_discharged ;
			}
			
			/*电量信息来自电池的计算，需要用到此数据*/
			Battery_Info.Q_remain_real_last = Battery_Info.Q_remain_real;
			
			/*电池非满充状态*/
			if( flagBatteryFull == 0 )
			{
				/*将一定轮圈数消耗的电量，写入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
			}
		}
	}
}

/******************************************************************************
* @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 )
					{
						/*更新平均功耗*/
						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;
}

uint8_t Battery_SocCal(uint16_t Voltage_mV)
{
	
  static uint32_t PeriodTimeCnt = 0;
	static uint8_t Result = 0,Result_Pre=0;
	uint16_t i;
	uint16_t num;
	uint32_t batteryTotalQ,batteryRemainQ;
	uint16_t Voltage;
	
	Voltage = Voltage_mV / 10;
	
	num = sizeof(battery_VoltageQuantity)/4;
	
	batteryTotalQ = battery_VoltageQuantity[num-1][1];
	batteryRemainQ = batteryTotalQ;
	
	Result_Pre = MC_RunInfo.SOC;
	Result = Result_Pre;

	if((HAL_GetTick() - PeriodTimeCnt) > 2000)//计算周期2s
	{
		PeriodTimeCnt = HAL_GetTick();
		for(i=0;i<num;i++)
		{
			if(battery_VoltageQuantity[i][0] <=  Voltage)
			{
				batteryRemainQ = batteryTotalQ - battery_VoltageQuantity[i][1] ;    //A*s
				break;
			}
			else//电池电压低于最小值
			{
				batteryRemainQ = 0;
			}
		}
		Result = batteryRemainQ * 100 / batteryTotalQ;
		if(Result >= Result_Pre)
		{
		  Result = Result_Pre;
		}
	}
	return Result;
}