Code uses M5stick library built-in IMU.getAhrsData()
function which is built on the Mahony algorithm. Note that the Yaw values seem to fluctuate significantly and are thus unreliable.
#include <M5StickC.h>
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;
void setup() {
// put your setup code here, to run once:
M5.begin();
M5.IMU.Init();
M5.Lcd.setRotation(1);
M5.Lcd.fillScreen(BLACK);
M5.Lcd.setTextSize(1);
M5.Lcd.setCursor(40, 0);
M5.Lcd.println("IMU TEST");
M5.Lcd.setCursor(0, 10);
M5.Lcd.println(" X Y Z");
M5.Lcd.setCursor(0, 50);
M5.Lcd.println(" Pitch Roll Yaw");
Serial.begin(9600);
}
float temp = 0;
/*****************************************
M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
M5.IMU.getAccelData(&accX,&accY,&accZ);
M5.IMU.getAhrsData(&pitch,&roll,&yaw);
M5.IMU.getTempData(&temp);
*****************************************/
void loop() {
// put your main code here, to run repeatedly:
M5.IMU.getGyroData(&gyroX,&gyroY,&gyroZ);
M5.IMU.getAccelData(&accX,&accY,&accZ);
M5.IMU.getAhrsData(&pitch,&roll,&yaw);
M5.IMU.getTempData(&temp);
M5.Lcd.setCursor(0, 20);
M5.Lcd.printf("%6.2f %6.2f %6.2f ", gyroX, gyroY, gyroZ);
M5.Lcd.setCursor(140, 20);
M5.Lcd.print("o/s");
M5.Lcd.setCursor(0, 30);
M5.Lcd.printf(" %5.2f %5.2f %5.2f ", accX, accY, accZ);
M5.Lcd.setCursor(140, 30);
M5.Lcd.print("G");
M5.Lcd.setCursor(0, 60);
M5.Lcd.printf(" %5.2f %5.2f %5.2f ", pitch, roll, yaw);
M5.Lcd.setCursor(0, 70);
M5.Lcd.printf("Temperature : %.2f C", temp);
// Plotting
Serial.print("pitch:");
Serial.print(pitch);
Serial.print(",");
Serial.print("roll:");
Serial.print(roll);
Serial.print(",");
Serial.print("yaw:");
Serial.println(yaw);
delay(100);
}