Test IMU filter.

1 files changed, 39 insertions, 2 deletions

diff --git a/main.cpp b/main.cpp
index 911e760..14f8a8a 100644
--- a/main.cpp
+++ b/main.cpp
@@ -7,6 +7,8 @@
 #include <ch.h>

 #include <hal.h>

 

+#include "IMU.h"

+

 class LEDThread : public BaseThread<LEDThread, 128> {

 	public:

 		noreturn_t thread_main() {

@@ -61,6 +63,11 @@ USBThread usb_thread;
 USBSerial usbs;

 

 #include "foo.h"

+#include <cmath>

+

+uint8_t syncword[] = {0xff, 0x00, 0xaa, 0x55};

+uint8_t buf[64];

+int16_t* sensordata = (int16_t*)buf;

 

 class I2CThread : public BaseThread<I2CThread, 256> {

 	public:

@@ -77,7 +84,9 @@ class I2CThread : public BaseThread<I2CThread, 256> {
 			acc.init();

 			magn.init();

 			

+			systime_t nexttime = chTimeNow();

 			while (1) {

+				nexttime += MS2ST(100);

 				gyro.update();

 				acc.update();

 				magn.update();

@@ -85,11 +94,39 @@ class I2CThread : public BaseThread<I2CThread, 256> {
 				y = gyro.y;

 				z = gyro.z;

 				

+				IMUupdate(gyro.x * 0.0012141420883438813, gyro.y * 0.0012141420883438813, gyro.z * 0.0012141420883438813, acc.x, acc.y, acc.z);

+				

+				//float pitch = asinf(2*(q0*q2 - q3*q1));

+				int16_t pitch = atan2f(2*(q2*q3 + q0*q1), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767;

+				int16_t roll = atan2f(2*(-q1*q3 + q0*q2), 1 - 2 * (q1*q1 + q2*q2)) / M_PI * 32767;

+				int16_t yaw = atan2f(2*(q2*q1 + q0*q3), 1 - 2 * (q3*q3 + q2*q2)) / M_PI * 32767;

+				

+				sensordata[0] = gyro.x;

+				sensordata[1] = gyro.y;

+				sensordata[2] = gyro.z;

+				sensordata[3] = acc.x;

+				sensordata[4] = acc.y;

+				sensordata[5] = acc.z;

+				sensordata[6] = magn.x;

+				sensordata[7] = magn.y;

+				sensordata[8] = magn.z;

+				sensordata[9] = pitch;

+				sensordata[10] = roll;

+				sensordata[11] = yaw;

+				

+				

 				

+				usbs.write(syncword, sizeof(syncword));

+				usbs.write(buf, sizeof(buf));

 				

-				usbprintf(usbs, "%6d, %6d, %6d\r\n", x, y, z);

+				/*usbprintf(usbs, "%6d, %6d, %6d | %6d, %6d, %6d | %6d, %6d, %6d | %6d, %6d, %6d, %6d | %6d, %6d, %6d\r\n",

+					gyro.x, gyro.y, gyro.z,

+					acc.x, acc.y, acc.z,

+					magn.x, magn.y, magn.z,

+					int(q0 * 10000), int(q1 * 10000), int(q2 * 10000), int(q3 * 10000),

+					int(pitch * 10000), int(roll * 10000), int(yaw * 10000));*/

 				

-				chThdSleepMilliseconds(100);

+				//chThdSleepUntil(nexttime);

 			}

 		}

 };