Julio Fajardo
/
Galileo_HoverBoardRider
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: L3GD20H LSM303DLHC kalman mbed-dsp mbed-rtos mbed
Revision 0:7fd305c81a8e, committed 2016-04-09
- Comitter:
- julioefajardo
- Date:
- Sat Apr 09 04:27:35 2016 +0000
- Child:
- 1:7eb3df293055
- Commit message:
- Roll & Pith (Kalman Filter)
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/L3GD20H.lib Sat Apr 09 04:27:35 2016 +0000 @@ -0,0 +1,1 @@ +L3GD20H#81b60e58e9e1
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/LSM303DLHC.lib Sat Apr 09 04:27:35 2016 +0000 @@ -0,0 +1,1 @@ +LSM303DLHC#ffed7ef0b248
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/kalman.lib Sat Apr 09 04:27:35 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/cdonate/code/kalman/#8a460b0d6f09
--- /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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-dsp.lib Sat Apr 09 04:27:35 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/mbed_official/code/mbed-dsp/#3762170b6d4d
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Sat Apr 09 04:27:35 2016 +0000 @@ -0,0 +1,1 @@ +https://developer.mbed.org/users/mbed_official/code/mbed-rtos/#bdd541595fc5
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sat Apr 09 04:27:35 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/99a22ba036c9 \ No newline at end of file