From 20921aab5ca8c7a68fc1562e6e9cc95013352456 Mon Sep 17 00:00:00 2001 From: Philipp Klaus Date: Thu, 8 Feb 2018 14:07:28 +0100 Subject: [PATCH] Magn Sens: Code to enable temperature compensation (TC) --- sensors/MagneticFieldSensor/MagnetSensor.c | 35 ++++++++++++++++++---- sensors/MagneticFieldSensor/MagnetSensor.h | 4 +-- sensors/MagneticFieldSensor/main.c | 27 +++++++++-------- 3 files changed, 45 insertions(+), 21 deletions(-) diff --git a/sensors/MagneticFieldSensor/MagnetSensor.c b/sensors/MagneticFieldSensor/MagnetSensor.c index 3beddbe..f17415a 100644 --- 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 5a71ec7..65eb7f1 100644 --- 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 5669d61..f51c346 100644 --- a/sensors/MagneticFieldSensor/main.c +++ b/sensors/MagneticFieldSensor/main.c @@ -14,6 +14,7 @@ volatile uint8_t mode = 0; int main(void) { char s[30]; uint8_t Status[4]={0,0,0,0}; + uint8_t TC_Enabled[4]={0,0,0,0}; uint8_t Status_Mag[4]={0,0,0,0}; uint16_t TRef[4]={0,0,0,0}; float SensX[4]; @@ -32,10 +33,10 @@ int main(void) twi_init(); //Init Magnetic field Sensor - Status[0] = InitMagSensor(ADDR_MAG_0, &TRef[0], &SensX[0], &SensY[0], &SensZ[0]); - Status[1] = InitMagSensor(ADDR_MAG_1, &TRef[1], &SensX[1], &SensY[1], &SensZ[1]); - Status[2] = InitMagSensor(ADDR_MAG_2, &TRef[2], &SensX[2], &SensY[2], &SensZ[2]); - Status[3] = InitMagSensor(ADDR_MAG_3, &TRef[3], &SensX[3], &SensY[3], &SensZ[3]); + Status[0] = InitMagSensor(ADDR_MAG_0, &TRef[0], &SensX[0], &SensY[0], &SensZ[0], &TC_Enabled[0]); + Status[1] = InitMagSensor(ADDR_MAG_1, &TRef[1], &SensX[1], &SensY[1], &SensZ[1], &TC_Enabled[1]); + Status[2] = InitMagSensor(ADDR_MAG_2, &TRef[2], &SensX[2], &SensY[2], &SensZ[2], &TC_Enabled[2]); + Status[3] = InitMagSensor(ADDR_MAG_3, &TRef[3], &SensX[3], &SensY[3], &SensZ[3], &TC_Enabled[3]); // sprintf(s,"%u", Status[0]); //uputs(s); @@ -52,10 +53,10 @@ int main(void) mode = 1; } else if (strcmp(uart_string,"END") == 0 ) { mode = 0; - InitMagSensor(ADDR_MAG_0, &TRef[0], &SensX[0], &SensY[0], &SensZ[0]); - InitMagSensor(ADDR_MAG_1, &TRef[1], &SensX[1], &SensY[1], &SensZ[1]); - InitMagSensor(ADDR_MAG_2, &TRef[2], &SensX[2], &SensY[2], &SensZ[2]); - InitMagSensor(ADDR_MAG_3, &TRef[3], &SensX[3], &SensY[3], &SensZ[3]); + InitMagSensor(ADDR_MAG_0, &TRef[0], &SensX[0], &SensY[0], &SensZ[0], &TC_Enabled[0]); + InitMagSensor(ADDR_MAG_1, &TRef[1], &SensX[1], &SensY[1], &SensZ[1], &TC_Enabled[1]); + InitMagSensor(ADDR_MAG_2, &TRef[2], &SensX[2], &SensY[2], &SensZ[2], &TC_Enabled[2]); + InitMagSensor(ADDR_MAG_3, &TRef[3], &SensX[3], &SensY[3], &SensZ[3], &TC_Enabled[3]); } else if (strcmp(uart_string,"EXIT") == 0 ) { status2 = twi_writeTo(0x0C, 0x80, 1,1, true); sprintf(s,"Status:0x%x",status2); @@ -170,15 +171,15 @@ int main(void) _delay_ms(200); }// END if uart_str_complete - //running mode if (mode == 0) { - if (Status[0] == 0) { MeasureMagSens(ADDR_MAG_0, TRef[0], &SensX[0], &SensY[0], &SensZ[0]);} + // normal running mode + if (Status[0] == 0) { MeasureMagSens(ADDR_MAG_0, TRef[0], &SensX[0], &SensY[0], &SensZ[0], TC_Enabled[0]);} else { uputsnl("ERR Mag 0 NO_DEVICE"); } - if (Status[1] == 0) { MeasureMagSens(ADDR_MAG_1, TRef[1], &SensX[1], &SensY[1], &SensZ[1]);} + if (Status[1] == 0) { MeasureMagSens(ADDR_MAG_1, TRef[1], &SensX[1], &SensY[1], &SensZ[1], TC_Enabled[1]);} else { uputsnl("ERR Mag 1 NO_DEVICE"); } - if (Status[2] == 0) { MeasureMagSens(ADDR_MAG_2, TRef[2], &SensX[2], &SensY[2], &SensZ[2]);} + if (Status[2] == 0) { MeasureMagSens(ADDR_MAG_2, TRef[2], &SensX[2], &SensY[2], &SensZ[2], TC_Enabled[2]);} else { uputsnl("ERR Mag 2 NO_DEVICE"); } - if (Status[3] == 0) { MeasureMagSens(ADDR_MAG_3, TRef[3], &SensX[3], &SensY[3], &SensZ[3]);} + if (Status[3] == 0) { MeasureMagSens(ADDR_MAG_3, TRef[3], &SensX[3], &SensY[3], &SensZ[3], TC_Enabled[3]);} else { uputsnl("ERR Mag 3 NO_DEVICE"); } _delay_ms(500); -- 2.43.0