TLE5012的速度计算

只看该作者 · 2025-3-25 16:08

首先阅读用户手册，查看到角速度计算算式如下图：

第14bit是方向，低14位[13:0]是速度数据，去掉高两位，使用低14位计算速度，发现磁铁静止时计算出的速度值在10左右和16374左右变化，磁铁旋转时只有一个方向的速度正确，另一个方向计算出的速度完全不对，于是查找资料，查到了代码资料：https://github.com/Infineon/TLE5012-Magnetic-Angle-Sensor
相关代码：
errorTypes Tle5012b::getAngleSpeed(double &finalAngleSpeed, int16_t &rawSpeed, updTypes upd, safetyTypes safe)
{
int8_t numOfData = 0x6;
uint16_t rawData[numOfData] = {};

errorTypes status = readMoreRegisters(reg.REG_ASPD + numOfData, rawData, upd, safe);
if (status != NO_ERROR)
{
return (status);
}

// Prepare raw speed
rawSpeed = rawData[0];
rawSpeed = (rawSpeed & (DELETE_BIT_15));
// check if the value received is positive or negative
if (rawSpeed & CHECK_BIT_14)
{
rawSpeed = rawSpeed - CHANGE_UINT_TO_INT_15;
}

// Prepare firMDVal
uint16_t firMDVal = rawData[3];
firMDVal >>= 14;

// Prepare intMode2Prediction
uint16_t intMode2Prediction = rawData[5];
if (intMode2Prediction & 0x0004)
{
intMode2Prediction = 3;
}else{
intMode2Prediction = 2;
}

// Prepare angle range
uint16_t rawAngleRange = rawData[5];
rawAngleRange &= GET_BIT_14_4;
rawAngleRange >>= 4;
double angleRange = ANGLE_360_VAL * (POW_2_7 / (double) (rawAngleRange));

//checks the value of fir_MD according to which the value in the calculation of the speed will be determined
//according to if prediction is enabled then, the formula for speed changes
finalAngleSpeed = calculateAngleSpeed(angleRange, rawSpeed, firMDVal, intMode2Prediction);
return (status);
}

发现里面的rawSpeed根据方向不同，赋值不一样，也就是说正转（15位是0）时速度值（低14位[13:0]）是从小到大，但是反转（15位是1）时速度值是从大到小，所以当第15位是1时，速度需要使用最大值减去比如使用0x100 - 速度值，也可以像上述源码中一样带着正负号进行运算：rawSpeed = rawSpeed - CHANGE_UINT_TO_INT_15;但是用户手册的公式中并未提到反转时的计算方法，踩坑了。