前回の続きです。
回転数計算処理
加速度の測定を行って、最終的には回転数をLCDに表示します。
手順は、
- バッファに加速度データをためる
- バッファを2つ用意し、片方では測定データの書き込みを行う、もう一方では測定データの読み出し、FFTを行うというようにし、
- ためた加速度データに対してFFTを実施
- FFT結果からピーク周波数算出
- ピーク周波数からrpm値(1分あたりの回転の数)を算出
といった感じです。
前の2つについては、現状FFTモードで実装済みです。
回転数測定スペック
かなり今更ですが、今回のRXタコメータでの回転数測定のスペックをまとめてみます。
- 加速度測定周波数 : 1600[Hz]
- 加速度センサ(ADXL345)への設定でいくつか変えられますが、下記のスペックを得るための値として決定しました。
- 測定点数 : 512点
- RX220のRAM容量、およびバッファを2つ持つことから、この点数になりました。
↓
- 周波数分解能 : 1600[Hz] / 512 = 3.125[Hz] ⇒ 187.5[rpm]
- ちょっと中途半端…
- rpmの値なので、中途半端にならざるを得ないか
- 十の桁以下は四捨五入表示とするか
- 最大周波数 : 1600[Hz] / 2 = 800[Hz] ⇒ 48000[rpm]
- 最小更新間隔 : 512 / 1600[Hz] = 0.32[s]
加速度データ取得
バッファは以下のように用意しています。加速度ログ用のバッファと共有の領域にしています。
また、バッファの他に書き込みポインタ、バッファセレクタの変数も用意しています。
#define N_GBUF 512 ... static struct{ ... union{ LOG_UNIT g_buf[N_LOGBUF]; struct{ FIX_T g_buf_a[N_GBUF]; FIX_T g_buf_b[N_GBUF]; }; }; uint8_t g_buf_sel; // Buffer side to save G sensor data uint16_t g_buf_wp_a; uint16_t g_buf_wp_b; uint16_t g_buf_wcnt_a; // For logging mode uint16_t g_buf_rp; uint16_t g_buf_rcnt; // For logging mode uint8_t g_overrun; uint8_t g_buf_filled; // All elements of the buffer are filled with valid data ... }vars;
ここで、FIX_Tは固定小数点演算用の型で、fix_math.hで以下のように定義されています。
//------------------------------------------------------ // fix point data type #define FIX_T int // decimal point #define FIX_P 16
結局4byteのint型にはなっています。
今回の固定小数点演算で一番大事なのは掛け算の処理ですが、以下の関数を用意しています。
/** * * @param op1 Operand 1 (FIX_T type) * @param op2 Operand 2 (FIX_T type) * @return FIX_T value of (op1 * op2) */ static inline FIX_T fix_mul(FIX_T op1, FIX_T op2){ return (FIX_T)((((uint64_t)op1) * op2) >> FIX_P); }
この関数を、FFT計算の中で使っています。
加速度センサの測定完了割り込みで2つのバッファに交互に書き込みを行います。
書き込みポインタは、割り込みごとにインクリメントしていき、バッファの最終データまで書いたらそれ以降は更新しません。
メイン処理で書き込みポインタがクリアされると再度書き込みを実施するようになります。
前々回の測定間隔測定も追加しました。
void adxl345_int_routine() { signed short d[3]; FIX_T f; LOG_UNIT log_d; uint16_t int_cnt = ADXL345_INT_CNTR; ... ADXL345_get(d); // Put data into buffer if(vars.logging){ ... } else{ if(vars.g_buf_sel == SEL_A){ if(vars.g_buf_wp_a < N_GBUF){ vars.g_buf_a[vars.g_buf_wp_a] = fix_int2fix((int)d[vars.axis_sel]); vars.g_buf_wp_a++; if(vars.g_buf_wp_a == N_GBUF-1){ // Measure the interval to fill the buffer vars.g_int_intvl = int_cnt - vars.g_int_cnt_last; vars.g_int_cnt_last = int_cnt; } } else{ // Update last counter value while waiting for buffer read vars.g_int_cnt_last = int_cnt; } } else{ if(vars.g_buf_wp_b < N_GBUF){ vars.g_buf_b[vars.g_buf_wp_b] = fix_int2fix((int)d[vars.axis_sel]);; vars.g_buf_wp_b++; if(vars.g_buf_wp_b == N_GBUF-1){ // Measure the interval to fill the buffer vars.g_int_intvl = int_cnt - vars.g_int_cnt_last; vars.g_int_cnt_last = int_cnt; } } else{ // Update last counter value while waiting for buffer read vars.g_int_cnt_last = int_cnt; } } } }
FFT処理
FFT計算には、京都大学の大浦氏のライブラリを使わせてもらっています。このソースコード自体は浮動小数点演算を使用していますが、今回使っているマイコンはそれほどCPU能力、クロック周波数等高くなく、浮動小数点演算のハードウェアがあるというわけでもないので、固定小数点演算化して使いました。(ソースコード : fft4g.c(.h))
#include "fix_math.h" void cdft_fix(int n, int isgn, FIX_T *a, int *ip, FIX_T *w); void rdft_fix(int n, int isgn, FIX_T *a, int *ip, FIX_T *w); void ddct_fix(int n, int isgn, FIX_T *a, int *ip, FIX_T *w); void ddst_fix(int n, int isgn, FIX_T *a, int *ip, FIX_T *w); void dfct_fix(int n, FIX_T *a, FIX_T *t, int *ip, FIX_T *w); void dfst_fix(int n, FIX_T *a, FIX_T *t, int *ip, FIX_T *w);
これらのFFT計算では、作業領域が必要になるので、用意しています。
#define N_IP 32 // >= 2+sqrt(N_GBUF/2) #define N_W 640 // = N_GBUF * 5 / 4 ... static struct{ ... int fft_ip[N_IP]; FIX_T fft_w[N_W]; }vars;
メイン処理のほうで、以下のようにFFT計算を実施しています。
- 下記の
proc_meter()関数を繰り返し呼んでいます。 - 現在選択している書き込みバッファがいっぱいになっているか確認
- いっぱいになっていれば、書き込みバッファ選択をもう一方のほうに変えて、書き込みポインタもクリアします。
- 書き込み完了したバッファの加速度データに、まず窓関数を適用します。不要であれば省略して処理時間短縮したいところ。
- これに対して、
rdft_fix()でFFTを実施します。 - FFT実施後、回転数計算と表示を行います。
//------------------------------------------------------------------------------------------- // Process for meter mode //------------------------------------------------------------------------------------------- static void proc_meter(uint8_t spush, uint8_t lpush) { int i; if(vars.mode_init){ adxl345_stop(); lcdc_fill_area(LCDC_BLACK, 0, LCDC_ROW-1, 0, LCDC_COL-1); lcdc_puts("METER mode", LCDC_WHITE, 0, 0); vars.g_buf_sel = SEL_A; vars.g_buf_wp_a = 0; vars.g_buf_wp_b = 0; vars.g_int_cnt_last = ADXL345_INT_CNTR; adxl345_start(); vars.mode_init = 0; } if(vars.g_buf_sel == SEL_A){ if(vars.g_buf_wp_a == N_GBUF){ vars.g_buf_sel = SEL_B; for(i = 0; i < 512; i++) vars.g_buf_a[i] = fix_mul(vars.g_buf_a[i], han_window_fix16_512[i]); // Window function rdft_fix(N_GBUF, -1, vars.g_buf_a, vars.fft_ip, vars.fft_w); //// Calculate and display rotation frequency vars.g_buf_wp_a = 0; } } else{ if(vars.g_buf_wp_b == N_GBUF){ vars.g_buf_sel = SEL_A; for(i = 0; i < 512; i++) vars.g_buf_b[i] = fix_mul(vars.g_buf_b[i], han_window_fix16_512[i]); // Window function rdft_fix(N_GBUF, -1, vars.g_buf_b, vars.fft_ip, vars.fft_w); //// Calculate and display rotation frequency vars.g_buf_wp_b = 0; } } if(lpush){ // Mode change adxl345_stop(); vars.mode = MODE_SETTING; vars.mode_init = 1; } }
回転数計算 : 検討
回転数は、FFT結果のうち、低い周波数成分から見ていって最初に現れるピーク周波数、から得られるかと。
一旦以前の加速度ログからどんなFFT結果になるか確認してみます。
まず気になるのは窓関数が必要かどうか。
窓関数を掛けずにグラフを出してみます。
FFT計算のスクリプトはこちら。
function ffts = fft_slice(d, u)
# d : data for FFT
# u : unit size for FFT
n_ffts = cast(floor(size(d)/u), "uint32");
for i = 1:n_ffts
ffts(:,i) = fft(d(1+(i-1)*u : i*u));
# ffts(:,i) = fft(d(1+(i-1)*u : i*u) .* hamming(u));
endfor
endfunction
軸の単位等は今はいらないので、手っ取り早くグラフ表示します。
>> [gData ovrn] = read_adxl345_log_bin_2byte('adxl345_log_002.bin');
Header detected, Ver:1, Size:8
Rate:1600[Hz], Range:2[G]
>> gFFT = fft_slice(gData(:,2),512);
>> h = pcolor(abs(gFFT(1:128,:)))
h = -1.0524
>> set(h, "edgecolor", "none");
800番目データでの断面を表示してみます。
>> figure(2) >> plot(abs(gFFT(1:128,800)), "*-")
窓関数ありだと、こうなります。ちなみに、ハミング窓です。
今回はピーク周波数を出したいだけなので、窓関数は不要かと考えられます。今回はなしとします。
また、このグラフから、どのようにピーク周波数を出せばいいかが見えてきそうです。
- DC成分は除いて考えます。このデータでは、加速度センサのZ軸方向測定値を使っているので、重力によるDC成分が出ているものと考えられます。
- 単純に最大値を調べればよさそうです。ただ、最初の大きなピークを見つけられればそれでいいようにも思えます。その場合はデータによって処理時間が変わるのが少し気持ち悪い。
- 最大周波数は、回転数にすると48000[rpm]になりますが、実際はここまでいきません。上限を設定してやればピーク検出の最大時間も短縮できそう。
改めて加速度ログからのFFTグラフを出してみます。 周波数点数は64点までにします。
やっぱりDC成分を除いた最大値を64点の中から探すのでよさそうです。
すなわち、回転数の値のパターンは64個までしかないということになります。
ちょっとそれはどうかな…
今回はここまで
長くなったので、続きは次回にします。
回転数推定の見直しをしていきます。
