電子コンパスと加速度センサを用いた姿勢推定(6軸)

以下の記事を参考に。
watako-lab.com

M5StackのBMM150からは以下のコードを利用してデータを取得する。
github.com
あらかじめBMM150のサンプルコードを用いてキャリブレーションしておき、pref（ESP32のNon Volatile Storage：NVS）に保存しておく。

Arduinoコード

#define M5STACK_MPU6886 
#include <M5Stack.h>
#include <ArduinoJson.h>
#include "Preferences.h"
#include "math.h"
#include "bmm150.h"
#include "bmm150_defs.h"

Preferences prefs;

struct bmm150_dev dev;
bmm150_mag_data mag_offset;
bmm150_mag_data mag_max;
bmm150_mag_data mag_min;

float accX = 0.0F;
float accY = 0.0F;
float accZ = 0.0F;

float gyroX = 0.0F;
float gyroY = 0.0F;
float gyroZ = 0.0F;

StaticJsonDocument<200> doc;
int8_t i2c_read(uint8_t dev_id, uint8_t reg_addr, uint8_t *read_data, uint16_t len)
{
    if(M5.I2C.readBytes(dev_id, reg_addr, len, read_data))
    {
        return BMM150_OK;
    }
    else
    {
        return BMM150_E_DEV_NOT_FOUND;
    }
}

int8_t i2c_write(uint8_t dev_id, uint8_t reg_addr, uint8_t *read_data, uint16_t len)
{
    if(M5.I2C.writeBytes(dev_id, reg_addr, read_data, len))
    {
        return BMM150_OK;
    }
    else
    {
        return BMM150_E_DEV_NOT_FOUND;
    }
}

int8_t bmm150_initialization()
{
    int8_t rslt = BMM150_OK;

    /* Sensor interface over SPI with native chip select line */
    dev.dev_id = 0x10;
    dev.intf = BMM150_I2C_INTF;
    dev.read = i2c_read;
    dev.write = i2c_write;
    dev.delay_ms = delay;

    /* make sure max < mag data first  */
    mag_max.x = -2000;
    mag_max.y = -2000;
    mag_max.z = -2000;

    /* make sure min > mag data first  */
    mag_min.x = 2000;
    mag_min.y = 2000;
    mag_min.z = 2000;

    rslt = bmm150_init(&dev);
    dev.settings.pwr_mode = BMM150_NORMAL_MODE;
    rslt |= bmm150_set_op_mode(&dev);
    dev.settings.preset_mode = BMM150_PRESETMODE_ENHANCED;
    rslt |= bmm150_set_presetmode(&dev);
    return rslt;
}

void bmm150_offset_save()
{
    prefs.begin("bmm150", false);
    prefs.putBytes("offset", (uint8_t *)&mag_offset, sizeof(bmm150_mag_data));
    prefs.end();
}

void bmm150_offset_load()
{
    if(prefs.begin("bmm150", true))
    {
        prefs.getBytes("offset", (uint8_t *)&mag_offset, sizeof(bmm150_mag_data));
        prefs.end();
        Serial.printf("bmm150 load offset finish.... \r\n");
    }
    else
    {
        Serial.printf("bmm150 load offset failed.... \r\n");
    }
}

void setup() {
  M5.begin();
  Wire.begin(21, 22, 400000);

  M5.IMU.Init();

    if(bmm150_initialization() != BMM150_OK)
    {
        for(;;)
        {
            delay(100);
        }
    }
    bmm150_offset_load();

}

void loop() {
  bmm150_read_mag_data(&dev);
  M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
  M5.IMU.getAccelData(&accX,&accY,&accZ);
  
  doc["accX"] = accX;
  doc["accY"] = accY;
  doc["accZ"] = accZ;
  doc["gyroX"] = gyroX;
  doc["gyroY"] = gyroY;
  doc["gyroZ"] = gyroZ;
  
  doc["magX"] = dev.data.x-mag_offset.x;
  doc["magY"] = dev.data.y-mag_offset.y;
  doc["magZ"] = dev.data.z-mag_offset.z;

  doc["t"] = millis();
  
  serializeJson(doc, Serial);
  Serial.println();

  delay(500);
}

Matlabサーバ（シリアルデータ読み込み）

clear;  close all;

global h; global dir;
global r; global p; global y;
global t; global ts; global rs; global ys; global ps;
global axs; global ays; global azs; 
global wxs; global wys; global wzs;
global ax_bias; global ay_bias; global az_bias;
global wx_bias; global wy_bias; global wz_bias;
global dxs; global dys; global dzs;
global g;
global count;

g = 9.81; % gravity constant
count=0;

s = serialport("/dev/cu.SLAB_USBtoUART",115200);
configureTerminator(s,"CR/LF");
configureCallback(s,"terminator",@readSerialData)

pt = [0 0 0];
dir = [1 0 0 1];
ts=[];
rs = []; ps = []; ys = [];
axs=[]; ays=[];azs=[];
wxs=[];wys=[];wzs=[];
dxs=[];dys=[];dzs=[];
r = 0; p = 0; y = 0;

wx_bias = 10.8236;
wy_bias = -14.1968;
wz_bias = -6.4555;
ax_bias = -0.0003337;
ay_bias = 0.0258;
az_bias = 1.06-1.0;

global maxn; global avet;
maxn = 20; avet=0.1;

Matlabコールバック関数

function readSerialData(src,~)
  global t;global y; global p; global r;
  global ts; global ys; global ps; global rs;
  global maxn; global avet;
  global ax_bias; global ay_bias; global az_bias; 
  global axs; global ays; global azs;
  global count;

  json = readline(src);
  data = jsondecode(json);

  ax = (data.accX)-ax_bias;
  ay = (data.accY)-ay_bias;
  az = (data.accZ)-az_bias;
  dx = data.magX;
  dy = data.magY;
  dz = data.magZ;

  axs = horzcat(axs, ax);
  ays = horzcat(ays, ay);
  azs = horzcat(azs, az);

  t = data.t;  
  
  r = atan2(ay,az);
  p = atan2(-ax,sqrt(ay^2+az^2));  
  y = atan2((-cos(r)*dy+sin(r)*dz),(cos(p)*dx+sin(p)*sin(r)*dy+sin(p)*cos(r)*dz));

  ts = horzcat(ts,t);
  ys = horzcat(ys,y);
  ps = horzcat(ps,p);
  rs = horzcat(rs,r);
   
  if(length(ts)>maxn)
    ts = ts(2:end);
    rs = rs(2:end);
    ps = ps(2:end);
    ys = ys(2:end);

    axs = axs(2:end);
    ays = ays(2:end);
    azs = azs(2:end);
  end

  subplot(2,1,1);
  plot(ts,axs,'r');
  hold on;
  plot(ts,ays,'b');
  plot(ts,azs,'g');
  legend('ax','ay','az');

  subplot(2,1,2);
  plot(ts,rs*180/pi,'r');
  hold on;
  plot(ts,ps*180/pi,'b');
  plot(ts,ys*180/pi,'g');  
  plot(ts,ones(size(ts))*90,'k--');
  plot(ts,ones(size(ts))*(-90),'k--');
  ylim([-181,181]);
  legend('Roll','Pitch','Yaw');
     
  drawnow;  

disp(count);
count=count+1;
  
end