一. 簡(jiǎn)介

由于在項(xiàng)目中需要使用的MPU6050，進(jìn)行姿態(tài)解算，計(jì)算中設(shè)計(jì)到arctan 和 sqr(x*2 + y * 2),這兩部分的計(jì)算，在了解了一番之后，發(fā)現(xiàn)Cordic算法可以很方便的一次性求出這兩個(gè)這兩部分的計(jì)算。另外也可以一次性求出sin和cos的值。另外該算法還可以計(jì)算其他的一些公式(沒(méi)做過(guò)多的了解)。

二.Cordic算法

該算法的核心實(shí)現(xiàn)就是旋轉(zhuǎn)逼近，每次旋轉(zhuǎn)一定的角度，無(wú)限的逼近所給定的角度值。

1. 理論基礎(chǔ)

首先有向量P0，現(xiàn)在要將其旋轉(zhuǎn)θ角度，到Pm。那么Pm的坐標(biāo)值如下

xm = x0cosθ - y0sinθ = cosθ(x0 – y0tanθ)

ym = x0sinθ + y0cosθ = cosθ(y0 + x0tanθ)

P0和Pm均在單位圓上，另外假設(shè)現(xiàn)在P0在X軸上，即 X0 = 1，y0 = 0。上式就可以變?yōu)槿缦嘛@示

xm = x0cosθ - y0sinθ = cosθ

ym = x0sinθ + y0cosθ = sinθ

可以看到Pm的坐標(biāo)值，就是sinθ 和 cosθ的值。這就是理論基礎(chǔ)。

2. sinθ 和 cosθ 算法實(shí)現(xiàn)

有了理論支持后，我們只需要求解Pm的坐標(biāo)即可。直接旋轉(zhuǎn)θ不太可能，但是我們可以每次旋轉(zhuǎn)特定的角度θi (tanθi = 1/2^i),讓我們的角度值逼近θ即可。于是就有了如下迭代公式。

x(i+1) = cosθi* (xi – yi * tanθi)

y(i+1) = cosθi * (yi + xi * tanθi)

θ(i+1) = θi (+-) dθi

如果當(dāng)前角度小于設(shè)定角度，那么就加dθ ，大于設(shè)定角度 ， 那么就減dθ。由于每次旋轉(zhuǎn)的dθ，會(huì)越來(lái)越小，所以旋轉(zhuǎn)的當(dāng)前角度會(huì)越來(lái)越來(lái)接近設(shè)定角度。

計(jì)算過(guò)程中 ，cosθi，只充當(dāng)縮放因子，對(duì)旋轉(zhuǎn)方向沒(méi)有影響。可以先在軟件中提取技術(shù)出來(lái)。每次旋轉(zhuǎn)角度值 和 對(duì)應(yīng)的 cos值如下。

3. arctan (x,y)和 sqr(x*2 + y * 2)算法實(shí)現(xiàn)

在求解sinθ 和 cosθ 的時(shí)候，知道，給定一個(gè)角度，按照上述方法就可以求解。現(xiàn)在將其反過(guò)來(lái)，給定sinθ 和 cosθ的值，也就是Pm的坐標(biāo)(可能不在單位圓上，只是模值縮放了)，現(xiàn)在只需要將其旋轉(zhuǎn)到X軸的正半軸上，即Y = 0 ，X > 0的時(shí)候，所旋轉(zhuǎn)過(guò)的角度值即arctan (x,y)。

然后P0的X坐標(biāo)值即sqr(x*2 + y * 2)。旋轉(zhuǎn)過(guò)程中，向量的模值是不會(huì)改變的，而Pm的模值就是sqr(x*2 + y * 2)。

三.Cordic算法實(shí)現(xiàn)

首先將上述角度值，存儲(chǔ)到verilog中，需要進(jìn)行擴(kuò)大處理。由于tanθi = 1/2^i)，所以對(duì)應(yīng)的tanθ也是知道的。在相乘的時(shí)候，只需要將對(duì)應(yīng)的數(shù)右移對(duì)應(yīng)的位數(shù)即可

`define rot0 32'd2949120    //45度*2^16
`define rot1 32'd1740992    //26.5651度*2^16
`define rot2 32'd919872    //14.0362度*2^16
`define rot3 32'd466944    //7.1250度*2^16
`define rot4 32'd234368    //3.5763度*2^16
`define rot5 32'd117312    //1.7899度*2^16
`define rot6 32'd58688     //0.8952度*2^16
`define rot7 32'd29312     //0.4476度*2^16
`define rot8 32'd14656     //0.2238度*2^16
`define rot9 32'd7360     //0.1119度*2^16
`define rot10 32'd3648     //0.0560度*2^16
`define rot11 32'd1856     //0.0280度*2^16
`define rot12 32'd896      //0.0140度*2^16
`define rot13 32'd448      //0.0070度*2^16
`define rot14 32'd256      //0.0035度*2^16
`define rot15 32'd128      //0.0018度*2^16

然后就是迭代過(guò)程了,迭代16次足夠了。最后的Zn和Xn就是想要結(jié)果。

//旋轉(zhuǎn)
genvar i;
generate
  for( i = 1 ;i < 17 ;i = i+1)
 ? begin: loop2
 ? ? ? always@(posedge clk or negedge rst_n)
 ? ? ? begin
 ? ? ? ? ? if( rst_n == 1'b0)
 ? ? ? ? ? begin
 ? ? ? ? ? ? ? Xn[i] <= 'd0;
 ? ? ? ? ? ? ? Yn[i] <= 'd0;
 ? ? ? ? ? ? ? Zn[i] <= 'd0;
 ? ? ? ? ? end
 ? ? ? ? ? else if( cal_delay[i -1] == 1'b1)
 ? ? ? ? ? begin
 ? ? ? ? ? ? ? if( Yn[i-1][31] == 1'b0)
 ? ? ? ? ? ? ? begin
 ? ? ? ? ? ? ? ? ? Xn[i] <= Xn[i-1] + (Yn[i-1] >>> (i-1));
          Yn[i] <= Yn[i-1] - (Xn[i-1] >>> (i-1));
          Zn[i] <= Zn[i-1] + rot[i-1];
 ? ? ? ? ? ? ? end
 ? ? ? ? ? ? ? else
 ? ? ? ? ? ? ? begin
 ? ? ? ? ? ? ? ? ? Xn[i] <= Xn[i-1] - (Yn[i-1] >>> (i-1));
          Yn[i] <= Yn[i-1] + (Xn[i-1] >>> (i-1));
          Zn[i] <= Zn[i-1] - rot[i-1];
 ? ? ? ? ? ? ? end
 ? ? ? ? ? end
 ? ? ? ? ? else
 ? ? ? ? ? begin
 ? ? ? ? ? ? ? Xn[i] <= Xn[i];
 ? ? ? ? ? ? ? Yn[i] <= Yn[i];
 ? ? ? ? ? ? ? Zn[i] <= Zn[i];
 ? ? ? ? ? end
 ? ? ? end
 ? end
endgenerate

這里沒(méi)有乘cosθ，最后的Xn會(huì)比真實(shí)值大1.64倍左右，所以還需要對(duì)其進(jìn)行一個(gè)縮小操作，通過(guò)右移來(lái)近似實(shí)現(xiàn)。

assign cordic_ack = cal_delay[16];
assign theta   = Zn[16];
assign amplitude = (Xn[16] >>> 1) + (Xn[16] >>> 3); ////幅度，偏大1.64倍，這里做了近似處理

然后就是仿真了，給了X=Y=15，也就是角度為45度，幅值21.213，擴(kuò)大65536倍為1,376,256。可以看到結(jié)果近似。

審核編輯：劉清