JuReceiver/system_sensors/compass.h

#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_Data 0
#define jCommand_Compass_Calibrate 1

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;

bool compass_is_calibrating = false;

void initCOMP() {
    compass.init();
    compass.setCalibrationOffsets(preferences.getFloat("compOffX", 0),
                                  preferences.getFloat("compOffY", 0),
                                  preferences.getFloat("compOffZ", 0));
    compass.setCalibrationScales(preferences.getFloat("compScaleX", 1),
                                 preferences.getFloat("compScaleY", 1),
                                 preferences.getFloat("compScaleZ", 1));
}

void COMPLoop() {
    if (!compass_is_calibrating) 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() {
    switch (jRCbuff[1]) {
        case jCommand_Compass_Data: {
            compData data;
            data.X = compass.getX();
            data.Y = compass.getY();
            data.Z = compass.getZ();
            data.angle = compass.getAzimuth();
            jSendNRF(&data, sizeof(data));
        } break;
        case jCommand_Compass_Calibrate: {
            compass_is_calibrating = true;
            delay(10);
            compass.calibrate();

            preferences.putFloat("compOffX", compass.getCalibrationOffset(0));
            preferences.putFloat("compOffY", compass.getCalibrationOffset(1));
            preferences.putFloat("compOffZ", compass.getCalibrationOffset(2));

            preferences.putFloat("compScaleX", compass.getCalibrationScale(0));
            preferences.putFloat("compScaleY", compass.getCalibrationScale(1));
            preferences.putFloat("compScaleZ", compass.getCalibrationScale(2));
            compass_is_calibrating = false;
        } break;
    }
}

#else

void initCOMP() {}

void COMPLoop() {}

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

#endif

#endif