Diff: main.cpp

Revision:

71:a6a052fd3d22

Parent:

70:c6b61c5cadf4

--- a/main.cpp	Sun Nov 12 20:48:31 2017 +0000
+++ b/main.cpp	Mon Nov 13 10:46:13 2017 +0000
@@ -54,6 +54,7 @@
 #include "mbed.h"
 #include "assert.h"
 #include "x_nucleo_iks01a1.h"
+#include "Kalman.h"
 #include <math.h>
 
 #include <Ticker.h>
@@ -61,7 +62,7 @@
 
 /*** Constants ---------------------------------------------------------------- ***/
 namespace {
-	const int MS_INTERVALS = 1000;
+	const int MS_INTERVALS = 5;
 	const double RAD_TO_DEG = 57.2957786;
 	const double PI = 3.14159265;
 }
@@ -77,6 +78,19 @@
 #endif
 #define LED_ON  (!LED_OFF)
 
+#define RESTRICT_PITCH // Comment out to restrict roll to ±90deg instead
+#define DECLINATION 2.23 
+
+Kalman kalmanX; // Create the Kalman instances
+Kalman kalmanY;
+Kalman kalmanZ;
+
+double gyroXangle, gyroYangle; // Angle calculate using the gyro only
+double compAngleX, compAngleY; // Calculated angle using a complementary filter
+double kalAngleX, kalAngleY, kalAngleZ; // Calculated angle using a Kalman filter
+
+Timer t;
+uint32_t timer;
 
 /*** Typedefs ----------------------------------------------------------------- ***/
 typedef struct {
@@ -138,42 +152,9 @@
 
 
 /*** Helper Functions (2/2) ------------------------------------------------------------ ***/
-/* print floats & doubles */
-static char *printDouble(char* str, double v, int decimalDigits=2)
-{
-  int i = 1;
-  int intPart, fractPart;
-  int len;
-  char *ptr;
-
-  /* prepare decimal digits multiplicator */
-  for (;decimalDigits!=0; i*=10, decimalDigits--);
-
-  /* calculate integer & fractinal parts */
-  intPart = (int)v;
-  fractPart = (int)((v-(double)(int)v)*i);
-
-  /* fill in integer part */
-  sprintf(str, "%i.", intPart);
-
-  /* prepare fill in of fractional part */
-  len = strlen(str);
-  ptr = &str[len];
-
-  /* fill in leading fractional zeros */
-  for (i/=10;i>1; i/=10, ptr++) {
-	  if(fractPart >= i) break;
-	  *ptr = '0';
-  }
-
-  /* fill in (rest of) fractional part */
-  sprintf(ptr, "%i", fractPart);
-
-  return str;
-}
-
 /* Initialization function */
 static void init(void) {
+	t.start();
 	uint8_t id1, id2;
 	
 	/* Determine ID of Gyro & Motion Sensor */
@@ -193,48 +174,35 @@
 		mems_expansion_board->gyro_lsm6ds3->Attach_Free_Fall_Detection_IRQ(ff_irq);
 		mems_expansion_board->gyro_lsm6ds3->Enable_Free_Fall_Detection();
 	}
-}
-
-/* Main cycle function */
-static void main_cycle(void) {
+	
 	AxesRaw_TypeDef MAG_Value;
 	AxesRaw_TypeDef ACC_Value;
 	AxesRaw_TypeDef GYR_Value;
-	char buffer1[32];
-	char buffer2[32];
-	char buffer3[32];
-	char buffer4[32];
 	unsigned int ret = 0;
 	
-	/* Declaration of sensors variables */
-	double accX,accY,accZ;
-	double gyroX,gyroY,gyroZ;
-
-	
 	/* Switch LED On */
 	myled = LED_ON;
-	printf("===\n");
+	//printf("===\n");
 
 	/* Determine Environmental Values */
 	ret |= (!CALL_METH(magnetometer, get_m_axes, (int32_t *)&MAG_Value, 0) ? 0x0 : 0x10);;
 	ret |= (!CALL_METH(accelerometer, get_x_axes, (int32_t *)&ACC_Value, 0) ? 0x0 : 0x20);;
 	ret |= (!CALL_METH(gyroscope, get_g_axes, (int32_t *)&GYR_Value, 0) ? 0x0 : 0x40);
+	
+	/* IMU Data */
+	double accX, accY, accZ;
+	double gyroX, gyroY, gyroZ;
 
-	/* Print Values Out */
-        printf("I2C [errors]: 0x%.2x    X         Y         Z\n", ret); 
-        printf("MAG [mgauss]: %9ld %9ld %9ld\n", 
-	       MAG_Value.AXIS_X, MAG_Value.AXIS_Y, MAG_Value.AXIS_Z);
-        printf("ACC [mg]:     %9ld %9ld %9ld\n", 
-	       ACC_Value.AXIS_X, ACC_Value.AXIS_Y, ACC_Value.AXIS_Z);
-        printf("GYR [mdps]:   %9ld %9ld %9ld\n", 
-	       GYR_Value.AXIS_X, GYR_Value.AXIS_Y, GYR_Value.AXIS_Z);
-	       
 	accX = ACC_Value.AXIS_X;
   	accY = ACC_Value.AXIS_Y;
   	accZ = ACC_Value.AXIS_Z;
+  	/*
+  	**
   	gyroX = GYR_Value.AXIS_X;
   	gyroY = GYR_Value.AXIS_Y;
   	gyroZ = GYR_Value.AXIS_Z;
+  	**
+  	*/
   	
   	#ifdef RESTRICT_PITCH // Eq. 25 and 26
   	double roll  = atan2(accY, accZ) * RAD_TO_DEG;
@@ -243,20 +211,134 @@
 	double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
   	double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
 	#endif
-	double yaw = atan2(-accZ, sqrt(accY * accY + accZ * accZ)) * 180.0/PI;
+	//double yaw = atan2(-accZ, sqrt(accY * accY + accZ * accZ)) * 180.0/PI;
+	kalmanX.setAngle(roll); // Set starting angle
+	kalmanY.setAngle(pitch);
+	gyroXangle = roll;
+	gyroYangle = pitch;
+	compAngleX = roll;
+	compAngleY = pitch;
+	
+	timer = t.read_us();
+}
+
+/* Main cycle function */
+static void main_cycle(void) {
+	AxesRaw_TypeDef MAG_Value;
+	AxesRaw_TypeDef ACC_Value;
+	AxesRaw_TypeDef GYR_Value;
+	unsigned int ret = 0;
 	
-	/* Print Serially *//*
-	ser.printf("%lf",pitch);
-	ser.printf(":");
-	ser.printf("%lf",roll);
-	ser.printf(":");
-	ser.printf("%lf\n",yaw);
-	*/
-	ser.printf("1");
-	ser.printf(":");
-	ser.printf("2");
-	ser.printf(":");
-	ser.printf("3\n");
+	/* Switch LED On */
+	myled = LED_ON;
+	//printf("===\n");
+
+	/* Determine Environmental Values */
+	ret |= (!CALL_METH(magnetometer, get_m_axes, (int32_t *)&MAG_Value, 0) ? 0x0 : 0x10);;
+	ret |= (!CALL_METH(accelerometer, get_x_axes, (int32_t *)&ACC_Value, 0) ? 0x0 : 0x20);;
+	ret |= (!CALL_METH(gyroscope, get_g_axes, (int32_t *)&GYR_Value, 0) ? 0x0 : 0x40);
+
+	/* Print Values Out */
+        //printf("I2C [errors]: 0x%.2x    X         Y         Z\n", ret); 
+        /*
+        **
+        printf("%9ld:%9ld:%9ld:", ACC_Value.AXIS_X, ACC_Value.AXIS_Y, ACC_Value.AXIS_Z);
+	    printf("%9ld:%9ld:%9ld:", GYR_Value.AXIS_X, GYR_Value.AXIS_Y, GYR_Value.AXIS_Z);
+        printf("%9ld:%9ld:%9ld\n", MAG_Value.AXIS_X, MAG_Value.AXIS_Y, MAG_Value.AXIS_Z);
+        **
+        */
+        
+	/* IMU Data */
+	double accX, accY, accZ;
+	double gyroX, gyroY, gyroZ;
+	double magX, magY, magZ;
+    
+	accX = ACC_Value.AXIS_X;
+  	accY = ACC_Value.AXIS_Y;
+  	accZ = ACC_Value.AXIS_Z;
+  	gyroX = GYR_Value.AXIS_X;
+  	gyroY = GYR_Value.AXIS_Y;
+  	gyroZ = GYR_Value.AXIS_Z;
+  	
+  	double dt = (double)(t.read_us() - timer) / 1000000; // Calculate delta time
+  	timer = t.read_us();
+  	
+  	#ifdef RESTRICT_PITCH // Eq. 25 and 26
+  	double roll  = atan2(accY, accZ) * RAD_TO_DEG;
+  	double pitch = atan(-accX / sqrt(accY * accY + accZ * accZ)) * RAD_TO_DEG;
+  	#else // Eq. 28 and 29
+  	double roll  = atan(accY / sqrt(accX * accX + accZ * accZ)) * RAD_TO_DEG;
+  	double pitch = atan2(-accX, accZ) * RAD_TO_DEG;
+  	#endif
+  	
+  	double gyroXrate = gyroX / 131.0; // Convert to deg/s
+  	double gyroYrate = gyroY / 131.0; // Convert to deg/s
+  	
+  	#ifdef RESTRICT_PITCH
+  	// This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
+  	if ((roll < -90 && kalAngleX > 90) || (roll > 90 && kalAngleX < -90)) {
+  		kalmanX.setAngle(roll);
+  		compAngleX = roll;
+  		kalAngleX = roll;
+  		gyroXangle = roll;
+  	}
+  	else
+    kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
+    
+    if (abs(kalAngleX) > 90)
+    gyroYrate = -gyroYrate; // Invert rate, so it fits the restriced accelerometer reading
+    kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt);
+    #else
+    // This fixes the transition problem when the accelerometer angle jumps between -180 and 180 degrees
+    if ((pitch < -90 && kalAngleY > 90) || (pitch > 90 && kalAngleY < -90)) {
+    	kalmanY.setAngle(pitch);
+    	compAngleY = pitch;
+    	kalAngleY = pitch;
+    	gyroYangle = pitch;
+    }
+    else
+    kalAngleY = kalmanY.getAngle(pitch, gyroYrate, dt); // Calculate the angle using a Kalman filter
+    
+    if (abs(kalAngleY) > 90)
+    gyroXrate = -gyroXrate; // Invert rate, so it fits the restriced accelerometer reading
+    kalAngleX = kalmanX.getAngle(roll, gyroXrate, dt); // Calculate the angle using a Kalman filter
+    #endif
+    
+    gyroXangle += gyroXrate * dt; // Calculate gyro angle without any filter
+    gyroYangle += gyroYrate * dt;
+    //gyroXangle += kalmanX.getRate() * dt; // Calculate gyro angle using the unbiased rate
+    //gyroYangle += kalmanY.getRate() * dt;
+    
+    compAngleX = 0.93 * (compAngleX + gyroXrate * dt) + 0.07 * roll; // Calculate the angle using a Complimentary filter
+    compAngleY = 0.93 * (compAngleY + gyroYrate * dt) + 0.07 * pitch;
+    
+    // Reset the gyro angle when it has drifted too much
+    if (gyroXangle < -180 || gyroXangle > 180)
+    gyroXangle = kalAngleX;
+    if (gyroYangle < -180 || gyroYangle > 180)
+    gyroYangle = kalAngleY;
+    
+    // Compute the Heading
+    
+    magX = MAG_Value.AXIS_X;
+  	magY = MAG_Value.AXIS_Y;
+  	magZ = MAG_Value.AXIS_Z;
+	
+	float heading;
+	if (magY== 0)
+    heading = (magX < 0) ? 180.0 : 0;
+    else
+    heading = atan2(magX , magY);
+    //arctan(imu.mx / sqrt(imu.mz*imu.mz + imu.my*imu.my)):
+    heading -= DECLINATION * PI / 180;
+    
+    if (heading > PI)
+    heading -= (2 * PI);
+    else if (heading < -PI || heading < 0)
+    heading += (2 * PI);
+    heading *= 180.0 / PI;
+  
+	printf("%lf:%1f:%1f\n", kalAngleY, kalAngleX, heading);
 	
 	
 	/* Switch LED Off */
@@ -271,7 +353,7 @@
 int main()
 {
 	/* Start & initialize */
-	printf("\n--- Starting new run ---\n");
+	//printf("\n--- Starting new run ---\n");
 	init();
 
 	/* Start timer irq */
@@ -289,4 +371,5 @@
 				    System Control Register is NOT set */
 		}
 	}
+	t.stop();
 }