#チョコプロ生ちゃん https://twitter.com/shonen9th/status/1492384263135985667

2022pronamachoco.ino

#define M5STACK_MPU6886
#include <M5Stack.h>

TFT_eSprite g_lcd_buffer(&M5.Lcd);

void setup() {
  M5.begin();
  Serial.begin(115200);
  
  Wire.begin();
  M5.IMU.Init();

  g_lcd_buffer.setColorDepth(8); // reduce memory
  auto buffer_ptr = g_lcd_buffer.createSprite(M5.Lcd.width(), M5.Lcd.height());
  if (buffer_ptr == nullptr)
    Serial.println("malloc failed!!");
}

void vec2q2euler(float vx, float vy, float vz, float& roll, float& pitch, float& yaw) {
  const float dx = 0.f;
  const float dy = 0.f;
  const float dz = 1.f;
  float x = vy*dz - vz*dy;
  float y = vz*dx - vx*dz;
  float z = vx*dy - vy*dx;
  float w = vx*dx + vy*dy + vz*dz +1.f;
  float n = sqrt(x*x + y*y + z*z + w*w);
  x /= n;
  y /= n;
  z /= n;
  w /= n;
  roll = atan2(2.f*(x*w + y*z), 1.f - 2.f*(w*w - z*z));
  pitch = asin(2.f*(-x*z + w*y));
  yaw = atan2(2.f*(x*y + z*w), 1.f - 2.f*(y*y - z*z));
}

float gx, gy, gz;
float ax, ay, az;
void loop() {
  M5.IMU.getGyroData(&gx, &gy, &gz);
  M5.IMU.getAccelData(&ax, &ay, &az);

  int kW = g_lcd_buffer.width();
  int kH = g_lcd_buffer.height();
  g_lcd_buffer.fillRect(0, 0, kW, kH, TFT_BLACK);

  float norm = (sqrt(ax*ax+ay*ay+az*az));
  if (abs(norm) < 1e-6) return;
  float vx = ax/norm;
  float vy = ay/norm;
  float vz = az/norm;

  float roll, pitch, yaw;
  vec2q2euler(vx,vy,vz,roll,pitch,yaw);

  {
    float lx = 0, ly = 0; 
    for (float th = 0; th < 2*PI; th += 0.01) {
      float s = sin(th);
      float c1 = cos(th);
      float c2 = cos(th*2);
      float c3 = cos(th*3);
      float x = (s*s*s)*100;
      float y = (c1-c2/3-c3/6)*100;
      y *= cos(roll);
      x *= cos(pitch);
      if (th != 0)
        g_lcd_buffer.drawLine(150 + lx, 110 + ly, 150 + x, 110 + y, TFT_RED);
      lx = x;
      ly = y;
    }
  }

  g_lcd_buffer.setCursor(0, 0);
  g_lcd_buffer.print(vx);
  g_lcd_buffer.print(' ');
  g_lcd_buffer.print(vy);
  g_lcd_buffer.print(' ');
  g_lcd_buffer.print(vz);
  g_lcd_buffer.print(' ');
  g_lcd_buffer.print(roll*180/PI);
  g_lcd_buffer.print(' ');
  g_lcd_buffer.print(pitch*180/PI);
  g_lcd_buffer.print(' ');
  g_lcd_buffer.print(yaw*180/PI);
  
  g_lcd_buffer.pushSprite(0, 0);
  
  delay(50);
}