#define jType_S_COMP 2
#define jCommand_COMP_fail 0xff
//#define jCommand_Compass_Raw 0

typedef struct{
  byte type = jType_S_COMP;
  byte command = 0;
  int X;
  int Y;
  int Z;
  int angle;
}compData;

#ifdef jCOMP

#include <QMC5883LCompass.h>
QMC5883LCompass compass;

void initCOMP(){
  compass.init();
}

void COMPLoop(){
  compass.read();
}

/*float Altitude = mpu6050.getAngleX()/180*PI;
float Roll = mpu6050.getAngleY()/180*PI;
float xh=compass.getX()*cos(Altitude)+compass.getZ()*sin(Altitude);
float yh=compass.getX()*sin(Roll)*sin(Altitude)+compass.getY()*cos(Roll)-compass.getZ()*sin(Roll)*cos(Altitude);
float heading = atan2(yh, xh);*/

void COMPradioTask(){
  compData data;
  data.X = compass.getX();
  data.Y = compass.getY();
  data.Z = compass.getZ();
  data.angle = compass.getAzimuth();
  jSendNRF(&data, sizeof(data));
}

#else

void initCOMP(){}

void COMPLoop(){}

void MPUradioTask(){
  compData data;
  data.command = jCommand_COMP_fail;
  jSendNRF(&data, sizeof(data));
}

#endif