JuPad/DefaultReceiver
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

mpu offsets

Browse source
This commit is contained in:
jusax23 2022-08-21 15:13:33 +02:00
parent e439ac81ec
commit 10ad073003
1 changed files with 100 additions and 2 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

102
MPU.h
View file

@ -2,6 +2,21 @@
#define jCommand_MPU_Acc 0
#define jCommand_MPU_Gyro 1
#define jCommand_MPU_Angle 2
#define jCommand_MPU_reset_offset_calc 0x10
#define jCommand_MPU_set_calc_offsets_acc 0x11
#define jCommand_MPU_set_calc_offsets_gyro 0x12
#define jCommand_MPU_get_calc_offsets_acc 0x13
#define jCommand_MPU_get_calc_offsets_gyro 0x14
#define jCommand_MPU_set_offsets_acc 0x15
#define jCommand_MPU_set_offsets_gyro 0x16
#define jCommand_MPU_get_offsets_acc 0x17
#define jCommand_MPU_get_offsets_gyro 0x18
#define jCommand_MPU_fail 0xff
typedef struct{
@ -20,8 +35,9 @@ MPU6050 mpu6050(Wire, 0.001f, 0.999f);
void initMPU(){
mpu6050.begin();
mpu6050.setGyroOffsets(-7.8,2.32,-0.22);
mpu6050.setAccXoffset(0.03,0.0,0.3);
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.calcGyroOffsets(false,50,50);
}
@ -55,6 +71,88 @@ void MPUradioTask(){
data.Z = mpu6050.getAngleZ();
jSendNRF(&data, sizeof(data));
}break;
case jCommand_MPU_reset_offset_calc: {
mpu6050.restGyroOffsetGlide();
mpu6050.resetAccOffsetWatch();
}
break;
case jCommand_MPU_set_calc_offsets_acc:{
float offX = mpu6050.calcAccXoffset();
float offY = mpu6050.calcAccYoffset();
float offZ = mpu6050.calcAccZoffset();
preferences.putFloat("mpuOffAccX",offX);
preferences.putFloat("mpuOffAccY",offY);
preferences.putFloat("mpuOffAccZ",offZ);
mpu6050.setAccXoffset(offX,offY,offZ);
}
break;
case jCommand_MPU_set_calc_offsets_gyro:{
float offX = mpu6050.getGyroXoffsetGlide();
float offY = mpu6050.getGyroYoffsetGlide();
float offZ = mpu6050.getGyroZoffsetGlide();
preferences.putFloat("mpuOffGyrX",offX);
preferences.putFloat("mpuOffGyrY",offY);
preferences.putFloat("mpuOffGyrZ",offZ);
mpu6050.setGyroOffsets(offX,offY,offZ);
}
break;
case jCommand_MPU_get_calc_offsets_acc:{
mpuSendData data;
data.command = jCommand_MPU_get_calc_offsets_acc;
data.X = mpu6050.getGyroXoffsetGlide();
data.Y = mpu6050.getGyroYoffsetGlide();
data.Z = mpu6050.getGyroZoffsetGlide();
jSendNRF(&data, sizeof(data));
}
break;
case jCommand_MPU_get_calc_offsets_gyro:{
mpuSendData data;
data.command = jCommand_MPU_get_calc_offsets_gyro;
data.X = mpu6050.calcAccXoffset();
data.Y = mpu6050.calcAccYoffset();
data.Z = mpu6050.calcAccZoffset();
jSendNRF(&data, sizeof(data));
}
break;
case jCommand_MPU_set_offsets_acc:
{
mpuSendData data;
memmove(&data, &jRCbuff, sizeof(data));
preferences.putFloat("mpuOffGyrX", data.X);
preferences.putFloat("mpuOffGyrY", data.Y);
preferences.putFloat("mpuOffGyrZ", data.Z);
mpu6050.setGyroOffsets(data.X, data.Y, data.Z);
}
break;
case jCommand_MPU_set_offsets_gyro:
{
mpuSendData data;
memmove(&data, &jRCbuff, sizeof(data));
preferences.putFloat("mpuOffAccX", data.X);
preferences.putFloat("mpuOffAccY", data.Y);
preferences.putFloat("mpuOffAccZ", data.Z);
mpu6050.setAccXoffset(data.X, data.Y, data.Z);
}
break;
case jCommand_MPU_get_offsets_acc:{
mpuSendData data;
data.command = jCommand_MPU_get_offsets_acc;
data.X = preferences.getFloat("mpuOffGyrX", -7.8);
data.Y = preferences.getFloat("mpuOffGyrY", 2.32);
data.Z = preferences.getFloat("mpuOffGyrZ", -0.22);
jSendNRF(&data, sizeof(data));
}
break;
case jCommand_MPU_get_offsets_gyro:{
mpuSendData data;
data.command = jCommand_MPU_get_offsets_gyro;
data.X = preferences.getFloat("mpuOffAccX", 0.03);
data.Y = preferences.getFloat("mpuOffAccY", 0.0);
data.Z = preferences.getFloat("mpuOffAccZ", 0.3);
jSendNRF(&data, sizeof(data));
}
}
}