imu.cpp

// imu.cpp
#include "imu.h"
#include "tca9548a.h"

Adafruit_MPU6050 mpu;

bool imuInit() {
  tcaSelect(0); // channel 0 for IMU
  if (!mpu.begin()) return false;
  mpu.setAccelerometerRange(MPU6050_RANGE_8_G);
  mpu.setGyroRange(MPU6050_RANGE_500_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  return true;
}

ImuData imuRead() {
  ImuData data;
  sensors_event_t a, g, temp;
  tcaSelect(0);
  mpu.getEvent(&a, &g, &temp);

  data.pitch = atan2(a.acceleration.y, a.acceleration.z) * 180 / PI;
  data.roll  = atan2(-a.acceleration.x,
                    sqrt(a.acceleration.y * a.acceleration.y + 
                         a.acceleration.z * a.acceleration.z)) * 180 / PI;
  return data;
}