56 lines
1.1 KiB
C
56 lines
1.1 KiB
C
|
#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
|