【倒立振子part31】タイマー割り込みで角度を取得する

前回はタイマー割り込みを初めて実装してみた。
今回はタイマー割り込みを使ってカルマンフィルタの結果を配列に入れてサンプリングタイムごとに値を取得する。

プログラム
解説
次回

プログラム

#include <M5Atom.h>
#include "Kalman.h"

#define samp_time 500 //[ms]
#define data 10

//タイマー割り込み用設定
hw_timer_t *timer = NULL; //タイマー0の割り込みtimerで定義
volatile SemaphoreHandle_t timerSemaphore;  //セマフォの宣言（割込み発生の確認用）
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED; //排他制御の利用を宣言

//タイマー割り込み用変数
volatile int count = 0;   //1msタイマカウンタ用
volatile int sec_up = 0;  //1秒経過フラグ用
// グローバル変数宣言
int start = 0;    //カウント開始フラグ
int sec = 0;      //秒カウント用
int sec_disp = 0;      //秒カウント用
float array_data[data];
int serial_frag = 0;  //シリアル通信用のフラグ

unsigned long oldTime = 0, loopTime, nowTime;
float dt;

//カルマンフィルタ用変数
float gyroX = 0, gyroY = 0, gyroZ = 0;
float gyroXoffset = 0, gyroYoffset = 0, gyroZoffset = 0;
float accX = 0, accY = 0, accZ = 0;
float accXoffset = 0, accYoffset = 0, accZoffset = 0;
Kalman kalmanY;
float kalAngleY, kalAngleDotY;
float theta_acc;
float theta_dot;
long tick = 0;
bool IMU6886Flag = false;

// tim0のタイマー割り込みハンドラ。この処理はsetupより先に書く
// タイマ割込み発生時に実行される処理 (IRAM_ATTRで宣言してRAM上に配置）
void IRAM_ATTR onTimer() {
  portENTER_CRITICAL_ISR(&timerMux);  //排他制御で以下を実行（割込み禁止）
  if(start == 1) {                    //カウント開始フラグが1なら
    count++;                          //1msタイマカウンタ +1
    if (count >= samp_time) {               //1000回＝1秒経過していたら
      sec_up = 1;                     //1秒経過フラグをON (メイン処理でリセットする）
      count = 0;                      //1msタイマカウンタをリセット
    }
  }
  portEXIT_CRITICAL_ISR(&timerMux);   //排他制御終了（割り込み許可）
  xSemaphoreGiveFromISR(timerSemaphore, NULL);  //セマフォを開放
}

//==============================================================
//ジャイロオフセット関数
//==============================================================
void offset_cal(){
  delay(5000);
  accXoffset = 0;
  accYoffset = 0;
  accZoffset = 0;
  gyroXoffset = 0;
  gyroYoffset = 0;
  gyroZoffset = 0;
 
  for(int i=0; i<10; i++) {
    M5.IMU.getAccelData(&accX,&accY,&accZ);
    M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
    delay(10);
    accXoffset += accX;
    accYoffset += accY;
    accZoffset += accZ;
    gyroXoffset += gyroX;
    gyroYoffset += gyroY;
    gyroZoffset += gyroZ;
  }
 
  accXoffset /= 10;
  accYoffset /= 10;
  accZoffset = accZoffset / 10 + 1.0;
  gyroXoffset /= 10;
  gyroYoffset /= 10;
  gyroZoffset /= 10;
}

//==============================================================
//初期設定関数
//==============================================================
void setup() {
  M5.begin(false, false, true);
  Serial.begin(115200);

  //タイマー割り込み用初期設定
  timerSemaphore = xSemaphoreCreateBinary();  //バイナリセマフォを作成(0か1のバイナリ)
  timer = timerBegin(0, 80, true);  // タイマ作成
  timerAttachInterrupt(timer, &onTimer, true);  // タイマ割り込みサービス・ルーチン onTimer を登録
  timerAlarmWrite(timer, 1000, true);  // 割り込みタイミング1ms(1us × 10000)の設定
  timerAlarmEnable(timer);  // タイマ有効化

  //カルマンフィルタ用初期設定
  if (M5.IMU.Init() != 0)
    IMU6886Flag = false;
  else
    IMU6886Flag = true;
  delay(500);
  //フルスケールレンジ
  M5.IMU.SetGyroFsr(M5.IMU.GFS_500DPS);
  M5.IMU.SetAccelFsr(M5.IMU.AFS_4G);
  //センサオフセット算出
  offset_cal();
  kalmanY.setAngle(get_theta_acc());
}

//==============================================================
//メイン関数
//==============================================================
void loop() {
  M5.update();
    //オフセット再計算
  if (M5.Btn.isPressed()){
    M5.dis.drawpix(10, 0xff0000);
    start = !start;                 //カウント開始フラグ反転
    if(start == 1) {                //カウント開始フラグが1なら
      M5.dis.drawpix(0, 0x00ff00);  //本体LED（緑）
    } else {                        //カウント開始フラグが0なら
      M5.dis.drawpix(0, 0xffffff); //本体LED（白）
    }
    Serial.println("オフセット再計算");
    offset_cal();
  }
  //カルマンフィルタによる角度取得
  nowTime = micros();
  loopTime = nowTime - oldTime;
  oldTime = nowTime;
  dt = (float)loopTime / 1000.0; //sec
  kalAngleY = kalmanY.getAngle(get_theta_acc(), get_gyro_data(), dt);

  if (xSemaphoreTake(timerSemaphore, 0) == pdTRUE) {  //セマフォが解放されたら
    M5.dis.drawpix(20, 0x00ff00);
    if (sec_up == 1) {
      //割り込み後に実行する内容
      array_data[sec] =  -kalAngleY;
      portENTER_CRITICAL(&timerMux);  //排他制御で以下を実行（割込み禁止）
      sec_up = 0;
      Serial.print(sec);
      Serial.print(",");
      Serial.println(array_data[sec]);
      sec ++;
      portEXIT_CRITICAL(&timerMux); //排他制御終了（割り込み許可）
      if(sec == (data)){  //配列に0からdata-1までのdata個のデータが入ったとき
        start = !start;     //タイマー割り込みのカウントを止める
        sec = 0;            //配列を0にして再測定できるようにする
      }
    }
  }
  delay(5);
}

//==============================================================
//ジャイロ関数
//==============================================================
float get_theta_acc() {
  M5.IMU.getAccelData(&accX,&accY,&accZ);
  //傾斜角導出 単位はdeg
  theta_acc  = atan(-1.0 * (accX - accXoffset) / (accZ - accZoffset))* RAD_TO_DEG;
  return theta_acc;
}
 
//Y軸 角速度取得
float get_gyro_data() {
  M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
  theta_dot = (gyroY - gyroYoffset);
  return theta_dot;
}

解説

カルマンフィルタの設定とタイマー割り込みの設定は過去のものをそのまま使用する。

5行目の定義文で取得するデータの個数を指定し、19行目の配列にデータを格納していく。
ここで覚えておきたいのは、array_dataにはdata個のデータが入るが、配列は0からdata-1の合計data個であるということを覚えておく。(間違ってdata+1個のデータを取ろうするとvfする)

#define data 10
float array_data[data];

115行目から125行目までで、カルマンフィルタ用のオフセットと割り込みときにサンプリングをカウントするための変数startの値を立ち上げるで測定を始める。

  if (M5.Btn.isPressed()){
    M5.dis.drawpix(10, 0xff0000);
    start = !start;                 //カウント開始フラグ反転
    if(start == 1) {                //カウント開始フラグが1なら
      M5.dis.drawpix(0, 0x00ff00);  //本体LED（緑）
    } else {                        //カウント開始フラグが0なら
      M5.dis.drawpix(0, 0xffffff); //本体LED（白）
    }
    Serial.println("オフセット再計算");
    offset_cal();
  }

138行目でカルマンフィルタで測定したデータを配列に入れる。カルマンフィルタはメイン文で常時取り続けておりサンプリングタイムになったときのデータのみを配列に入れるイメージ。
ここでonTime()関数にこの文を入れるのはNGらしい。
どうやら割り込みが発生するタイミングと、loop() 関数内での処理の競合が起きる可能性があるらしいのでメイン文に入れた。

array_data[sec] =  -kalAngleY;

145行目から148行目で配列にdata個のデータが入ると変数startの数値を反転してカウントを止めて配列にデータに入れないようにする。

     if(sec == (data)){  //配列に0からdata-1までのdata個のデータが入ったとき
        start = !start;     //タイマー割り込みのカウントを止める
        sec = 0;            //配列を0にして再測定できるようにする
      }