55 lines
1,009 B
C
55 lines
1,009 B
C
|
#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
|