STM32G4シリーズのCORDICを使ってみる1

STM32

こんにちは、もりゅーです。

今日は前回の続きみたいなもんで、STM32G4シリーズに搭載されている
CORDICプロセッサーの使い方をざっくり説明したいと思います。

CORDICとは

Coordinate Rotation Digital Computerらしいです。
反復演算で三角比などを求めていく手法で、テーブル参照・シフト演算・加算のみで演算できるため
高速かつハードウェア化しやすいらしいです。
詳しくはググってください。

1. 環境

Windows10+STM32CubeIDE+NucleoG474RE

2. CubeMXの設定

設定するほどのものもありませんが、いつものとおりCubeMXでCORDICを有効にします。
f:id:Moryu_io:20200216135136p:plain

3. sinを求める

もっともよく使うであろう三角関数sinをCORDICで求めてみたいと思います。
といってもやりかたは非常に簡単で
　1. CORDIC_CSRレジスタに各種設定を書き込み
　2. CORDIC_WDATAレジスタに角度を書き込み
　3. CORDIC_CSRにRRDYフラグが立つまで待つ
　4. CORDIC_RDATAレジスタにsinの値が出力される
といった感じです。
全てHALで関数が用意されているので、それで行ってもいいですし、直接レジスタを弄っても良いです。

具体的には以下のコードのようにします。

  /* CORDICの設定 */
  CORDIC_ConfigTypeDef sCordicConfig;
  sCordicConfig.Function	= CORDIC_FUNCTION_SINE;    /* sinを計算 */
  sCordicConfig.Precision	= CORDIC_PRECISION_6CYCLES; 	/* 反復回数（計算精度）を設定する */
  sCordicConfig.Scale      	= CORDIC_SCALE_0;           /* scale設定。sinでは使用しない */
  sCordicConfig.NbWrite  	= CORDIC_NBWRITE_1;         	/* 入力データの数 */
  sCordicConfig.NbRead   	= CORDIC_NBREAD_1;          	/* 出力データの数 */
  sCordicConfig.InSize      	= CORDIC_INSIZE_32BITS;     	/* 入力データの固定小数点位置 */
  sCordicConfig.OutSize   	= CORDIC_OUTSIZE_32BITS;    /* 出力データの固定小数点位置 */
  HAL_CORDIC_Configure(&hcordic, &sCordicConfig);

  // 入力引数
  int32_t _s32_x = 0x15555555;		// Q1.31。pi/6
  int32_t _s32_hal_res = 0;
  int32_t _s32_res = 0;

  // 結果の確認用
  float  _sf_x = 0.0f;
  float  _sf_hal_res = 0.0f;
  float  _sf_res = 0.0f;

  /* HALライブラリで求める */
  HAL_CORDIC_Calculate(&hcordic, &_s32_x, &_s32_hal_res, 1, 100);

  /* レジスタ直打ち */
  CORDIC->WDATA = _s32_x;		// 入力レジスタにθを書き込み
  while((CORDIC->CSR&0x80000000)==0);	// 計算終了まで待つ
  _s32_res = CORDIC->RDATA;		// 出力レジスタにsinθの結果がQ1.31で出力される

  _sf_x = (float)_s32_x / 0x80000000;		// 浮動小数点へ変換
  _sf_hal_res = (float)_s32_hal_res / 0x80000000;	   // 浮動小数点へ変換
  _sf_res = (float)_s32_res / 0x80000000;	   // 浮動小数点へ変換

固定小数点

入力角度は-pi~piが-1.0~+1.0になるような固定小数点
出力は-1.0~+1.0が固定小数点としてそのまま出力されます。
sCordicConfig.InSizeで固定小数点位置を15.16か1.31に設定できます。

計算精度

sCordicConfig.Precision = CORDIC_PRECISION_6CYCLES;
でCORDICアルゴリズムの反復回数を設定できます。
反復回数が増えると計算精度も上がっていきますが、もちろん時間がかかっていきます。
リファレンスマニュアルによると、6CYCLESで最大誤差は2^-19ということで、これが最高精度っぽいです。

f:id:Moryu_io:20200216141151p:plain
STM32G4 Series Reference manual p.436より

計算結果

f:id:Moryu_io:20200216135120p:plain
ということで、上記コードでの計算結果は↑の右下のようになりました。
　sin(pi/6) = 0.5
なので、ほぼ合っていますね。

ちなみにHAL関数では、角度の配列を引数としてNbCalcを増やせば、配列全てのsinが求められたりします。

 _s32_x[2] = {0x15555555, 0x2AAAAAAA};
 _s32_hal_res[2] = {};
  HAL_CORDIC_Calculate(&hcordic, _s32_x, _s32_hal_res, 2, 100);

4. sinとcosを同時に求める

角度を配列として渡す、とは別の話で、入力種類や出力種類を増やすことができます。
例えばsinであれば、同時にcosを求められたりします。

具体的には以下のように、sCordicConfig.NbReadにCORDIC_NBREAD_2を書き込み、
RDATAレジスタから二回データを読み出すと、一回目がsin、二回目がcosで出力されます。

  /* CORDICの設定 */
  CORDIC_ConfigTypeDef sCordicConfig;
  sCordicConfig.Function	= CORDIC_FUNCTION_SINE;    /* sinを計算 */
  sCordicConfig.Precision	= CORDIC_PRECISION_6CYCLES; 	/* 反復回数（計算精度）を設定する */
  sCordicConfig.Scale      	= CORDIC_SCALE_0;           /* scale設定。sinでは使用しない */
  sCordicConfig.NbWrite  	= CORDIC_NBWRITE_1;         	/* 入力データの数。1ならば位相のみ、2ならば位相θと径m[0,1]を設定できる */
  sCordicConfig.NbRead   	= CORDIC_NBREAD_2;          	/* 出力データの数。1がm*sinθ, 2がm*cosθ */
  sCordicConfig.InSize      	= CORDIC_INSIZE_32BITS;     	/* 入力データの固定小数点位置 */
  sCordicConfig.OutSize   	= CORDIC_OUTSIZE_32BITS;    /* 出力データの固定小数点位置 */
  HAL_CORDIC_Configure(&hcordic, &sCordicConfig);

  // 入力引数
  int32_t _s32_xy[2] = {0x15555555, 0x00000000};		// Q1.31。{pi/6, 0}
  int32_t _s32_res[2] = {};

  // 結果の確認用
  float  _sf_xy[2] = {};
  float  _sf_res[2] = {};

  /* レジスタ直打ち */
  CORDIC->WDATA = _s32_xy[0];		// 入力レジスタにθを書き込み
  while((CORDIC->CSR&0x80000000)==0);	// 計算終了まで待つ
  _s32_res[0] = CORDIC->RDATA;		// 出力レジスタにsinθの結果がQ1.31で出力される
  _s32_res[1] = CORDIC->RDATA;		// 出力レジスタにcosθの結果がQ1.31で出力される

  _sf_xy[0] = (float)_s32_xy[0] / 0x80000000;		// 浮動小数点へ変換
  _sf_res[0] = (float)_s32_res[0] / 0x80000000;	   // 浮動小数点へ変換
  _sf_res[1] = (float)_s32_res[1] / 0x80000000;	   // 浮動小数点へ変換

計算結果

f:id:Moryu_io:20200216132223p:plain
　sin(pi/6) = 0.5
　cos(pi/6) = 0.8660254...
なので、ほぼ合っていますね。

5. atan2を求める

atan2も求めることができます。atan2はatan2という名前ではなくphaseとして求めます。
ちなみにatanは別で存在するので注意。
また、入力は-1~+1のみなので、これまた注意。

具体的には以下のように、
・sCordicConfig.NbReadはCORDIC_NBREAD_2
・sCordicConfig.NbWriteはCORDIC_NBWRITE_2
として、WDATAレジスタにx,yを順に書き込みます。
そうすると計算が開始され、RDATAレジスタから
・1回目：位相-1~+1 (-pi~+piに対応)
・2回目：径m 0~+1 (√x^2+y^2)
が読み出せます。

  /* CORDICの設定 */
  CORDIC_ConfigTypeDef sCordicConfig;
  sCordicConfig.Function	= CORDIC_FUNCTION_PHASE;    /* phaseを計算 */
  sCordicConfig.Precision	= CORDIC_PRECISION_6CYCLES; 	/* 反復回数（計算精度）を設定する */
  sCordicConfig.Scale      	= CORDIC_SCALE_0;           	/* scale設定。phaseでは使用しない */
  sCordicConfig.NbWrite  	= CORDIC_NBWRITE_2;          /* 入力データの数。phaseは2必要 */
  sCordicConfig.NbRead   	= CORDIC_NBREAD_2;          	/* 出力データの数。1が位相[-1,1], 2が径[0,1] */
  sCordicConfig.InSize      	= CORDIC_INSIZE_32BITS;     	/* 入力データの固定小数点位置 */
  sCordicConfig.OutSize   	= CORDIC_OUTSIZE_32BITS;    /* 出力データの固定小数点位置 */
  HAL_CORDIC_Configure(&hcordic, &sCordicConfig);

  // 入力引数
  int32_t _s32_xy[2] = {0xC0000000, 0x40000000};		// Q1.31。{-0.5, 0.5}
  int32_t _s32_res[2] = {};

  // 結果の確認用
  float  _sf_xy[2] = {};
  float  _sf_res[2] = {};

  /* レジスタ直打ち */
  CORDIC->WDATA = _s32_xy[0];		// 入力レジスタにx座標を書き込み
  CORDIC->WDATA = _s32_xy[1];		// 入力レジスタにy座標を書き込み
  while((CORDIC->CSR&0x80000000)==0);		// 計算終了まで待つ
  _s32_res[0] = CORDIC->RDATA;		// 出力レジスタにatan2(x,y)の結果がQ1.31で出力される
  _s32_res[1] = CORDIC->RDATA;		// 出力レジスタにsqrt(x^2+y^2)の結果がQ1.31で出力される

  _sf_xy[0] = (float)_s32_xy[0] / 0x80000000;		// 浮動小数点へ変換
  _sf_xy[1] = (float)_s32_xy[0] / 0x80000000;		// 浮動小数点へ変換
  _sf_res[0] = (float)_s32_res[0] / 0x80000000;	   // 浮動小数点へ変換
  _sf_res[1] = (float)_s32_res[1] / 0x80000000;	   // 浮動小数点へ変換

計算結果

f:id:Moryu_io:20200216132243p:plain
atan2(-0.5, 0.5) = 3*pi/4 は正規化すると0.75
√0.5^2+0.5^2 = 0.7071067...
なのでほぼ合っていますね。

6. 平方根を求める

三角関数だけでなく、平方根なども求めることができます。
ただし、どんな平方根でも求めることができるわけではなく、
0.027～2.341の平方根のみ求めることができます。
また、その間も区分が3つあって、
・x = 0.027～0.75はscale0として、そのまま入力引数とする
・x = 0.75～1.75はscale1として、x/2を入力引数とし、出力に2をかけた値が√x
・x = 1.75～2.341はscale2として、x/4を入力引数とし、出力に4をかけた値が√x
これ以外の場合は事前にこのスケールに収まるよう、値を弄っておかないといけないですね。

f:id:Moryu_io:20200216155040p:plain
STM32G4 Series Reference manual p.432より
f:id:Moryu_io:20200216155233p:plain
STM32G4 Series Reference manual p.432より

scale_0の場合

  /* CORDICの設定 */
  CORDIC_ConfigTypeDef sCordicConfig;
  sCordicConfig.Function	= CORDIC_FUNCTION_SQUAREROOT;   /* sqrtを計算 */
  sCordicConfig.Precision	= CORDIC_PRECISION_6CYCLES; 	/* 反復回数（計算精度）を設定する */
  sCordicConfig.Scale      	= CORDIC_SCALE_0;              /* scale設定 */
  sCordicConfig.NbWrite  	= CORDIC_NBWRITE_1;         /* 入力データの数。sqrtは1 */
  sCordicConfig.NbRead   	= CORDIC_NBREAD_1;          /* 出力データの数。sqrtは1 */
  sCordicConfig.InSize      	= CORDIC_INSIZE_32BITS;     	/* 入力データの固定小数点位置 */
  sCordicConfig.OutSize   	= CORDIC_OUTSIZE_32BITS;    /* 出力データの固定小数点位置 */
  HAL_CORDIC_Configure(&hcordic, &sCordicConfig);

  // 入力引数
  int32_t _s32_x = 85899346;		// Q1.31。0.04
  int32_t _s32_res = 0;

  // 結果の確認用
  float  _sf_x = 0.0f;
  float  _sf_res = 0.0f;

  /* レジスタ直打ち */
  CORDIC->WDATA = _s32_x;					// 入力レジスタにxを書き込み
  while((CORDIC->CSR&0x80000000)==0);		// 計算終了まで待つ
  _s32_res = CORDIC->RDATA;				// 出力レジスタにsqrt(x)の結果がQ1.31で出力される

  _sf_x = (float)_s32_x / 0x80000000;		// 浮動小数点へ変換
  _sf_res = (float)_s32_res / 0x80000000;	// 浮動小数点へ変換
計算結果

f:id:Moryu_io:20200216132301p:plain
√0.04 = 0.2なので大体合ってますね

scale_1の場合

  /* CORDICの設定 */
  CORDIC_ConfigTypeDef sCordicConfig;
  sCordicConfig.Function	= CORDIC_FUNCTION_SQUAREROOT;   /* sqrtを計算 */
  sCordicConfig.Precision	= CORDIC_PRECISION_6CYCLES; 	/* 反復回数（計算精度）を設定する */
  sCordicConfig.Scale      	= CORDIC_SCALE_1;              /* scale設定 */
  sCordicConfig.NbWrite  	= CORDIC_NBWRITE_1;         /* 入力データの数。sqrtは1 */
  sCordicConfig.NbRead   	= CORDIC_NBREAD_1;          /* 出力データの数。sqrtは1 */
  sCordicConfig.InSize      	= CORDIC_INSIZE_32BITS;     	/* 入力データの固定小数点位置 */
  sCordicConfig.OutSize   	= CORDIC_OUTSIZE_32BITS;    /* 出力データの固定小数点位置 */
  HAL_CORDIC_Configure(&hcordic, &sCordicConfig);

  // 入力引数
  int32_t _s32_x = (int32_t) (1.2f/2.0f*0x80000000);		// Q1.31。1.2/2
  int32_t _s32_res = 0;

  // 結果の確認用
  float  _sf_x = 0.0f;
  float  _sf_res = 0.0f;

  /* レジスタ直打ち */
  CORDIC->WDATA = _s32_x;					// 入力レジスタにxを書き込み
  while((CORDIC->CSR&0x80000000)==0);		// 計算終了まで待つ
  _s32_res = CORDIC->RDATA;				// 出力レジスタにsqrt(x)の結果がQ1.31で出力される

  _sf_x = (float)_s32_x / 0x80000000;		// 浮動小数点へ変換
  _sf_res = (float)_s32_res / 0x80000000 * 2.0f;	// 浮動小数点へ変換
計算結果

f:id:Moryu_io:20200216132313p:plain
√1.2 = 1.095445...なので大体合ってますね

7. まとめ

CORDICでsinやatan2、sqrtなどの計算が可能
精度も申し分なし。


ちなみに、今回は
・設定はHAL関数
・計算はレジスタ直打ち
で行いましたが、リファレンスマニュアルを見ればわかる通り、
コンフィグレジスタは1つなので設定は非常に簡単です。

CORDICは一つしかないので、sinやatanを両方とも頻繁に計算する、などの運用の場合は
全部レジスタ直打ちした方が早そうですね。

ということで次回はその辺の計算速度などを検証したいと思います。