Diff: AHRS.cpp

Revision:

5:eee47600b772

Parent:

4:3c21fb0c9e84

Child:

6:5824bd96b6cf

diff -r 3c21fb0c9e84 -r eee47600b772 AHRS.cpp
--- a/AHRS.cpp	Fri May 03 13:46:40 2019 +0000
+++ b/AHRS.cpp	Fri May 03 14:43:47 2019 +0000
@@ -29,7 +29,7 @@
     LinearCharacteristics int2magx( -1.0,0.0);     // x-axis reversed
     LinearCharacteristics int2magy( -1.0,0.0);     // y-axis reversed
     LinearCharacteristics int2magz(  1.0,0.0);
-    matrix measurement(6,1,0.0);
+    //matrix measurement(6,1,0.0);
     while (!imu.begin()) {
         wait(1);
         printf("Failed to communicate with LSM9DS1.\r\n");
@@ -47,17 +47,16 @@
         //imu.readMag_calibrated();
         imu.readMag();
         imu.readGyro();
-        
+        matrix measurement(6,1,0.0);
         //Perform Madgwick-filter update
         //RPY_filter.update(raw_gx2gx(imu.gx), raw_gy2gy(imu.gy),  raw_gz2gz(imu.gz) ,
         //              raw_ax2ax(imu.ax), raw_ay2ay(imu.ay),  raw_az2az(imu.az),
-        //              int2magx(imu.mx), int2magy(imu.my),  int2magz(imu.mz)); 
+        //              int2magx(imu.mx), int2magy(imu.my),  int2magz(imu.mz));
         measurement.put_entry(1,1,raw_gx2gx(imu.gyroX));
         measurement.put_entry(2,1,raw_gy2gy(imu.gyroY));
         measurement.put_entry(3,1,raw_ax2ax(imu.accX));
         measurement.put_entry(4,1,raw_ay2ay(imu.accY));
-        //RPY_filter.loop(&measurement);
- 
+        RPY_filter.loop(&measurement);
         }       // while(1)
 }
 void AHRS::sendSignal() {