Diff: Magneto.cpp

Revision:

0:28d5622d1a3e

Child:

1:3591e7df4ff4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Magneto.cpp	Sun Mar 12 14:58:06 2017 +0000
@@ -0,0 +1,169 @@
+#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");  // Open "out.txt" on the local file system for writing
+        // 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");  // Open "out.txt" on the local file system for writing
+        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, 6);
+}
+
+bool Magneto::Test(){
+    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;
+}