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

//全局变量定义
TorqueSensor_Param_Struct_t TorqueSensor_1_Param;//传感器1参数
TorqueSensor_Param_Struct_t TorqueSensor_2_Param;//传感器2参数
TorqueSensor_Param_Struct_t TorqueSensor_3_Param;//传感器3参数

TorsueSensorFaultStatus_Struct_t TorsueSensorFaultStatus;//3个传感器的故障状态

uint16_t TorqueSensorStartData = TORQUE_SENSOR_STARTDATA;//启动值
TrueOrFalse_Flag_Struct_t IsTorqueOffSetUpdateEnable = FALSE;//运行中零点值动态更新标志
TorqueSensor_LinearCorrectionParam_Struct_t TorqueSensor_LinearCorrectionParam[15];//线性校正参数

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

/**************************全局函数定义*************************/
//力矩传感器参数设为默认值
void TorqueSensor_ParamSetDefaultData_Init(TorqueSensor_Param_Struct_t* p_TorqueSensor_Param, uint16_t AdcData)
{
	uint8_t i;
	
	//校正系数设为1
	p_TorqueSensor_Param->Torque_Cal_K = 100;
	
	//零点初始值	
	for(i=0; i<sizeof(TorqueOffSetData_Struct_t) / 2; i++)
	{
	  memcpy((uint16_t*)(&p_TorqueSensor_Param->Torque_OffSetData.Data[0] + i), (uint16_t*)&AdcData, 2);
	}
	
	//线性校正参数初始化为默认值，间隔10N.m取值
	for(i=0; i<7; i++)
	{
	  TorqueSensor_LinearCorrectionParam[i].Torque_Measure = 10 * (i + 1) * 28;
		TorqueSensor_LinearCorrectionParam[i].Torque_Load = 10 * (i + 1);
		TorqueSensor_LinearCorrectionParam[i].Correction_K = 128;//系数放大128倍
	}
	for(i=7; i<15; i++)   //兼容间隔5N.m取值，后半部分数据进行非零填充，防止计算时出错
	{
	  TorqueSensor_LinearCorrectionParam[i].Torque_Measure = 0xFFFF;
		TorqueSensor_LinearCorrectionParam[i].Torque_Load = 80;
		TorqueSensor_LinearCorrectionParam[i].Correction_K = 128;//系数放大128倍
	}
}

//零点偏移历史值处理值
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;
		}
		//数据存储，根据参数确定存储地址
		if(p_TorqueOffSetData == &TorqueSensor_1_Param.Torque_OffSetData)
		{
			SaveParamToEEprom_24C02(&I2C_Handle_EEPROM, EEPROM_24C02_ADDR_TORQUE_OFFSET_1, sizeof(TorqueOffSetData_Struct_t), (uint8_t*)p_TorqueOffSetData->Data);
		}
		else if(p_TorqueOffSetData == &TorqueSensor_2_Param.Torque_OffSetData)
		{
			SaveParamToEEprom_24C02(&I2C_Handle_EEPROM, EEPROM_24C02_ADDR_TORQUE_OFFSET_2, sizeof(TorqueOffSetData_Struct_t), (uint8_t*)p_TorqueOffSetData->Data);
		}
		else if(p_TorqueOffSetData == &TorqueSensor_3_Param.Torque_OffSetData)
		{
			SaveParamToEEprom_24C02(&I2C_Handle_EEPROM, EEPROM_24C02_ADDR_TORQUE_OFFSET_3, sizeof(TorqueOffSetData_Struct_t), (uint8_t*)p_TorqueOffSetData->Data);
		}
	}	
	else
	{
	  IsTorqueOffSetUpdateEnable = TRUE;
	}
}

//零点偏移值动态更新
void TorqueOffSetData_Present_Update1(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)
	{
	  TorsueSensorFaultStatus.FaultBit.Sensor1 = 1;	
	}
}

//零点偏移值动态更新
void TorqueOffSetData_Present_Update2(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)
	{
	  TorsueSensorFaultStatus.FaultBit.Sensor2 = 1;
	}
}

//零点偏移值动态更新
void TorqueOffSetData_Present_Update3(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)
	{
	  TorsueSensorFaultStatus.FaultBit.Sensor3 = 1;
	}
}