Diff: main.cpp

Revision:

5:f6727800e43f

Parent:

4:fc8ef84a7dbc

Child:

7:2c48702d99e3

--- a/main.cpp	Wed Dec 05 04:44:17 2018 +0000
+++ b/main.cpp	Thu Dec 13 11:17:03 2018 +0000
@@ -37,6 +37,7 @@
 #include "max32630fthr.h"
 #include "USBSerial.h"
 #include "stdlib.h"
+#include "math.h"
 
 //MAX32630HSP icarus(MAX32630HSP::VIO_3V3);
 MAX32630FTHR pegasus(MAX32630FTHR::VIO_3V3);
@@ -64,6 +65,14 @@
 void printRegister(BMI160 &imu, BMI160::Registers reg);
 void printBlock(BMI160 &imu, BMI160::Registers startReg, BMI160::Registers stopReg);
 void writeReg(BMI160 &imu, BMI160::Registers reg, uint8_t data);
+//float compFilter(float K, float pitch, float gyroX, float accY, float accZ,float DT);
+
+
+float compFilter(float K, float pitch, float gyroX, float accY, float accZ,float DT)
+{
+    return ((K * (pitch + (gyroX * DT))) + ((1.0F - K) * ((180.0F / 3.1459F) * atan(accY/accZ))));
+}
+
 
 int main()
 {
@@ -142,6 +151,8 @@
         pc.printf("GYRO Range = %d\r\n", gyroConfig.range);
         pc.printf("GYRO BandWidthParam = %d\r\n", gyroConfig.bwp);
         pc.printf("GYRO OutputDataRate = %d\r\n\r\n", gyroConfig.odr);
+        
+        
     }
     else
     {
@@ -177,15 +188,28 @@
         BMI160::SensorTime sensorTime;
         
         //PwmPin = 1;
-
+        float apitch = 0;
+        float k = 0.6;
+        
+        time_1 = sensorTime.seconds;
         while(1)
         {
             
             imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
             imu.getTemperature(&imuTemperature);
             
+            time_2 = sensorTime.seconds;
+            apitch = compFilter(k, apitch, gyroData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled, time_2 - time_1);
+            daplink.printf("Forward: %s%4.3f\r\n", "\033[K", apitch);
+            time_1 = time_2;
+            
              //printRegister(imu, BMI160::GYR_CONF);
              
+       }
+    }
+}     
+             
+/*             
             if(timeFlag == true){
                 
                 currentGyroX = gyroData.xAxis.scaled;
@@ -214,12 +238,12 @@
                 {
                     M_1 = 0;
                     M_2 = 1;
-                    pc.printf("Forward: %s%4.3f\r\n", "\033[K", xDeviation);
+                    daplink.printf("Forward: %s%4.3f\r\n", "\033[K", xDeviation);
                 }
                     else{
                     M_1 = 1;
                     M_2 = 0;
-                    pc.printf("Backward: %s%4.3f\r\n", "\033[K", xDeviation);
+                    daplink.printf("Backward: %s%4.3f\r\n", "\033[K", xDeviation);
                 }
                 
                 //------------------------
@@ -251,7 +275,9 @@
             wait(0.6);
         }
     }
-}
+*/
+
+
 
 
 //*****************************************************************************