#include <Adafruit_BNO08x.h>
#include <Wire.h>

Adafruit_BNO08x bno;
sh2_SensorValue_t sensorValue;

void setup() {
  Serial.begin(115200);
  while (!Serial) delay(10);

  Wire.begin(5, 6);

  if (!bno.begin_I2C(0x4A, &Wire)) {
    Serial.println("BNO08x not found, check wiring!");
    while (1) delay(10);
  }

  bno.enableReport(SH2_ARVR_STABILIZED_RV);
  Serial.println("BNO08x connected!");
}

void loop() {
  if (bno.getSensorEvent(&sensorValue)) {
    if (sensorValue.sensorId == SH2_ARVR_STABILIZED_RV) {

      float qw = sensorValue.un.arvrStabilizedRV.real;
      float qx = sensorValue.un.arvrStabilizedRV.i;
      float qy = sensorValue.un.arvrStabilizedRV.j;
      float qz = sensorValue.un.arvrStabilizedRV.k;

      float yaw = atan2(2.0*(qw*qz + qx*qy), 1.0 - 2.0*(qy*qy + qz*qz));
      yaw = yaw * 180.0 / PI;

      Serial.println(yaw);
    }
  }
}