Diff: main.cpp

Revision:

26:e35dd673fd13

Parent:

25:6e43bbe76aec

--- a/main.cpp	Sat Sep 24 06:10:54 2016 +0000
+++ b/main.cpp	Thu Feb 02 03:08:13 2017 +0000
@@ -1,9 +1,9 @@
 #include "mbed.h"
-#include "FXOS8700Q.h"
+#include "FXOS8700CQ.h"
 #include "FXAS21000.h"
 #include "MBed_Adafruit_GPS.h"
 #include "Hexi_OLED_SSD1351.h"
-#include "sensor_fusion.h"
+#include "MadgwickAHRS.h"
 #include "images.h"
 
 #include "rtos.h"
@@ -35,20 +35,11 @@
 #define RED 0xFF0000
 #define BLUE 0x0000FF
 
-FXOS8700Q_acc acc( PTC11, PTC10, FXOS8700CQ_SLAVE_ADDR0); // Proper Ports and I2C Address for Hexiwear
-FXOS8700Q_mag mag( PTC11, PTC10, FXOS8700CQ_SLAVE_ADDR0); // Proper Ports and I2C Address for Hexiwear
-MotionSensorDataUnits mag_data;
-MotionSensorDataUnits acc_data;
-
-MotionSensorDataCounts mag_raw;
-MotionSensorDataCounts acc_raw;
+FXOS8700CQ combo( PTC11, PTC10); // Proper Ports and I2C Address for Hexiwear
 
 FXAS21000 gyro( PTC11, PTC10); // Proper Ports for Hexiwear
 
-float faX, faY, faZ;
-float fmX, fmY, fmZ;
-int16_t raX, raY, raZ;
-int16_t rmX, rmY, rmZ;
+Data combo_data;
 float gyro_data[3];
 
 Timer t;
@@ -78,45 +69,8 @@
 void imu_thread(void)
 {
     while(true) {
-        acc.getAxis(acc_data);
-        mag.getAxis(mag_data);
         gyro.ReadXYZ(gyro_data);
-        
-        LOG("FXOS8700Q ACC: X=%1.4f Y=%1.4f Z=%1.4f  ", acc_data.x, acc_data.y, acc_data.z);
-        LOG("    MAG: X=%4.1f Y=%4.1f Z=%4.1f\r\n", mag_data.x, mag_data.y, mag_data.z);
-        LOG("FXAS21000      X=%4.2f Y=%4.2f Z=%4.1f\r\n", gyro_data[0], gyro_data[1], gyro_data[2]);
-        
-        acc.getX(&faX);
-        acc.getY(&faY);
-        acc.getZ(&faZ);
-        mag.getX(&fmX);
-        mag.getY(&fmY);
-        mag.getZ(&fmZ);
-        
-        LOG("FXOS8700Q ACC: X=%1.4f Y=%1.4f Z=%1.4f  ", faX, faY, faZ);
-        LOG("    MAG: X=%4.1f Y=%4.1f Z=%4.1f\r\n", fmX, fmY, fmZ);
-        
-        acc.getAxis(acc_raw);
-        mag.getAxis(mag_raw);
-        
-        LOG("FXOS8700Q ACC: X=%d Y=%d Z=%d  ", acc_raw.x, acc_raw.y, acc_raw.z);
-        LOG("    MAG: X=%d Y=%d Z=%d\r\n", mag_raw.x, mag_raw.y, mag_raw.z);
-        
-        acc.getX(&raX);
-        acc.getY(&raY);
-        acc.getZ(&raZ);
-        mag.getX(&rmX);
-        mag.getY(&rmY);
-        mag.getZ(&rmZ);
-        
-        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);
-        
-        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);
-        
+        combo_data = combo.get_values();
         Thread::wait(10);
     }
 }
@@ -148,9 +102,10 @@
 //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());
+//Initialise gyro
+    char whoami = gyro.getWhoAmI();
+//Initialise combo (acc&mag)
+    combo.init();
 
     /* Get OLED Class Default Text Properties */
     oled_text_properties_t textProperties = {0};
@@ -211,16 +166,6 @@
     imu_t.start(imu_thread);
 
     while (true) {     
-        /* Format the MAG reading */
-        sprintf(text,"%.1f %.1f %.1f",fmX,fmY,fmZ);
-        /* Display Mag readings to 1 decimal */
-        oled.TextBox((uint8_t *)text,15,15,80,15); /*Expand textbox for more digits*/
-
-        /* Format the ACC reading */
-        sprintf(text,"%.2f %.2f %.2f",faX,faY,faZ);
-        /* Display acceleration to 2 decimal */
-        oled.TextBox((uint8_t *)text,15,30,80,15); /*Expand textbox for more digits*/
-
         /* Format the GYRO reading */
         sprintf(text,"%.2f %.2f %.2f",gyro_data[0],gyro_data[1],gyro_data[2]);
         /* Display gyro to 2 decimal */