Diff: main.cpp

Revision:

3:3b88d6ae24f2

Parent:

2:c08efa9effc8

Child:

5:4438d5665b4f

--- a/main.cpp	Fri May 17 20:13:32 2013 +0000
+++ b/main.cpp	Thu May 23 11:42:10 2013 +0000
@@ -1,54 +1,284 @@
 #include "mbed.h"
 #include "WiGo_BattCharger.h"
 #include "WiGo_AmbLight.h"
+#include "TSISensor.h"
+#include "math.h"
+#include "MPL3115A2.h"
+ 
+#define MPL3115A2_I2C_ADDRESS (0x60<<1)
+ MPL3115A2 wigo_sensor1(PTE0, PTE1, MPL3115A2_I2C_ADDRESS);
+
+#define MAG_ADDR 0x1D
+
+// define registers
+#define MAG_DR_STATUS 0x00
+#define MAG_OUT_X_MSB 0x01
+#define MAG_OUT_X_LSB 0x02
+#define MAG_OUT_Y_MSB 0x03
+#define MAG_OUT_Y_LSB 0x04
+#define MAG_OUT_Z_MSB 0x05
+#define MAG_OUT_Z_LSB 0x06
+#define MAG_WHO_AM_I  0x07
+#define MAG_SYSMOD    0x08
+#define MAG_OFF_X_MSB 0x09
+#define MAG_OFF_X_LSB 0x0A
+#define MAG_OFF_Y_MSB 0x0B
+#define MAG_OFF_Y_LSB 0x0C
+#define MAG_OFF_Z_MSB 0x0D
+#define MAG_OFF_Z_LSB 0x0E
+#define MAG_DIE_TEMP  0x0F
+#define MAG_CTRL_REG1 0x10
+#define MAG_CTRL_REG2 0x11
+
+// what should WHO_AM_I return?
+#define MAG_3110_WHO_AM_I_VALUE 0xC4
+
+
+// Fields in registers
+// CTRL_REG1: dr2,dr1,dr0  os1,os0  fr tm ac
+
+// Sampling rate from 80Hz down to 0.625Hz
+#define MAG_3110_SAMPLE80 0
+#define MAG_3110_SAMPLE40 0x20
+#define MAG_3110_SAMPLE20 0x40
+#define MAG_3110_SAMPLE10 0x60
+#define MAG_3110_SAMPLE5 0x80
+#define MAG_3110_SAMPLE2_5 0xA0
+#define MAG_3110_SAMPLE1_25 0xC0
+#define MAG_3110_SAMPLE0_625 0xE0
+
+// How many samples to average (lowers data rate)
+#define MAG_3110_OVERSAMPLE1 0
+#define MAG_3110_OVERSAMPLE2 0x08
+#define MAG_3110_OVERSAMPLE3 0x10
+#define MAG_3110_OVERSAMPLE4 0x18
+
+// read only 1 byte per axis
+#define MAG_3110_FASTREAD 0x04
+// do one measurement (even if in standby mode)
+#define MAG_3110_TRIGGER 0x02
+// put in active mode
+#define MAG_3110_ACTIVE 0x01
+
+// CTRL_REG2: AUTO_MRST_EN  _ RAW MAG_RST _ _ _ _ _
+// reset sensor after each reading
+#define MAG_3110_AUTO_MRST_EN 0x80
+// don't subtract user offsets
+#define MAG_3110_RAW 0x20
+// reset magnetic sensor after too-large field
+#define MAG_3110_MAG_RST 0x10
+
+// DR_STATUS Register ZYXOW ZOW YOW XOW ZYXDR ZDR YDR XDR
+#define MAG_3110_ZYXDR  0x08
+
+
+#define PI 3.14159265359
+#define ON  0
+#define OFF 1
 
 #define RGB_LED_ON  0
 #define RGB_LED_OFF 1
 
-DigitalOut myled1(LED1);
-DigitalOut myled2(LED2);
-DigitalOut myled3(LED3);
+// Some LEDs for showing status
+DigitalOut redLed(LED_RED);
+DigitalOut greenLed(LED_GREEN);
+DigitalOut blueLed(LED_BLUE);
+
+// Slide sensor acts as a button
+TSISensor tsi;
+
+// I2C used to communicate with sensor
+I2C i2c(PTE0, PTE1);
+
+int avgX, avgY, newX, tempXmin, tempXmax, newY, tempYmin, tempYmax;
+// Ideally these would be saved in eeprom/flash
+struct settings_t {
+    long maxX, minX, maxY, minY;
+}
+settings;
+
+const int addr = MAG_ADDR;
 
 Serial pc(USBTX, USBRX);
 
 WiGo_BattCharger Batt( BATT_LOW, BATT_MED, BATT_FULL, CHRG_EN1, CHRG_EN2, CHRG_SNS_EN, CHRG_SNS, CHRG_POK, CHRG_CHG);
 WiGo_AmbLight Light( AMBLIGHT_EN, AMBLIGHT_LVL);
 
+// Read a single byte form 8 bit register, return as int
+int readReg(char regAddr)
+{
+    char cmd[1];
+
+    cmd[0] = regAddr;
+    i2c.write(addr, cmd, 1);
+
+    cmd[0] = 0x00;
+    i2c.read(addr, cmd, 1);
+    return (int)( cmd[0]);
+}
+
+
+// read a register per, pass first reg value, reading 2 bytes increments register
+// Reads MSB first then LSB
+int readVal(char regAddr)
+{
+    char cmd[2];
+
+    cmd[0] = regAddr;
+    i2c.write(addr, cmd, 1);
+
+    cmd[0] = 0x00;
+    cmd[1] = 0x00;
+    i2c.read(addr, cmd, 2);
+    return (int)( (cmd[1]|(cmd[0] << 8))); //concatenate the MSB and LSB
+}
+
+
+void initMag() {
+    char cmd[2];
+
+    cmd[0] = MAG_CTRL_REG2;
+    cmd[1] = 0x80;
+    i2c.write(addr, cmd, 2);
+
+    cmd[0] = MAG_CTRL_REG1;
+    cmd[1] = MAG_3110_SAMPLE80+MAG_3110_OVERSAMPLE2+MAG_3110_ACTIVE; // 0x91;
+    i2c.write(addr, cmd, 2);
+    }
+    
+    
+void calXY() //magnetometer calibration: finding max and min of X, Y axis
+{
+    int tempXmax, tempXmin, tempYmax, tempYmin, newX, newY;
+    redLed = ON;
+
+    printf("Waiting for initial press\n");
+    // Wait for slider to be pressed
+    while( tsi.readDistance() == 0 ) {
+        redLed = ON;
+        wait(0.2);
+        redLed = OFF;
+        wait(0.2);
+    }
+    
+    printf("Waiting for release\n");
+
+    // Wait for release
+    while( tsi.readDistance() != 0 ) {
+        redLed = OFF;
+        wait(0.2);
+        redLed = ON;
+        wait(0.2);
+    }
+    redLed = OFF;
+    wait(0.5);
+    
+    printf("Rotate\n");
+        
+    tempXmax = tempXmin = readVal(MAG_OUT_X_MSB);
+    tempYmax = tempYmin = readVal(MAG_OUT_Y_MSB);
+
+    while(tsi.readDistance() == 0) {
+        greenLed = ON;
+        wait(0.1);
+        greenLed = OFF;
+        wait(0.1);
+        newX = readVal(MAG_OUT_X_MSB);
+        newY = readVal(MAG_OUT_Y_MSB);
+        if (newX > tempXmax) tempXmax = newX;
+        if (newX < tempXmin) tempXmin = newX;
+        if (newY > tempYmax) tempYmax = newY;
+        if (newY < tempYmin) tempYmin = newY;
+    }
+
+    settings.maxX = tempXmax;
+    settings.minX = tempXmin;
+    settings.maxY = tempYmax;
+    settings.minY = tempYmin;
+
+    //store new X, Y values in EEPROM/Flash
+
+    // Calculate average from min/max
+    avgX=(settings.maxX+settings.minX)/2;
+    avgY=(settings.maxY+settings.minY)/2;
+
+    // Wait for release
+    while( tsi.readDistance() != 0 ) {
+        greenLed = OFF;
+        wait(0.2);
+        greenLed = ON;
+        wait(0.2);
+    }
+    greenLed = OFF;
+    wait(1.0);
+
+
+}
+
 float max_batt;
 float batt_lvl;
+float sensor_data[3];
 
 int main()
 {
+    
     Batt.init(CHRG_500MA);
     Batt.sense_en(1);
     Light.en(1);
     wait(0.5);
     max_batt = Batt.read();
+    wigo_sensor1.Oversample_Ratio( OVERSAMPLE_RATIO_32);
 
+    printf("MAG3110 Test\n");
+
+    redLed = OFF;
+    greenLed = OFF;
+    blueLed = OFF;
+    
+    initMag();
+
+    // Get some values
+    printf("DR_STATUS %X\n", readReg( MAG_DR_STATUS ));
+    printf("WHO_AM_I %X\n", readReg( MAG_WHO_AM_I ));
+    printf("SYSMOD %X\n", readReg( MAG_SYSMOD ));
+    printf("DIE_TEMP %d\n", readReg( MAG_DIE_TEMP ));
+
+    printf("OFF_X %d\n", readVal( MAG_OFF_X_MSB ));
+    printf("OFF_Y %d\n", readVal( MAG_OFF_Y_MSB ));
+    printf("OFF_Z %d\n", readVal( MAG_OFF_Z_MSB ));
+
+    printf("CTRL_REG1 %X\n", readReg( MAG_CTRL_REG1 ));
+    printf("CTRL_REG2 %X\n", readReg( MAG_CTRL_REG2 ));
+
+    printf("calibrate\n");
+    calXY();
+    printf("....Finished\n");
+    printf("avgX = %d, avgY = %d\n", avgX, avgY);
+
+    redLed = OFF;
+    greenLed = OFF;
+    blueLed = OFF;
 
     while(1) {
 
-        //If charging then blink Blue led to show we are alive
-        if( Batt.charging() == CHARGING ) {
-            myled2 = RGB_LED_OFF;
-            myled3 = RGB_LED_OFF;
-
-            myled1 = RGB_LED_ON;
-            wait(0.1);
-            myled1 = RGB_LED_OFF;
-            wait(0.9);
-        }
+        wait(2);
+        int xVal = readVal(MAG_OUT_X_MSB);
+        int yVal = readVal(MAG_OUT_Y_MSB);
+        float heading = (atan2((double)(yVal-avgY),(double)(xVal-avgX)))*180/PI;
+        
+        // Do something with heading - display direction and turn on blue LED if heading approx north
+        if (abs(heading) <= 22.5) { printf("N\n"); blueLed = ON; } else blueLed = OFF;
+        if (abs(heading) >= 157.5) printf("S\n");
+        if (heading >= 67.5 && heading <= 112.5) printf("E \n");
+        if (heading <= -67.5 && heading >= -112.5) printf("W \n");
+        if (heading > 22.5 && heading < 67.5) printf("NE\n");
+        if (heading < -22.5 && heading > -67.5) printf("NW\n");
+        if (heading > 112.5 && heading < 157.5) printf("SE\n");
+        if (heading < -112.5 && heading > -157.5) printf("SW\n");
 
-        //If no power applied (via USB) then turn all on, blink Bled led off
-        if( Batt.supply() != POWER_OK ) {
-            myled2 = RGB_LED_ON;
-            myled3 = RGB_LED_ON;
-
-            myled1 = RGB_LED_OFF;
-            wait(0.1);
-            myled1 = RGB_LED_ON;
-            wait(0.9);
-        }
+        if (heading < 0) heading += 360.0;
+        printf("xVal - avgX = %d, yVal - avgY = %d  ", xVal-avgX, yVal-avgY);
+        printf("X = %d, Y = %d, Heading %f\n", xVal, yVal, heading);
 
         Batt.LEDupdate(1);
 
@@ -57,6 +287,11 @@
             max_batt = batt_lvl;
         }
 
-        pc.printf(">%f Max:%f %i %i\n\r", batt_lvl, max_batt, Batt.level(), Light.level());
+        pc.printf(">%f Max:%f %i %i\n", batt_lvl, max_batt, Batt.level(), Light.level());
+        
+        if ( wigo_sensor1.isDataAvailable()) {
+            wigo_sensor1.getAllData( &sensor_data[0]);
+            pc.printf("\Altitude: %f\tTemperature: %f\r\n", sensor_data[0], sensor_data[1]);
+        }
     }
 }