Diff: main.cpp

Revision:

0:4e1d43dc608f

Child:

2:e4ae1d748311

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 07 21:04:11 2014 +0000
@@ -0,0 +1,123 @@
+#include "mbed.h"
+#include "eCompass_Lib_V3.h"
+#include "MAG3110.h"
+#include "MMA8451Q.h"
+#include "rtos.h"
+#include "SLCD.h"
+
+#define MMA8451_I2C_ADDRESS (0x1d<<1)
+
+
+MAG3110  mag( PTE25, PTE24);
+MMA8451Q acc( PTE25, PTE24, MMA8451_I2C_ADDRESS);
+I2C      i2c( PTE25, PTE24);
+DigitalOut led(LED_RED);
+Serial pc(USBTX, USBRX);
+SLCD slcd;
+
+axis6_t axis6;
+unsigned int seconds;
+unsigned int compass_type;
+int tcount;
+
+// This routing move and negates data as needed the
+// properly align the sensor axis for our desired compass (NED)
+//
+void hal_map(int16_t * acc, int16_t * mag)
+{
+    int16_t t;
+    // swap accelerometer x & y
+    t = acc[1];
+    acc[1] = acc[0];
+    acc[0] = t;
+    // negate accelerometer x & y
+    acc[0] *= -1;
+    acc[1] *= -1;
+    // negate magnetometer y
+    mag[1] *= -1;
+    
+}
+    
+
+void manage_axis6(int16_t * acc_raw, int16_t * mag_raw)
+{
+    axis6.timestamp = tcount;
+    hal_map(acc_raw, mag_raw);
+    //
+    // raw data
+    axis6.acc_x = acc_raw[0];
+    axis6.acc_y = acc_raw[1];
+    axis6.acc_z = acc_raw[2];
+    axis6.mag_x = mag_raw[0];
+    axis6.mag_y = mag_raw[1];
+    axis6.mag_z = mag_raw[2];
+    //
+    // raw data converted to floating ouing
+    axis6.fax = (float) acc_raw[0];
+    axis6.fay = (float) acc_raw[1];
+    axis6.faz = (float) acc_raw[2];
+    axis6.fmx = (float) mag_raw[0];
+    axis6.fmy = (float) mag_raw[1];
+    axis6.fmz = (float) mag_raw[2];
+    //
+    // Accelerometer data converted to Gs
+    axis6.fGax = ((float) acc_raw[0]) / 4096.0;
+    axis6.fGay = ((float) acc_raw[1]) / 4096.0;
+    axis6.fGaz = ((float) acc_raw[2]) / 4096.0;
+    //
+    // Magnetometer data converted to microteslas
+    axis6.fUTmx = ((float) mag_raw[0]) / 10.0;
+    axis6.fUTmy = ((float) mag_raw[1]) / 10.0;
+    axis6.fUTmz = ((float) mag_raw[2]) / 10.0; 
+    }
+
+//
+// This is the 50Hz thread where the magic happens
+//
+void compass_thread(void const *argument) {
+    static int16_t acc_raw[3], mag_raw[3];
+    static int l = 0;
+    while (true) {
+        // Signal flags that are reported as event are automatically cleared.
+        Thread::signal_wait(0x1);
+        // get raw data from the sensors
+        mag.ReadXYZraw(mag_raw);
+        acc.getAccXYZraw(acc_raw);
+        
+        manage_axis6( acc_raw, mag_raw); // copy needed data into the axis6 structure for use by the eCompass
+        if(tcount) run_eCompass(); // calculate the eCompass
+        slcd.printf("%04d", axis6.yaw); // print the heading (NED compass) to the LCD
+        if(l++ >= 50) { // take car of business once a second
+            seconds++;
+            calibrate_eCompass(); // re-calibrate the eCompass every second
+            l = 0;
+            /* Some usful printf statements for debug
+            printf("roll=%d, pitch=%d, yaw=%d\r\n", axis6.roll, axis6.pitch, axis6.yaw);
+            printf("Acc: X= %2.3f Y= %2.3f Z= %2.3f    ", axis6.fGax, axis6.fGay, axis6.fGaz);
+            printf("Mag: X= %4.1f Y= %4.1f Z= %4.1f\r\n\r\n", axis6.fUTmx, axis6.fUTmy, axis6.fUTmz);
+            */
+            led = !led;
+            }
+        tcount++;
+    }
+}
+                                   
+int main() {
+    Thread thread(compass_thread);
+    float mag_data[3];
+    
+    slcd.clear();
+    tcount = 0;
+
+    init_eCompass();
+    compass_type = NED_COMPASS;
+    seconds = 0;
+    // make sure our sensors are talking to us
+    printf("\r\n%X\r\n", acc.getWhoAmI());
+    printf("%X\r\n", mag.readReg(MAG_WHO_AM_I));
+    mag.ReadXYZ(mag_data); // flush the magnetmeter
+    while (1) {
+        Thread::wait(20); // run out thread every 20ms for a 50Hz rate
+        thread.signal_set(0x1);    
+        }
+}