motor_ctl-3/User/Src/torque_sensor.c

#include "torque_sensor.h"
#include "stdlib.h"
#include "string.h"
#include "math_tools.h"
#include "eeprom_24c02.h"
#include "log_save.h"

//全局变量定义
TorqueOffSetData_Struct_t TorqueOffSetData;
TrueOrFalse_Flag_Struct_t IsTorqueOffSetUpdateEnable = FALSE;

/**************************局部函数定义*************************/


/**************************全局函数定义*************************/
//获取默认零点偏移值
void TorqueOffSetDefaultData_Init(TorqueOffSetData_Struct_t* p_TorqueOffSetData, uint16_t AdcData)
{
	uint8_t i;
	
	for(i=0; i<sizeof(TorqueOffSetData_Struct_t) / 2; i++)
	{
	  memcpy((uint16_t*)(&p_TorqueOffSetData->Data[0] + i), (uint16_t*)&AdcData, 2);
	}
}

//零点偏移历史值处理值
void TorqueOffSetData_Process(TorqueOffSetData_Struct_t* p_TorqueOffSetData, uint16_t AdcData)
{
	int16_t DiffData, TempData;
	uint8_t i, ArraySize;
	
	//更新零点值，运行中动态更新
	p_TorqueOffSetData->PresentData = AdcData;
	
	//历史缓存大小
	ArraySize = sizeof(p_TorqueOffSetData->Data) / 2;
	//历史缓存数据平均值和当此数据差值
	DiffData = GetAverageData(p_TorqueOffSetData->Data, ArraySize) - AdcData;
		
	//考虑力矩传感器零偏±20%的蠕变，零偏允许变化范围为：
	//±0.5mV/V * 20% * 5V * 125 = ±62.5mV
	//对应的ADC转换结果允许变化范围为：0.1mV/V * 5V * 125 / 3300mV * 4095 = 78
	if((DiffData > -120) && (DiffData < 120))//本次开机零偏相对历史数据变化较小
	{
		//缓存队列移位 
		for(i=0; i<ArraySize - 1; i++)
		{
		  p_TorqueOffSetData->Data[i] = p_TorqueOffSetData->Data[i+1];
		}
		p_TorqueOffSetData->Data[ArraySize - 1] = AdcData;
		//获取最大值
		TempData = GetMaxData(p_TorqueOffSetData->Data, ArraySize);
		if(TempData > p_TorqueOffSetData->MaxData)
		{
		  p_TorqueOffSetData->MaxData = TempData;
		}
		//获取最小值
		TempData = GetMinData(p_TorqueOffSetData->Data, ArraySize);
		if(TempData < p_TorqueOffSetData->MinData)
		{
		  p_TorqueOffSetData->MinData = TempData;
		}
		//数据存储
		SaveParamToEEprom_24C02(&I2C_Handle_EEPROM, EEPROM_24C02_ADDR_TORQUE_OFFSET, sizeof(TorqueOffSetData), (uint8_t*)TorqueOffSetData.Data);
	}	
	else
	{
	  IsTorqueOffSetUpdateEnable = TRUE;
	}
}

//零点偏移值动态更新
void TorqueOffSetData_Present_Update(uint16_t* PresentData, uint16_t AdcData, uint8_t Cal_K, MC_ErrorCode_Struct_t* p_MC_ErrorCode)
{
  static uint16_t Array[50];
	static TrueOrFalse_Flag_Struct_t IsArrayFullFalg = FALSE;
	static uint8_t i = 0;
	uint16_t TempData;
	static uint32_t ErrorDelayTimeCnt = 0;
	static uint32_t JudgeDelayTimeCnt = 0;
	
	//刚开机时，数组未存满
	if(IsArrayFullFalg == FALSE)
	{
	  Array[i++] = AdcData;
		if(i >= 50)
		{
		  IsArrayFullFalg = TRUE;
			i = 0;
		}
	}
	//存满后取滑动均值
	else
	{
	  TempData = MovingAverageFilter(AdcData, Array, sizeof(Array) / 2);
		if(TempData < *PresentData)
		{
			if((HAL_GetTick() - JudgeDelayTimeCnt) > 1000)
			{
		  	//更新零点值
			  *PresentData = TempData;
			}
		}
		else
		{
		  JudgeDelayTimeCnt = HAL_GetTick();
		}
	}
	//判断零点是否超出范围
	//零位上限计算方法：(4096 - 零位AD) / 标定系数 >= 2100，控制的有效空间是2100
	if(*PresentData <= (4096 - (2100 * Cal_K / 100)))
	{
	  ErrorDelayTimeCnt = HAL_GetTick();		
	}
	if((HAL_GetTick() - ErrorDelayTimeCnt) > 10000)
	{
	  p_MC_ErrorCode->ERROR_Bit.Fault_TorqueSensor = 1;
		//记录故障日志
		MC_ErrorLogSaveInfo.NotesInfo1 = 3;
		MC_ErrorLogSaveInfo.NotesInfo2 = *PresentData;
		ErrorLogSave_Update(&MC_ErrorLogSaveInfo);
		IsErrorLogSaveInfoUpdateFlag = TRUE;
		//存储故障次数
		MC_RunLog1.TQS_FaultCnt++;
		RunLogSaveIndex = 1;
	}
	
}


//力矩传感器两点校正
uint16_t TorqueSensorTwoPointCalibrate(uint16_t torque_AD, uint16_t torqueOffSet, MC_TorqueCorrectParam_Struct_t* torqueCorrectParam_Temp)
{
	int16_t torqueData;
  uint16_t	torqueCorrectParam_StarData, torqueCorrectParam_StopData, torqueResult;
	
	torqueData = torque_AD - torqueOffSet;
	torqueCorrectParam_StarData = torqueCorrectParam_Temp->StarData * torqueCorrectParam_Temp->K /100;
	torqueCorrectParam_StopData = torqueCorrectParam_Temp->K * 21;
	if(torqueData < 0)
	{
		torqueResult = 0;
	}
	else if(torqueData < torqueCorrectParam_StarData)
	{
		torqueResult= torqueData*364/torqueCorrectParam_StarData;
	}
	else
	{
		torqueResult = 364 + (torqueData-torqueCorrectParam_StarData)*1736/(torqueCorrectParam_StopData-torqueCorrectParam_StarData);
	}
	
	return torqueResult;
}