104 lines
No EOL
2.5 KiB
C
104 lines
No EOL
2.5 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_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
|
|
|
|
typedef struct {
|
|
float x;
|
|
float y;
|
|
float z;
|
|
} compassPrefData;
|
|
|
|
#include <QMC5883LCompass.h>
|
|
QMC5883LCompass compass;
|
|
|
|
bool compass_is_calibrating = false;
|
|
|
|
void initCOMP() {
|
|
compass.init();
|
|
|
|
compassPrefData buffer;
|
|
|
|
if (!fs_read("compOff", &buffer, sizeof(buffer))) {
|
|
buffer.x = 0;
|
|
buffer.y = 0;
|
|
buffer.z = 0;
|
|
}
|
|
compass.setCalibrationOffsets(buffer.x, buffer.y, buffer.z);
|
|
if (!fs_read("compScale", &buffer, sizeof(buffer))) {
|
|
buffer.x = 1;
|
|
buffer.y = 1;
|
|
buffer.z = 1;
|
|
}
|
|
compass.setCalibrationScales(buffer.x, buffer.y, buffer.z);
|
|
}
|
|
|
|
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();
|
|
|
|
compassPrefData buffer;
|
|
buffer.x = compass.getCalibrationOffset(0);
|
|
buffer.y = compass.getCalibrationOffset(1);
|
|
buffer.z = compass.getCalibrationOffset(2);
|
|
fs_write("compOff", &buffer, sizeof(buffer));
|
|
buffer.x = compass.getCalibrationScale(0);
|
|
buffer.y = compass.getCalibrationScale(1);
|
|
buffer.z = compass.getCalibrationScale(2);
|
|
fs_write("compScale", &buffer, sizeof(buffer));
|
|
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 |