Magn Sens: Code to enable temperature compensation (TC)

diff --git a/sensors/MagneticFieldSensor/MagnetSensor.c b/sensors/MagneticFieldSensor/MagnetSensor.c
index 3beddbe91de2d4a88e1125c6fe6f47560a3e3d2c..f17415a17da8bacc5a96ee576f9cff894627addc 100644 (file)
--- a/sensors/MagneticFieldSensor/MagnetSensor.c
+++ b/sensors/MagneticFieldSensor/MagnetSensor.c
@@ -1,7 +1,7 @@
 #include "MagnetSensor.h"
 //#define CHIP_OLD 1
 
-uint8_t InitMagSensor(uint8_t Addr, uint16_t* Tref, float* SensX, float* SensY, float* SensZ)
+uint8_t InitMagSensor(uint8_t Addr, uint16_t* Tref, float* SensX, float* SensY, float* SensZ, uint8_t *TC_Enabled)
 {
     
     // Write register command, AH = 0x00, AL = 0x5C, Hall plate spinning rate = DEFAULT, GAIN_SEL = 5(0x60, 0x00, 0x5C, 0x00)
@@ -12,11 +12,18 @@ uint8_t InitMagSensor(uint8_t Addr, uint16_t* Tref, float* SensX, float* SensY,
     uint8_t Gain,Res_x,Res_y,Res_z;
     char s[30];
    
+    // Read temperature compensation 
+    txBuffer[0] = 0x50; //write in MagSensor
+    txBuffer[1] = 0x04; //Register 0x1 (shiftet to left by 2 Bits)
+    twi_writeTo(Addr, txBuffer, 2,1, true);
+    twi_readFrom(Addr, &ReadReg, 3, true);
+    *TC_Enabled = (ReadReg[1] & 0x4 ) >> 2;
+
+    // Write reg addr 0x0 (hall config...)
     txBuffer[0] = 0x60; //write in MagSensor
     txBuffer[1] = 0x00; //Upper Byte
     txBuffer[2] = 0x5C; //Lower Byte
     txBuffer[3] = 0x00; //Register Address (shiftet to left by 2 Bits)
-    
     status2 = twi_writeTo(Addr, txBuffer, 4,1, true);
     twi_readFrom(Addr, &status, 1, true);
     
@@ -68,7 +75,7 @@ uint8_t InitMagSensor(uint8_t Addr, uint16_t* Tref, float* SensX, float* SensY,
 }
 
 
-void MeasureMagSens(uint8_t Addr, uint16_t Tref, float* SensX, float* SensY, float* SensZ){
+void MeasureMagSens(uint8_t Addr, uint16_t Tref, float* SensX, float* SensY, float* SensZ, const uint8_t TC_Enabled){
     uint8_t MagData[9] = {255,255,255,255,255,255,255,255,255};
     char    s[30];
 
@@ -89,9 +96,25 @@ void MeasureMagSens(uint8_t Addr, uint16_t Tref, float* SensX, float* SensY, flo
 
     if (MagData[0] == 3) {
        uint16_t tMag = (uint16_t)( MagData[1] * 256 + MagData[2]);
-       int16_t xMag = MagData[3] * 256 + MagData[4];
-       int16_t yMag = MagData[5] * 256 + MagData[6];
-       int16_t zMag = MagData[7] * 256 + MagData[8];
+        int16_t xMag;
+        int16_t yMag;
+        int16_t zMag;
+       if (TC_Enabled) {
+         /*
+          uint16_t u_xMag = MagData[3] * 256 + MagData[4];
+         uint16_t u_yMag = MagData[5] * 256 + MagData[6];
+         uint16_t u_zMag = MagData[7] * 256 + MagData[8];
+         xMag = u_xMag ^ 0x8000;
+         yMag = u_yMag ^ 0x8000;
+         zMag = u_zMag ^ 0x8000;*/
+         xMag = (MagData[3] ^ 0x80) * 256 + MagData[4];
+         yMag = (MagData[5] ^ 0x80) * 256 + MagData[6];
+         zMag = (MagData[7] ^ 0x80) * 256 + MagData[8];
+        } else {
+         xMag = MagData[3] * 256 + MagData[4];
+         yMag = MagData[5] * 256 + MagData[6];
+         zMag = MagData[7] * 256 + MagData[8];
+        }
        int16_t temp_temp = (uint16_t)(tMag) - (uint16_t)(Tref);
        float Temperature = 35.0 + (temp_temp/45.2);
        uputs("M");uputs(Num);uputs("_T ");

diff --git a/sensors/MagneticFieldSensor/MagnetSensor.h b/sensors/MagneticFieldSensor/MagnetSensor.h
index 5a71ec753f7dd10745f08e83ad3ba38e11f4f22f..65eb7f100ea08b536a9cdb6f2e3792a69c45a809 100644 (file)
--- a/sensors/MagneticFieldSensor/MagnetSensor.h
+++ b/sensors/MagneticFieldSensor/MagnetSensor.h
@@ -1,6 +1,6 @@
 #include "main.h"
 
-uint8_t InitMagSensor(uint8_t Addr, uint16_t* Tref, float *SensX, float* SensY, float *SensZ);
-void MeasureMagSens(uint8_t Addr, uint16_t Tref, float* SensX, float* SensY, float* SensZ);
+uint8_t InitMagSensor(uint8_t Addr, uint16_t* Tref, float *SensX, float* SensY, float *SensZ, uint8_t *TC_Enabled);
+void MeasureMagSens(uint8_t Addr, uint16_t Tref, float* SensX, float* SensY, float* SensZ, const uint8_t TC_Enabled);
 float SensitivityXY(uint8_t Gain, uint8_t Res);
 float SensitivityZ(uint8_t Gain, uint8_t Res);

diff --git a/sensors/MagneticFieldSensor/main.c b/sensors/MagneticFieldSensor/main.c
index 5669d61f2c9b231973ed36c34be3083ba47e5c03..f51c346174db56fc6cc61ade0943f82bd137aaa0 100644 (file)
--- a/sensors/MagneticFieldSensor/main.c
+++ b/sensors/MagneticFieldSensor/main.c
@@ -14,6 +14,7 @@ volatile uint8_t mode = 0;
 int main(void)\r
 {   char s[30];\r
     uint8_t Status[4]={0,0,0,0};\r
+    uint8_t TC_Enabled[4]={0,0,0,0};\r
     uint8_t Status_Mag[4]={0,0,0,0};\r
     uint16_t TRef[4]={0,0,0,0};\r
     float SensX[4];\r
@@ -32,10 +33,10 @@ int main(void)
     twi_init();\r
 \r
     //Init Magnetic field Sensor\r
-    Status[0] = InitMagSensor(ADDR_MAG_0, &TRef[0], &SensX[0], &SensY[0], &SensZ[0]);\r
-    Status[1] = InitMagSensor(ADDR_MAG_1, &TRef[1], &SensX[1], &SensY[1], &SensZ[1]);\r
-    Status[2] = InitMagSensor(ADDR_MAG_2, &TRef[2], &SensX[2], &SensY[2], &SensZ[2]);\r
-    Status[3] = InitMagSensor(ADDR_MAG_3, &TRef[3], &SensX[3], &SensY[3], &SensZ[3]);\r
+    Status[0] = InitMagSensor(ADDR_MAG_0, &TRef[0], &SensX[0], &SensY[0], &SensZ[0], &TC_Enabled[0]);\r
+    Status[1] = InitMagSensor(ADDR_MAG_1, &TRef[1], &SensX[1], &SensY[1], &SensZ[1], &TC_Enabled[1]);\r
+    Status[2] = InitMagSensor(ADDR_MAG_2, &TRef[2], &SensX[2], &SensY[2], &SensZ[2], &TC_Enabled[2]);\r
+    Status[3] = InitMagSensor(ADDR_MAG_3, &TRef[3], &SensX[3], &SensY[3], &SensZ[3], &TC_Enabled[3]);\r
 \r
      // sprintf(s,"%u", Status[0]);\r
      //uputs(s);\r
@@ -52,10 +53,10 @@ int main(void)
              mode = 1;\r
            } else if (strcmp(uart_string,"END") == 0 ) {\r
              mode = 0;\r
-             InitMagSensor(ADDR_MAG_0, &TRef[0], &SensX[0], &SensY[0], &SensZ[0]);\r
-             InitMagSensor(ADDR_MAG_1, &TRef[1], &SensX[1], &SensY[1], &SensZ[1]);\r
-             InitMagSensor(ADDR_MAG_2, &TRef[2], &SensX[2], &SensY[2], &SensZ[2]);\r
-             InitMagSensor(ADDR_MAG_3, &TRef[3], &SensX[3], &SensY[3], &SensZ[3]);\r
+             InitMagSensor(ADDR_MAG_0, &TRef[0], &SensX[0], &SensY[0], &SensZ[0], &TC_Enabled[0]);\r
+             InitMagSensor(ADDR_MAG_1, &TRef[1], &SensX[1], &SensY[1], &SensZ[1], &TC_Enabled[1]);\r
+             InitMagSensor(ADDR_MAG_2, &TRef[2], &SensX[2], &SensY[2], &SensZ[2], &TC_Enabled[2]);\r
+             InitMagSensor(ADDR_MAG_3, &TRef[3], &SensX[3], &SensY[3], &SensZ[3], &TC_Enabled[3]);\r
            } else if (strcmp(uart_string,"EXIT") == 0 ) {\r
              status2 = twi_writeTo(0x0C, 0x80, 1,1, true);\r
              sprintf(s,"Status:0x%x",status2);\r
@@ -170,15 +171,15 @@ int main(void)
            _delay_ms(200);\r
          }// END if uart_str_complete\r
          \r
-         //running mode\r
          if (mode == 0) {\r
-           if (Status[0] == 0) { MeasureMagSens(ADDR_MAG_0, TRef[0], &SensX[0], &SensY[0], &SensZ[0]);}\r
+           // normal running mode\r
+           if (Status[0] == 0) { MeasureMagSens(ADDR_MAG_0, TRef[0], &SensX[0], &SensY[0], &SensZ[0], TC_Enabled[0]);}\r
            else { uputsnl("ERR Mag 0 NO_DEVICE"); }\r
-           if (Status[1] == 0) { MeasureMagSens(ADDR_MAG_1, TRef[1], &SensX[1], &SensY[1], &SensZ[1]);}\r
+           if (Status[1] == 0) { MeasureMagSens(ADDR_MAG_1, TRef[1], &SensX[1], &SensY[1], &SensZ[1], TC_Enabled[1]);}\r
            else { uputsnl("ERR Mag 1 NO_DEVICE"); }\r
-           if (Status[2] == 0) { MeasureMagSens(ADDR_MAG_2, TRef[2], &SensX[2], &SensY[2], &SensZ[2]);}\r
+           if (Status[2] == 0) { MeasureMagSens(ADDR_MAG_2, TRef[2], &SensX[2], &SensY[2], &SensZ[2], TC_Enabled[2]);}\r
            else { uputsnl("ERR Mag 2 NO_DEVICE"); }\r
-           if (Status[3] == 0) { MeasureMagSens(ADDR_MAG_3, TRef[3], &SensX[3], &SensY[3], &SensZ[3]);}\r
+           if (Status[3] == 0) { MeasureMagSens(ADDR_MAG_3, TRef[3], &SensX[3], &SensY[3], &SensZ[3], TC_Enabled[3]);}\r
            else { uputsnl("ERR Mag 3 NO_DEVICE"); }\r
            \r
            _delay_ms(500);\r