Firstly, a little bit of fun with making a compass with the Prop Shield using the Madgwick filter:
Teensy Prop Shield Compass (Madgwick)
Now onto the more analytical videos. This video shows the Madgwick algorithm's initial startup:
Teensy Prop Shield Compass (Initial Madgwick orientation)
As you might expect, the current implementation of Madgwick starts up instantly and gimbles slowly to North. So the Madgwick filter might benefit from having a FirstOrientationLock feature like NXPSensorFusion.
Now over to the NXPSensorFusion filter:
Teensy Prop Shield Compass (NXPSensorFusion FirstOrientationLock and ValidMagCal issue)
0:04 - I plug in the teensy to my PC so I can see the serial monitor which is displaying the ValidMagCal and FirstOrientationLock flags from inside the NXPSensorFusion library.
0:09 - I rotate the board until the conditions for setting FirstOrientationLock become valid (i.e. ValidMagCal -> true).
0:27 - I rotate the board back to it's original position where ValidMagCal = false. This means the heading is now just using the Gyro data.
0:27 - 1:50 - Due to not using the Mag data, the heading drifts due to Gyro offset. (Fast forward & compare frame at 0:30 to frame at 1:50 to see that heading has drifted)
1:54 - I rotate the board to a position where ValidMagCal -> true again and you see the heading drift being corrected
Interestingly the NXPSensorFusion heading is 180 degrees different to Madgwick. Before anyone asks - I used the MotionCal to get a very good calibration before all these videos.
I was able to find a workaround for the loss of ValidMagCal in SensorFusion.cpp by changing
to
I don't think this workaround is technically correct but it might be helpful for others just to get things working.
I'm interested to see what others have observed, please feel free to post on this thread.
Teensy Prop Shield Compass (Madgwick)
Now onto the more analytical videos. This video shows the Madgwick algorithm's initial startup:
Teensy Prop Shield Compass (Initial Madgwick orientation)
As you might expect, the current implementation of Madgwick starts up instantly and gimbles slowly to North. So the Madgwick filter might benefit from having a FirstOrientationLock feature like NXPSensorFusion.
Now over to the NXPSensorFusion filter:
Teensy Prop Shield Compass (NXPSensorFusion FirstOrientationLock and ValidMagCal issue)
0:04 - I plug in the teensy to my PC so I can see the serial monitor which is displaying the ValidMagCal and FirstOrientationLock flags from inside the NXPSensorFusion library.
0:09 - I rotate the board until the conditions for setting FirstOrientationLock become valid (i.e. ValidMagCal -> true).
0:27 - I rotate the board back to it's original position where ValidMagCal = false. This means the heading is now just using the Gyro data.
0:27 - 1:50 - Due to not using the Mag data, the heading drifts due to Gyro offset. (Fast forward & compare frame at 0:30 to frame at 1:50 to see that heading has drifted)
1:54 - I rotate the board to a position where ValidMagCal -> true again and you see the heading drift being corrected
Interestingly the NXPSensorFusion heading is 180 degrees different to Madgwick. Before anyone asks - I used the MotionCal to get a very good calibration before all these videos.
I was able to find a workaround for the loss of ValidMagCal in SensorFusion.cpp by changing
Code:
if (fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f) {
ValidMagCal = 1;
} else {
ValidMagCal = 0;
}
to
Code:
if(FirstOrientationLock == 0){
if (fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f && fabsf(mx) >= 20.0f) {
ValidMagCal = 1;
} else {
ValidMagCal = 0;
}
} else{
ValidMagCal = 1;
}
I don't think this workaround is technically correct but it might be helpful for others just to get things working.
I'm interested to see what others have observed, please feel free to post on this thread.
Last edited: