74 lines
1.5 KiB
C
74 lines
1.5 KiB
C
|
#define jType_S_MPU 1
|
||
|
|
#define jCommand_MPU_Acc 0
|
||
|
|
#define jCommand_MPU_Gyro 1
|
||
|
|
#define jCommand_MPU_Angle 2
|
||
|
|
#define jCommand_MPU_fail 0xff
|
||
|
|
|
||
|
|
typedef struct{
|
||
|
|
byte type = jType_S_MPU;
|
||
|
|
byte command;
|
||
|
|
float X;
|
||
|
|
float Y;
|
||
|
|
float Z;
|
||
|
|
}mpuSendData;
|
||
|
|
|
||
|
|
#ifdef jMPU
|
||
|
|
|
||
|
|
#include <MPU6050_jusax23.h>
|
||
|
|
|
||
|
|
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.calcGyroOffsets(false,50,50);
|
||
|
|
}
|
||
|
|
|
||
|
|
void MPULoop(){
|
||
|
|
mpu6050.update();
|
||
|
|
}
|
||
|
|
|
||
|
|
void MPUradioTask(){
|
||
|
|
switch (jRCbuff[1]) {
|
||
|
|
case jCommand_MPU_Acc:{
|
||
|
|
mpuSendData data;
|
||
|
|
data.command = jCommand_MPU_Acc;
|
||
|
|
data.X = mpu6050.getAccX();
|
||
|
|
data.Y = mpu6050.getAccY();
|
||
|
|
data.Z = mpu6050.getAccZ();
|
||
|
|
jSendNRF(&data, sizeof(data));
|
||
|
|
}break;
|
||
|
|
case jCommand_MPU_Gyro:{
|
||
|
|
mpuSendData data;
|
||
|
|
data.command = jCommand_MPU_Gyro;
|
||
|
|
data.X = mpu6050.getGyroX();
|
||
|
|
data.Y = mpu6050.getGyroY();
|
||
|
|
data.Z = mpu6050.getGyroZ();
|
||
|
|
jSendNRF(&data, sizeof(data));
|
||
|
|
}break;
|
||
|
|
case jCommand_MPU_Angle:{
|
||
|
|
mpuSendData data;
|
||
|
|
data.command = jCommand_MPU_Angle;
|
||
|
|
data.X = mpu6050.getAngleX();
|
||
|
|
data.Y = mpu6050.getAngleY();
|
||
|
|
data.Z = mpu6050.getAngleZ();
|
||
|
|
jSendNRF(&data, sizeof(data));
|
||
|
|
}break;
|
||
|
|
}
|
||
|
|
}
|
||
|
|
|
||
|
|
#else
|
||
|
|
|
||
|
|
void initMPU(){}
|
||
|
|
|
||
|
|
void MPULoop(){}
|
||
|
|
|
||
|
|
void MPUradioTask(){
|
||
|
|
mpuSendData data;
|
||
|
|
data.command = jCommand_MPU_fail;
|
||
|
|
jSendNRF(&data, sizeof(data));
|
||
|
|
}
|
||
|
|
|
||
|
|
#endif
|