File content as of revision 1:d45fc466a46e:

#include "mbed.h"
#include "TSISensor.h"
#include "math.h"

//PwmOut r (LED_RED);
//PwmOut g (LED_GREEN);
//PwmOut b (LED_BLUE);



#define MAG_ADDR 0x1D
// (0x0E << 1)
//0x1D

// define register values
#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


//0x0E //7-bit address for the MAG3110, doesn't change

DigitalOut myled(PTB10);
DigitalOut redLed(LED_RED);
DigitalOut greenLed(LED_GREEN);
DigitalOut blueLed(LED_BLUE);

TSISensor tsi;
   
I2C i2c(PTE0, PTE1);

int avgX, avgY, newX, tempXmin, tempXmax, newY, tempYmin, tempYmax;
struct settings_t {
    long maxX, minX, maxY, minY;
}
settings;

const int addr = MAG_ADDR;

// 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
}

#define PI 3.14159265359
#define ON  0
#define OFF 1

void calXY() //magnetometer calibration: finding max and min of X, Y axis
{
    int tempXmax, tempXmin, tempYmax, tempYmin, newX, newY;
    redLed = ON;
//    lcd.setCursor(0,1);
//    lcd.print(“Rotate the car “);
//    delay(1000);
//    lcd.setCursor(0,1);
//    lcd.print(“and press button”);
//    delay(1000);
//    lcd.setCursor(0,1);
//    lcd.print(“Now, begin! “);
//    delay(500);

    // Wait for slider to be pressed at one end > 30mm
    while( tsi.readDistance() < 30 && tsi.readDistance() != 0 ) {
        redLed = OFF;
        wait(0.2);
        redLed = ON;
        wait(0.2);
    }

    // Wait for release
    while( tsi.readDistance() != 0 ) {
        redLed = OFF;
        wait(0.2);
        redLed = ON;
        wait(0.2);
    }
    redLed = OFF;
    wait(1.0);
        
    tempXmax = tempXmin = readVal(MAG_OUT_X_MSB);
    tempYmax = tempYmin = readVal(MAG_OUT_Y_MSB);
    int changeCount = 10000;
    bool change = false;
    while(changeCount) {   // digitalRead(10) == LOW) {
//    for(int x=0; x<100000; x++); {
        //newX = readx();
        //newY = ready();
        newX = readVal(MAG_OUT_X_MSB);
        newY = readVal(MAG_OUT_Y_MSB);
        if (newX > tempXmax) {
            tempXmax = newX;
            change = true;
        }
        if (newX < tempXmin) {
            tempXmin = newX;
            change = true;
        }
        if (newY > tempYmax) {
            tempYmax = newY;
            change = true;
        }
        if (newY < tempYmin) {
            tempYmin = newY;
            change = true;
        }
        if( change )
            change = false;
        else
            changeCount--;
//        printf("X max %d min %d, Y max %d min %d\n",tempXmax, tempXmin, tempYmax, tempYmin);
    }
    settings.maxX = tempXmax;
    settings.minX = tempXmin;
    settings.maxY = tempYmax;
    settings.minY = tempYmin;
    //X max 65173 min 64850, Y max 490 min 141

//store new X, Y values in the EEPROM
//    eeprom_write_block((const void*)&settings, (void*)0, sizeof(settings));

    avgX=(settings.maxX+settings.minX)/2;
    avgY=(settings.maxY+settings.minY)/2;

    redLed = OFF;
    // Wait for slider to be pressed at one end > 30mm
    while( tsi.readDistance() < 30 && tsi.readDistance() != 0 ) {
        greenLed = OFF;
        wait(0.2);
        greenLed = ON;
        wait(0.2);
    }

    // Wait for release
    while( tsi.readDistance() != 0 ) {
        greenLed = OFF;
        wait(0.2);
        greenLed = ON;
        wait(0.2);
    }
    greenLed = OFF;
    wait(1.0);

//    lcd.setCursor(0,1);
//    lcd.print(“Calibration done”);
    //delay(3000);
//    lcd.setCursor(0,1);
//    lcd.print(” “);

}

int main()
{
    char cmd[2];
//    r.period(0.001);
//    g.period(0.001);
//    b.period(0.001);

    printf("MAG3110 Test\n");

    redLed = OFF;
    greenLed = OFF;
    blueLed = OFF;
    
    cmd[0] = MAG_CTRL_REG2;
    cmd[1] = 0x80;
    i2c.write(addr, cmd, 2);

//    wait(0.1);
    cmd[0] = MAG_CTRL_REG1;
    cmd[1] = MAG_3110_SAMPLE80+MAG_3110_OVERSAMPLE2+MAG_3110_ACTIVE; // 0x91;
    i2c.write(addr, cmd, 2);

    // 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 ));

//    r = 1;
//    g = 0;
//    b = 0;
    settings.maxX = 65173;
    settings.minX = 64850;
    settings.maxY = 490;
    settings.minY = 141;
    //X max 65173 min 64850, Y max 490 min 141
    avgX=(settings.maxX+settings.minX)/2;
    avgY=(settings.maxY+settings.minY)/2;
    printf("avgX = %d, avgY = %d\n", avgX, avgY);

//    avgX = 0;
//    avgY = 0;

    printf("calibrate\n");
    calXY();
    printf("....Finished\n");
    printf("avgX = %d, avgY = %d\n", avgX, avgY);

 //   greenLed = 1;
    redLed = OFF;
    greenLed = OFF;
    blueLed = OFF;
    
    while (1) {
        wait(0.5);
        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;
//        float heading = (atan2((double)(xVal-avgX),(double)(yVal-avgY)))*180/PI;

        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 (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);

//        printf("2 byte: X = %d ", readVal( 0x01 ));
//        printf(" Y = %d ", readVal( 0x03 ));
//        printf(" Z = %d\n", readVal( 0x05 ));

//        printf("1 byte: X = %d ", readx());
//        printf(" Y = %d ", ready());
//        printf(" Z = %d\n", readz());
    }
}