In this video lesson we show how to create a tilt compensated digital compass. Calculating heading based simply on the measured magnetometer values in the X and Y directions only works accurately when the compass is sitting flat, or horizontal with the earth’s surface. If we introduce a tilt, either by applying pitch or roll to the system, calculated heading, or yaw will no longer be accurate. In the video above, we show you how to mathematically ‘un-tilt’ the sensor to get accurate heading readings when the device is not perfectly flat.
We are working with a GY-87 9-axis IMU, and an Arduino Uno R4 WiFi. Below is the schematic we are using in this project:
For your convenience, the code developed in this video lesson is included below. Please notice that the calibration constants in the code below are for my GY-87 module. You need to calibrate your own module, as my numbers below would likely be different from your numbers. We showed how to do the calibration in THIS LESSON.
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 | #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); } |