#define jType_S_MPU 1
#define jCommand_MPU_Acc 0
#define jCommand_MPU_Gyro 1
#define jCommand_MPU_Angle 2
#define jCommand_MPU_fail 0xff

typedef struct{
  byte type = jType_S_MPU;
  byte command;
  float X;
  float Y;
  float Z;
}mpuSendData;

#ifdef jMPU

#include <MPU6050_jusax23.h>

MPU6050 mpu6050(Wire, 0.001f, 0.999f);

void initMPU(){
  mpu6050.begin();
  mpu6050.setGyroOffsets(-7.8,2.32,-0.22);
  mpu6050.setAccXoffset(0.03,0.0,0.3);
  //mpu6050.calcGyroOffsets(false,50,50);
}

void MPULoop(){
  mpu6050.update();
}

void MPUradioTask(){
  switch (jRCbuff[1]) {
    case jCommand_MPU_Acc:{
      mpuSendData data;
      data.command = jCommand_MPU_Acc;
      data.X = mpu6050.getAccX();
      data.Y = mpu6050.getAccY();
      data.Z = mpu6050.getAccZ();
      jSendNRF(&data, sizeof(data));
    }break;
    case jCommand_MPU_Gyro:{
      mpuSendData data;
      data.command = jCommand_MPU_Gyro;
      data.X = mpu6050.getGyroX();
      data.Y = mpu6050.getGyroY();
      data.Z = mpu6050.getGyroZ();
      jSendNRF(&data, sizeof(data));
    }break;
    case jCommand_MPU_Angle:{
      mpuSendData data;
      data.command = jCommand_MPU_Angle;
      data.X = mpu6050.getAngleX();
      data.Y = mpu6050.getAngleY();
      data.Z = mpu6050.getAngleZ();
      jSendNRF(&data, sizeof(data));
    }break;
  }
}

#else

void initMPU(){}

void MPULoop(){}

void MPUradioTask(){
  mpuSendData data;
  data.command = jCommand_MPU_fail;
  jSendNRF(&data, sizeof(data));
}

#endif