Revision 3:78cf56b8eb21, committed 2018-02-10

Comitter:

rsmits

Date:

Sat Feb 10 12:42:40 2018 +0000

Parent:

2:405f8e1a01d3

Commit message:

calibration

Changed in this revision

--- a/main.cpp	Sat Feb 03 15:21:25 2018 +0000
+++ b/main.cpp	Sat Feb 10 12:42:40 2018 +0000
@@ -11,21 +11,84 @@
 #define toRadians(x) (x * 0.01745329252f)
 //Convert from radians to degrees.
 #define toDegrees(x) (x * 57.2957795)
-
+//Number of samples to average.
+#define SAMPLES 4
+//Number of samples to be averaged for a null bias calculation
+//during calibration.
+#define CALIBRATION_SAMPLES 128
+//Sampling gyroscope at 200Hz.
+#define GYRO_RATE   0.005
+//Sampling accelerometer at 200Hz.
+#define ACC_RATE    0.005
+//Updating filter at 40Hz.
+#define FILTER_RATE 0.1
 const int   LSM6DS3_ADDRESS = 0xD4;
-float       DEGREES[3];
-float       ACCELERATIONS[3];
-float       ANGLES[3];
 //const int   LSM6DS3_ADDRESS = 0x69;
 
 Serial      pc(SERIAL_TX, SERIAL_RX); 
 AnalogIn    PRESSURE_SENSOR(PA_0);
 LSM6DS3     imu(PB_9, PB_8, LSM6DS3_ADDRESS);
-IMUfilter   imuFilter(0.1, 10);
+//At rest the gyroscope is centred around 0 and goes between about
+//-5 and 5 counts. As 1 degrees/sec is ~15 LSB, error is roughly
+//5/15 = 0.3 degrees/sec.
+IMUfilter   imuFilter(FILTER_RATE, 0.3);
+Ticker      filterTicker;
 Ticker      ScreenTicker; 
 
+//Offsets for the gyroscope.
+//The readings we take when the gyroscope is stationary won't be 0, so we'll
+//average a set of readings we do get when the gyroscope is stationary and
+//take those away from subsequent readings to ensure the gyroscope is offset
+//or "biased" to 0.
+float w_xBias;
+float w_yBias;
+float w_zBias;
+ 
+//Offsets for the accelerometer.
+//Same as with the gyroscope.
+float a_xBias;
+float a_yBias;
+float a_zBias;
+ 
+//Accumulators used for oversampling and then averaging.
+volatile float a_xAccumulator = 0;
+volatile float a_yAccumulator = 0;
+volatile float a_zAccumulator = 0;
+volatile float w_xAccumulator = 0;
+volatile float w_yAccumulator = 0;
+volatile float w_zAccumulator = 0;
+ 
+//Accelerometer and gyroscope readings for x, y, z axes.
+volatile float a_x;
+volatile float a_y;
+volatile float a_z;
+volatile float w_x;
+volatile float w_y;
+volatile float w_z;
+ 
+//Buffer for accelerometer readings.
+float readings[3];
+//Number of accelerometer samples we're on.
+int accelerometerSamples = 0;
+//Number of gyroscope samples we're on.
+int gyroscopeSamples = 0;
+ 
+/**
+ * Prototypes
+ */
+//Calculate the null bias.
+void calibrateAccelerometer(void);
+//Take a set of samples and average them.
+void sampleAccelerometer(void);
+//Calculate the null bias.
+void calibrateGyroscope(void);
+//Take a set of samples and average them.
+void sampleGyroscope(void);
+//Update the filter and calculate the Euler angles.
+void filter(void);
+
 //Init Serial port and LSM6DS3 chip
-void setup()
+void setup_LSM6DS3()
 {
     // Use the begin() function to initialize the LSM9DS0 library.
     // You can either call it with no parameters (the easy way):
@@ -47,21 +110,136 @@
     pc.printf("Should be 0x69\r\n");
 }
 
-void screenUpdate(){
-    pc.printf("Accel (m/s^2):\r\n");
-    pc.printf("AX: %2f\r\n", ACCELERATIONS[0]);
-    pc.printf("AY: %2f\r\n", ACCELERATIONS[1]);
-    pc.printf("AZ: %2f\r\n", ACCELERATIONS[2]);
+void screenUpdate(){    
+    pc.printf("%f,%f,%f\r\n",toDegrees(imuFilter.getRoll()),
+          toDegrees(imuFilter.getPitch()),
+          toDegrees(imuFilter.getYaw()));
+}
+
+void calibrateAccelerometer(void) {
+ 
+    a_xAccumulator = 0;
+    a_yAccumulator = 0;
+    a_zAccumulator = 0;
+    
+    //Take a number of readings and average them
+    //to calculate the zero g offset.
+    for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
+
+        imu.readAccel();
+        a_xAccumulator += imu.ax;
+        a_yAccumulator += imu.ay;
+        a_zAccumulator += imu.az;  
+ 
+        wait(ACC_RATE);
+    }
+ 
+    a_xAccumulator /= CALIBRATION_SAMPLES;
+    a_yAccumulator /= CALIBRATION_SAMPLES;
+    a_zAccumulator /= CALIBRATION_SAMPLES;
+ 
+    a_xBias = a_xAccumulator;
+    a_yBias = a_yAccumulator;
+    // When laying on the table, default a_z is 1g
+    a_zBias = (a_zAccumulator - 1.0f);
+ 
+    a_xAccumulator = 0;
+    a_yAccumulator = 0;
+    a_zAccumulator = 0;
+}
+
+void sampleAccelerometer(void) {
+ 
+    //Have we taken enough samples?
+    if (accelerometerSamples == SAMPLES) {
+ 
+        //Average the samples, remove the bias and convert to m/s/s.
+        a_x = ((a_xAccumulator / SAMPLES) - a_xBias) * g0;
+        a_y = ((a_yAccumulator / SAMPLES) - a_yBias) * g0;
+        a_z = ((a_zAccumulator / SAMPLES) - a_zBias) * g0;
+ 
+        a_xAccumulator = 0;
+        a_yAccumulator = 0;
+        a_zAccumulator = 0;
+        accelerometerSamples = 0;
+ 
+    } else {
+        //Take another sample.
+        imu.readAccel();
+        a_xAccumulator += imu.ax;
+        a_yAccumulator += imu.ay;
+        a_zAccumulator += imu.az;        
+ 
+        accelerometerSamples++;
+    }
+}
 
-    pc.printf("Gyro (degrees/s):\r\n");
-    pc.printf("GX: %f\r\n", DEGREES[0]);
-    pc.printf("GY: %f\r\n", DEGREES[1]);
-    pc.printf("GZ: %2f\r\n", DEGREES[2]);
-    
-    pc.printf("Angle (degrees):\r\n");
-    pc.printf("X: %f\r\n", ANGLES[0]);
-    pc.printf("Y: %f\r\n", ANGLES[1]);
-    pc.printf("Z: %f\r\n\r\n", ANGLES[2]); 
+void calibrateGyroscope(void) {
+ 
+    w_xAccumulator = 0;
+    w_yAccumulator = 0;
+    w_zAccumulator = 0;
+ 
+    //Take a number of readings and average them
+    //to calculate the gyroscope bias offset.
+    for (int i = 0; i < CALIBRATION_SAMPLES; i++) {
+
+        imu.readGyro();
+        w_xAccumulator += imu.gx;
+        w_yAccumulator += imu.gy;
+        w_zAccumulator += imu.gz;
+        
+        wait(GYRO_RATE);
+    }
+ 
+    //Average the samples.
+    w_xAccumulator /= CALIBRATION_SAMPLES;
+    w_yAccumulator /= CALIBRATION_SAMPLES;
+    w_zAccumulator /= CALIBRATION_SAMPLES;
+ 
+    w_xBias = w_xAccumulator;
+    w_yBias = w_yAccumulator;
+    w_zBias = w_zAccumulator;
+ 
+    w_xAccumulator = 0;
+    w_yAccumulator = 0;
+    w_zAccumulator = 0;
+}
+ 
+void sampleGyroscope(void) {
+ 
+    //Have we taken enough samples?
+    if (gyroscopeSamples == SAMPLES) {
+ 
+        //Average the samples, remove the bias, and calculate the angular
+        //velocity in rad/s.
+        w_x = toRadians(((w_xAccumulator / SAMPLES) - w_xBias));
+        w_y = toRadians(((w_yAccumulator / SAMPLES) - w_yBias));
+        w_z = toRadians(((w_zAccumulator / SAMPLES) - w_zBias));
+ 
+        w_xAccumulator = 0;
+        w_yAccumulator = 0;
+        w_zAccumulator = 0;
+        gyroscopeSamples = 0;
+ 
+    } else {
+        //Take another sample.
+        imu.readGyro();
+        w_xAccumulator += imu.gx;
+        w_yAccumulator += imu.gy;
+        w_zAccumulator += imu.gz;
+ 
+        gyroscopeSamples++;
+    }
+}
+ 
+void filter(void) {
+ 
+    //Update the filter variables.
+    imuFilter.updateFilter(w_y, w_x, w_z, a_y, a_x, a_z);
+    //Calculate the new Euler angles.
+    imuFilter.computeEuler();
+ 
 }
  
 int main()
@@ -72,37 +250,28 @@
         if(PRESSURE_SENSOR > 0.9f){
             
             if (not started){
-                setup();  //Setup sensor and Serial
+                setup_LSM6DS3();  //Setup sensor and Serial
                 pc.printf("\r\n------ LSM6DS3 Demo -----------\r\n");
                 started=true;
-                ScreenTicker.attach(&screenUpdate, 1);
+                
+                calibrateAccelerometer();
+                calibrateGyroscope();
+                pc.printf("\r\n------ Calibrated -----------\r\n");
+                
+                //Set up timers.
+                filterTicker.attach(&filter, FILTER_RATE);
+                ScreenTicker.attach(&screenUpdate, FILTER_RATE);
             }
             
-            wait_ms(REFRESH_TIME_MS);     
-            imu.readAccel();
-            imu.readGyro();
-            
-            
-            DEGREES[0] = imu.gx;
-            DEGREES[1] = imu.gy;
-            DEGREES[2] = imu.gz;
-            
-            // m/(s^2) = g0*g
-            ACCELERATIONS[0] = g0*imu.ax;
-            ACCELERATIONS[1] = g0*imu.ay;
-            ACCELERATIONS[2] = g0*imu.az;
-                        
-            imuFilter.updateFilter(toRadians(imu.gx), toRadians(imu.gy), toRadians(imu.gz), 
-                                   g0*imu.ax, g0*imu.ay, g0*imu.az);
-            imuFilter.computeEuler();
-            
-            ANGLES[0] = toDegrees(imuFilter.getRoll());
-            ANGLES[1] = toDegrees(imuFilter.getPitch());
-            ANGLES[2] = toDegrees(imuFilter.getYaw());            
+            wait(0.005);
+            sampleAccelerometer();
+            sampleGyroscope();
+               
         }
         else {
             started=false;
             imuFilter.reset();
+            filterTicker.detach();
             ScreenTicker.detach();
         }
     }