一. 簡介

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

二.Cordic算法

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

1. 理論基礎

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

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

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

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

xm = x0cosθ - y0sinθ = cosθ

ym = x0sinθ + y0cosθ = sinθ

可以看到Pm的坐標值，就是sinθ 和 cosθ的值。這就是理論基礎。

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

有了理論支持后，我們只需要求解Pm的坐標即可。直接旋轉(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θ ，大于設定角度 ， 那么就減dθ。由于每次旋轉(zhuǎn)的dθ，會越來越小，所以旋轉(zhuǎn)的當前角度會越來越來接近設定角度。

計算過程中 ，cosθi，只充當縮放因子，對旋轉(zhuǎn)方向沒有影響?？梢韵仍谲浖刑崛〖夹g出來。每次旋轉(zhuǎn)角度值 和 對應的 cos值如下。

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

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

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

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

首先將上述角度值，存儲到verilog中，需要進行擴大處理。由于tanθi = 1/2^i)，所以對應的tanθ也是知道的。在相乘的時候，只需要將對應的數(shù)右移對應的位數(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

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

//旋轉(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

這里沒有乘cosθ，最后的Xn會比真實值大1.64倍左右，所以還需要對其進行一個縮小操作，通過右移來近似實現(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，擴大65536倍為1,376,256?？梢钥吹浇Y果近似。

審核編輯：劉清