ESP32S3 6軸センサ LSM6DSV16X

6軸センサ LSM6DSV16Xを用い，xyz3軸方向の加速度，xyz3軸周りの角速度とオイラー角（pitch,roll,yaw）を求めます。
6軸センサ値からオイラー角を算出するには，madgwickfilterを使用します。

6軸センサ LSM6DSV16Xは自分自身のxyz3軸方向の加速度およびxyz3軸周りの角速度を測定するセンサです。
xyz3軸方向の加速度およびxyz3軸周りの角速度から，姿勢を表すオイラー角（pitch,roll,yaw）の変換すると，センサの姿勢を表現できます。
そして，倒立振り子台車やドローンなどに取り付けると，機器の姿勢を得ることができます。

3軸加速度・3軸角速度の値から，pitch,roll,yawに変換するフィルタはいろいろあるのですが，
演算量が少なくよく使われているmadgwickfilterを使用します。このフィルタはライブラリとして公開されていますので，
そのライブラリを取り込んで使います。
pitch,roll,yawの３つの角はいろいろな定義があるようですが，姿勢制御の場面では次のような定義が多く使われているようです。
飛行機での機首上げではpitchは正の値です。また左翼上げでrollは正の値になります。

しかし，LSM6DSV16Xから3軸加速度・3軸角速度を受け取り，そのままのその受け取った値からmadgwickfilterライブラリで得られるpitch,roll,yawの３つの角は次のようになっています。

LSM6DSV16Xのｘ軸方向を機器の前方と考えると，飛行機の機首上げでpitchの値が負になって符号が逆になっています。これはプログラム内で解決することにします。
なお，実際の機器にLSM6DSV16Xを取り付けたときに，LSM6DSV16Xのy軸方向を機器の前方になってしまうこともあります。
その場合，pitchとrollが逆になりますが，プログラム内で解決することにします。

LSM6DSV16X（秋月基板）では，はんだショートによってI2Cアドレスを決めます。次の例は6Bと書いてあるところをはんだショートしたものです。
配線前にこのはんだショート作業を行います。

５　6軸センサ LSM6DSV16Xを用い，オイラー角を得るプログラム

LSM6DSV16Xの操作はLSM6DSV16Xライブラリを使用するので，ArduinoIDEのライブラリマネージャを使って先にライブラリを取り込んでおきます。
「LSM6DSV16X」で検索すると，「LSM6DSV16X by STM32duinot」が見つかるので取り込みます。
また，madgwickfilterもライブラリを使用するので，先にライブラリを取り込んでおきます。
「madgwick」で検索すると，「madgwick by Arduino」が見つかるので取り込みます。
上記2つの準備ができたら，次のプログラムを実行できます。
なお，プログラム中，LSM6DSV16Xから直接得られるxyz軸方向加速度とxyz軸周り角速度の単位は次のようになっています。
　加速度　mg（milliG） 9.8/1000倍して[m/s^2]
　角速度　 mdps（ millideg/sec） 1/1000倍して[deg/sec]
また，madgwickから得られるpitch,roll,yawの３つの角の単位はdeg（度）です。

ライブラリの初期化関数begin()がよくできていて，使用しているデバイスのアドレスを見つけ出してくれます。

arduinoIDEでの作業のときは次の２つのライブラリを読み込んでおいてください。
　　LSM6DSV16X by STM32duino
　　Madgwick by Arduino

　　　6軸センサ LSM6DSV16Xを用い，オイラー角を得るプログラム

//LSM6DSV16X_test.ino
//library: LSM6DSV16X by STM32duino
//library: Madgwick by Arduino

/*
STM32duinoのLSM6DSV16Xライブラリの関数から得られる値の単位
Get_X_Axes(accel)（加速度）： mg（milliG）   9.8/1000倍して[m/s^2]
Get_G_Axes(gyro)（角速度）： mdps（ millideg/sec）   1/1000倍して[deg/sec]
*/

#include <LSM6DSV16XSensor.h>
#include <MadgwickAHRS.h>

LSM6DSV16XSensor Lsm6dsv16x(&Wire);
Madgwick madgwickfilter;

unsigned long microsPerReading, microsPrevious;
int frequency = 200; //Hz データ取得周波数

void setup() {
int ret;
Serial.begin(115200);
//while(!Serial) {}
delay(300);

Wire.begin();
// Initializing the LSM6DSV16X
Serial.println("LSM6DSV16X initialization started");
int result;
result = Lsm6dsv16x.begin();
result |= Lsm6dsv16x.Enable_X();
result |= Lsm6dsv16x.Enable_G();
if (result != 0) {
    Serial.print("LSM6DSV16X initialization failed : " + String(result));
    while(1);
}
Serial.println("LSM6DSV16X initialization completed");
delay(100);

//Acceleration Range = 2G
Lsm6dsv16x.Set_X_FS(2);
//Angular velocity Range = 250 dps
Lsm6dsv16x.Set_G_FS(250);

// Wait LSM6DSV16X to start
delay(100);

madgwickfilter.begin(frequency);
microsPerReading = 1000000 / frequency; //Period measured in microseconds
microsPrevious = micros();

Serial.print("\ntime[sec], acceleration[m/s2] x,y,z,");
Serial.print(" angular_velocity[deg/s] x,y,x,");
Serial.println(" pitch[deg],roll[deg],yaw[deg]");
}

void loop() {
int32_t accel[3], gyro[3]; //x,y,z軸方向加速度，x,y,z軸周りの角速度
unsigned long microsNow;
static int cntr = 0;

microsNow = micros();
if (microsNow - microsPrevious >= microsPerReading) {
    //Serial.println(microsNow - microsPrevious);
    microsPrevious = microsPrevious + microsPerReading;

    Lsm6dsv16x.Get_X_Axes(accel);
    Lsm6dsv16x.Get_G_Axes(gyro);
    float ax = accel[0] / 1000. * 9.8; //acceleration_x [m/s2]
    float ay = accel[1] / 1000. * 9.8; //acceleration_y [m/s2]
    float az = accel[2] / 1000. * 9.8; //acceleration_z [m/s2]
    float gx = gyro[0] / 1000.; //angular_velocity_x [deg/s]
    float gy = gyro[1] / 1000.; //angular_velocity_y [deg/s]
    float gz = gyro[2] / 1000.; //angular_velocity_z [deg/s]

    madgwickfilter.updateIMU(gx, gy, gz, ax, ay, az);

    // print the heading, pitch and roll
    float roll = madgwickfilter.getRoll();
    float pitch = madgwickfilter.getPitch();
    float yaw = madgwickfilter.getYaw() - 180.0;

    cntr++;
    if (cntr != frequency) return;
    cntr = 0;

    // Format data for Serial Plotter
    Serial.print(microsNow/1000000);
    Serial.print(", ");
    Serial.print(ax);
    Serial.print(", ");
    Serial.print(ay);
    Serial.print(", ");
    Serial.print(az);
    Serial.print(", ");
    Serial.print(gx,5);
    Serial.print(", ");
    Serial.print(gy,5);
    Serial.print(", ");
    Serial.print(gz,5);
    Serial.print(", ");
    Serial.print(pitch);
    Serial.print(", ");
    Serial.print(roll);
    Serial.print(", ");
    Serial.println(yaw);
}
}

LSM6DSV16Xをほぼ水平に静止させ，得られた姿勢データは次のようになりました。
これは1秒間隔のデータです。

LSM6DSV16X initialization started
LSM6DSV16X initialization completed

time[sec], acceleration[m/s2] x,y,z, angular_velocity[deg/s] x,y,x, pitch[deg],roll[deg],yaw[deg]
1, -0.04, 0.19, 9.79, -0.57700, 0.08700, -0.36700, 0.27, 1.13, -0.38
2, -0.04, 0.20, 9.79, -0.56800, 0.07000, -0.37600, 0.25, 1.11, -0.76
3, -0.04, 0.21, 9.80, -0.59500, 0.14000, -0.33200, 0.25, 1.23, -1.14
4, -0.04, 0.21, 9.79, -0.60300, 0.10500, -0.36700, 0.20, 1.19, -1.53
5, -0.05, 0.21, 9.80, -0.52500, 0.08700, -0.37600, 0.27, 1.19, -1.91
6, -0.04, 0.20, 9.78, -0.49800, 0.10500, -0.39300, 0.24, 1.14, -2.29
7, -0.04, 0.21, 9.79, -0.52500, 0.07000, -0.36700, 0.22, 1.26, -2.67
8, -0.04, 0.21, 9.80, -0.50700, 0.11300, -0.37600, 0.23, 1.20, -3.05
9, -0.05, 0.20, 9.80, -0.54200, 0.01700, -0.41100, 0.30, 1.13, -3.43
10, -0.04, 0.20, 9.80, -0.55100, 0.03500, -0.38500, 0.23, 1.14, -3.81
11, -0.04, 0.20, 9.80, -0.50700, 0.10500, -0.40200, 0.27, 1.15, -4.19
12, -0.05, 0.20, 9.80, -0.52500, 0.03500, -0.42800, 0.25, 1.16, -4.58
13, -0.04, 0.21, 9.79, -0.59500, 0.07800, -0.39300, 0.25, 1.16, -4.95
14, -0.04, 0.20, 9.79, -0.56800, 0.03500, -0.39300, 0.23, 1.14, -5.34
15, -0.04, 0.21, 9.80, -0.56000, 0.07000, -0.41100, 0.25, 1.24, -5.72
16, -0.04, 0.21, 9.79, -0.56000, 0.09600, -0.42000, 0.17, 1.19, -6.10
17, -0.04, 0.21, 9.79, -0.56000, 0.03500, -0.39300, 0.23, 1.19, -6.48
18, -0.04, 0.20, 9.79, -0.56000, 0.11300, -0.39300, 0.25, 1.19, -6.86
19, -0.04, 0.20, 9.79, -0.54200, 0.02600, -0.39300, 0.24, 1.13, -7.24
20, -0.05, 0.21, 9.80, -0.52500, 0.09600, -0.37600, 0.29, 1.20, -7.62
21, -0.04, 0.20, 9.79, -0.51600, 0.07000, -0.41100, 0.20, 1.13, -8.00
22, -0.04, 0.20, 9.79, -0.53300, 0.08700, -0.35000, 0.23, 1.14, -8.38
23, -0.04, 0.20, 9.79, -0.56000, 0.08700, -0.33200, 0.23, 1.16, -8.76
24, -0.05, 0.21, 9.79, -0.57700, 0.09600, -0.40200, 0.32, 1.24, -9.15
25, -0.04, 0.21, 9.79, -0.59500, 0.05200, -0.39300, 0.19, 1.23, -9.53
26, -0.04, 0.20, 9.80, -0.56000, 0.07800, -0.36700, 0.19, 1.13, -9.91
27, -0.04, 0.21, 9.79, -0.60300, 0.08700, -0.39300, 0.24, 1.17, -10.29
28, -0.04, 0.20, 9.79, -0.61200, 0.07800, -0.39300, 0.22, 1.18, -10.67
29, -0.04, 0.20, 9.78, -0.57700, 0.06100, -0.36700, 0.25, 1.18, -11.05
30, -0.04, 0.21, 9.79, -0.61200, 0.09600, -0.35000, 0.24, 1.26, -11.43
31, -0.04, 0.20, 9.79, -0.56800, 0.09600, -0.40200, 0.20, 1.16, -11.82
32, -0.04, 0.20, 9.80, -0.59500, 0.07000, -0.37600, 0.25, 1.14, -12.19
33, -0.04, 0.20, 9.79, -0.54200, 0.09600, -0.36700, 0.28, 1.17, -12.57

センサをほぼ水平の置いている状態で，Accelのz（鉛直方向の）加速度が9.8m/s2程度を示しています。
センサをほぼ水平の置いている状態で，pitchとrollは正しく測定出来ているかどうかは検証できませんが，0度近いので良いことにします。
yawは最初に0度ですが毎秒0.5度程度ずれていきます。
得られたpith,roll,yawの取得値をグラフにしてみました。

静止した状態でx,y,z軸周りの角速度をそれぞれ数秒間取得し，その平均値がx,y,z軸周りの角速度のそれぞれのオフセット誤差になります。
x,y,z軸周りの角速度からそれぞれ角速度のオフセット誤差を減じた値を用いれば，yawのずれも解消できると思います。

Xiao ESP32S3で，6軸センサLSM6DSV16Xおよびmadgwickfilterを用いて，機器の姿勢を得ました。
ここでのテストではブレッドボード上で，Xiao ESP32S3に6軸センサLSM6DSV16Xだけを接続したものです。これだけシンプルな構成であるのに，3.3V電源ラインにバイパスコンデンサを付けないと正しく動作しませんでした。コンデンサの適用は当然ですが，電源ノイズに敏感なセンサであることがわかりました。
6軸センサLSM6DSV16Xをほぼ水平にして固定しているのにyawが時間とともに一定の割合で変化してしまいました。 LSM6DSV16XのGyroに関するoffset誤差をあらかじめ求めて差し引けば，より安定したGyro値を得ることができ，より安定したyawが得られると思います。

ESP32S3 6軸センサ LSM6DSV16Xで姿勢を検出