Julio Fajardo / Mbed 2 deprecated Freescale_Multi-Sensor_Quadcopter

Freescale Quadcopter with Freedom K64F Board

Dependencies:   FXAS21000 FXLS8471Q FXOS8700Q MAG3110 MMA8652 MPL3115A2 mbed kalman mbed-dsp

Fork of Freescale_Multi-Sensor_Shield by Shields

Quadcopter based on Freescale FRDM-K64F, Freescale FRDM-FXS-9AXIS, Hobbypower X525 V3 Quadcopter Foldable Kit with 1000KV Motors and SimonK 30A ESC /media/uploads/julioefajardo/1.jpg

Files at this revision

API Documentation at this revision

Revision 3:58ebc00f1a68, committed 2015-09-01

Comitter:
julioefajardo
Date:
Tue Sep 01 18:03:27 2015 +0000
Parent:
2:4bc4e25328cc
Child:
4:1bc3ca07a412
Commit message:
V0.1; Kalman Filter

Changed in this revision

kalman.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file 
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/kalman.lib	Tue Sep 01 18:03:27 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/cdonate/code/kalman/#8a460b0d6f09

--- a/main.cpp	Mon Aug 31 23:37:07 2015 +0000
+++ b/main.cpp	Tue Sep 01 18:03:27 2015 +0000
@@ -19,15 +19,18 @@
 #include "mbed.h"
 #include "FXOS8700Q.h"
 #include "FXAS21000.h"
-
+#include "kalman.c"
 
-//FXOS8700Q_acc combo_acc(D14, D15, FXOS8700CQ_SLAVE_ADDR0);
-//FXOS8700Q_mag combo_mag(D14, D15, FXOS8700CQ_SLAVE_ADDR0);
+#define PI             3.1415926535897932384626433832795
+#define Rad2Dree       57.295779513082320876798154814105
+
 FXOS8700Q_acc combo_acc(A5, A4, FXOS8700CQ_SLAVE_ADDR0);
 FXOS8700Q_mag combo_mag(A5, A4, FXOS8700CQ_SLAVE_ADDR0);
-//FXAS21000 gyro(D14, D15);
 FXAS21000 gyro(A5, A4);
 
+Timer GlobalTime;
+Timer ProgramTimer;
+
 //PwmOut M1(PTB18);
 PwmOut M1(D13);
 PwmOut M2(D12);
@@ -36,6 +39,14 @@
 
 Serial pc(USBTX, USBRX);
 
+kalman filter_pitch; 
+kalman filter_roll;
+
+float R;
+double angle[3];
+unsigned long timer;
+long loopStartTime;
+
 int main()
 {
     float gyro_data[3];
@@ -44,6 +55,8 @@
     
     printf("\r\nStarting\r\n\r\n");
     
+    GlobalTime.start();
+    
     M1.period(0.02f);               //Comparten el mismo timer
     M1.pulsewidth(0.0006f);
     M2.pulsewidth(0.0006f);
@@ -55,18 +68,36 @@
     printf("FXOS8700 Combo = %X\r\n", combo_acc.whoAmI());
     printf("FXAS21000 Gyro = %X\r\n", gyro.getWhoAmI());
     
+    kalman_init(&filter_pitch, R_matrix, Q_Gyro_matrix, Q_Accel_matrix); 
+    kalman_init(&filter_roll, R_matrix, Q_Gyro_matrix, Q_Accel_matrix);
+    
     wait(3);
     
+    ProgramTimer.start();
+    loopStartTime = ProgramTimer.read_us();
+    timer = loopStartTime;
+    
     while(1) {
         combo_acc.getAxis(adata);
-        printf("FXOS8700 Acc:   X:%6.3f Y:%6.3f Z:%6.3f\r\n", adata.x, adata.y, adata.z);
+        combo_mag.getAxis(mdata);
+        gyro.ReadXYZ(gyro_data);
+        
+        R = sqrt(std::pow(adata.x, 2) + std::pow(adata.y, 2) + std::pow(adata.z, 2));
         
-        combo_mag.getAxis(mdata);
+        kalman_predict(&filter_pitch, gyro_data[0],  (ProgramTimer.read_us() - timer)); 
+        kalman_update(&filter_pitch, acos(adata.x/R));
+        kalman_predict(&filter_roll, gyro_data[1],  (ProgramTimer.read_us() - timer)); 
+        kalman_update(&filter_roll, acos(adata.y/R));
+        
+        angle[0] = kalman_get_angle(&filter_pitch);
+        angle[1] = kalman_get_angle(&filter_roll);
+        
+        timer = ProgramTimer.read_us();
+    
+        printf("FXOS8700 Acc:   X:%6.3f Y:%6.3f Z:%6.3f\r\n", adata.x, adata.y, adata.z);
         printf("FXOS8700 Mag:   X:%6.2f Y:%6.2f Z:%6.2f\r\n", mdata.x, mdata.y, mdata.z);
-
-        gyro.ReadXYZ(gyro_data);
         printf("FXAS21000 Gyro: X:%6.2f Y:%6.2f Z:%6.2f\r\n", gyro_data[0], gyro_data[1], gyro_data[2]);
-    
+        printf("Roll Angle X: %.6f   Pitch Angle Y: %.6f \n\r", Rad2Dree * angle[1], Rad2Dree * angle[0]);
         printf("\r\n");
         
         wait(0.5);