Diff: main.cpp

Revision:

6:9712c04e13bf

Parent:

5:f66e4666de56

Child:

7:337faf16a8d6

--- a/main.cpp	Fri Jan 13 07:29:19 2017 +0000
+++ b/main.cpp	Sun Jan 15 22:11:22 2017 +0000
@@ -39,6 +39,9 @@
 * Reading a IMU Without Kalman: The Complementary Filter
 * http://www.pieter-jan.com/node/11
 *
+* The Balance Filter
+* http://d1.amobbs.com/bbs_upload782111/files_44/ourdev_665531S2JZG6.pdf
+*
 */
 
 
@@ -47,9 +50,11 @@
 #include "bmi160.h"
 #include "SDFileSystem.h"
 
+
+float compFilter(float K, float pitch, float gyroX, float accY, float accZ, float DT);
                  
 void saveData(uint32_t numSamples, float * loopCoeffs, float * gyroX, 
-              float * accY, float * accZ, float * pw);
+              float * accY, float * accZ, int32_t * pw);
 
 
 //Setup interrupt in from imu
@@ -156,7 +161,7 @@
     float accYaxisBuff[N];
     float accZaxisBuff[N];
     float gyroXaxisBuff[N];
-    float pulseWidthBuff[N];
+    int32_t pulseWidthBuff[N];
     
     //Setup I2C bus for IMU
     I2C i2cBus(P5_7, P6_0);
@@ -167,10 +172,10 @@
     
     BMI160::AccConfig accConfig;
     BMI160::AccConfig accConfigRead;
-    accConfig.range = BMI160::SENS_2G;
+    accConfig.range = BMI160::SENS_4G;
     accConfig.us = BMI160::ACC_US_OFF;
     accConfig.bwp = BMI160::ACC_BWP_2;
-    accConfig.odr = BMI160::ACC_ODR_12;
+    accConfig.odr = BMI160::ACC_ODR_11;
     if(imu.setSensorConfig(accConfig) == BMI160::RTN_NO_ERROR)
     {
         if(imu.getSensorConfig(accConfigRead) == BMI160::RTN_NO_ERROR)
@@ -209,7 +214,7 @@
     BMI160::GyroConfig gyroConfigRead;
     gyroConfig.range = BMI160::DPS_2000;
     gyroConfig.bwp = BMI160::GYRO_BWP_2;
-    gyroConfig.odr = BMI160::GYRO_ODR_12;
+    gyroConfig.odr = BMI160::GYRO_ODR_11;
     if(imu.setSensorConfig(gyroConfig) == BMI160::RTN_NO_ERROR)
     {
         if(imu.getSensorConfig(gyroConfigRead) == BMI160::RTN_NO_ERROR)
@@ -273,13 +278,12 @@
         BMI160::SensorTime sensorTime;
         
         //Complementary Filter vars, filter weight K
-        static const float K = 0.99995F;
+        static const float K = 0.999F;
         float pitch = 0.0F;
         
         //PID coefficients
-        //Kc = 2.5, Pc = 0.1; Critical gain and period of osc
-        //when adjusting KP until marginally stable
-        static const float DT = 0.000625F;
+
+        static const float DT = 0.00125F;
         static const float KP = 2.0F;
         static const float KI = (0.0F*DT);
         static const float KD = (0.0F/DT);
@@ -343,7 +347,7 @@
                 rLED = LED_ON;
                 gLED = LED_ON;
                 
-                while(offsetCount < 3200)
+                while(offsetCount < 1600)
                 {
                     if(drdy)
                     {
@@ -353,9 +357,8 @@
                         //Read feedback sensors
                         imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
                         
-                        //Feedback Block, Complementary filter.
-                        //Pitch estimate in degrees
-                        pitch = ((K * (pitch + (gyroData.xAxis.scaled * DT))) + ((1.0F - K) * accData.yAxis.scaled));
+                        //Feedback Block, pitch estimate in degrees
+                        pitch = compFilter(K, pitch, gyroData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled, DT);
                         
                         //Accumalate pitch measurements
                         setPoint += pitch;
@@ -388,10 +391,9 @@
                     //Read feedback sensors
                     imu.getGyroAccXYZandSensorTime(accData, gyroData, sensorTime, accConfig.range, gyroConfig.range);
                     
-                    //Feedback Block, Complementary filter.
-                    //Pitch estimate in degrees
-                    pitch = ((K * (pitch + (gyroData.xAxis.scaled * DT))) + ((1.0F - K) * accData.yAxis.scaled));
-                    
+                    //Feedback Block, pitch estimate in degrees
+                    pitch = compFilter(K, pitch, gyroData.xAxis.scaled, accData.yAxis.scaled, accData.zAxis.scaled, DT);
+                        
                     //PID Block
                     //Error signal
                     currentError = (setPoint - pitch);
@@ -472,7 +474,7 @@
                         accYaxisBuff[samples] = accData.yAxis.scaled;
                         accZaxisBuff[samples] = accData.zAxis.scaled;
                         gyroXaxisBuff[samples] = gyroData.xAxis.scaled;
-                        pulseWidthBuff[samples] = pulseWidth;
+                        pulseWidthBuff[samples] = static_cast<int32_t>(pulseWidth);
                         samples++;
                     }
                     
@@ -524,7 +526,7 @@
 
 //*****************************************************************************
 void saveData(uint32_t numSamples, float * loopCoeffs, float * gyroX, 
-              float * accY, float * accZ, float * pw)
+              float * accY, float * accZ, int32_t * pw)
 {
     SDFileSystem sd(P0_5, P0_6, P0_4, P0_7, "sd");  // mosi, miso, sclk, cs
     FILE *fp;
@@ -543,21 +545,21 @@
         float accPitch = 0.0F;
         float gyroDegrees = 0.0F;
         float pitch = 0.0F;
-        float k = loopCoeffs[1];
-        float dt = loopCoeffs[5];
+        float K = loopCoeffs[1];
+        float DT = loopCoeffs[5];
         
         for(uint32_t idx = 0; idx < numSamples; idx++) 
         {
-            fprintf(fp, "%f,", idx*dt);
+            fprintf(fp, "%f,", idx*DT);
             fprintf(fp, "%5.4f,", accY[idx]);
             fprintf(fp, "%6.2f,", gyroX[idx]);
-            gyroDegrees += (gyroX[idx] * dt);
+            gyroDegrees += (gyroX[idx] * DT);
             fprintf(fp, "%5.2f,", gyroDegrees);
             accPitch = ((180.0F/3.14159F) * atan(accY[idx]/accZ[idx]));
             fprintf(fp, "%5.2f,", accPitch);
-            pitch = ((k * (pitch + (gyroX[idx] * dt))) + ((1.0F - k) * accY[idx]));
+            pitch = compFilter(K, pitch, gyroX[idx], accY[idx], accZ[idx], DT);
             fprintf(fp, "%5.2f,", pitch);
-            fprintf(fp, "%f", pw[idx]);
+            fprintf(fp, "%d", pw[idx]);
             fprintf(fp, "\n");
         }
         fprintf(fp, "\n");
@@ -568,3 +570,10 @@
         printf("Failed to open file\n");
     }
 }
+
+
+//*****************************************************************************
+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))));
+}