Diff: main.cpp

Revision:

0:7fd305c81a8e

Child:

2:f3043132a959

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sat Apr 09 04:27:35 2016 +0000
@@ -0,0 +1,136 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "arm_math.h" 
+#include "kalman.c"
+#include "LSM303DLHC.h"
+#include "L3GD20H.h"
+
+#define Rad2Dree       57.295779513082320876798154814105f
+
+DigitalOut led_red(LED_RED);
+DigitalOut led_green(LED_GREEN);
+DigitalIn sw2(SW2);
+DigitalIn sw3(SW3);
+Serial pc(USBTX, USBRX);
+
+LSM303DLHC imu(D14,D15);
+L3GD20H gyro(D14,D15);
+
+LSM303DLHC imu2(PTC11,PTC10);
+L3GD20H gyro2(PTC11,PTC10);
+
+Timer GlobalTime;
+Timer ProgramTimer;
+
+kalman filter_pitch; 
+kalman filter_roll;
+
+kalman filter_pitch2; 
+kalman filter_roll2;
+
+int acc[3];
+int mag[3];
+short gyr[3];
+
+int acc2[3];
+int mag2[3];
+short gyr2[3];
+
+struct vector {
+  float x;
+  float y;
+  float z;
+} Acc, Gyr, Acc2, Gyr2;
+
+float R;
+double angle[3];
+
+float R2;
+double angle2[3];
+
+unsigned long timer;
+long loopStartTime;
+
+void AccRaw2G(int acc[3]);
+void GyrRaw2D(short gyr[3]);
+void AccRaw2G2(int acc[3]);
+void GyrRaw2D2(short gyr[3]);
+
+int main() {
+    GlobalTime.start();
+    imu.init();
+    imu2.init();
+    led_green = 1;
+    led_red = 1;
+    pc.baud(115200);
+    pc.printf("Hello World from FRDM-K64F board.\r\n");
+    kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
+    kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
+    kalman_init(&filter_pitch2, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
+    kalman_init(&filter_roll2, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
+    
+    ProgramTimer.start();
+    loopStartTime = ProgramTimer.read_us();
+    timer = loopStartTime;
+
+    while (true) {
+        imu.readAcc(acc);
+        AccRaw2G(acc);
+        gyro.read(gyr);
+        GyrRaw2D(gyr);
+        
+        imu2.readAcc(acc2);
+        AccRaw2G2(acc2);
+        gyro2.read(gyr2);
+        GyrRaw2D2(gyr2);
+        
+        R = sqrt(std::pow(Acc.x, 2) + std::pow(Acc.y, 2) + std::pow(Acc.z, 2));
+        R2 = sqrt(std::pow(Acc2.x, 2) + std::pow(Acc2.y, 2) + std::pow(Acc2.z, 2));
+        
+        kalman_predict(&filter_pitch, Gyr.x,  (ProgramTimer.read_us() - timer)); 
+        kalman_update(&filter_pitch, acos(Acc.x/R));
+        kalman_predict(&filter_roll, Gyr.y,  (ProgramTimer.read_us() - timer)); 
+        kalman_update(&filter_roll, acos(Acc.y/R));
+        
+        kalman_predict(&filter_pitch2, Gyr2.x,  (ProgramTimer.read_us() - timer)); 
+        kalman_update(&filter_pitch2, acos(Acc2.x/R2));
+        kalman_predict(&filter_roll2, Gyr.y,  (ProgramTimer.read_us() - timer)); 
+        kalman_update(&filter_roll2, acos(Acc2.y/R2));
+        
+        angle[0] = kalman_get_angle(&filter_pitch);
+        angle[1] = kalman_get_angle(&filter_roll);
+        
+        angle2[0] = kalman_get_angle(&filter_pitch2);
+        angle2[1] = kalman_get_angle(&filter_roll2);
+        
+        timer = ProgramTimer.read_us();
+         
+        printf("IMU1\tangle0=%6.2f\tangle1=%6.2f\r\n",Rad2Dree*angle[0],Rad2Dree*angle[1]);
+        printf("IMI2\tangle0=%6.2f\tangle1=%6.2f\r\n",Rad2Dree*angle2[0],Rad2Dree*angle2[1]);
+        wait(0.2);
+    }
+}
+
+void AccRaw2G(int acc[3]){
+    Acc.x = acc[0]/1024.0f; 
+    Acc.y = acc[1]/1024.0f; 
+    Acc.z = acc[2]/1024.0f;
+}
+
+void GyrRaw2D(short gyr[3]){
+    Gyr.x = gyr[0]/225.0f; 
+    Gyr.y = gyr[1]/225.0f; 
+    Gyr.z = gyr[2]/225.0f;
+}
+
+void AccRaw2G2(int acc[3]){
+    Acc2.x = acc[0]/1024.0f; 
+    Acc2.y = acc[1]/1024.0f; 
+    Acc2.z = acc[2]/1024.0f;
+}
+
+void GyrRaw2D2(short gyr[3]){
+    Gyr2.x = gyr[0]/225.0f; 
+    Gyr2.y = gyr[1]/225.0f; 
+    Gyr2.z = gyr[2]/225.0f;
+}
\ No newline at end of file