// Example #7 sketch for the ICM20948_WE library
#include <Wire.h>
#include <ICM20948_WE.h>
#define ICM20948_ADDR 0x68

char s[100];  // for formatted output using sprintf
ICM20948_WE myIMU = ICM20948_WE(ICM20948_ADDR);

void setup() {
  //delay(2000); // maybe needed for some MCUs, in particular for startup after power off
  Wire.begin(4,5);
  Serial.begin(115200);
  while(!Serial) {}
 
  if(!myIMU.init()){
    Serial.println("ICM20948 does not respond");
  }
  else{
    Serial.println("ICM20948 is connected");
  }
  //myIMU.setAccOffsets(-16330.0, 16450.0, -16600.0, 16180.0, -16520.0, 16690.0);
  Serial.println("Position your ICM20948 flat and don't move it - calibrating...");
  delay(1000);
  myIMU.autoOffsets();
  Serial.println("Done!");
  /* enables or disables the acceleration sensor, default: enabled */
  // myIMU.enableAcc(true);

  myIMU.setAccRange(ICM20948_ACC_RANGE_2G);
  myIMU.setAccDLPF(ICM20948_DLPF_6);    
  //myIMU.setAccSampleRateDivider(10);
}

void loop() {
  xyzFloat gValue;
  xyzFloat angle;
  myIMU.readSensor();
  myIMU.getGValues(&gValue);
  myIMU.getAngles(&angle);
 
  float pitch = myIMU.getPitch();
  float roll  = myIMU.getRoll();

  sprintf(s, "<M-GAPR:%6.3f, %6.3f, %6.3f, %7.2f, %7.2f, %7.2f, %5.1f, %5.1f>",
     gValue.x, gValue.y, gValue.z,
     angle.x, angle.y, angle.z,
     pitch, roll);
  Serial.println(s);

  delay(100);
}