Revision 25:6e43bbe76aec, committed 2016-09-24

Comitter:

whatnick

Date:

Sat Sep 24 06:10:54 2016 +0000

Parent:

24:cbdf0f7d33bd

Child:

26:e35dd673fd13

Commit message:

Add BLE/haptic module as dependency and start threads properly.

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Hexi_KW40Z.lib	Sat Sep 24 06:10:54 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/teams/Hexiwear/code/Hexi_KW40Z/#8c7c1cc024ed

--- a/main.cpp	Tue Sep 20 04:18:34 2016 +0000
+++ b/main.cpp	Sat Sep 24 06:10:54 2016 +0000
@@ -12,6 +12,8 @@
 DigitalOut myled(LED1);
 
 Serial gps(PTD3,PTD2);
+Serial camera(PTC17,PTC16);
+
 Adafruit_GPS myGPS(&gps);
 char c; //when read via Adafruit_GPS::read(), the class returns single character stored here
 
@@ -51,7 +53,12 @@
 
 Timer t;
 
-void gps_thread(void const *args)
+//Worker threads
+Thread gps_t,imu_t;
+
+
+//GPS Reader worker task
+void gps_thread(void)
 {
     while (true) {
         c = myGPS.read();   //queries the GPS
@@ -67,7 +74,8 @@
     }
 }
 
-void imu_thread(void const *args)
+//IMU reader and sensor fusion worker task
+void imu_thread(void)
 {
     while(true) {
         acc.getAxis(acc_data);
@@ -104,7 +112,12 @@
         LOG("FXOS8700Q ACC: X=%d Y=%d Z=%d  ", raX, raY, raZ);
         LOG("    MAG: X=%d Y=%d Z=%d\r\n\n", rmX, rmY, rmZ);
         
-        Thread::wait(100);
+        MadgwickQuaternionUpdate(faX,faY,faZ,gyro_data[0],gyro_data[1],gyro_data[2],fmX,fmY,fmZ);
+        calcEuler();
+        
+        LOG("EULER: Y=%f R=%f P=%f\n", yaw,roll,pitch);
+        
+        Thread::wait(10);
     }
 }
 
@@ -132,6 +145,9 @@
     pc.baud(115200);
 #endif
 
+//Connect to Nikon D800 UART at 4800baud
+    camera.baud(4800);
+
     acc.enable();
     LOG("\r\n\nFXOS8700Q Who Am I= %X\r\n", acc.whoAmI());
     LOG("\r\n\nFXAS21000 Who Am I= %X\r\n", gyro.getWhoAmI());
@@ -191,8 +207,8 @@
     oled.SetTextProperties(&textProperties);
 
     //Start read and update threads
-    Thread gps_t(gps_thread);
-    Thread imu_t(imu_thread);
+    gps_t.start(gps_thread);
+    imu_t.start(imu_thread);
 
     while (true) {     
         /* Format the MAG reading */

--- a/sensor_fusion.h	Tue Sep 20 04:18:34 2016 +0000
+++ b/sensor_fusion.h	Sat Sep 24 06:10:54 2016 +0000
@@ -16,6 +16,7 @@
 float q[4] = {1.0f, 0.0f, 0.0f, 0.0f};           // vector to hold quaternion
 float eInt[3] = {0.0f, 0.0f, 0.0f};              // vector to hold integral error for Mahony method
 float pitch, yaw, roll;
+float mag_dec = 7.97;                            // Adelaide Magnetic declination http://www.magnetic-declination.com/Australia/Adelaide/116230.html
 float deltat = 0.0f;                             // integration interval for both filter schemes
 int lastUpdate = 0, firstUpdate = 0, Now = 0;    // used to calculate integration interval
 
@@ -208,4 +209,15 @@
 
 }
 
+void calcEuler()
+{
+    yaw   = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
+    pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
+    roll  = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
+    pitch *= 180.0f / PI;
+    yaw   *= 180.0f / PI;
+    yaw   -= mag_dec; // Declination from BLE/SD storage at GPS location
+    roll  *= 180.0f / PI;
+}
+
 #endif
\ No newline at end of file