回复: 112

超高速的反正切算法，纯整数运算

(132093786)

出0入0汤圆

电梯直达

发表于 2017-3-14 14:52:38

|

只看该作者

|倒序浏览

|阅读模式

本帖最后由 s1j2h3 于 2017-3-14 21:02 编辑

加入这个论坛，大部时间都在潜水，今天刚完成一个纯整数乘加运算的快速反正切算法，用于加速度传感器解析倾角。

大家评价下，看能否再优化

解析出的为放大了1600倍的角度值，右移四位就是精度为0.01度的角度值，范围为0~360度

编译完成的整个代码不到500字节，大家可以用IAR FOR STM8验证一下。

const static INT16U __tanz[] =

{

32767        ,

16384        ,

8192        ,

4096        ,

2048        ,

1024        ,

512        ,

256        ,

128        ,

64        ,

32        ,

16        ,

8        ,

4        ,

2        ,

1        ,

};

const static INT32U __angle[] =

{

72000        ,

42504        ,

22458        ,

11400        ,

5722        ,

2864        ,

1432        ,

716        ,

358        ,

179        ,

90        ,

45        ,

22        ,

11        ,

6        ,

3        ,

};

static INT32U __atan_core(INT16U tan)

{

INT32S  x,y;

INT32S  xt,yt,t;

INT16U *ptan;

INT32U *pang;

INT32U  m,ang;

x = 65535;

y = tan;

ptan = (INT16U *)&__tanz;

pang = (INT32U *)&__angle;

m    =  0;

for(INT8U i=0; i<16; i++)

{

t   = *ptan++;

ang = *pang++;

yt  = y*t;

xt  = x*t;

yt >>= 15;

xt >>= 15;

if(y>0)

{

x  += yt;

y  -= xt;

m  += ang;

}

else

{

x  -= yt;

y  += xt;

m  -= ang;

}

}

return m;

}

INT32U __FastIntAtan2__(INT16S x, INT16S y)

{

INT8U   flag = 0;

INT32U  angle;

if(x<0)

{

x = -x;

flag |= 0x01;

}

if(y<0)

{

y = -y;

flag |= 0x02;

}

if(y>x)

{

INT16S t;

t = y;

y = x;

x = t;

flag |= 0x04;

}

{

INT32U m = y;

m *= 65535;

m /= (INT16U)x;

angle = __atan_core(m);

}

if(flag & 0x04)

{

angle = 144000 - angle;

}

if(flag & 0x01)

{

angle = 288000 - angle;

}

if(flag & 0x02)

{

angle = 576000 - angle;

}

return angle;

}

(132091839)

出0入0汤圆

发表于 2017-3-14 15:25:05

|

只看该作者

恩，这种算法类的东西还是要支持下的。有价值

(132091543)

出240入221汤圆

发表于 2017-3-14 15:30:01

|

只看该作者

标记一下，说不定哪天就用上了！谢谢

(132090740)

出10入14汤圆

发表于 2017-3-14 15:43:24

来自手机

|

只看该作者

y = 3.5714 * x / (x * x + 3.5714)

(132089561)

出0入0汤圆

楼主|

发表于 2017-3-14 16:03:03

|

只看该作者

你这个是不错的近似算法；

如果是STM32或上位机都可以；

但相对于8位单片机来说，有以下不利之处：

1. 需要使用浮点运算，速度与空间上都不利；

2. 精度不够，在360度范围内，最大误差有近0.3度，而我的是<0.01度

当然，算法并没有对错，只是应用场合不同。我的是用于STM8S003上的，

空间不够，速度还有要求，所以不得不做了这个整数算法

(132088159)

出0入0汤圆

发表于 2017-3-14 16:26:25

|

只看该作者

本帖最后由 xiaolaba 于 2017-3-14 16:28 编辑

沒有詳細讀碼

請問是不是類似CORDIC的方法

(132087951)

出0入0汤圆

楼主|

发表于 2017-3-14 16:29:53

|

只看该作者

是的，就是根据CORDIC算法改写的

当前离线

精华汤圆VIP

{*}

(132087317)

出0入0汤圆

发表于 2017-3-14 16:40:27

|

只看该作者

标记学习

(132086345)

出0入0汤圆

发表于 2017-3-14 16:56:39

来自手机

|

只看该作者

支持！下载收藏。

(132083658)

出0入0汤圆

发表于 2017-3-14 17:41:26

|

只看该作者

有用的东西还是要顶一下

(132082818)

出0入0汤圆

发表于 2017-3-14 17:55:26

|

只看该作者

(132067701)

出0入0汤圆

发表于 2017-3-14 22:07:23

|

只看该作者

LZ厉害，很久之前也研究过cordic。。

(132028023)

出0入26汤圆

发表于 2017-3-15 09:08:41

|

只看该作者

标记一下

(132027758)

出0入0汤圆

楼主|

发表于 2017-3-15 09:13:06

|

只看该作者

本帖最后由 s1j2h3 于 2017-3-15 09:22 编辑

源代码都出来了，还需说明？

原理网上都有的啊

况且这个代码不大，原理也很简单啊

(132027418)

出0入0汤圆

发表于 2017-3-15 09:18:46

|

只看该作者

收藏了，这个很有价值，谢谢楼主分享！

(132019360)

出0入0汤圆

发表于 2017-3-15 11:33:04

|

只看该作者

收藏了，多谢楼主分享

(132018902)

出0入0汤圆

发表于 2017-3-15 11:40:42

|

只看该作者

收藏一下，谢谢楼主，不过现在产品都转到A8平台了

(132018307)

出0入0汤圆

发表于 2017-3-15 11:50:37

|

只看该作者

其实看不明白 "右移四位就是精度为0.01度的角度值"

或者漏眼了

(132017207)

出0入0汤圆

发表于 2017-3-15 12:08:57

|

只看该作者

大概是说，输出角度除以1600才是真实值，右移4位就是除以16 等于是原来的值只放大了100倍，自然就是精确到0.01了。

(132017043)

出0入0汤圆

楼主|

发表于 2017-3-15 12:11:41

|

只看该作者

开头就有说明，解析出的角度放大了1600倍，右移4位就是除了16，就成了放大100倍，当然就是输出精度(更准确的说法是解析率)为0.01度了

(132016905)

出0入0汤圆

发表于 2017-3-15 12:13:59

|

只看该作者

所以角度表格是45度的1600倍开始然后半分的吗?

看来又不像

const static INT32U __angle[] =

{72000,42504,22458,11400,5722,2864,1432,716,358,179,90,45,22,11,6,3};

(132016795)

出0入0汤圆

发表于 2017-3-15 12:15:49

|

只看该作者

很不错啊，要是能解释下就更好了

(132016672)

出0入0汤圆

楼主|

发表于 2017-3-15 12:17:52

|

只看该作者

本帖最后由 s1j2h3 于 2017-3-15 12:20 编辑

发表于 2017-3-15 12:20:05

|

只看该作者

这个必须mark一下的，必须支持。

(132010604)

出0入0汤圆

发表于 2017-3-15 13:59:00

|

只看该作者

楼主的开源思想还是应该给个赞的！

(132004941)

出0入0汤圆

发表于 2017-3-15 15:33:23

|

只看该作者

标记下，不同情况下的平衡的算法还是比较有意义的

(132004504)

出0入0汤圆

发表于 2017-3-15 15:40:40

|

只看该作者

好，支持下，留待下次使用

积分

精华汤圆VIP

{*}

(132002956)

出0入0汤圆

发表于 2017-3-15 16:06:28

|

只看该作者

支持！

(132002271)

出0入0汤圆

楼主|

发表于 2017-3-15 16:17:53

|

只看该作者

其实这个算法还可以优化的，甚至不需乘法，仅用加减和移位就能完成。

这样效率能赶上16位甚至32位机了吧(在同样的运行频率上)

但，为何我不能对代码进行编辑了？？？

(131995258)

出0入0汤圆

发表于 2017-3-15 18:14:46

|

只看该作者

(131991683)

出0入0汤圆

发表于 2017-3-15 19:14:21

|

只看该作者

收藏了,多谢楼主分享.

(131945288)

出0入4汤圆

发表于 2017-3-16 08:07:36

来自手机

|

只看该作者

正好弄得着，好思路

(131938754)

出0入0汤圆

楼主|

发表于 2017-3-16 09:56:30

|

只看该作者

其实思路是一样的。不过我上面的代码只是稍微变化了点，更适合于我的应用而已。

而且，整数应用是没法做出通用版的。

最终精度是以0.1度，0.01度，还是0.001度为好？

应用不同，需要也不同。

(131937928)

出0入0汤圆

发表于 2017-3-16 10:10:16

来自手机

|

只看该作者

非常不错的思路

(131935262)

出0入0汤圆

发表于 2017-3-16 10:54:42

|

只看该作者

換個方向想

如果1600倍就是SCALAR, 那單位圓的半徑為1

tan (angle) = y / 1 = ya / (1600+1)

因為 1600 >>> 1

則大約的 ya / y = 1600 或 ya / y =1601

這樣讀值可不可以看成 (0- 360) X scalar, 而不需要關心0,0xx 或移位乘除呢, 限制就在16BIT或32BIT, 這樣做可以通用嗎

(131933326)

出0入0汤圆

楼主|

发表于 2017-3-16 11:26:58

|

只看该作者

这个，好像不行

预算好的正切值也需支持到相应的精度，否则光比例缩放是没有用的。

精度的大小也同样对应于查找的次数。

按你的想法，最好的就是提供足够的精度与足够的查找次数，而对于0.1度精度就能满足要求的需要求来说，你提供了0.001度的精度，那就多浪费了约7次的查找过程。

如果你不在乎这7次处理时间，那差不多也不在乎存储空间，那直接调用浮点库函数就行了

(131919720)

出0入0汤圆

发表于 2017-3-16 15:13:44

|

只看该作者

不错，收藏一下！

(131834424)

出0入0汤圆

楼主|

发表于 2017-3-17 14:55:20

|

只看该作者

唉，没有研究算法的吗？

后续还有SIN,COS,TAN,ASIN,ACOS等衍生算法的。只用加减与移位就能完成的

(131826011)

出0入0汤圆

发表于 2017-3-17 17:15:33

|

只看该作者

个人觉得改成+-Pi 范围的Q15输出，而不是固定1600倍会更通用一点

(131819399)

出0入0汤圆

发表于 2017-3-17 19:05:45

|

只看该作者

好东西，必须点赞。

(131780260)

出0入0汤圆

发表于 2017-3-18 05:58:04

|

只看该作者

非常好的资料，感谢楼主分享。

(131778312)

出0入0汤圆

发表于 2017-3-18 06:30:32

来自手机

|

只看该作者

感谢楼主提供算法优化方案

发表于 2017-3-19 15:10:44

|

只看该作者

记号一下，以后会用到，感谢楼主分享！

当前离线

精华汤圆VIP

{*}

(131656950)

出0入170汤圆

发表于 2017-3-19 16:13:14

|

只看该作者

学习学习

当前离线积分

精华汤圆注册会员

{*}

(131567179)

出0入0汤圆

发表于 2017-3-20 17:09:25

|

只看该作者

有价值，收藏下！

(131565932)

出0入0汤圆

发表于 2017-3-20 17:30:12

|

只看该作者

给楼主打赏了50莫元。

发帖发的太及时了，刚好碰到这个问题，已经用上了，用来解析线性霍尔的交流伺服电机的绝对位置，很不错。

拜谢楼主。

精华汤圆VIP

{*}

(131562537)

出0入0汤圆

发表于 2017-3-20 18:26:47

|

只看该作者

收藏了，谢谢楼主分享

发表于 2017-3-20 18:28:54

来自手机

|

只看该作者

s1j2h3 发表于 2017-3-17 14:55

唉，没有研究算法的吗？

后续还有SIN,COS,TAN,ASIN,ACOS等衍生算法的。只用加减与移位就能完成的 ...

你告诉大家cordic，大家自己就会玩了

发表于 2017-3-20 18:38:38

|

只看该作者

非常好的资料，感谢楼主分享

(131558850)

出0入0汤圆

楼主|

发表于 2017-3-20 19:28:14

|

只看该作者

感谢你的理解与支持

(131558074)

出0入0汤圆

发表于 2017-3-20 19:41:10

|

只看该作者

顶一个，用在姿态解算上？

当前离线

精华汤圆VIP

{*}

(131464747)

出0入0汤圆

发表于 2017-3-21 21:36:37

|

只看该作者

有些单片机可以用

(131462609)

出0入0汤圆

发表于 2017-3-21 22:12:15

来自手机

|

只看该作者

非常好的资料，感谢楼主分享!

(131280852)

出0入0汤圆

发表于 2017-3-24 00:41:32

|

只看该作者

没细看，看前面两个表就感觉是cordic算法

(131280825)

出0入0汤圆

发表于 2017-3-24 00:41:59

|

只看该作者

没细看，看前面两个表就感觉是cordic算法

(131254510)

出0入0汤圆

发表于 2017-3-24 08:00:34

来自手机

|

只看该作者

用在什么领域？

(131247570)

出0入0汤圆

楼主|

发表于 2017-3-24 09:56:14

|

只看该作者

其基本原理就是CORDIC算法。不过少部分人不知如何变换成整数运算，所以在此献丑了。

这样，低端的8位单片机也能跑出16位，甚至32位单片机的性能来了。

至于用途，频谱分析，波形输出，三角计算，图形处理等都会用到

积分

精华汤圆VIP

{*}

(131244431)

出0入0汤圆

发表于 2017-3-24 10:48:33

来自手机

|

只看该作者

实用，感谢分享

(131146254)

出0入0汤圆

发表于 2017-3-25 14:04:50

|

只看该作者

看STM32的无感FOC 也用到cordic算法。

(131137056)

出0入0汤圆

发表于 2017-3-25 16:38:08

|

只看该作者

标记学习

(131136741)

出0入0汤圆

发表于 2017-3-25 16:43:23

|

只看该作者

支持！标记收藏。

(131135704)

出0入0汤圆

发表于 2017-3-25 17:00:40

|

只看该作者

赞一个。收藏了

(130983157)

出0入0汤圆

发表于 2017-3-27 11:23:07

|

只看该作者

收藏了，多谢楼主分享

(130811945)

出0入0汤圆

发表于 2017-3-29 10:56:39

|

只看该作者

谢谢楼主！

(130117561)

出0入8汤圆

发表于 2017-4-6 11:49:43

|

只看该作者

楼主，我在STM32F301上实现你的算法，算一次要700个CLOCK啊，这个也太慢了吧，我用ARM_DSP库，算一次sin或是cos都是70多个CLOCK，楼主有没有更快的算法实现？

(130114636)

出0入0汤圆

发表于 2017-4-6 12:38:28

|

只看该作者

好东西，收藏了

(130111821)

出10入14汤圆

发表于 2017-4-6 13:25:23

来自手机

|

只看该作者

mtswz.213 发表于 2017-4-6 11:49

楼主，我在STM32F301上实现你的算法，算一次要700个CLOCK啊，这个也太慢了吧，我用ARM_DSP库，算一次sin或 ...

301带浮点运算单元吧？不带这么欺负楼主的

(130110805)

出0入0汤圆

楼主|

发表于 2017-4-6 13:42:19

|

只看该作者

本帖最后由 s1j2h3 于 2017-4-6 13:44 编辑

楼上说出了我的心声。

这个算法适合于低端单片机，如C51,STM8等。

还有，上面的算法是可以再优化的

我刚出道时的总工对我说过的一句话我还是记忆犹新：用好东西做出好东西不算本事，用蹩脚东西也能做出好东西，才算本事。

这个算法就是对这句话的很好诠释。

当然，光这一个算法也不能媲美高端芯片，如加上好的框架，系统工程与设计理念，就能达到了

(130110297)

出0入0汤圆

发表于 2017-4-6 13:50:47

|

只看该作者

我也标记一下，感谢楼主分享

(130104761)

出0入8汤圆

发表于 2017-4-6 15:23:03

|

只看该作者

(130104595)

出10入14汤圆

发表于 2017-4-6 15:25:49

来自手机

|

只看该作者

mtswz.213 发表于 2017-4-6 15:23

我用301浮点运算，算反正切，要3000多个机器周期啊！

开了fpu么？

(130100200)

出0入0汤圆

发表于 2017-4-6 16:39:04

|

只看该作者

我也标记一下，感谢楼主分享

(130098938)

出0入8汤圆

发表于 2017-4-6 17:00:06

来自手机

|

只看该作者

有没有定点计算反正切，快一点的函数，最好是q格式的，我这边做电机控制需要

(130097541)

出0入0汤圆

发表于 2017-4-6 17:23:23

|

只看该作者

MARK，琢磨琢磨

(130037923)

出0入0汤圆

楼主|

发表于 2017-4-7 09:57:01

|

只看该作者

这个算法就是定点运算的

当前离线

精华汤圆注册会员

{*}

(130035266)

出0入0汤圆

发表于 2017-4-7 10:41:18

|

只看该作者

好吧必须顶一下，电角度计算需要用到

(130031896)

出0入8汤圆

发表于 2017-4-7 11:37:28

|

只看该作者

发表于 2017-6-23 14:20:07

|

只看该作者

感谢楼主分享。

不过我在STC的51上面测试， 反而比标准库函数 atan2要慢一倍以上。

测试平台：

MCU: STC15W408AS， KEIL C51 V9.52， 开9级优化， 常量表放在xdata

测试结果：

标准数学函数atan2()

t=8066, time=0.729347ms

angle = 46.047980

网友算法测试:

t=20427, time=1.847059ms

angle = 460

(123290503)

出0入0汤圆

发表于 2017-6-24 12:14:01

|

只看该作者

留名，有用………………

(81907609)

出0入0汤圆

发表于 2018-10-16 11:28:55

|

只看该作者

我测试了一下，这个速度还是不够快，我在dspic33ep上测试，耗时大约231us。而我使用查表做的耗时29us，使用汇编做的，耗时间6us。

(81787598)

出0入0汤圆

发表于 2018-10-17 20:49:06

来自手机

|

只看该作者

zddvszw 发表于 2018-10-16 11:28

我测试了一下，这个速度还是不够快，我在dspic33ep上测试，耗时大约231us。而我使用查表做的耗时29us，使用 ...

查表空间多大

(81786669)

出0入0汤圆

发表于 2018-10-17 21:04:35

|

只看该作者

表有多大？查找算法能不能介绍一下？

发表于 2018-10-18 09:38:10

|

只看该作者

标记下，以后可能真的能用到

当前离线

精华汤圆VIP＋＋

{*}

(81740503)

出0入85汤圆

发表于 2018-10-18 09:54:01

|

只看该作者

好东西，标记一下。

当前离线

精华汤圆VIP＋＋

{*}

(81739818)

出0入85汤圆

发表于 2018-10-18 10:05:26

|

只看该作者

标准库函数 atan2有没有源码？贴出来看看。

(81734375)

出0入0汤圆

发表于 2018-10-18 11:36:09

|

只看该作者

标记备用

(81733768)

出0入0汤圆

发表于 2018-10-18 11:46:16

|

只看该作者

收藏了，多谢楼主分享

(81732726)

出0入0汤圆

发表于 2018-10-18 12:03:38

|

只看该作者

不错不错，收藏备用。

(81729184)

出0入0汤圆

发表于 2018-10-18 13:02:40

来自手机

|

只看该作者

标记备用，多谢

(81727766)

出0入0汤圆

发表于 2018-10-18 13:26:18

|

只看该作者

mark，谢谢楼主

(81722031)

出0入4汤圆

发表于 2018-10-18 15:01:53

|

只看该作者

了解一下，谢谢分享

当前离线

精华汤圆中级会员

{*}

(81719804)

出0入0汤圆

发表于 2018-10-18 15:39:00

|

只看该作者

看了文章都看懂了！！！！！！！！11

当前离线

精华汤圆中级会员

{*}

(81715105)

出0入0汤圆

发表于 2018-10-18 16:57:19

|

只看该作者

if(y>0)

{

x  += yt;

y  -= xt;

m  += ang;

}

else

{

x  -= yt;

y  += xt;

m  -= ang;

}

楼主这段为何判断Y 决定是正转还是反转呢？  不是该判断角度大于目标还是小于目标吗？

(81708186)

出20入0汤圆

发表于 2018-10-18 18:52:38

|

只看该作者

感谢楼主分享

发表于 2018-11-3 15:15:59

来自手机

|

只看该作者

Mark^O^

(80316549)

出0入0汤圆

发表于 2018-11-3 21:26:35

|

只看该作者

支持！下载收藏

发表于 2018-11-5 09:46:52

|

只看该作者

MARK

(80158630)

出0入0汤圆

发表于 2018-11-5 17:18:34

|

只看该作者

支持！下载收藏。

(80142591)

出0入4汤圆

发表于 2018-11-5 21:45:53

来自手机

|

只看该作者

crod的方法！不错