勉強しないとな～blog

ちゃんと勉強せねば…な電気設計エンジニアです。

OpenCVやってみる-2. 画像表示

OpenCV

OpenCVやってみるの続きです。

やってみたいこと

ヤマザキ春のパン祭りのシールを集めています。
このシール点数集計がいいネタになるのではと。

OpenCVチュートリアル

OpenCVのチュートリアルを一度ざっと読んでみたので、これを元に春のパン祭り点数集計をやってみたいと思います。

Welcome to OpenCV-Python Tutorials’s documentation! — OpenCV-Python Tutorials 1 documentation

和訳されているページもあります。

OpenCV-Python チュートリアル文書のページへようこそ! — OpenCV-Python Tutorials 1 documentation

画像表示

まずはライブラリのインポート。

>>> import cv2

画像を読み込んで表示します。

>>> img = cv2.imread('harupan_200317_1.jpg', cv2.IMREAD_COLOR)
>>> cv2.imshow('image', img)
>>> cv2.waitKey(0)
  • img変数に画像データを読み込み
  • imshow関数でウィンドウを出し、画像を表示します。実際には、waitKey関数を呼んだ時点で表示されます。
    waitKeyを呼ぶ前まではウィンドウ内はグレー一色になっています。

表示された画像。

f:id:nokixa:20210315234057p:plain

元の解像度と一対一の倍率で表示されているようです。
縮小して全体を表示できるようにしたい。

ひとまず進めますが、この画像ウィンドウ上で適当なキーを押すと、pythonコンソールがまた入力可能な状態となります。

>>> cv2.imshow('image', img)
>>> cv2.waitKey(0)
13                                      ← 画像ウィンドウ上でEnterキーを押した
>>>

以下のdestroyAllWindows関数で画像ウィンドウを消します。

>>> cv2.destroyAllWindows()

画像のリサイズ

まず、shapeで画像サイズが得られます。

Python, OpenCV, Pillow(PIL)で画像サイズ（幅、高さ）を取得 | note.nkmk.me

>>> img.shape
(4032, 3024, 3)

shapeはタプルになっています。今回はカラー画像なので、要素は(高さ, 幅, チャネル数)となっています。
タプルなので、アンパックを行います。

>>> h,w = img.shape[:2]
>>> print('height:',h,'width:',w)
height: 4032 width: 3024

resize関数を使って、リサイズします。 shapeでは(高さ, 幅)の順になっていましたが、 resize関数の引数では(幅, 高さ)の順になるようです。

OpenCVで画像サイズの変更をしてみた - Qiita

あと、整数型にしておかないとエラーメッセージが出たので、int関数を使っています。

>>> img2 = cv2.resize(img, (w/10, h/10))
Traceback (most recent call last):
  File "<stdin>", line 1, in <module>
TypeError: integer argument expected, got float
>>> img2 = cv2.resize(img, (int(w/10), int(h/10)))
>>> img2.shape
(403, 302, 3)
>>> cv2.imshow('image2', img2)
>>> cv2.waitKey(0)

縮小された画像が表示されました。

f:id:nokixa:20210317224642p:plain

ここまで

とりあえずここまで。
pythonもOpenCVも慣れていきたい。


OpenCVやってみる-1

OpenCV

画像処理の勉強のために、OpenCVを使ってみたいと思っています。
色々やってみようと思います。

まず環境

Anaconda + python + OpenCVという組み合わせでやろうと思います。
参考にしたサイト。

Anaconda のインストール: Python環境構築ガイド - python.jp

PythonでOpenCVをはじめる（Windows10、Anaconda 2018.12、Python3.7.1、OpenCV4.0.0） - Qiita

まずはAnacondaのインストール。
上記のサイトによると、Anacondaは「データサイエンス向けの環境を提供するプラットフォーム」だということです。
色々できそうですが、まだよくわかりません。

Anaconda | The World's Most Popular Data Science Platform

まずはインストーラをダウンロード、指示に従ってインストールします。
ちなみに、Windows 10 64bitのPC環境です。

インストール後、Anaconda Promptを起動します。

f:id:nokixa:20210315072846p:plain:w300

参考サイトにならって、仮想環境を作っておきます。
名前はpy37cv2としておきます。

conda create -n py37cv2 python=3.7 anaconda
conda activate py37cv2

この環境に、OpenCVをインストールします。

python -m pip install opencv-python
python -m pip install opencv-contrib-python

pythonを起動し、バージョンを確認します。

import cv2
print(cv2.__version__)

実行結果は以下の通り。

(py37cv2) C:\work\OpenCV\harupan>python
Python 3.7.9 (default, Aug 31 2020, 17:10:11) [MSC v.1916 64 bit (AMD64)] :: Anaconda, Inc. on win32
Type "help", "copyright", "credits" or "license" for more information.
>>> import cv2
>>> print(cv2.__version__)
4.5.1
>>>

ここから色々やってみます。

マイコンと加速度センサでタコメータを作る - 15. メータモード実装

RXタコメータ マイコン

また時間が空いてしまいましたが、続きです。
一応今回で完成になります。
実際にバイクに載せて動かして、動画も撮ってみました。 ※音入ってます。

うまくいったかというと、微妙なところ…


GitHubリポジトリです。

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に表示します。


コードを以下に示します。

  • 2つのバッファおよびセレクタ
    vars構造体の中に入っています。ついでに、FFT用のワーク領域も入っています。
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;
}
  • FFT演算、回転数計算、LCD表示
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結果から半端な周波数の成分を検出する方法を考えました。サンプリング時間を短く抑えつつ周波数分解能を確保する方法として有用と思います。


ひとまず完了〜〜


マイコンと加速度センサでタコメータを作る - 14. メータモード実装

RXタコメータ マイコン

回転数計算処理

回転数測定のスペックをちゃんと考えてみると、思ったより刻みが粗かった(187.5[rpm]で64ステップ程度)ので、ちょっと検討してみます。

実データ確認

FFTのグラフで、ピーク前後のスペクトルを使ったりでなんとか周波数をより細かく出せないか…

Octaveでちょっと加速度の時間波形を出してみます。

>> plot(gData(800*512+1:801*512,2))

f:id:nokixa:20201210002853p:plain:w600

この波形を見ると、21個の山があるように見えます。
正弦波を重ね書きしてみます。

>> hold on
>> plot(sin(2*pi*21*[1:512]/512))

f:id:nokixa:20201210004613p:plain:w600

hold onで前のグラフを消さずに上書きできます。
この正弦波とだいたい形は合っていますが、ちょっとずつずれていっています。

FFT結果を再確認します。ピーク周波数周辺を見てみます。

>> figure(2)
>> plot(abs(gFFT(1:100,800)), "*-")
>> xticks([1:1:800])
>> axis([11 30 100 300])

f:id:nokixa:20201210005850p:plain:w400

このグラフでは、横軸の値1がDC成分を表すので、横軸の値22が先ほどの正弦波周波数にあたります。 確かにここがピークになっています。
1つ前後の周波数もそれなりの値を持っているので、使えそうな気も。

周波数計算検討

FFT対象の波形が本当に純粋な単一周波数、ただしサンプリング周波数の整数倍でないとします。

f(t) = A\exp(j(\omega_n + \omega_f)t)

\omega_n = n\Delta\omega, \Delta\omega=\cfrac{2\pi}{N\Delta t}, \Delta t : サンプリング周期, n : 整数, N : サンプリング点数

| \omega_f | \lt \Delta\omega / 2

ここで、角周波数 \omega_m = m\Delta\omega の成分を取り出すべく、 \exp(-j\omega_m t) を掛けてサンプリング時間積分、サンプリング時間で割ると、

\cfrac{1}{N\Delta t} \int_0^{N\Delta t}\exp(j(\omega_n+\omega_f -\omega_m)t)dt

=\cfrac{1}{N\Delta t (\omega_n+\omega_f -\omega_m)}[\exp(j(\omega_n+\omega_f -\omega_m)t) ] _0 ^{N\Delta t}

=\cfrac{1}{N\Delta t (\omega_n -\omega_m +\omega_f)}(\exp(jN\omega_f \Delta t)-1)

この絶対値 r は、

r = \cfrac{1}{N\Delta t|\omega_n -\omega_m +\omega_f|} \sqrt{(\cos(N\omega_f \Delta t)-1) ^2 + \sin ^2(N\omega_f \Delta t) }

=\cfrac{1}{N\Delta t|\omega_n -\omega_m +\omega_f|} \sqrt{2-2\cos(N\omega_f \Delta t)}

1-\cos\theta = 2\sin ^2\cfrac{\theta}{2} より、

r = \cfrac{2|\sin(N\omega_f \Delta t)|}{N\Delta t|\omega_n -\omega_m +\omega_f|}

n, mの組み合わせについて、以下の3パターンを考えます。

  1. m = n ・・・mは、\omega_mが実際の角周波数に最も近くなる整数値
  2. m = n-1 ・・・mは、a. の1つ前
  3. m = n+1 ・・・mは、a. の1つ後

それぞれについて、FFTスペクトル絶対値 r は、

  1. r_a = \cfrac{2|\sin(N\omega_f \Delta t)|}{|N\omega_f\Delta t|}
  2. r_b = \cfrac{2|\sin(N\omega_f \Delta t)|}{|N\omega_f\Delta t + 2\pi|}
  3. r_c = \cfrac{2|\sin(N\omega_f \Delta t)|}{|N\omega_f\Delta t - 2\pi|}

これについて考えてみます。

まず、a. が最も大きい値となります。
b. とc. については、\omega_f の符号により大小関係が変わります。

  • \omega_f \lt 0 の場合・・・b. のほうが大きい
  • \omega_f \gt 0 の場合・・・c. のほうが大きい

これらをグラフにしてみると、こうなります。横軸にN\omega_f \Delta tを、縦軸に上の3つのスペクトルそれぞれの r をプロットしています。

>> omega_f = [-pi:pi/1000:pi]
>> r_a = 2*sin(omega_f)./omega_f
>> r_b = 2*sin(omega_f)./(omega_f + 2*pi)
>> r_c = 2*sin(omega_f)./(omega_f - 2*pi)
>> plot(omega_f, r_a, omega_f, r_b, omega_f, r_c)

f:id:nokixa:20201211005733p:plain:w400

まず、ピーク周波数前後のスペクトルの大小関係により、ピーク周波数よりどちらに本来の周波数があるかわかります。
また、ピーク周波数前後のスペクトルの大きさの比から、\omega_fの値が推定できるのでは、と考えられます。この比をプロットすると、

>> plot(omega_f, r_c ./r_b)

f:id:nokixa:20201212004244p:plain:w400

と、\omega_f の値にしたがって単調増加する形になります。
なので、2つの比で\omega_fが一意に決まります。

※ここで、./は配列の要素ごとの除算になります。

r_a と r_b または r_c の比でもいいかもしれません。
下のようになります。

>> plot(omega_f, r_b ./ r_a)
>> hold on
>> plot(omega_f, r_c ./ r_a)
>>

f:id:nokixa:20201211235802p:plain:w400

こっちだと、単純な単調変化とならないので、最初にr_b, r_cの大きさ比較を行う必要が出てきます。
前者のもののほうがシンプルに思うので、こちらでいきたいと思います。
ただし、r_b, r_cの値があまり小さかったら中心の周波数をそのまま採用することとします。 大きさの基準としては、スペクトル絶対値の平均の2倍、というくらいかなと。

実データで検証

元のデータに戻って、前後のスペクトルの大きさの比を調べてみます。

>> r_21 = abs(gFFT(21,800))
r_21 =  141.03
>> r_22 = abs(gFFT(22,800))
r_22 =  278.40
>> r_23 = abs(gFFT(23,800))
r_23 =  126.31

1つ前のスペクトルのほうが大きいので、ピーク周波数より少し小さい周波数が本来の周波数のようです。
前後の比およびそこから\omega_fを逆算すると、

>> r_23 / r_21
ans =  0.89568
>> [w, iw] = min(abs(r_c ./ r_b - r_23/r_21))
w =  0.000056458
iw =  891
>> omega_f(iw)
ans = -0.34558
>> omega_f(iw)/pi
ans = -0.11000

となります。だいたい周波数単位の0.11倍小さいところが本来の周波数らしい。
加速度の時間波形にこれを重ねてみます。

>> plot(gData(800*512+1:801*512,2))
>> hold on
>> plot(sin(2*pi*(21-0.11)*[1:512]/512))

f:id:nokixa:20201211014848p:plain:w600

結構いい線行ってる！周期がばっちり。
これでやってみたいと思います。

時系列FFTグラフから回転数列を出してみたいと思います。
以下のOctaveスクリプトを作りました。

function rpm = calc_rpm(fft_abs, df, amp_ratio)

  nt = size(fft_abs)(2);
  nf = size(fft_abs)(1);
  fft_mean = mean(fft_abs(2:end));
  
  for i = 1:nt
    [v, iv] = max(fft_abs(2:end, i));
    v_m1 = 0; v_p1 = 0;
    if(iv >= 2)
      v_m1 = fft_abs(iv-1, i);
    endif
    if(iv != nf)
      v_p1 = fft_abs(iv+1, i);
    endif
    
    if( (v_m1 > fft_mean) && (v_p1 > fft_mean) )
      [r, ir] = min(abs(amp_ratio - v_p1/v_m1));
      rpm(i) = iv + (ir / size(amp_ratio)(2) - 0.5);
    else
      rpm(i) = iv;
    endif
    
  endfor
  
endfunction

これを動かしてみます。

まず、今回の処理を追加する前の時系列FFTグラフ。 上記のスクリプトで、 rpm(i) = iv + (ir / size(amp_ratio)(2) - 0.5); の部分をrpm(i) = iv; で置き換えて表示しました。

>> rpm = calc_rpm(gFFT_abs(1:64,:), 1, r_c./r_b);
>> plot(rpm)

f:id:nokixa:20201212022046p:plain:w600

処理を追加すると、

f:id:nokixa:20201212021725p:plain:w600

うーん…それほどギザギザ感がなくなったかというと…
多少はよくなったぐらいか。
とりあえずこれでやってみようと思います。

マイコン用テーブル作成

マイコンでの実装としては、前後周波数成分比のテーブルを用意して、実際の測定値での前後周波数成分比が入る範囲を探す、ということをしようと思います。
テーブルの要素数分だけ分解能が上がることになります。
今回の設定では187.5[rpm]の分解能なので、20要素用意すれば10[rpm]くらいの分解能を持つことができます。

先ほどの計算から、テーブルの値として使うものを取り出しておきます。

>> r_c(1:100:end)./r_b(1:100:end)
ans =

 Columns 1 through 17:

   0.33333   0.37931   0.42857   0.48148   0.53846   0.60000   0.66667   0.73913   0.81818   0.90476       NaN   1.10526   1.22222   1.35294   1.50000   1.66667   1.85714

 Columns 18 through 21:

   2.07692   2.33333   2.63636   3.00000

>>

\omega_f = 0 のところではr_b, r_c いずれも0になるので、NaNが出てしまっています。
ここは1.0で埋めておけばいいかと思います。

ここまで

一旦ここまでにします。

今回分かったことはどういうことかというと、

  • FFTを使って周波数解析を行う場合、周波数分解能は\cfrac{1}{N\Delta t} (\Delta t : サンプリング間隔、N : サンプリング数)となる
  • 周波数分解能を上げたい場合、N\Delta tを大きくする必要があるが、そうするとサンプリング時間が長くなり、結果処理時の更新レートが下がってしまう
  • ピーク周波数の前後の周波数成分を使うことで、\cfrac{1}{N\Delta t}の非整数倍の周波数成分検出ができ、短いサンプリング時間のままで周波数分解能を上げることができる
    • ただし対象の波形には単一周波数しか含まれていない前提

といったところです。

実際にはすでにマイコンのほうの実装もやって、実際にバイクに載せて動かしてみています。
次回はこれを紹介したいと思います。

マイコンと加速度センサでタコメータを作る - 13. メータモード実装

RXタコメータ マイコン

前回の続きです。

回転数計算処理

加速度の測定を行って、最終的には回転数を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]
    • 1回のFFT用データ取得時間で考えています。これよりデータ処理時間のほうが長くなると、もっと更新間隔は延びます。
    • 1秒に3回程度の更新で、本当はもう少し頻度を上げたいですが、ここは単純に周波数分解能とのトレードオフになります。ここがFFT方式での欠点かな。

加速度データ取得

バッファは以下のように用意しています。加速度ログ用のバッファと共有の領域にしています。
また、バッファの他に書き込みポインタ、バッファセレクタの変数も用意しています。

#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))

FFT Package 1-dim / 2-dim

#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");

f:id:nokixa:20201209000755p:plain:w600

800番目データでの断面を表示してみます。

>> figure(2)
>> plot(abs(gFFT(1:128,800)), "*-")

f:id:nokixa:20201209005320p:plain:w400

窓関数ありだと、こうなります。ちなみに、ハミング窓です。

f:id:nokixa:20201209010343p:plain:w400

今回はピーク周波数を出したいだけなので、窓関数は不要かと考えられます。今回はなしとします。

また、このグラフから、どのようにピーク周波数を出せばいいかが見えてきそうです。

  • DC成分は除いて考えます。このデータでは、加速度センサのZ軸方向測定値を使っているので、重力によるDC成分が出ているものと考えられます。
  • 単純に最大値を調べればよさそうです。ただ、最初の大きなピークを見つけられればそれでいいようにも思えます。その場合はデータによって処理時間が変わるのが少し気持ち悪い。
  • 最大周波数は、回転数にすると48000[rpm]になりますが、実際はここまでいきません。上限を設定してやればピーク検出の最大時間も短縮できそう。
    • 前の速度・回転数換算表によると、私のドラッグスター250では1速、60kmで約12000[rpm]になります。
    • 1速でここまで引っ張ることはないので、これを上限としてよさそうです。
    • 12000[rpm] = 200[Hz] = 64 * 3.125[Hz]

改めて加速度ログからのFFTグラフを出してみます。 周波数点数は64点までにします。

f:id:nokixa:20201209022601p:plain

やっぱりDC成分を除いた最大値を64点の中から探すのでよさそうです。
すなわち、回転数の値のパターンは64個までしかないということになります。

ちょっとそれはどうかな…

今回はここまで

長くなったので、続きは次回にします。
回転数推定の見直しをしていきます。

マイコンと加速度センサでタコメータを作る - 12. メータモード実装

RXタコメータ マイコン

早くRXタコメータを仕上げたい事情が出てきたので、まだ評価作業に微妙なところはありますが、タコメータとして動かせるように機能を実装したいと思います。

タコメータ機能

基本的には、現在の回転数[rpm]を液晶に数字で表示したいと思います。
本当に単純ですが、こんな感じ。

f:id:nokixa:20201205231627p:plain

f:id:nokixa:20201205232658j:plain:w400

  • 真ん中に大きく表示
  • 単位(rpm)も表示しておく
  • 緑色が目に優しいかと、背景は黒か

タコメータ実装

既にFFTグラフリアルタイム表示機能を実装していて、FFT動作周りは同じ処理になります。
回転数の推定(ピーク位置探索およびrpm計算)、LCD表示あたりが新しく実装しないといけないところです。

表示用フォント

GitHubリポジトリのリンクを再掲します。

GitHub - hubnoki/RXtacho: Designing tachometer using RX220

LCD表示について、今は1文字あたり5x8ドットサイズのデータを用意して、これで文字表示を行っています。(ソースコードfont.c,font.h)

このフォントデータは、以下のサイトからもらってきました。

libstock.mikroe.com

実際にLCDに表示してみた様子。

f:id:nokixa:20201206222145j:plain:w500

これを元に、数字とrpmの表示だけ大きなフォントを用意し、LCD表示を行いたいと思います。

typedef struct{
    unsigned char f[5];
}FONT_DATA;

extern const FONT_DATA font[];

const FONT_DATA font[]=
{
     {{0x00, 0x00, 0x00, 0x00, 0x00}} // 20
    ,{{0x00, 0x00, 0x5f, 0x00, 0x00}} // 21 !
    ,{{0x00, 0x07, 0x00, 0x07, 0x00}} // 22 "
    ,{{0x14, 0x7f, 0x14, 0x7f, 0x14}} // 23 #
    ,{{0x24, 0x2a, 0x7f, 0x2a, 0x12}} // 24 $
    ,{{0x23, 0x13, 0x08, 0x64, 0x62}} // 25 %
    ,{{0x36, 0x49, 0x55, 0x22, 0x50}} // 26 &
    ,{{0x00, 0x05, 0x03, 0x00, 0x00}} // 27 '
    ,{{0x00, 0x1c, 0x22, 0x41, 0x00}} // 28 (
    ,{{0x00, 0x41, 0x22, 0x1c, 0x00}} // 29 )
    ,{{0x14, 0x08, 0x3e, 0x08, 0x14}} // 2a *
    ,{{0x08, 0x08, 0x3e, 0x08, 0x08}} // 2b +
    ,{{0x00, 0x50, 0x30, 0x00, 0x00}} // 2c ,
    ,{{0x08, 0x08, 0x08, 0x08, 0x08}} // 2d -
    ,{{0x00, 0x60, 0x60, 0x00, 0x00}} // 2e .
    ,{{0x20, 0x10, 0x08, 0x04, 0x02}} // 2f /
    ,{{0x3e, 0x51, 0x49, 0x45, 0x3e}} // 30 0
    ,{{0x00, 0x42, 0x7f, 0x40, 0x00}} // 31 1
    ,{{0x42, 0x61, 0x51, 0x49, 0x46}} // 32 2
    ,{{0x21, 0x41, 0x45, 0x4b, 0x31}} // 33 3
    ...

unsigned char型の5個の配列で、5x8ドットの文字を表しています。

LCD自体は160x128の解像度ですが、1文字20x32ドット(元の4倍)くらいにすれば数字4桁+rpm は表示されそうなので、そうしてみます。

4倍フォント

以下のように4倍フォントを用意しました。

  • font.hで、FONT_DATA_X4型を用意 (unsigned int型を20個で1文字)
  • FONT_DATA_X4型の配列font_x4[]
#define W_FONT_X4 20

typedef struct{
    unsigned int f[W_FONT_X4];
}FONT_DATA_X4;

extern const FONT_DATA_X4 font_x4[];
  • font.cで、元のフォントデータ(font[])を元に、4倍フォント(font_x4[])を作成
    • 今回使っているRX220のRAM(16KB)にはとても入りませんが、ROM(256KB)には十分入るので、定数となるようにして、ROMに入れてもらうことを期待します。
  • まず、8bitデータを32bitデータに拡張するマクロを用意
#define BYTE_X4(d) \
( ((((unsigned int)d) >> 7) & 0x1) << 31 ) \
( ((((unsigned int)d) >> 7) & 0x1) << 30 ) \
( ((((unsigned int)d) >> 7) & 0x1) << 29 ) \
( ((((unsigned int)d) >> 7) & 0x1) << 28 ) \
( ((((unsigned int)d) >> 6) & 0x1) << 27 ) \
( ((((unsigned int)d) >> 6) & 0x1) << 26 ) \
( ((((unsigned int)d) >> 6) & 0x1) << 25 ) \
( ((((unsigned int)d) >> 6) & 0x1) << 24 ) \
( ((((unsigned int)d) >> 5) & 0x1) << 23 ) \
( ((((unsigned int)d) >> 5) & 0x1) << 22 ) \
( ((((unsigned int)d) >> 5) & 0x1) << 21 ) \
( ((((unsigned int)d) >> 5) & 0x1) << 20 ) \
( ((((unsigned int)d) >> 4) & 0x1) << 19 ) \
( ((((unsigned int)d) >> 4) & 0x1) << 18 ) \
( ((((unsigned int)d) >> 4) & 0x1) << 17 ) \
( ((((unsigned int)d) >> 4) & 0x1) << 16 ) \
( ((((unsigned int)d) >> 3) & 0x1) << 15 ) \
( ((((unsigned int)d) >> 3) & 0x1) << 14 ) \
( ((((unsigned int)d) >> 3) & 0x1) << 13 ) \
( ((((unsigned int)d) >> 3) & 0x1) << 12 ) \
( ((((unsigned int)d) >> 2) & 0x1) << 11 ) \
( ((((unsigned int)d) >> 2) & 0x1) << 10 ) \
( ((((unsigned int)d) >> 2) & 0x1) << 9 ) \
( ((((unsigned int)d) >> 2) & 0x1) << 8 ) \
( ((((unsigned int)d) >> 1) & 0x1) << 7 ) \
( ((((unsigned int)d) >> 1) & 0x1) << 6 ) \
( ((((unsigned int)d) >> 1) & 0x1) << 5 ) \
( ((((unsigned int)d) >> 1) & 0x1) << 4 ) \
( ((((unsigned int)d) >> 0) & 0x1) << 3 ) \
( ((((unsigned int)d) >> 0) & 0x1) << 2 ) \
( ((((unsigned int)d) >> 0) & 0x1) << 1 ) \
( ((((unsigned int)d) >> 0) & 0x1) << 0 )
  • 元のフォントデータを、垂直方向にBYTE_X4マクロで4倍に、水平方向にデータを繰り返すことで4倍にします。エディタの力を借りてゴリゴリ書きます。
const FONT_DATA_X4 font_x4[] = 
{

     {{
        BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00),
        BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00),
        BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00),
        BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00),
        BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00)
    }} // 20
    ,{{
        BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00),
        BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00),
        BYTE_X4(0x5f),BYTE_X4(0x5f),BYTE_X4(0x5f),BYTE_X4(0x5f),
        BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00),
        BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00)
    }} // 21 !
    ,{{
        BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00),
        BYTE_X4(0x07),BYTE_X4(0x07),BYTE_X4(0x07),BYTE_X4(0x07),
        BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00),
        BYTE_X4(0x07),BYTE_X4(0x07),BYTE_X4(0x07),BYTE_X4(0x07),
        BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00),BYTE_X4(0x00)
    }} // 22 "
    , ...

LCD表示

上記の4倍フォントをLCD上に表示できるようにします。

LCD制御の処理はlcdc_ST7735S.c(.h)に作ってあり、現状、文字表示の関数として以下を用意しています。

void lcdc_putchar(char c, _UWORD color, _UWORD x0, _UWORD y0);
void lcdc_puts(char *s, _UWORD color, _UWORD x0, _UWORD y0);

基準位置と表示したい文字(列)を指定する形です。
これらの4倍フォントを使用するバージョンを用意します。

void lcdc_putchar_x4(char c, _UWORD color, _UWORD x0, _UWORD y0);
void lcdc_puts_x4(char *s, _UWORD color, _UWORD x0, _UWORD y0);

関数名を変えただけです。
これらの関数の実装は、元のものから少しいじって用意します。

//-------------------------------------------------
// 4x font functions
//-------------------------------------------------

// bit_ptn[j].bit(b) : (j)th y, (b)th x
// display area : rectangle of (x0, y0), (x0 + 31, y0 + dy - 1)
static void lcdc_draw_32x(const unsigned int *bit_ptn, _UWORD color, _UWORD x0, _UWORD y0, _UWORD dy)
{
    int i, j;
    unsigned int tmp;


    lcdc_set_area(x0, x0 + 31, y0, y0 + dy-1);

    write_command(0x2C);

    DEVICE_RS_SET();
    DEVICE_CS_CLEAR();

    for(j = 0; j < dy; j++){          // Row loop

        tmp = bit_ptn[j];

        for(i = 0; i < 32; i++){ // Column loop

            if( tmp & 0x01 ){
                DEVICE_SPI_TX_BUFW( (_UBYTE)(color >> 8) );
                DEVICE_SPI_TX_BUFW( (_UBYTE)color );
            }
            else{
                DEVICE_SPI_TX_BUFW(0x0);
                DEVICE_SPI_TX_BUFW(0x0);
            }

            tmp >>= 1;
        }
    }

    while(!DEVICE_SPI_TX_END)
        ;
    DEVICE_CS_SET();

}


void lcdc_putchar_x4(char c, _UWORD color, _UWORD x0, _UWORD y0)
{
    int i, j;
    unsigned char index;
    
    if( (c < 0x20) || (c > 0x7F) )
        index = 0x20;
    else
        index = c - 0x20;
        
    lcdc_row_col_exchange();

    lcdc_draw_32x(font_x4[index].f, color, y0, x0, W_FONT_X4);

    lcdc_row_col_exchange();
}

void lcdc_puts_x4(char *s, _UWORD color, _UWORD x0, _UWORD y0)
{
    _UWORD x_tmp, y_tmp;
    char *p;

    p = s;

    x_tmp = x0;
    y_tmp = y0;

    while(*p != '\0'){
        if(x_tmp > LCDC_X_WIDTH - W_FONT_X4){
            // If there is not enough space for x direction,
            // rewind x position, and increment y position.
            x_tmp = x0;
            y_tmp += 32 + VERTICAL_PITCH;
            if(y_tmp >= LCDC_Y_WIDTH)
                break;
        }
        else{
            lcdc_putchar_x4(*p, color, x_tmp, y_tmp);
            p++;
            x_tmp += (W_FONT_X4 + HORIZONTAL_PITCH);
        }
    }

}

4倍フォント表示確認

詳細は省略しますが、UARTからのコマンド入力で機能テストができるようにしているので、ここに4倍フォント表示テストコマンドを追加して動かしてみたいと思います。

TeratermでCOMポートに接続し、以下のように入力します。

**** program start ****
Built on e2studio version 6.2.0
RX UART > lcd_area 0 9F 0 7F

RX UART > lcd_fill 0 0 0
color:0
Elapsed time : 79ms

RX UART > lcd_puts_x4
string to display :
1234rpm

RX UART >

f:id:nokixa:20201206221544p:plain:w400

LCD表示はこうなりました。

f:id:nokixa:20201206230316j:plain:w500

いい感じ。
ぎざぎざですが。

ひとまずここまで

タコメータ用の文字表示ができるようになりました。
どんどんタコメータ機能の実装を進めていきます。

マイコンと加速度センサでタコメータを作る - 11. 加速度ログ取得変更

RXタコメータ マイコン

前回、大まかに加速度センサの測定間隔を見ることができたので、今度は加速度ログにこの測定間隔を追加、これを使って結果の解析をできるようにしたいと思います。

加速度ログの変更

加速度ログの中に、ある程度の頻度で割り込み間隔測定値を混ぜます。あまり頻度高く混ぜるとSD書き込みが間に合わなくなるので、バッファを1周したくらいで記録、というのでいいかと思います。

先ほどは1回1回の割り込み間隔を測定しましたが、複数回の割り込み間隔を見たほうが精度が上げられるので、そのようにします。

加速度バッファは1024データを保持できるようになっているので、1600Hzの測定レートで考えると、バッファ1周分の時間は 0.625ms x 1024 = 640ms となります。

CMTモジュールでは、カウンタ1周の時間が約26msだったので、これでは足りません。
ただ、カウント元クロックをPCLK/8、PCLK/32、PCLK/128、PCLK/512と変えられるので、最大で25.6us単位、約1.67秒までカウントすることができます。

これであれば、それなりの分解能を持ちつつ測定レート1600Hzにも対応できるので、カウント元クロックPCLK/512のCMTモジュールで測定しようと思います。

    //// 16bit freerun counter (CMT1) setting ////
    // CMT1.CMCR.BIT.CKS = 0x0;    // count source : PCLK / 8
    CMT1.CMCR.BIT.CKS = 0x3;   // count source : PCLK / 512

以下のように加速度ログフォーマットを変更します。

  • ヘッダに割り込み間隔測定頻度、CMTモジュールのCMCRレジスタCKSの値を追加
  • バッファの一番最初にデータを書いてから、次に同じ位置にデータを書いたら割り込み間隔測定値を挿入します。
  • 加速度データ(2byte) フラグ(2byte) 加速度データ(2byte) フラグ(2byte) ... とデータを入れていましたが、その途中に割り込み間隔測定値を追加します。割り込み間隔測定値は16bit(2byte)データですが、加速度データと同様にデータ→フラグの形で記録しようと思います。フラグのところに、「割り込み間隔測定値」ビットを用意します。

データファイルフォーマット :

アドレス(byte) サイズ(byte) 内容 値
0x0 0x4 ヘッダ識別子 'LOGH' (0x4C4F4748)
0x4 0x4 ヘッダバージョン 0x1
0x8 0x4 ヘッダデータサイズ(ヘッダ識別子、バージョンは含まない) 0xC
0xC 0x4 測定レート ADXL345レジスタに設定する4bitコード
0x10 0x4 測定レンジ ADXL345レジスタに設定する2bitコード
0x14 0x2 割り込み間隔測定頻度 ログバッファのサイズ
0x16 0x2 CMTモジュール CMCRレジスタCKS値
0x18以降 加速度データ、割り込み間隔データ


コードの変更は以下の通り。

  • ログデータの1単位を示す構造体です。
// Flags of log data
//     flags.bit0    overrun - Overrun occurred, overwriting data which are not read
//  flags.bit1    measureIntCnt - Data contains measurement interval count
typedef union{
    struct{
        uint8_t overrun : 1;
        uint8_t measureIntCnt : 1;
    };
    uint16_t wd;
} LOG_FLAGS;

typedef struct{
    signed short d;
    LOG_FLAGS flags;
} LOG_UNIT;
  • バッファに1周分以上書き込みを行ったかどうかのフラグが必要になるので、以下を追加しました。
static struct{
    ...

    uint8_t            g_buf_filled; // All elements of the buffer are filled with valid data
    ...

}vars;
  • 以下のようにヘッダ書き込み部分を関数化、内容変更しました。CMTモジュールのCMCRレジスタ値は、後でカウント元クロック分周比が分かるようにするため、追加しています。
#include "iodefine.h"

static FRESULT write_log_header_bin(FIL *f)
{
    FRESULT fres = 0;
    unsigned int tmp;
    UINT btw;

    // Header identifier
    tmp = 'LOGH';
    fres |= f_write(f, &tmp, sizeof(int), &btw);
    // Header version
    tmp = 2;
    fres |= f_write(f, &tmp, sizeof(int), &btw);
    // Header contents size (in bytes)
    tmp = 12;
    fres |= f_write(f, &tmp, sizeof(int), &btw);
    //// Header contents ////
    // Rate setting
    tmp = value_list_rate[vars.cur_value_idx[STG_RATE]].val;
    fres |= f_write(f, &tmp, sizeof(int), &btw);
    // Range setting
    tmp = value_list_range[vars.cur_value_idx[STG_RANGE]].val;
    fres |= f_write(f, &tmp, sizeof(int), &btw);
    // Time measurement interval
    tmp = (N_LOGBUF & 0xFFFF) | (CMT1.CMCR.BIT.CKS << 16);
    fres |= f_write(f, &tmp, sizeof(int), &btw);

    return fres;
}
  • 加速度センサからの割り込み発生時、バッファを1周したときの時間を測定します。
void adxl345_int_routine()
{
    signed short d[3];
    FIX_T f;
    LOG_UNIT log_d;
    uint16_t int_cnt = ADXL345_INT_CNTR;

    // if(vars.g_int_update == 0){
    //     vars.g_int_intvl = int_cnt - vars.g_int_cnt_last;
    //     vars.g_int_update = 1;
    // }
    // vars.g_int_cnt_last = int_cnt;

    ADXL345_get(d);

    // Put data into buffer
    if(vars.logging){

        if(vars.g_buf_wp_a == 0){
            // 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;
        }

        log_d.d = d[vars.axis_sel];
        log_d.flags.wd = 0;
        log_d.flags.overrun = (vars.g_overrun = (vars.g_buf_wcnt_a - vars.g_buf_rcnt) >= N_LOGBUF-1);

        vars.g_buf[vars.g_buf_wp_a] = log_d;

        if(++vars.g_buf_wp_a >= N_LOGBUF){
            vars.g_buf_wp_a = 0;
            vars.g_buf_filled = 1;
        }
        vars.g_buf_wcnt_a++;
    }
    else{
            ...
    }
}
  • ログファイルへの書き込み時、バッファに1周分以上データが書かれていて、かつバッファの0番目要素を読み出す場合、加速度データを書いた後に割り込み間隔測定値を書き込みます。
    書き込みサイズは特に考えていませんでしたが、最低限のサイズ(16bit)にしておきます。
    (CSV記録は特に使うことはないと思うので、こちらは省きます。)
        // Continue logging
        else{
            td = timer_soft_count(&tm);
            timer_soft_reset(&tm);
            if(td > tmax)
                tmax = td;

            if(vars.g_buf_wcnt_a != vars.g_buf_rcnt){ // There remains unread data
                unsigned int tmp = 0;
                LOG_UNIT logData = vars.g_buf[vars.g_buf_rp];
                unsigned int ovrn = vars.g_overrun;

                if(vars.log_format == LOG_FMT_CSV){
                    fres = (
                            f_printf(&(wk.fl), "%d,%d\r\n"
                            , logData.d, logData.flags.wd)
                        == EOF);
                }
                else if(vars.log_format == LOG_FMT_BIN){
                    fres |= f_write(&(wk.fl), &logData, sizeof(LOG_UNIT), &btw);

                    // Write measure interval count after g data
                    if((vars.g_buf_rp == 0) && vars.g_buf_filled){
                        logData.d = vars.g_int_intvl;
                        logData.flags.measureIntCnt = 1;
                        fres |= f_write(&(wk.fl), &tmp, sizeof(unsigned int), &btw);
                    }

                }
                                ...

            }
        }

...
  • ついでに、ログファイル読み込みのoctaveスクリプトも変更します。
    • 読み出し結果のgDataに、その時々の測定時間間隔(dt)を追加します。FFTを行う際に、対象データの中のいずれかのdtを使って実周波数を計算します。
function [gData ovrn] = read_adxl345_log_bin_2byte(fname)

  # gData(:,1) : Time [s]
  # gData(:,2) : Acceleration [G]
  # gData(:,3) : Measure interval [s]

  fid = fopen(fname, "r");
  d = dir(fname);
  dataSize = d.bytes;

  ########################
  # Read header
  ########################
  hdrId = fread(fid, 1, "uint32");
  
  if (hdrId == 0x4C4F4748) # 'LOGH', little endian
    hdrVer = fread(fid, 1, "uint32");
    hdrSize = fread(fid, 1, "uint32");
    
    printf("Header detected, Ver:%d, Size:%d\n", hdrVer, hdrSize);

    if(hdrVer >= 1)
      hdrRate = fread(fid, 1, "uint32");
      hdrRange = fread(fid, 1, "uint32");
      
      rateCodes = [0x0, 0x1, 0x2, 0x3, 0x4, 0x5, 0x6, 0x7,  ...
                   0x8, 0x9, 0xA, 0xB, 0xC, 0xD, 0xE, 0xF];
      rateVals = [0.1, 0.2, 0.39, 0.78, 1.56, 3.13, 6.5, 12.5,  ...
                  25, 50, 100, 200, 400, 800, 1600, 3200]; # Measure frequency in Hz
      
      rangeCodes = [0x0, 0x1, 0x2, 0x3];
      rangeVals = [2.0, 4.0, 8.0, 16.0]; # Measure range in G

      rate = rateVals(find(rateCodes == hdrRate)(1));
      range = rangeVals(find(rangeCodes == hdrRange)(1));
      
      dt = 1 / rate;
      gUnit = range * 2 / 1024.0; # 10bit ADC, -range ~ +range
      
      printf("Rate:%d[Hz], Range:%d[G]\n", rate, range);
      
    endif

    if(hdrVer >= 2)
      intMeasureFreq = fread(fid, 1, "uint16");
      
      hdrIntMeasureClkDiv = fread(fid, 1, "uint16");
      switch(hdrIntMeasureClkDiv)
        case 0x0
          intMeasureClkDiv = 8;
        case 0x1
          intMeasureClkDiv = 32;
        case 0x2
          intMeasureClkDiv = 128;
        case 0x3
          intMeasureClkDiv = 512;
        otherwise
          intMeasureClkDiv = 8;
      endswitch

      intMeasureCntUnit = 50 * 10^-9 * intMeasureClkDiv; # 50 ns * (divide count)

    else
      # Interval count is not included in the log
      intMeasureFreq = 0;
      intMeasureCntUnit = 1;

    endif

    dataTop = 12 + hdrSize;
    dataSize -= 12 + hdrSize;

  else # Header was not detected
    dt = 1;
    gUnit = 0.004; # 4mg/LSB
    fseek(fid, 0, SEEK_SET);
    dataTop = 0;

    intMeasureFreq = 0;
    intMeasureCntUnit = 1;

    printf("Header was not detected\n");
  endif

  ########################
  # Read data body
  ########################
  if(intMeasureFreq == 0)
    gData = fread(fid, Inf, "int16", 2);
    gData = [dt*[1:size(gData)(1)].', gUnit*gData, dt];
      # gData(:,1) : Time [s]
      # gData(:,2) : Acceleration [G]
      # gData(:,3) : Measure interval [s]
    fseek(fid, dataTop + 2, SEEK_SET);
    ovrn = fread(fid, Inf, "int16", 2);

  else
    # Allocate data array (Reduced later)
    gData = zeros(dataSize/2/2, 3);
      # gData(:,1) : Time [s]
      # gData(:,2) : Acceleration [G]
      # gData(:,3) : Measure interval [s]
    ovrn = zeros(dataSize/2/2);

    p = 0;
    t = 0;
    while(1)
      # Get data unit from log file
      [d c] = fread(fid, 2, "int16=>int16");
      if(c != 2)
        break;
      endif

      ov = bitget(d(2),1);
      MsIntCnt = bitget(d(2),2);

      if(MsIntCnt == 1)
        # Update dt
        dt = double(d(1)) * intMeasureCntUnit / intMeasureFreq;
      else
        p++; # Increment in advance, finally p indicates the number of acceleration data
        gData(p,1) = t;
        gData(p,2) = d(1);
        gData(p,3) = dt;
        ovrn(p) = ov;
        t += dt;
      endif

    endwhile

    # Remove unnecessary area
    gData = gData(1:p,:);
    ovrn = ovrn(1:p);

  endif

  fclose(fid);


endfunction

ちょっとごちゃごちゃしてきました…

いくつか備忘録。

  • freadの精度指定 (freadのフォーマット : val = fread (fid, size, precision, skip, arch)におけるprecision)で、"int16=>int16"のように書くと、"読み出し元をint16として解釈して読み出し"→"int16型に格納"となります。
  • bitgetで指定の位置のビット値を得られます。

次回

次回は実際に加速度ログを取ってみて確認したいと思います。