22#include < MadgwickAHRS.h>
33
44Madgwick filter;
5- unsigned long microsPerReading, microsPrevious;
65float accelScale, gyroScale;
76
87void setup () {
@@ -18,10 +17,6 @@ void setup() {
1817 CurieIMU.setAccelerometerRange (2 );
1918 // Set the gyroscope range to 250 degrees/second
2019 CurieIMU.setGyroRange (250 );
21-
22- // initialize variables to pace updates to correct rate
23- microsPerReading = 1000000 / 25 ;
24- microsPrevious = micros ();
2520}
2621
2722void loop () {
@@ -30,11 +25,9 @@ void loop() {
3025 float ax, ay, az;
3126 float gx, gy, gz;
3227 float roll, pitch, heading;
33- unsigned long microsNow;
3428
35- // check if it's time to read data and update the filter
36- microsNow = micros ();
37- if (microsNow - microsPrevious >= microsPerReading) {
29+ // Check if new data is available from gyroscope or accelerometer
30+ if (CurieIMU.gyroDataReady () && CurieIMU.accelDataReady ()) {
3831
3932 // read raw data from CurieIMU
4033 CurieIMU.readMotionSensor (aix, aiy, aiz, gix, giy, giz);
@@ -60,9 +53,6 @@ void loop() {
6053 Serial.print (pitch);
6154 Serial.print (" " );
6255 Serial.println (roll);
63-
64- // increment previous time, so we keep proper pace
65- microsPrevious = microsPrevious + microsPerReading;
6656 }
6757}
6858
0 commit comments