CORDICアルゴリズム使用した論理回路を作成したので紹介をします。
今回の目標
三角関数（SINθ）を算出する回路をFPGAに実装します。
前提条件
- アルゴリズムについての説明は割愛
- FPGAに落とし込む際の具体的な方法について置き説明します
- 固定小数を使用
- 回路実装が容易な固定小数で実装します
- 最後の除算はしない
- CORDIC演算の最後の処理でX,Y座標を
変換ベクトルで割る処理はしません（除算回路を入れたくないから） - 代替案としてX,Y座標の初期位置（1,1）のベクトル(√2)を、
変換ベクトルで予め割った値を初期値とします。
- CORDIC演算の最後の処理でX,Y座標を
-
精度目標
- 正弦波振動しているデバイスの基準信号(1周期に1トリガ)から、現在の角度を求めたい
- 小数２桁くらいまで正確にもとめたい
-
パイプライン型の回路実装とする
- 入力角度が毎サイクル更新される条件に対応したいため
-
入力値（角度）
- 固定少数で正数9bit,小数10bit
- 正の数のみ
- 360°以上の値を入力した場合は無効値として処理する（出力ENAをネゲートする）
-
出力値
- 16bitの２の補数表現とする
実装前準備
下準備として以下の２つを計算します。
- 角度参照テーブル
- XY座標初期値
角度参照テーブル
底辺がＷ、高さＨの直角三角形を用意し、角度をθとします。
以下のとおり三角形をＮ回変形させて、角度θをテーブル化します。
- 底辺Ｈは１に固定
- 高さＨの 初期値を１ として、 Hを1/2^n倍ずつ 小さくする。
- 角度θをアークタンジェントをnの関数として算出。（下図中の式参照）
XY座標初期値
角度参照テーブルで求めた角度をもつ直角三角形を下図のように連結してゆきます。
このときn+1番目の三角の底辺Ｗがn番目の三角形の斜辺に一致するように、拡大しながら連結します。
このとき、以下の法則が成り立つので、拡大したｎ番目の三角形の斜辺Ｌ［ｎ］はピタゴラスの定理により算出できます。
- ｎ番目の三角形の底辺Ｗ ＝ ｎ-1番目の斜辺
- ｎ番目の三角形の高さＨ ＝ 底辺Ｗの 1/2＾n
最終Ｎ番目の三角形の斜辺の長さ【Ｌ】が変換ベクトル【Ｒ】 です。
1/Ｒを予め求め、固定小数に変換した値を"ＸＹ座標の初期値"として適用することで除算回路を省略します。
桁数と計算サイクル数
必要な桁数（小数bit幅）と計算サイクル数を予めＥｘｃｅｌで当たりをつけて以下のとおりとしました。
- 小数桁数Ｋ＝10[bit]
- 計算サイクル数Ｎ＝16[cycle]
誤差は出ていますが・・・
今回はパイプライン型の回路を組む為なるべくbit数/サイクル数は増やしたくないという理由で、この程度にとどめておきました。
事前検証
回路実装する前にPythonで事前検証した際のコードと結果を紹介します。
# -*- codinng:utf-8 -*-
import math
import sys
N=16 #計算サイクル数【Ｎ】
K=10 #固定小数点桁数【Ｋ】
#-----------------------------------------------
#角度テーブル【α】を求める
#-----------------------------------------------
def table_gen():
alph = [] #角度テーブル
for i in range(N):
# 1/2^n から高さが 1,1/2,1/4,1/8…となる角度(ラジアン)を算出
rad = math.atan( 1/(2**i))
#ラジアンから度に変換
deg = math.degrees(rad)
#固定小数分下駄をはかす
ang_int =int(deg * (2**K))
#角度テーブルに追加
alph.append( ang_int )
return alph
#-----------------------------------------------
#座標の初期値
#-----------------------------------------------
def init_pos():
#ベクトルＲの初期値は√２（45°の斜辺の長さ）
r=math.sqrt(2)
for i in range(1,N):
#三角形の高さ（短い方の辺の長さ）＝1つ前の三角形の斜辺の長さの 1/(2^n)
h = r / (2**i)
#次の斜辺：ピタゴラスの定理で求める
r = math.sqrt( (r**2) + (h**2) )
#固定小数分下駄をはかせた１をＲで割って整数型にまるめる
return int((2 ** K) / r)
#------------------------------------------------------
#ＣＯＲＤＩＣ演算：SINθの演算
# 入力は0～360°以内とする
#------------------------------------------------------
def cordic(theta , alph , init_position):
#入力範囲逸脱チェック
if theta>360 or theta<0 :
print("inpur theta error!!")
sys.exit()
#求める角度:固定小数分下駄をはかせて端数捨て整数にする
theta_int =int( theta * (2**K) )
#360°を90°で4分割したエリアの該当位置判定 （pol：出力極性 rvs：入力左右反転）
grp = theta_int // (90 * 2**K)
if grp ==0: pol = 1 ; rvs=False #0～90°の場合
elif grp ==1: pol = 1 ; rvs=True #90～180°の場合
elif grp ==2: pol = -1 ; rvs=False #180～270°の場合
elif grp ==3: pol = -1 ; rvs=True #27～360°の場合
#求めたい角度が0～90°の範囲になるようにする。
nom_ang = theta_int % (90 * 2**K)
if rvs ==False:
tgt_ang = nom_ang
else:
tgt_ang = 90 * 2**K - nom_ang
#x,ｙ座標の初期値
x = init_position
y = init_position
#角度の初期値：45°に固定小数分下駄をはかせる
ang = 45 * (2**K)
for n in range(1,N):
#演算方向決定
if ang >= tgt_ang:
cd = -1
else:
cd = 1
#角度移動
ang = ang + (cd * alph[n])
#座標演算用シフト処理
xd = x >> n
yd = y >> n
#座標移動先計算
tmp_x = x - (cd * yd)
tmp_y = y + (cd * xd)
#座標移動
x=tmp_x
y=tmp_y
#Y座標がそのままSIN（180°以上で負数）
sin = pol * y
#ＳＩＮθを返す
return sin
#---------------------------------------------------------------
#メイン関数
#---------------------------------------------------------------
#角度テーブルを生成
tbl =table_gen()
#座標初期値
ip=init_pos()
#CORDICで0～360°を0.1°ステップで計算する
for i in range(3600):
theta = i * 0.1
sin= cordic(theta,tbl,ip)
#SINθの結果（固定小数）をグラフプロット用の小数にする
print (sin/2**K)
今回のbit精度から得られたSINカーブを下図に示します。（多少粗い印象ですが、要求には達していそうです）
RTL実装
全体ブロック図
Pythonプログラムのforループ内の処理をCORDIC_COREモジュールに実装し、N段並べる構成にしました。
| ブロック | モジュール名 | 機能概要 |
|---|---|---|
| 角度テーブル | AngleTable | 予め計算しておいた角度デーブル（固定値） |
| 前処理部 | CORDIC_PRE | 0～360°入力を0～90°に変換する。180°以上では出力極性を負数にするフラグ(o_pol)を立てる。 |
| コア演算部 | CORDIC_CORE | XY座標演算のコアモジュール。Pythonコードのforループ１回転分の処理を行うモジュール。コアモジュールをシリアルN段接続してパイプラインを構成する。 |
| 後処理部 | CORDIC_POST | 極性フラグ(i_pol)に応じて符号反転するモジュール |
CORDICコアモジュール
- 目標角度(i_tgt)と現在角度(i_ang)を比較し、cd( :Calcrate Dirctionの略)フラグを決定する
- cdフラグに応じて加算or減算処理をセレクトする
- 現在角度(i_ang)が負数（MSB=1)の場合は常に加算方向とする
- シフト量nはパラメータ入力とする(ハード的には固定値)
シミュレーション結果
入力角度(正数)を0から１LSBずつインクリメントしてゆくシミュレーションで、出力が正弦波（２の補数）となっていることを確認できました。
ソースコード
前処理部
library ieee;
use ieee.std_logic_1164.all;
use ieee.std_logic_unsigned.all;
use ieee.std_logic_arith.all;
entity CORDIC_PRE is
port(
i_clk :in std_logic ;--クロック
i_tgt :in std_logic_vector(18 downto 0) ;--目標角度0～360°(整数9bit 小数10bit)
i_ena :in std_logic ;--イネーブル
o_tgt :out std_logic_vector(16 downto 0) ;--目標角度0～90°(整数7bit 小数10bit)
o_ena :out std_logic ;--イネーブル
o_pol :out std_logic );--出力極性フラグ()
end entity;
architecture rtl of CORDIC_PRE is
----------------------------------------------------------------
--CONSTANT
----------------------------------------------------------------
--90,180,270度を固定少数10bitにした値
constant DEG90 :std_logic_vector(18 downto 0) :=conv_std_logic_vector(90 ,9) & conv_std_logic_vector(0,10);
constant DEG180 :std_logic_vector(18 downto 0) :=conv_std_logic_vector(180,9) & conv_std_logic_vector(0,10);
constant DEG270 :std_logic_vector(18 downto 0) :=conv_std_logic_vector(270,9) & conv_std_logic_vector(0,10);
constant DEG360 :std_logic_vector(18 downto 0) :=conv_std_logic_vector(360,9) & conv_std_logic_vector(0,10);
constant ANG90 :std_logic_vector(16 downto 0) :=conv_std_logic_vector(90 ,7) & conv_std_logic_vector(0,10);
----------------------------------------------------------------
--SIGNAL
----------------------------------------------------------------
signal d0_ena :std_logic :='0' ;
signal d0_tgt :std_logic_vector(18 downto 0) :=(others=>'0') ;
signal d1_ena :std_logic :='0' ;--
signal d1_pol :std_logic :='0' ;--出力極性反転フラグ(180°以上で負数に反転させる)
signal d1_rvs :std_logic :='0' ;--角度90°反転
signal nom_ang :std_logic_vector(18 downto 0) :=(others=>'0') ;--0～90°にノーマライズした角度
signal d2_ena :std_logic :='0' ;--
signal d2_pol :std_logic :='0' ;--
signal tgt_ang :std_logic_vector(16 downto 0) :=(others=>'0') ;--
begin
--Stage0
process(i_clk)begin
if Rising_edge(i_clk) then
--入力FF
d0_ena <=i_ena;
d0_tgt <=i_tgt;
end if;
end process;
--Stage1
process(i_clk)begin
if Rising_edge(i_clk) then
if(d0_tgt>=DEG360)then
d1_ena <='0';--360°以降は無効入力としてENAを落とす
else
d1_ena <=d0_ena;
end if;
if(d0_tgt>=DEG360)then
d1_pol <='0';
d1_rvs <='0';
nom_ang <=(others=>'0');--360°以上が入力されたら0°として処理する
elsif(d0_tgt>=DEG270)then
d1_pol <='1';
d1_rvs <='1';
nom_ang <=d0_tgt - DEG270;
elsif(d0_tgt>=DEG180)then
d1_pol <='1';
d1_rvs <='0';
nom_ang <=d0_tgt - DEG180;
elsif(d0_tgt>=DEG90)then
d1_pol <='0';
d1_rvs <='1';
nom_ang <=d0_tgt - DEG90;
else
d1_pol <='0';
d1_rvs <='0';
nom_ang <=d0_tgt;
end if;
end if;
end process;
--Stage2
process(i_clk)begin
if Rising_edge(i_clk) then
d2_ena <=d1_ena;
d2_pol <=d1_pol;
--90°反転フラグが立っていたら
if(d1_rvs='0')then
tgt_ang <=nom_ang(16 downto 0);
else
tgt_ang <=ANG90 - nom_ang(16 downto 0);
end if;
end if;
end process;
o_ena <=d2_ena ;
o_pol <=d2_pol ;
o_tgt <=tgt_ang ;
end architecture;
コア演算部
library ieee;
use ieee.std_logic_1164.all;
use ieee.std_logic_unsigned.all;
entity CORDIC_CORE is
generic(
N :integer ;--シフト量
ANGLE_TABLE :std_logic_vector(16 downto 0) );--角度参照テーブル
port(
i_clk :in std_logic ;--クロック
i_tgt :in std_logic_vector(17 downto 0) ;--目標角度
i_ena :in std_logic ;--イネーブル
i_pol :in std_logic ;--出力極性反転フラグ
i_ang :in std_logic_vector(17 downto 0) ;--現在角度
i_x :in std_logic_vector(15 downto 0) ;--座標X
i_y :in std_logic_vector(15 downto 0) ;--座標Y
o_tgt :out std_logic_vector(17 downto 0) ;--
o_ena :out std_logic ;--
o_pol :out std_logic ;--
o_ang :out std_logic_vector(17 downto 0) ;--
o_x :out std_logic_vector(15 downto 0) ;--
o_y :out std_logic_vector(15 downto 0) );--
end entity;
architecture rtl of CORDIC_CORE is
signal tgt :std_logic_vector(17 downto 0) :=(others=>'0') ;
signal ena :std_logic :='0' ;
signal pol :std_logic :='0' ;
signal cd :std_logic :='0' ;
signal ang :std_logic_vector(17 downto 0) :=(others=>'0') ;
signal x :std_logic_vector(15 downto 0) :=(others=>'0') ;
signal y :std_logic_vector(15 downto 0) :=(others=>'0') ;
signal ang_tbl :std_logic_vector(17 downto 0) ;
signal sft_x :std_logic_vector(15 downto 0) ;
signal sft_y :std_logic_vector(15 downto 0) ;
begin
--入力FF
process(i_clk)begin
if Rising_edge(i_clk) then
if(i_ena='1')then
tgt <=i_tgt;
ena <='1';
if(i_ang(i_ang'high)='1')then --MSBが１は負数なので常に足す
cd <='0';--角度足す方向
else
if(i_ang>=i_tgt)then
cd <='1';--角度引く方向
else
cd <='0';--角度足す方向
end if;
end if;
x <=i_x;
y <=i_y;
ang <=i_ang;
pol <=i_pol;
else
tgt <=(others=>'0');
ena <='0';
cd <='0';
x <=(others=>'0');
y <=(others=>'0');
ang <=(others=>'0');
pol <='0';
end if;
end if;
end process;
--シフト
GEN:for i in 0 to 15 generate
GENIF:if i >=(15-N)generate
sft_x(i) <=x(15);
sft_y(i) <=y(15);
end generate;
GENIF2:if i <(15-N)generate
sft_x(i) <=x(i+N+1);
sft_y(i) <=y(i+N+1);
end generate;
end generate;
--符号bit付加
ang_tbl <='0' & ANGLE_TABLE;
--目標角度
o_tgt <=tgt;
--イネーブル
o_ena <=ena;
o_pol <=pol;
--現在角度
o_ang <= ang - ang_tbl when(cd='1')else
ang + ang_tbl;
--X座標
o_x <= x - sft_y when(cd='0')else
x + sft_y;
--Y座標
o_y <= y + sft_x when(cd='0')else
y - sft_x;
end architecture;
後処理部
library ieee;
use ieee.std_logic_1164.all;
use ieee.std_logic_unsigned.all;
use ieee.std_logic_arith.all;
entity CORDIC_POST is
port(
i_clk :in std_logic ;--クロック
i_tgt :in std_logic_vector(17 downto 0) ;--目標角度
i_ena :in std_logic ;--イネーブル
i_pol :in std_logic ;--出力極性反転フラグ
i_y :in std_logic_vector(15 downto 0) ;--座標Y
o_ena :out std_logic ;--
o_sin :out std_logic_vector(15 downto 0) );--
end entity;
architecture rtl of CORDIC_POST is
----------------------------------------------------------------
--SIGNAL
----------------------------------------------------------------
signal ena :std_logic :='0' ;
signal pol :std_logic :='0' ;
signal y :std_logic_vector(15 downto 0) :=(others=>'0') ;
signal ena1:std_logic :='0' ;
signal sin :std_logic_vector(15 downto 0) :=(others=>'0') ;
begin
--Stage0
process(i_clk)begin
if Rising_edge(i_clk) then
if(i_ena='1')then
ena <='1' ;
pol <=i_pol ;
y <=i_y ;
else
ena <='0' ;
pol <='0' ;
y <=(others=>'0') ;
end if;
end if;
end process;
--Stage1
process(i_clk)begin
if Rising_edge(i_clk) then
ena1<=ena;
if(pol='0')then
sin <=y ;
else
sin <=(not y) + 1;
end if;
end if;
end process;
o_ena <=ena1;
o_sin <=sin;
end architecture;
トップ
library ieee;
use ieee.std_logic_1164.all;
use ieee.std_logic_arith.all;
use ieee.std_logic_unsigned.all;
entity CORDIC is
port(
i_clk :in std_logic ;--クロック
i_theta :in std_logic_vector(18 downto 0) ;--目標角度0～360°(整数9bit 小数10bit)
i_ena :in std_logic ;--イネーブル
o_ena :out std_logic ;--イネーブル
o_sin :out std_logic_vector(15 downto 0) );--
end entity;
architecture rtl of CORDIC is
----------------------------------------------------
--User type
----------------------------------------------------
--コア間接続信号をまとめたタイプ
type typ_LINK_CORE is record
tgt :std_logic_vector(17 downto 0) ;--
ena :std_logic ;--
pol :std_logic ;--
ang :std_logic_vector(17 downto 0) ;--
x :std_logic_vector(15 downto 0) ;--
y :std_logic_vector(15 downto 0) ;--
end record;
--コア間接続信号タイプのアレイ
type typ_LINK_CORE_ARY is
array(integer range<>) of typ_LINK_CORE;
type typ_ANGLE_TABLE is
array(integer range<>) of std_logic_vector(16 downto 0);
----------------------------------------------------
--Signal
----------------------------------------------------
signal lnk :typ_LINK_CORE_ARY(0 to 15) ;
constant N :integer :=15;--パイプライン段数
constant INIT_ANG :std_logic_vector(17 downto 0):=conv_std_logic_vector(46080,18);
constant INIT_X :std_logic_vector(15 downto 0):='0'&conv_std_logic_vector(621,15);
constant INIT_Y :std_logic_vector(15 downto 0):='0'&conv_std_logic_vector(621,15);
constant ANGLE_TABLE :typ_ANGLE_TABLE(0 to N)
:= ("0"&x"B400"
,"0"&x"6A42"
,"0"&x"3825"
,"0"&x"1C80"
,"0"&x"0E4E"
,"0"&x"0728"
,"0"&x"0394"
,"0"&x"01CA"
,"0"&x"00E5"
,"0"&x"0072"
,"0"&x"0039"
,"0"&x"001C"
,"0"&x"000E"
,"0"&x"0007"
,"0"&x"0003"
,"0"&x"0001"
);
begin
--前処理部：角度0～90°正規化
u_CORDIC_PRE:entity work.CORDIC_PRE
port map
(i_clk =>i_clk
,i_tgt =>i_theta
,i_ena =>i_ena
,o_tgt =>lnk(0).tgt(16 downto 0)
,o_ena =>lnk(0).ena
,o_pol =>lnk(0).pol
);
--初期値
lnk(0).tgt(17) <='0' ;
lnk(0).ang <='0'&ANGLE_TABLE(0);--INIT_ANG ;
lnk(0).x <=INIT_X ;
lnk(0).y <=INIT_Y ;
--コアモジュールシリアル接続
GEN_CORDIC: for i in 0 to N-1 generate
u_CORDIC_CORE:entity work.CORDIC_CORE
generic map
(N =>i
,ANGLE_TABLE =>ANGLE_TABLE(i+1)
)
port map
(i_clk =>i_clk
,i_tgt =>lnk( i ).tgt
,i_ena =>lnk( i ).ena
,i_pol =>lnk( i ).pol
,i_ang =>lnk( i ).ang
,i_x =>lnk( i ).x
,i_y =>lnk( i ).y
,o_tgt =>lnk(i+1).tgt
,o_ena =>lnk(i+1).ena
,o_pol =>lnk(i+1).pol
,o_ang =>lnk(i+1).ang
,o_x =>lnk(i+1).x
,o_y =>lnk(i+1).y
);
end generate;
CORDIC_POST:entity work.CORDIC_POST
port map
(i_clk =>i_clk
,i_tgt =>lnk(N).tgt
,i_ena =>lnk(N).ena
,i_pol =>lnk(N).pol
,i_y =>lnk(N).y
,o_ena =>o_ena
,o_sin =>o_sin
);
end architecture;
注意点
- 実機検証までは至っていないので、回路規模や速度まで含めた最適化までは完了していません。