JuControl/JuReceiver
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

blocking callibrate, fix

Browse source
This commit is contained in:
jusax23 2023-09-21 11:51:54 +02:00
parent 0cd62f9db7
commit b5fb77fe35
Signed by: jusax23
GPG key ID: 499E2AA870C1CD41
2 changed files with 42 additions and 13 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

37
system_sensors/compass.h
View file

@ -3,7 +3,8 @@
#define jType_S_COMP 0x22
#define jCommand_COMP_fail 0xff
// #define jCommand_Compass_Raw 0
#define jCommand_Compass_Data 0
#define jCommand_Compass_Calibrate 1
typedef struct {
byte type = jType_S_COMP;
@ -19,9 +20,21 @@ typedef struct {
#include <QMC5883LCompass.h>
QMC5883LCompass compass;
void initCOMP() { compass.init(); }
bool compass_is_calibrating = false;
void COMPLoop() { compass.read(); }
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;
@ -31,12 +44,30 @@ yh=compass.getX()*sin(Roll)*sin(Altitude)+compass.getY()*cos(Roll)-compass.getZ(
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

6
system_sensors/mpu.h
View file

@ -42,14 +42,12 @@ void initMPU() {
Serial.println("began mpu6050");
#endif
/*mpu6050.setGyroOffsets(preferences.getFloat("mpuOffGyrX", -7.8),
mpu6050.setGyroOffsets(preferences.getFloat("mpuOffGyrX", -7.8),
preferences.getFloat("mpuOffGyrY", 2.32),
preferences.getFloat("mpuOffGyrZ", -0.22));
mpu6050.setAccXoffset(preferences.getFloat("mpuOffAccX", 0.03),
preferences.getFloat("mpuOffAccY", 0.0),
preferences.getFloat("mpuOffAccZ", 0.3));*/
mpu6050.setGyroOffsets(-7.8, 2.32, -0.22);
preferences.getFloat("mpuOffAccZ", 0.3));
#if debug
Serial.println("set mpu6050 gyro");
#endif