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

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

加速度ログの変更

加速度ログの中に、ある程度の頻度で割り込み間隔測定値を混ぜます。あまり頻度高く混ぜると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で指定の位置のビット値を得られます。