Used for testing battery sense circuit, looking for max and min levels. Allow for finding true empty, half and full values for driving LEDs for example
Dependencies: mbed MPL3115A2 TSI WiGo_BattCharger
Diff: main.cpp
- Revision:
- 3:3b88d6ae24f2
- Parent:
- 2:c08efa9effc8
- Child:
- 5:4438d5665b4f
diff -r c08efa9effc8 -r 3b88d6ae24f2 main.cpp --- 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]); + } } }