M5Stackでマルチタスクを使ってセンサ取得・ログ記録を行う

概要

センサ値の取得とログの記録は切っても切れない関係である。

M5Stack Grayに内蔵された加速度センサやジャイロセンサや磁気センサ、外付けの気圧センサの値を高頻度に取得、SDへ記録をしたいが、 何も工夫をせずに取得→記録のプログラムを実行すると、SDへの書き込み時間の変動のため、周期が変動してしまい、不都合である。

そこで、FreeRTOSの機能を使いセンサ値の取得とログの書き込みをそれぞれマルチタスクで処理させることで、 高頻度かつ安定した時間でセンサ値取得・ログ記録を行う方法を述べる。

背景

M5Stack Grayには加速度センサ・ジャイロセンサ・磁気センサが内蔵されている。
これらを使ってなにかやろうと思うと、まずは値をログしたくなる。
今回はそれらに加え、Groveで外付けの気圧センサをつなぎ、加速度・ジャイロ・磁気・気圧の4つのセンサ値をログすることにした。

幸いM5StackにはSDカードスロットがついているのでSDカードにログを記録できる。
だが、単純にセンサ取得、記録をしていくと問題が起きる。

例えば、下のグラフは単にセンサ値取得→SDへログ記録を25Hz（40ms）で繰り返したときの一周期あたりの処理時間である。
およそ4.5秒に1回、処理時間が伸びていることがわかる。

M5Stackでセンサ値取得→SDへログ記録を25Hzで繰り返した際の処理時間

原因を見るために、各処理ごとに処理時間を計測した結果がこちら。

処理時間内訳

凡例は上から「加速度ジャイロ取得」「磁気取得」「姿勢計算」「気圧取得」「ログ書き込み」だが、 処理時間が伸びる原因は一目瞭然で、ログ書き込みである。 SDは一般的にある書き込み量の単位を超えると書き込みに時間がかかってしまう。
これでは一定周期を前提とする制御などには不都合である。

1秒ごとにまとめて書き込む（失敗）

最初に思いつくのは、「毎周期SDに書き込むのは無駄だから、ログを一定周期分まとめて書き込めばいいんじゃね？」だが、 結局まとめて書き込む際にSDの一定の書き込み量を超えると処理時間遅延が生じる。

以下はこの処理を実装したときの周期あたりの処理時間。結局書き込みに時間がかかっている。

1秒ごとにまとめて書き込んで見る（失敗）

割り込みを使ってセンサ取得する（失敗）

SDに書き込んでいる間センサ取得が止まってしまうのが問題なので、 割り込みを使ってセンサ値取得を優先的に行ってみる。
こうすると、SDへのログ書き込み中でも優先的にセンサ値が取得される。
幸い、SDは書き込みを中断しても処理的に問題ないため、割り込みセンサ取得も可能である。

だが、M5Stackでこれを行うと、以下のエラーが生じて再起動してしまう。

[IGuru Meditation Error: Core  1 panic'ed (Interrupt wdt timeout on CPU1)

どうやら割り込み中にセンサ値取得などの長ったらしい処理をしているとWDTが発動して再起動してしまうようだ。
WDTを停止するという方法もあるかもしれないが、他の処理に悪影響を及ぼす可能性もあるので、やめておく。

マルチタスクを使う（成功）

さて、ここで活躍するのがタスク機能である。

ESP32-Arduino-coreではオープンソースのリアルタイムOSである、freeRTOSが使われている。
リアルタイムOSの詳細やその使い方については以下のウェブサイトが非常に詳しい。

miqn.net

リアルタイムOSが何たるかは上のページを参照していただくとして、 ここではごくごく簡単に使い方を書くと、

• タスク用関数を定義する。

• xTaskCreatePinnedToCore()でタスクを生成する。その際、タスク用関数とその優先度などを指定する。

Arduino環境でESP32の開発をすると、setup()やloop()の関数を使ってプログラムを書くことになるが、 これらの関数もfreeRTOSのタスクを使って実装されている。

以下はesp32-1.0.4\cores\esp32\main.cppのコードの一部である。
メイン関数でloopTaskがタスクとして生成され、loopTask関数内でsetup()とloop()のループが呼ばれていることがわかる。
setup()が一度だけ呼ばれ、その後loop()が無限ループするのはこのためである。

#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_task_wdt.h"
#include "Arduino.h"
TaskHandle_t loopTaskHandle = NULL;
#if CONFIG_AUTOSTART_ARDUINO
bool loopTaskWDTEnabled;
void loopTask(void *pvParameters)
{
    setup();
    for(;;) {
        if(loopTaskWDTEnabled){
            esp_task_wdt_reset();
        }
        loop();
    }
}
extern "C" void app_main()
{
    loopTaskWDTEnabled = false;
    initArduino();
    xTaskCreateUniversal(loopTask, "loopTask", 8192, NULL, 1, &loopTaskHandle, CONFIG_ARDUINO_RUNNING_CORE);
}
#endif

また、M5Stackに内蔵されているESP32はデュアルコアのため、コア0、コア1の2つを使うことができる。
これらのコアを有効利用するためにも、freeRTOSを有効に使う必要がある。

xTaskCreateUniversalがタスク生成の関数だが、最後の引数がそのタスクを実行するコアを指定している。 CONFIG_ARDUINO_RUNNING_COREは1と定義されているため、loopTaskはコア1で実行されることになる。

これと同様に、センサ取得タスクとログ記録タスクを作成し、各コアに割り付けてやる。

タスク関数の作成

センサ取得タスクとログ記録タスク関数を記述する。

タスク関数は、今回のように周期的に行うタスクの場合内部にwhileループを持つ。
返り値はvoid, 引数はvoid*だ。引数の型からわかるように、何でも受け取れる。
以下はセンサ取得タスク関数。

#define AHRS_SAMPLE_RATE 50
#define AHRS_SAMPLE_MS (1000.0 / AHRS_SAMPLE_RATE)

typedef struct _log_data_ {
    unsigned long time;
    float accX, accY, accZ;
    float gyroX, gyroY, gyroZ;
    float magX, magY, magZ;
    float roll, pitch, yaw;
    float temperature;
    float pressure;
} LOG_DATA;

LOG_DATA log_data[2][AHRS_SAMPLE_RATE];  // ダブルバッファ

unsigned long ms_begin;

void prvGetSensorTask(void *pvParameters)
{
  LOG_DATA *buf;

  lid = 0;
  double_buffer = 0;
  buf = log_data[double_buffer];

  portTickType xLastWakeTime;
  xLastWakeTime = xTaskGetTickCount();

  ms_begin = millis();

  while (1)
  {
   // ここで色々センサからデータを取得し、バッファに格納する・・・
  // （中略）

    buf[lid].time = millis() - ms_begin;  // 経過時間
    lid++;
    if (lid == AHRS_SAMPLE_RATE)  // ある程度バッファに溜めてからSDに書き込む
    {
      double_buffer = !double_buffer;  // ダブルバッファを使う
      buf = log_data[double_buffer];
      lid = 0;
      logged = true;  // ログ記録タスクで参照するフラグ
    }
    // このタスクは周期的に実行される
    vTaskDelayUntil(&xLastWakeTime, AHRS_SAMPLE_MS / portTICK_PERIOD_MS);
  }
}

上の関数では、センサの値を周期的に読み取るため、vTaskDelayUntil()を使っている。
今回の場合、AHRS_SAMPLE_MS[ms]ごとにこのwhileループが回ることになる。

原理的には、周期タスクとせずにメイン関数からタイマで毎回タスクを生成してもいいのだが、タスク生成のオーバーヘッドが大きいのでおすすめしない。というか多分正しい使い方ではない。

また、センサ取得とログ記録をマルチタスクで行うため、ダブルバッファを使っている。
ダブルバッファ自体は簡易的に実装していて、SDに書き込むデータ量の2回分のバッファを用意し、1回分が溜まったら裏側に切り替え、それが埋まったら表側に切り替え、ということをしている。

次に、ログ記録タスク関数はこちら。

void prvWriteLogTask(void *pvParameters)
{
  LOG_DATA *ld;
  while (1)
  {
    if (logged)
    {
      logged = false;
      ld = (LOG_DATA *)pvParameters + AHRS_SAMPLE_RATE * (!double_buffer);  // 書き込むバッファの表or裏

      File f = SD.open(log_filepath, FILE_APPEND);
      if (f)
      {
        for (int i = 0; i < AHRS_SAMPLE_RATE; i++)
        {
          sprintf(logtext, "%lu,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f",
                  ld->time,
                  ld->accX, ld->accY, ld->accZ,
                  ld->gyroX, ld->gyroY, ld->gyroZ,
                  ld->pitch, ld->roll, ld->yaw,
                  ld->magX, ld->magY, ld->magZ,
                  ld->pressure);
          f.println(logtext);
          ld++;
        }
        f.close();
      }
      else
      {
        M5.Lcd.println("Failed!");
      }
    }
    delay(1);  // これ大事
  }
}

センサ取得タスクで、バッファが溜まったらloggedフラグを立て、それを記録タスクで参照する。
バッファにデータが溜まったことをフラグで確認したら、バッファの表裏を間違えないようにして、SDのに書き込む。

この中で特に大事なのは最後のdelay(1)である。
マルチタスクをする場合、タスクにdelay()を入れないと優先度によってはRTOSの他の処理を行うことができず、WDTが作動してしまったりする。
なので、タスクの最後にdelay()を入れて、他の処理ができるようにする。

ちなみに、ESP32のArduinoライブラリでは、delay()が内部ではタスク用のvTaskDelay()で実装されている。

// esp32-hal-misc.c
void delay(uint32_t ms)
{
    vTaskDelay(ms / portTICK_PERIOD_MS);
}

タスクを生成する

タスク用関数ができたら、メインの関数からそれらを生成してやる。

// MultiTask setting
#define SENSORTASK_CORE 0
#define SENSORTASK_PRI 1
#define LOGTASK_CORE 1
#define LOGTASK_PRI 0

void logging()
{
  TaskHandle_t h_WriteLogTask, h_GetSensorTask;

  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setCursor(0, 0);
  M5.Lcd.println("Press B to stop logging!");
  setLogFileName();
  logWriteHeader();

  // Multi task start
  xTaskCreatePinnedToCore(prvGetSensorTask, "GetSensorTask", 4096, NULL, SENSORTASK_PRI, &h_GetSensorTask, SENSORTASK_CORE);
  xTaskCreatePinnedToCore(prvWriteLogTask, "WriteLogTask", 4096, (void *)log_data, LOGTASK_PRI, &h_WriteLogTask, LOGTASK_CORE);

  while (1)  // ボタンが押されたら終了
  {
    M5.update();
    if (M5.BtnB.wasReleased())
      break;
    delay(10);
  }
  M5.Lcd.printf("\nSaved as %s", log_filepath);

  vTaskDelete(h_GetSensorTask);
  vTaskDelete(h_WriteLogTask);

  delay(3000);
}

ESP32の場合は、デュアルコアのため明示的にコアを指定してタスクを生成するxTaskCreatePinnedToCore()を使う。
引数は左からタスク関数ポインタ、タスクの名前、スタックメモリ量、タスクへ渡すパラメータ、タスク優先度、タスクハンドラ、割り当てるコア　となっている。詳細は公式ドキュメント（以下）を参照。

docs.espressif.com

ESP32の場合コア0とコア1が使えるので、好きな方にタスクを割り当てる。
優先度は、0が最も低く、数字が増えるほど高くなる。あまり高くしすぎると内部の他の処理を妨げるので、必要最低限の優先度とするのが良さそう。

今回重要なのは、ログ記録タスクの優先度（LOGTASK_PRI）よりセンサ取得タスクの優先度（SENSORTASK_PRI）を高くすること。
こうすることで、ログ記録中でも優先度の高いセンサ取得をしてくれる。

以下が、マルチタスクでセンサ値取得タスクの優先度を上げて25Hz（40ms）でセンサ取得し、1秒毎（25回分ごと）にSDへ記録したときの、1周期あたりの処理時間である。

マルチタスク化して実行した場合の処理時間

図からわかるように、きれいに25Hzでセンサが取得できていることがわかる。

ちなみに、センサ取得とログ記録の優先度を逆転してみると、処理時間は以下の通り。

センサ取得タスクの優先度を下げた場合の処理時間

センサ取得タスクが優先度の高いログ記録タスクに邪魔され、処理時間がめちゃめちゃになっていることがわかる。

優先度を一緒にすると、今度は全体的に周期が遅くなる。これもダメ。

センサ取得とログ記録の優先度を同一（０）にした場合

やはり、センサ取得タスクの優先度をログ記録タスクのそれより高くする必要がある。

「センサ取得タスクの周期をvTaskDelayUntil()で限界以上に早く設定しちゃうとどうなるの？」という疑問もあろうと思うので、それをやってみると以下の通り。

限界以上（例:100Hz）の周期でタスク実行した場合の処理時間

100Hzでは処理しきれていないが、できるだけ早く処理するよう健闘していることがわかる。なお、これでもSDへのログ記録自体は問題なかった。

なお、今回100Hzではセンサが処理しきれていないが、処理時間の大半はGroveで外付けしている気圧センサである。
もしM5Stack内部の加速度・ジャイロ・磁気のみの取得であればもっと高い周期でも処理できるはずである。

気圧センサの使い方については以下の過去記事参照。

westgate-lab.hatenablog.com

まとめ

高頻度のセンサ取得とSDへのログ記録を実現させるため、M5Stack内蔵のデュアルコアを使ってマルチタスク化した。
センサ取得タスクとログ記録タスクを生成し、それぞれのコアに割付けセンサ取得タスクの優先度を上げることで乱れなく周期的なセンサ取得・記録ができるようになった。

おまけ

高頻度にセンサを処理することは、例えば姿勢のフィルタ・制御では必須である。
姿勢のフィルタでよく使われるMadgwickフィルタやMahonyフィルタなどでは、最低でも30Hz以上あったほうが良さそうである。

気圧センサは関係ないが、内部のセンサの処理により、今はようやくここまでできている。

M5Stackの姿勢フィルタ、だいぶいい感じになってきた。よく聞くMadgwickフィルタよりMahonyフィルタの方が何か安定してる pic.twitter.com/N6Qe876wOU

— 水田かなめ (@kmizta) March 1, 2020

姿勢の可視化については、こちらのページを参照した。

ambidata.io