我浪费了很多天与我的16MHz 8位AVR(大型2560)进行战斗。

目的是标准化我收到的值(加速度计,磁力计等)。
这些值是16位带符号的(int16),在我想要从0.0f-1.0f浮点后
我正在将此用于3d IMU。

常用方法:

int32_t tmp = (int32_t)a*a+b*b+c*c;
float magnitude = sqrt(tmp);
float a_v = a / magnitude;
float b_v = b / magnitude;
float c_v = c / magnitude;


更快的方法:

int32_t tmp = (int32_t)a*a+b*b+c*c;
float imagnitude = InvSqrt(tmp); // like the 'tricky' one for ID software quake source
float a_v = a * imagnitude;
float b_v = b * imagnitude;
float c_v = c * imagnitude;


第二个优点是,它使用近似而不是1 / sqrt(但也存在近似sqrt),并且需要3个乘法而不是除法,这很不错,因为AVR支持MUL但不支持DIV。
另一方面,由于浮点数和32位计算的原因,它还是很慢的。

这样的功能通常需要1-2毫秒,这对我的代码产生了巨大影响,我的代码尝试在一个周期为2.4ms max的周期内解决许多其他任务和规范化问题

我进行了很多研究,尝试了许多不同的近似方法和想法,但是无论我如何尝试,代码的执行速度都太慢了。

也许还有另一种方法可以标准化我的传感器值。

针对有特殊问题(加速度计幅度)的人的更新:
没有浮点和sqrt,我现在正在此工作:(忽略其他强制转换:)
int16 cal []保存3轴的校准加速度计值。

int16 average_sq_1g = CONST_1G / 256;
uint32_t work = (int32_t)((int32_t)cal[0]*cal[0] + (int32_t)cal[1]*cal[1] + (int32_t)cal[2]*cal[2])/256;
work = work * 100L / average_sq_1g;
attitude.acc_magnitude = work;


这对于我的事业来说是非常专业的,因为我正在努力获得加速度的大小,并且我知道我获得的1G值(大约16000)
因此公式(X ^ 2 + Y ^ 2 + Z ^ 2)* 100 / 1G ^ 2向我返回了幅度(100 =无附加加速度,可以不使用浮点数来完成。
我没有检查性能差异,但是应该更快。

最佳答案

使用定点算法。

通过合理数量的位缩放输入并使用整数运算。 (还有用于整数平方根计算的算法)

例如,如果您的值范围从-10m到10m,并且至少需要mm的分辨率,则我会添加11位(缩放为2048)

#define VEC_SHIFT 11
#define VEC_SCALE (1 << (VEC_SHIFT))
int16_t a =  7 * VEC_SCALE;
int16_t b =  3 * VEC_SCALE;
int16_t c = 10 * VEC_SCALE;

// calculations have to be done in larger data type so they do not overflow
int32_t snorm = (int32_t)a * a + (int32_t)b*b + (int32_t)c*c;  // snorm now is scaled by VEC_SCALE*VEC_SCALE (2*VEC_SHIFT)
int16_t norm = intsqrt(snorm); // norm is scaled with VEC_SCALE

// since norm and a,b,c is in VEC_SCALE, you have to scale up the divident so that one VEC_SCALE is chanceled out by division
int16_t as =  (((int32_t)a) * VEC_SCALE )/norm;
int16_t bs =  (((int32_t)b) * VEC_SCALE )/norm;
int16_t cs =  (((int32_t)c) * VEC_SCALE )/norm;

关于c - AVR/嵌入式:以快速方式归一化 vector ?,我们在Stack Overflow上找到一个类似的问题:https://stackoverflow.com/questions/24370511/

10-12 23:14