#ifndef _J_SYS_SENS_COMPASS_H
#define _J_SYS_SENS_COMPASS_H

#define jType_S_COMP 0x22
#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

#endif