【M5stack】行動認識用に加速度データをPCにBluetoothSerial送信したときにちょっと躓いた記憶

こんにちは.

奈良先端大には,毎年各研究室ごとにセミナーが開催されているのですが,その担当者に抜擢していただきました.8月の話です.

その準備としてサンプルコードを作成していたのですが,いつものごとく通信周りで躓いたので,問題点と完成形のソースコードを軽く共有しようと思います.

機能

一定時間ごとに加速度センサの値などを,無線(Bluetooth Serial)でM5stackからPCに送信.
それを.csv形式に保存して,Wekaというソフトで学習し行動認識するための学習データとする.

f:id:ume-boshi:20201018084931p:plain
流れ

ぶつかった罠①

問題

加速度センサの値を取得し,Kalmanフィルターで姿勢を導出しようと思ったのですが,おかしな角度が出力されました.

解決策

いろいろ調査していると,1軸を除いて加速度の値がめちゃくちゃぶれていることに気が付きました.

どうやら,M5stack Grayで加速度/ジャイロセンサを用いる場合,"M5.IMU.Init();”よりも前に ↓ のように"M5.Power.begin()"を追加しないといけないようです.

M5.Power.begin();
M5.IMU.Init();

M5stack界隈では常識なのかもしれませんが,慣れていない私には完全に盲点でした.


ぶつかった罠②

問題

次に,送信したいデータを一時的にString型にまとめてから,後で一気に送信しようとしていた際の問題です.
文字列の途中までしか送信できていないことに気づきました.

始めは,インスタンスにうまく文字列が格納できていないと予測していたのですが,調べていくとString型に保存した文字列の長さ計測法を間違っているようでした...

配列の長さを計測する要領で ↓ のように書いていたのですが, 文章長が変わっても「12」の値から変化しません.

sizeof(message)/sizeof(message[0])    // = 12 

解決策・コード

「関数を調べるの面倒くさい」からと記憶にある書き方に挑戦したのですが,当然,↑ のコードではString型のインスタンスのサイズしか取得できません.

String型の文字列の長さを取得するなら ↓ ですよね.

message.length()

しょうもなつらいミス.
あほ


使用したソースコード

ちょっくら編集しているので,完全に動作するか試していないですが,おおよそ似たようなコードで実装できるはずです.このサンプルコードがマイコンPython間でSPP通信したい方の参考程度になればと思います.

そういやぁ,受信側で改行されすぎる問題があった気もします.

Arduino

#define M5STACK_MPU6886 

#include "BluetoothSerial.h"
#include <M5Stack.h>
#include <time.h>
#include <Kalman.h>

#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
#endif

#define rad2deg 57.2958

BluetoothSerial SerialBT;
Kalman kalmanX;
Kalman kalmanY;
String message;

const char* message1 = "Sleeping?";

int count = 0;
int sendCount = 0;

float accX = 0.0F;
float accY = 0.0F;
float accZ = 0.0F;
float gyroX = 0.0F;
float gyroY = 0.0F;
float gyroZ = 0.0F;
float pitch = 0.0F;
float roll  = 0.0F;
float yaw   = 0.0F;

double kalAngleX, kalAngleY; // Calculated angle using a Kalman filter
double gyroXangle, gyroYangle; // Angle calculate using the gyro only

unsigned long t;
unsigned long beft;
int action = 0; 
// 0:standing 
// 1:sitting
// 2:walk


void setup() {
  Serial.begin(115200);
  SerialBT.begin("ESP32-SPP"); //Bluetooth device name
  Serial.println("The device started, now you can pair it with bluetooth!");

  M5.begin(true, false, true);
  M5.Power.begin();
  M5.IMU.Init();
  
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextColor(GREEN , BLACK);
  M5.Lcd.setTextSize(2);
  M5.Lcd.setCursor(0, 40);
  M5.Lcd.printf("standing state");
  M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
  M5.IMU.getAccelData(&accX,&accY,&accZ);

  roll  = atan2(accY, accZ) * rad2deg;
  pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * rad2deg;
  kalmanX.setAngle(roll); // Set starting angle
  kalmanY.setAngle(pitch);

  delay(100);

  t = micros();
  beft = micros();

  message = String("index");
  message = String(message + ",");
  message = String(message + "time");
  message = String(message + ",");
  message = String(message + "accX");
  message = String(message + ",");
  message = String(message + "accY");
  message = String(message + ",");
  message = String(message + "pitch");
  message = String(message + '\n');
  

  // 1回目のデータを送信
  for(int i = 0; i < message.length(); i++){ // receive data from micro-controller via Bluetooth 
    SerialBT.write(message[i]);
    delay(1);
  }    

}


void loop() {
  int i=0;

  // 各種センサの値を更新する
  M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
  M5.IMU.getAccelData(&accX,&accY,&accZ);
//  M5.IMU.getAhrsData(&pitch,&roll,&yaw);
  M5.update(); // ボタンの値を取得


  // Kalmanフィルタによる姿勢取得
  double dt = (double)(micros() - beft) / 1000000; // Calculate delta time
  beft = micros();

  double gyroXrate = gyroX / 131.0; // Convert to deg/s
  double gyroYrate = gyroY / 131.0; // Convert to deg/s

  roll  = atan2(accY, accZ) * rad2deg;
  pitch = -atan2(accX, sqrt(accY * accY + accZ * accZ)) * rad2deg;

  if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
    kalmanX.setAngle(roll);
  } else
    kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
    
  if (abs(kalAngleX) > 90)
    gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced Acclelerometer reading
  kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);

  if (gyroXangle < -180 || gyroXangle > 180)
    gyroXangle = kalAngleX;
  if (gyroYangle < -180 || gyroYangle > 180)
    gyroYangle = kalAngleY;


  // 画面に表示
  M5.Lcd.setCursor(0, 20);
  M5.Lcd.printf("%d   ", (int)((micros() - t)/1000000));
  M5.Lcd.setCursor(0, 40);
  M5.Lcd.printf(" %5.2f   %5.2f   ", kalAngleX, kalAngleY); // マイコンの姿勢を表示
  M5.Lcd.setCursor(0, 100);
  M5.Lcd.printf(" %5.2f   %5.2f   %5.2f   ", accX, accY, accZ); // マイコンの姿勢を表示
  M5.Lcd.setCursor(0, 120);
  M5.Lcd.printf("%6.2f  %6.2f  %6.2f      ", gyroX, gyroY, gyroZ);
  M5.Lcd.setCursor(0, 160);
  M5.Lcd.printf("%d  ", sendCount);
  

  if(M5.BtnA.wasReleased() == 1){ // 現在を起立状態とする
    M5.Lcd.setCursor(0, 80);
    M5.Lcd.printf("standing state");
    action = 0;
  }
  if(M5.BtnB.wasReleased() == 1){  // 現在を着席状態とする
    M5.Lcd.setCursor(0, 80);
    M5.Lcd.printf("sitting state ");
    action = 1;
  }


  // 送信するデータを一度 message に記録していく
  count = count + 1;
  message = String(sendCount, DEC);
  message = String(message + ",");
  message = String(message + action);
  message = String(message + ",");
  message = String(message + (int)((micros() - t)/1000000));
  message = String(message + ",");
  message = String(message + action);
  message = String(message + ",");
  message = String(message + roll);
  message = String(message + ",");
  message = String(message +  pitch);
  message = String(message + "\r\n");


  if(count == 10){  // 10サンプルに1回だけデータを送信
    for(i = 0; i < message.length(); i++){      
      SerialBT.write(message[i]);
      delay(1);
    }
    count = 0;
    sendCount++;
  }

//  if (SerialBT.available()) { // PCからデータを受信したい場合
//    Serial.write(SerialBT.read());
//  }

  delay(10);
}


PC(Python)側

import serial
import csv

path = 'action.csv'
f = open(path,mode='w') # ファイルを作成、初期化
f.close()


ser = serial.Serial('COM10',timeout=None)

index=0
while True:
    index=index+1
    # ser.write(bytes("Hello"+str(index)+"\r\n", 'UTF-8')) # メッセージを受信したら返信する

    line = ser.readline() # 改行コードまでメッセージを読み込む
    print(line.decode(), end='')

    with open('action.csv', 'a') as f: # 配列の内容をCSVファイルに1行書き込む
        writer = csv.writer(f)
        writer.writerow(line[:-1])

ser.close() 

おわりに

開発経験が長くなっても,勘だけに頼る開発は避けていきたいものです.


奈良先端大のサマーセミナーですが,今年はオンライン開催でリモートで教えました.やはり,リモート環境でマイコン周りの環境を整えたり,プログラムのバグを見てあげることは難しいですね.

ガチガチのHWが関わるリモート開発の場合,遠隔地からの指示がかなり難しくなるため,過去に試作した ↓ のようなデバイスが市販化されたら便利だなぁと思ってます...

ume-boshi.hatenablog.jp