Seeed XIAO BLE nRF52840 Senseで6軸センサを使う

初めに

内蔵のIMUを使おうと公式Wikiに従ってやってもいまくいかなかったので，まとめます．

ライブラリの導入

Seeed studioの公式WikiにはArduino_LSM6DS3を導入しろと書いてありますがこれハズレです． 正しくはこちらのライブラリSeeed_Arduino_LSM6DS3を使います．

ということでもし, Arduino_LSM6DS3がある場合は消してください．

Releaseから現時点での最新版のv2.0.3をダウンロードして展開しArduinoIDEのライブラリディレクトに入れてください．

6軸センサの値を取得して姿勢推定

姿勢推定にはMadgwickAHRSを使用します． 最新のコミットをダウンロードして先ほど同様に展開します．

測定周波数の設定は公式のデータシート見ればいいんですが， こんな感じのソースコードがあって参考になります
https://github.com/JohnsonShen/AHRS_RD/blob/master/SparkFunLSM6DS3.c

#include <LSM6DS3.h>
#include <Wire.h>
#include <MadgwickAHRS.h>

#define MEASURING_FREQ (1660)

//Create a instance of class LSM6DS3
LSM6DS3 IMU(I2C_MODE, 0x6A);  //I2C device address 0x6A

typedef struct {
  float x;
  float y;
  float z;
} pos3d_t;

pos3d_t gyr_ = { 0 };
pos3d_t acc_ = { 0 };
pos3d_t ang_ = { 0 };
double mes_time_ = 1.0 / (double) MEASURING_FREQ * 1000.0;
Madgwick m_;

void print_header() {
  Serial.println("Roll, Pitch, Yaw");
}

void print_values() {
  Serial.print(ang_.x);
  Serial.print(",");
  Serial.print(ang_.y);
  Serial.print(",");
  Serial.print(ang_.z);
  Serial.println("");
}

void blink(int pin, uint16_t delay_time = 500) {
  digitalWrite(pin, HIGH);
  delay(delay_time);
  digitalWrite(pin, LOW);
  delay(delay_time);
}

void read_gyr() {
  gyr_.x = IMU.readFloatGyroX();
  gyr_.y = IMU.readFloatGyroY();
  gyr_.z = IMU.readFloatGyroZ();
}

void read_acc() {
  acc_.x = IMU.readFloatAccelX();
  acc_.y = IMU.readFloatAccelY();
  acc_.z = IMU.readFloatAccelZ();
}

void setup() {
  // 初期化
  pinMode(LED_RED, OUTPUT);
  pinMode(LED_BLUE, OUTPUT);
  pinMode(LED_GREEN, OUTPUT);

  digitalWrite(LED_RED, HIGH);
  digitalWrite(LED_GREEN, HIGH);
  digitalWrite(LED_BLUE, HIGH);

  Serial.begin(115200);

  while (!Serial) {
    blink(LED_RED);
  }

  IMU.settings.gyroRange = 2000;
  IMU.settings.accelRange = 4;  
  while (IMU.begin() != 0) {
    blink(LED_GREEN);
  }

  IMU.writeRegister(LSM6DS3_ACC_GYRO_CTRL2_G,  0x8C);
  IMU.writeRegister(LSM6DS3_ACC_GYRO_CTRL1_XL, 0x8A);
  IMU.writeRegister(LSM6DS3_ACC_GYRO_CTRL7_G,  0x00);
  IMU.writeRegister(LSM6DS3_ACC_GYRO_CTRL8_XL, 0x09);

  // 測定周波数  
  m_.begin((int)MEASURING_FREQ);
}

void loop() {
  // update
  read_acc();
  read_gyr();
  m_.updateIMU(gyr_.x, gyr_.y, gyr_.z, acc_.x, acc_.y, acc_.z);
  // get
  ang_.x = m_.getRoll();
  ang_.y = m_.getPitch();
  ang_.z = m_.getYaw();
  // print
  print_values();
  delay(mes_time_);
}