また時間が空いてしまいましたが、続きです。
一応今回で完成になります。
実際にバイクに載せて動かして、動画も撮ってみました。
※音入ってます。
うまくいったかというと、微妙なところ…
GitHub - hubnoki/RXtacho: Designing tachometer using RX220
結果的に動作が少し変だったので、バグを含むかもしれません…
回転数計算処理の実装
前回までで考えた回転数計算処理を、マイコンのほうに実装します。
回転数計算の手順を改めてまとめると、
- FFT結果のうち、DC成分を除いて最大の絶対値をとる周波数成分を調べる、ただし、64番目要素まででOK
- 最大値を取る位置の前後周波数成分比を計算する
- 前後周波数成分比テーブルから、周波数の非整数分を算出する
- 前後周波数成分があまり大きくなかったら、ここはスキップ
- 今回の設定では、FFT結果の周波数間隔は3.125[Hz]なので、[rpm]の単位に直すために 60/3.125 = 19.2をかける
- やっぱりサンプリング周波数は可変に変更しました。計算の関数の引数に周波数間隔(df)を追加しました。
まずは前後周波数比のテーブル。
const double tbl_spectrum_ratio[] = { 0.33333 , 0.37931 , 0.42857 , 0.48148 , 0.53846 , 0.60000 , 0.66667 , 0.73913 , 0.81818 , 0.90476 , 1.00000 , 1.10526 , 1.22222 , 1.35294 , 1.50000 , 1.66667 , 1.85714 , 2.07692 , 2.33333 , 2.63636 , 3.00000 };
回転数を計算する関数を以下のように用意します。
// f[0] : Real part // f[1] : Imaginary part static double fix_cabs(FIX_T *f) { int i1; double d1, d2; i1 = fix_fix2int(f[0]); // Re d1 = i1 * i1;// Re^2 d2 = d1; i1 = fix_fix2int(f[1]); // Im d1 = i1 * i1; // Im^2 d2 += d1; return sqrt(d2); } ... #define N_SPECTRUM_RATIO (sizeof(tbl_spectrum_ratio)/sizeof(double)) #define SPECTRUM_FRAC_UNIT ((double)1.0/N_SPECTRUM_RATIO) #define MAX_FREQ_IDX 64 static int calc_rpm(FIX_T *fft_result, double df) { int i; FIX_T *p = fft_result + 2; // Exclude DC component double amp; double amp_max = 0; int idx_max = 0; double amp_max_m1, amp_max_p1; double amp_mean = 0; bool updated_max = false; double amp_ratio; double frac; int rtn; // Find the index with maximum amplitude (Except DC level) for(i = 1; i < MAX_FREQ_IDX; i++){ amp = fix_cabs(p); p += 2; amp_mean += amp; if(updated_max){ amp_max_p1 = amp; } if(amp > amp_max){ amp_max_m1 = amp_max; amp_max = amp; idx_max = i; updated_max = true; } else{ updated_max = false; } } amp_mean /= (MAX_FREQ_IDX-1); // PRINTF("%f, %d, %f, %f, %f\r\n", amp_max, idx_max, amp_max_m1, amp_max_p1, amp_mean); // Find the fractional value of the actual frequency // Calculate rpm frac = -1.0; if((amp_max_p1 > amp_mean) && (amp_max_m1 > amp_mean)){ amp_ratio = amp_max_p1 / amp_max_m1; for(i = 0; i < N_SPECTRUM_RATIO; i++){ if(amp_ratio < tbl_spectrum_ratio[i]){ rtn = (int)((idx_max + frac) * df * 60); // PRINTF("%d, %f\r\n", i, amp_ratio); break; } frac += SPECTRUM_FRAC_UNIT; if(i == N_SPECTRUM_RATIO-1) rtn = (int)(idx_max * df * 60); } } else{ rtn = (int)(idx_max * df * 60); } return rtn; }
メータモードの実装
最後に、メータモードとしての処理になります。
- 2つのバッファを用意、片方は加速度データの収集用に、もう片方はFFT演算および回転数計算に使います。これらを役割を入れ替えながら使うことで、回転数更新レートを上げることを図ります。
- 加速度センサ測定完了割り込みで、バッファにデータを入れていきます。バッファが埋まったらそれ以降は書き込みしません。
- main()関数の処理で、片方のバッファが埋まっていることを確認したら、こちらを計算用として、バッファの役割の切り替えを行います。
- そして、FFT演算と回転数計算をして、その回転数をLCDに表示します。
コードを以下に示します。
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 ... int fft_ip[N_IP]; FIX_T fft_w[N_W]; }vars;
- 加速度センサ割り込み
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{ uint16_t *wp; FIX_T *buf; if(vars.g_buf_sel == SEL_A){ wp = &vars.g_buf_wp_a; buf = vars.g_buf_a; } else{ wp = &vars.g_buf_wp_b; buf = vars.g_buf_b; } if(*wp < N_GBUF){ buf[*wp] = fix_int2fix((int)d[vars.axis_sel]); (*wp)++; if(*wp == 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; } } }
- main()関数
proc_meter()関数でメータの処理をやっています。
int main(void) { ... while (1) { ... //// Process for each mode //// //--------- if(vars.mode == MODE_LOGGING){ proc_logging(psw_spush, psw_lpush); } //--------- else if(vars.mode == MODE_FFT){ proc_FFT(psw_spush, psw_lpush); } //--------- else if(vars.mode == MODE_METER){ proc_meter(psw_spush, psw_lpush); } //--------- else if(vars.mode == MODE_SETTING){ proc_setting(psw_spush, psw_lpush); } ... } return 0; }
static void proc_meter(uint8_t spush, uint8_t lpush) { int i; int rpm; char s[10]; unsigned int tm, tm_d; uint16_t *wp; FIX_T *buf; 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); lcdc_puts_x4("rpm", LCDC_GREEN, 88, 48); 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){ wp = &vars.g_buf_wp_a; buf = vars.g_buf_a; } else{ wp = &vars.g_buf_wp_b; buf = vars.g_buf_b; } if(*wp == N_GBUF){ //t timer_soft_reset(&tm); // Toggle buffer to write vars.g_buf_sel = (vars.g_buf_sel == SEL_B) ? SEL_A : SEL_B; // for(i = 0; i < 512; i++) // buf[i] = fix_mul(buf[i], han_window_fix16_512[i]); // Window function rdft_fix(N_GBUF, -1, buf, vars.fft_ip, vars.fft_w); //// Calculate and display rotation frequency rpm = calc_rpm(buf, (1.0 / 25.6e-6) / (double)(vars.g_int_intvl)); if(rpm > 10000) rpm = 9999; sprintf(s, "%4d", rpm/10*10); lcdc_puts_x4(s, LCDC_GREEN, 0, 48); //t tm_d = timer_soft_count(&tm); //t PRINTF("Elapsed : %d\r\n", tm_d); *wp = 0; } if(lpush){ // Mode change adxl345_stop(); vars.mode = MODE_SETTING; vars.mode_init = 1; } }
実動作
上記のコードでコンパイル、書き込みをして、実際にバイクに載せて動かしてみました。
※音入ってます。
ギアニュートラルでアクセル回してみています。
- ある程度回すと、2000〜3000rpmぐらいに落ち着いています。エンジン音的にはいつも5速で60km/hぐらい出しているときの感じで、前の速度-回転数変換表からしても妥当そうです。
- アイドリング中や低い回転数のとき、360rpmのままになっています。これはちょっと変… バグの予感…
- アクセルを回したときにたまに7000rpmくらいが出ています。本来の倍ぐらい？高調波を拾ってしまった？
最初の公道を走った動画でも同じような感じです。
ここまで
色々怪しいところがありますが、時間的にここまでにしました。
得られたことも色々あります。
- RX220基本設定、SDカードアクセス、UARTなどは前に作っていたものの流用です。
- 加速度センサ(ADXL345)、液晶モジュールを初めて使いました。自分で液晶表示できると面白いですね。
- マイコンでのFFT、ライブラリを少し改造して使わせてもらいましたが、ちゃんと動いてよかったです。
- octaveでのデータ解析も勉強になりました。また何か使えそうです。
- 実データを収集して分析、試行錯誤するのがなかなかいい経験になったと思います。
- FFT結果から半端な周波数の成分を検出する方法を考えました。サンプリング時間を短く抑えつつ周波数分解能を確保する方法として有用と思います。
ひとまず完了〜〜
