File content as of revision 1:3591e7df4ff4:

#include "Magneto.h"

#define PI                3.14159265358979323f

namespace {
    char cmd[2];
    char buffer[6];
    
    Ticker calibrationTicker;
    
    int16_t minX = OR_MAX_VALUE; // The maximum value is put in the min so real values will always be smaller
    int16_t maxX = OR_MIN_VALUE; // The minimum value is put in the max so real values will always be bigger
    int16_t minY = OR_MAX_VALUE; // The maximum value is put in the min so real values will always be smaller
    int16_t maxY = OR_MIN_VALUE; // The minimum value is put in the max so real values will always be bigger
    
    int16_t offsetX = 0;
    int16_t offsetY = 0;
    
    int16_t scaleX = 0;
    int16_t scaleY = 0;
    
    void _UpdateScaleAndOffset(){
        offsetX = (maxX + minX)/2;
        scaleX = maxX - offsetX;
        
        offsetY = (maxY + minY)/2;
        scaleY = maxY - offsetY;
    }
    
    void _UpdateCalibration(int16_t x, int16_t y){
        bool changed = false;
            
        if (x < minX){
            minX = x;
            changed = true;
        }
        if (x > maxX){
            maxX = x;
            changed = true;
        }
        
        if (y < minY){
            minY = y;
            changed = true;
        }
        if (y > maxY){
            maxY = y;
            changed = true;
        }
        
        if(changed || true){
            _UpdateScaleAndOffset();
        }
    }

    LocalFileSystem local("local");
    
    void _LoadCalibration(){
        FILE *fp = fopen("/local/magneto.cfg", "r");
        // File doesn't exist
        if (fp == 0){
            printf("File doesn't exist!!\r\n");
            return;
        }
        
        fscanf(fp,"%04X %04X %04X %04X", &minX, &maxX, &minY, &maxY);
        fclose(fp);
        
        _UpdateScaleAndOffset();
    }
    
    void _SaveCalibration(){
        FILE *fp = fopen("/local/magneto.cfg", "w");
        fprintf(fp, "%04X %04X %04X %04X", minX & 0xFFFF, maxX & 0xFFFF, minY & 0xFFFF, maxY & 0xFFFF);
        fclose(fp);
    }
};

Magneto::Magneto(I2C & i2c) :
_i2c(&i2c)
{
    _LoadCalibration();

    // Every 10 minutes
    calibrationTicker.attach(_SaveCalibration, 600);
}

bool Magneto::TestDeviceConnection(){
    cmd[0] = MAGNETO_RA_ID_A;
    
    _i2c->write(MAGNETO_ADDRESS, cmd, 1, true);
    _i2c->read(MAGNETO_ADDRESS, buffer, 3);
    
    return (buffer[0] == IR_A_VALUE) && 
           (buffer[1] == IR_B_VALUE) && 
           (buffer[2] == IR_C_VALUE);
}

void Magneto::ActivateDevice(){
    cmd[0] = MAGNETO_RA_CONFIG_A;
    cmd[1] = (CRA_SAMPLE_AVG_8 << CRA_SAMPLE_AVG_BIT) | (CRA_DATA_RATE_15 << CRA_DATA_RATE_BIT);
    
    _i2c->write(MAGNETO_ADDRESS, cmd, 2, false);
    
    cmd[0] = MAGNETO_RA_CONFIG_B;
    cmd[1] = CRB_GAIN_130 << CRB_GAIN_BIT;
    
    _i2c->write(MAGNETO_ADDRESS, cmd, 2, false);
    
    cmd[0] = MAGNETO_RA_MODE;
    cmd[1] = MR_MODE_CONTINUOUS << MR_MODE_BIT;
    
    _i2c->write(MAGNETO_ADDRESS, cmd, 2, false);
}

void Magneto::ReadXZY(int16_t* raw){
    cmd[0] = MAGNETO_RA_X_MSB;
    
    _i2c->write(MAGNETO_ADDRESS, cmd, 1, true);
    _i2c->read(MAGNETO_ADDRESS, buffer, 6);
    
    for(int i = 0; i < 6; i += 2){
        int16_t value = buffer[i];
        value = (value << 8) | buffer[i+1];
        raw[i >> 1] = value;
    }
}

int16_t Magneto::GetHeadingXY(){
    int16_t data[3] = {0};
    
    ReadXZY(data);
    
    int16_t x = data[0];
    int16_t y = data[2];
    
    _UpdateCalibration(x, y);
    
    if (scaleX == 0 || scaleY == 0){
        return HEADING_ERROR;
    }
    
    float scaledX = (float)(x - offsetX) / (float) scaleX; // Scaled value between -1 and 1
    float scaledY = (float)(y - offsetY) / (float) scaleY; // Scaled value between -1 and 1
    
    int16_t heading = atan2(scaledY, scaledX) * 180.0f / PI;
    
    if (heading < 0) {
        heading += 360;
    } else if (heading > 360) {
        heading -= 360;
    }
    
    return heading;
}


void Magneto::ResetDevice(){
    minX = OR_MAX_VALUE;
    maxX = OR_MIN_VALUE;
    minY = OR_MAX_VALUE;
    maxY = OR_MIN_VALUE;
    
    offsetX = 0;
    offsetY = 0;
    
    scaleX = 0;
    scaleY = 0;
}