2024年9月19日木曜日

STM32 CMSIS FIRとFFTをお味見

おはよーございます。

セカイの信号処理LOVERSの皆さんはいかがお過ごしでしょうか? 今回は大鳥神社モスからお送りしています。

わたしは今、STM32シリーズの中でhighendなSTM32H723の自作基板でDSPしようとしています。FIRやFFTを自分で書く気は無くて、CMSISライブラリの関数を流用します。

CMSISにはいろいろな信号処理関数が用意されていて、窓関数を除く全てを同ライブラリ関数の引用で記述できました。しかも結構速い。

以下は、ADC→FIR→FFT をやるcode解説です。

netにはライブラリ関数のreferenceなら在るのですが、全体像をペロッと舐めた事例が乏しく、FFTと複素数の関係、入出力配列の個数みたいな痒いところの解説が少なくて、昨日は試行錯誤で丸一日潰しました。

ーーーー
SPECなど:
 STM32H723 中華品だが本物だったらしい @¥800ぐらい
 550MHz最高速度
 ADC,DAC 12bit
 サンプル周波数 32kHz
 ADC→DMA→ring−buffer→FIR→FFT→UART
 ring−bufferに512サンプル積んだら処理する
 FIR/FFTは浮動小数点演算(float)

ブロック図はこんなかんじ
FFT結果をUSB COMでPCへ飛ばします
↓①の信号
↓②の信号 窓関数 DC offsetあり
↓③のスペクトラムdB 500Hzがpeakになってる (縦軸変だな....)
↓③ 500Hzを歪ませて高調波を発生させて、FIRスルーと、4kHz LPF FIRのスペクトラム比較 (縦軸変だな....)

ーーーー
CMSISをSTM32CubeIDEに入れるやりかた →こちらの末尾に書いてあります

ーーーー
ring-bufferの取り扱い

FIRだのFFTだのを解説する前にDMAとring-bufについて説明しとかないと話が通じないと思います

ブロック図にあるように、ring-bufを2つ、ADC用とDAC用に設けてあります.各々1024サンプル分のサイズです.32kサンプルで1024個ですから、32Hz周期で満杯になりんす.割り込みも発生します.

これらのring-bufには書く人と読む人が同時アクセスするので一工夫せないけません.

最もhardware寄りな視点では、書く人と読む人が同時だったとしても、APBバスとかのバスアービタが優先順序づけしてくれるので、衝突をケアする必要はないです.こうゆうところはCPUは便利ですね.FPGAだと自分で衝突回避せないけませんけど.

もう少し上位で考えますと、
 ADC ring-bufは、
  ・32kHz逐次書き&バースト読み
  ・逐次書きをバースト読みが邪魔したらいけない
 DAC ring-bufは、
  ・バースト書き&32kHz逐次読み
  ・逐次読みをバースト書きが邪魔してはいけない
なので対策はこうします.
 ・ring-bufが半分進んだら割り込みさせる
 ・ring-buf半分を演算する(512サンプル演算)

「ring-buf半分で割り込み」を実現するcallback関数がHAL driverに用意されています.
 HAL_ADC_ConvHalfCpltCallback()  半分で割り込み
 HAL_ADC_ConvCpltCallback()    満杯で割り込み
この2つのcallback関数でゴニョゴニョと信号処理します.

ーーーー
以下はsource codeのキモのところを説明します

まずはmain()の初期化のところ

FIRの初期化です

↓FIRのパラメータ構造体を2つ確保
arm_fir_instance_f32 fir_params;
arm_fir_instance_f32 fir_params2;

↓FIRの内部変数を2本確保
長さは、タップ数99+ブロックサイズ512 です
float32_t fir_decimate_variables[TAP_NUM_DECIMATE+BLOCKSIZE];
float32_t fir_decimate_variables2[TAP_NUM_DECIMATE+BLOCKSIZE];

↓FIRパラメータ構造体の初期化関数 2つのFIRをインスタンスする
arm_fir_init_f32(
&fir_params,
TAP_NUM_DECIMATE,
fir_decimate_taps,
fir_decimate_variables,
BLOCKSIZE
);

arm_fir_init_f32(
&fir_params2,
TAP_NUM_DECIMATE,
fir_decimate_taps,
fir_decimate_variables2,
BLOCKSIZE
);

なんでFIRを2つインスタンスしているの? →ブロック図に描いたとおり2つ使うから

FIR関数を2度呼び出せばいいじゃん →ダメ、内部変数を破壊しちゃいけないから

ーーーー
FFTの初期化です

↓パラメータ構造体確保
arm_rfft_fast_instance_f32   fft_params;

↓初期化関数 512個のFFTをやります
arm_rfft_fast_init_f32( &fft_params, 512);

rfftは実数FFTの意味です.
CMSISには、cfftすなわち複素FFTも用意されていますが、ADC dataを突っ込むのですから実数FFTで構いません.

それと、CMSISには似た関数でarm_rfft_init_f32()のようなfast抜きの関数もあるのですが、古いらしいです.fast抜きの関数は使い方がよくわかりませんでした.

ーーーー
ADC,DACをDMAで自走させる設定

uint32_t ADCfifo[1024];  ring-buf ADC用
uint32_t DACfifo[1024];  ring-buf DAC用
HAL_ADC_Start_DMA(&hadc1, ADCfifo, 1024);
HAL_Delay(1);
HAL_DAC_Start_DMA(&hdac1, DAC_CHANNEL_1, DACfifo, 1024, DAC_ALIGN_12B_R);

1mSec delayの意味は、、、
 ・1024個のring-bufですから32kHzサンプルですと31mSecで一周します
 ・ADCの逐次書きから1mSec遅れてDACが逐次読みするように設定しておきます
 ・ring-bufの衝突防止のためです

DMAが1024個積んだら割り込みしてくれる設定方法はこちらに書きました.

ADC,DACにTIM15でトリガ(32kHz)させます.その設定はこちらに書きました.

説明は略しますが、DACが12bitなので、ADCも12bitに設定しました.ADCは16bitにもできます.

ーーーー
FIRのtap係数

カットオフ4kHzのLPFです.99tapです.

tap係数計算サイトはたくさんあります.これとか.

こんな感じで宣言しときます.
FIRもFFTもfloat演算なのでtap係数もfloatです.

float32_t fir_decimate_taps[99] = {

-0.0004885952754413765,

-0.0010871390633406717,

-0.0014610965092318009,

-0.0019028688834487318,

   中略

-0.0011277364894511205,

0.000009864619595940656,

0.0011669433057244123 }


ーーーー
窓関数

窓関数はCMSISに存在しないようですので自作です.ハミングだかハニングだかですよねこれ.FFTする前に被せます.窓関数の必要性は割愛します.BLOCKSIZE=512です.

void fft_window( float32_t* in, float32_t* winout ){
    double pp = 2.0*PI/(double)BLOCKSIZE;
    for(int i=0; i<BLOCKSIZE; i++){
        float32_t h = 0.5 - 0.5 * cos(pp*(double)i);
        winout[i] = in[i] * h;
    }
}

窓関数を被せるとこんなになります.両端がゼロにすぼむ.

ーーーー
それでは、キモのcallback関数です.

↓ring-buf半分でのcallback
void HAL_ADC_ConvHalfCpltCallback(){
    ①arm_copy_q31( (q31_t*)&ADCfifo[0], ADCcopy, 512);
    ②arm_q31_to_float( ADCcopy, ADCcopyf, 512);
    ③arm_fir_f32( &fir_params, ADCcopyf, firout, 512);
    ④arm_float_to_q31( firout, (q31_t*)&DACfifo[512], 512);
}

arm_xxxはCMSISライブラリ関数です.512は処理サンプル数

やってることは、、、
①ADC ring-bufは全1023のうち512界隈から先へ逐次書きしている最中(half=半分だから)
なので0番~511番までをスクショみたいにコピって使う →ADCcopyへ
q31_t ADCcopy[512];

②ADC dataはintなので、floatに変換する
float32_t ADCcopyf[512];

③FIRへ通す
float32_t firout[512];

④FIR出力をfloat→int変換し、格納先はDAC ring-bufの512番~1023番へ書き出し
DACは450番界隈を逐次読みしている最中なので衝突の心配はない

配列copyや配列型変換をできるのでCIMSISは便利です

ーーーー
次はring-buf全埋まりでのcallbackです.ここでFFTします.
動作確認のために冗長なcodeなので許してちょ.

void HAL_ADC_ConvCpltCallback() {
    ①arm_copy_q31( (q31_t*)&ADCfifo[512], ADCcopy, 512);
    ②arm_q31_to_float( ADCcopy, ADCcopyf, 512);
    ③arm_fir_f32( &fir_params, ADCcopyf, firout, 512);
    ④arm_float_to_q31( firout, (q31_t*)&DACfifo[0], 512);
①②③④は前記とほぼ同じ コピる場所が違うだけ

↓FFTです
    ⑤arm_offset_q31( ADCcopy, -2048, offset, 512);
    ⑥arm_q31_to_float( offset, offsetf, 512);
    ⑦arm_fir_f32( &fir_params2, offsetf, firout, 512);
    ⑧fft_window( firout, firout_window );
    ⑨arm_copy_f32( firout_window, firout_window_bak, 512);
    ⑩arm_rfft_fast_f32( &fft_params, firout_window, fftout, 0 );
    ⑪arm_cmplx_mag_f32( fftout, fftmag, 256 );
    float32_t max; uint32_t index;
    ⑫arm_max_f32( fftmag, 256, &max, &index );
    ⑬arm_scale_f32( fftmag, 1.0/max, fftmag_scale, 256 );
⑤12bit ADCはストレートバイナリなので2048を引き算して±2047の範囲にズラす
⑥int→floatに変換
⑦2つめのFIRフィルタを使う(内部変数を破壊しないため)
⑧窓関数をかぶせる
⑨検証のためにコピっておきます FFT関数は入力配列を破壊するからです
⑩512個のFFT
入力は512個の配列です.内容物はADC dataに由来する512個の実数です
    float32_t firout_window[512];
出力配列は512個なので512個の実数が出てくるのかと思いきやそうではなく256個の複素数です.先頭からreal,imagが交互に並んで総数512個です
    float32_t fftout[512];
256の意味は「0~ナイキスト周波数までしか出力しません」です.ナイキスト周波数から上半分は下半分の対称形なので自分で補完してねという意図なのでしょう
⑪複素数→スペクトラム変換します.256個の複素数を入れると、256個のスペクトラムが出てきます.もちろん実数です.周波数範囲は0~ナイキスト周波数です
    float32_t fftmag[256];
⑫スペクトラムの最大値を検索
⑬最大値=1で正規化

以上でFFT演算まで終了です.

この後で⑬の成果物をUARTへ送信します.sourceは割愛.

ーーーー
STM32H7の動作速度実測

①~④の処理に0.8mSecかかっています  ≒512点99タップ浮動小数点FIR

①~⑬の処理に2.3mSecかかっています  ≒FIR+FIR+FFT

割り込み周期はHALF→FULL→HALF→の間隔、すなわち16mSecですから、十分な演算速度だと思います

結論としては、、、
STM32H723は32kHzサンプルのDSPに十分な速度だと思います.
やったことはCMSISを導入しただけであって、cash memoryの最適化とかはやってません.たぶん裏でFPUを動かしたりしてるんじゃないかと思います.CPUでこうゆう性能を出せるようですと、DSP chipが不人気になってしまうのも仕方ないかな.
将来は4chの同時処理をやりたいので、その時にはギリギリ感があるかもしれません.

現状でwork memoryとして60kBぐらい消費していますが、検証のためダブダブな変数確保しているせいです.スリム化はできます.CPU搭載RAMは500kBぐらいです.

かしこ

0 件のコメント:

コメントを投稿