#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <QMC5883LCompass.h>
float AxRaw,AyRaw,AzRaw,GxRaw,GyRaw,GzRaw,MxRaw,MyRaw,MzRaw;
float AxCal,AyCal,AzCal,GxCal,GyCal,GzCal,MxCal,MyCal,MzCal;
float rollA,pitchA,yawM,rollC,pitchC,yawC;
float rollG = 0;
float pitchG =0 ;
float yawG = 0;
float deltaRoll = 0;
float deltaPitch = 0;
float deltaYaw = 0;
float alpha = .1;
float rollCrad;
float pitchCrad;
float MxCalH;
float MyCalH;
int tStart =millis();
Adafruit_MPU6050 mpu;
QMC5883LCompass compass;
void setup() {
// put your setup code here, to run once:
Serial.begin(115200);
mpu.begin();
mpu.setI2CBypass(true);
compass.init();
mpu.setGyroRange(MPU6050_RANGE_1000_DEG);
delay(100);
}
void calibrateSensors() {
const float axOffset = 0.45407887789180545;
const float ayOffset = 0.09432915233553185;
const float azOffset = -0.4614157209191969;
const float axScale = 0.10141341897341144;
const float ayScale = 0.10141341897341144;
const float azScale = 0.10141341897341144;
const float gxOffset = -2.277507235645022;
const float gyOffset = 0.8794902155258137;
const float gzOffset = -0.5729577951308232;
const float mxOffset = -103.48885017294003;
const float myOffset = -96.80085481152321;
const float mzOffset = 923.8042898128733;
const float mxScale = 0.001156333898601669;
const float myScale = 0.001012882116988885;
const float mzScale = 0.0009400860982065997;
AxCal = (AxRaw - axOffset) * axScale;
AyCal = (AyRaw - ayOffset) * ayScale;
AzCal = (AzRaw - azOffset) * azScale;
GxCal = GxRaw*180/PI - gxOffset;
GyCal = GyRaw*180/PI - gyOffset;
GzCal = GzRaw*180/PI - gzOffset;
MxCal = (MxRaw - mxOffset) * mxScale;
MyCal = (MyRaw - myOffset) * myScale;
MzCal = (MzRaw - mzOffset) * mzScale;
}
void loop() {
// put your main code here, to run repeatedly:
compass.read();
sensors_event_t a, g, temp;
mpu.getEvent(&a, &g, &temp);
AxRaw = a.acceleration.x;
AyRaw = a.acceleration.y;
AzRaw = a.acceleration.z;
GxRaw = g.gyro.x;
GyRaw = g.gyro.y;
GzRaw = g.gyro.z;
MxRaw= compass.getX();
MyRaw= compass.getY();
MzRaw= compass.getZ();
calibrateSensors();
rollA = atan2(AyCal,sqrt(AzCal*AzCal + AxCal*AxCal))*180/PI;
pitchA = atan2(AxCal,sqrt(AzCal*AzCal + AyCal*AyCal))*180/PI;
deltaRoll = GxCal*(millis()-tStart)/1000.;
deltaPitch = -GyCal*(millis()-tStart)/1000.;
deltaYaw = -GzCal*(millis()-tStart)/1000.;
tStart = millis();
rollG = rollG + deltaRoll;
pitchG = pitchG + deltaPitch;
yawG = yawG + deltaYaw;
rollC = alpha*(rollA) + (1-alpha)*(rollC+deltaRoll);
pitchC = alpha*(pitchA) + (1-alpha)*(pitchC+deltaPitch);
rollCrad = rollC*PI/180;
pitchCrad = pitchC*PI/180;
MxCalH = MxCal*cos(pitchCrad) -MyCal*sin(rollCrad)*sin(pitchCrad)-MzCal*cos(rollCrad)*sin(pitchCrad);
MyCalH = MyCal*cos(rollCrad) - MzCal*sin(rollCrad);
yawM = atan2(MyCalH,MxCalH)*180./PI;
yawC = alpha*(yawM) + (1-alpha)*(yawC + deltaYaw);
// Serial.print("yawC:");Serial.print(yawC);Serial.print(',');
// Serial.print("yawM:");Serial.print(yawM);Serial.print(',');
// Serial.print("yawG:");Serial.print(yawG);Serial.print(',');
// Serial.print("LL:");Serial.print(-90);Serial.print(',');
// Serial.print("UL:");Serial.println(90);
Serial.print("rollC:");Serial.print(rollC);Serial.print(',');
Serial.print("pitchC:");Serial.print(pitchC);Serial.print(',');
Serial.print("yawC:");Serial.print(yawC);Serial.print(',');
Serial.print("LL:");Serial.print(-90);Serial.print(',');
Serial.print("UL:");Serial.println(90);
delay(100);
}