Revision 2:4debef04091d, committed 2018-07-05

Comitter:

acracan

Date:

Thu Jul 05 17:57:00 2018 +0000

Parent:

1:fb6804e865fd

Child:

3:c2cd6cc71ee2

Commit message:

Add calibration transform function

Changed in this revision

--- a/HMC5983.cpp	Thu Jul 05 15:43:14 2018 +0000
+++ b/HMC5983.cpp	Thu Jul 05 17:57:00 2018 +0000
@@ -20,37 +20,43 @@
 {
     // Placement new to avoid additional heap memory allocation.
     new(i2cRaw) I2C(sda, scl);
-    
+
     init();
 }
 
-HMC5983::HMC5983(I2C &i2c): i2c_(i2c){
-  init();
+HMC5983::HMC5983(I2C &i2c): i2c_(i2c)
+{
+    init();
 }
 
 bool HMC5983::init()
 {
-  if ((fastRegister8(HMC5983_REG_IDENT_A) != 0x48)
-    || (fastRegister8(HMC5983_REG_IDENT_B) != 0x34)
-    || (fastRegister8(HMC5983_REG_IDENT_C) != 0x33)) {
-    return false;
-  }
+    const float initial_m[9] = {1.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 1.0f};
+    const float initial_b[3] = {0.0f, 0.0f, 0.0f};
+    if ((fastRegister8(HMC5983_REG_IDENT_A) != 0x48)
+            || (fastRegister8(HMC5983_REG_IDENT_B) != 0x34)
+            || (fastRegister8(HMC5983_REG_IDENT_C) != 0x33)) {
+        return false;
+    }
 
-  // Set Gain Range
-  setRange(HMC5983_RANGE_8_1GA);
-  // Set DataRate 220Hz ~4.5ms
-  setDataRate(HMC5983_DATARATE_220HZ);
-  // Set number of samples to average
-  setSampleAverages(HMC5983_SAMPLEAVERAGE_2);
-  // Set Mode
-  setMeasurementMode(HMC5983_CONTINOUS);
-
-  // Setup DRDY int
+    // Set Gain Range
+    setRange(HMC5983_RANGE_8_1GA);
+    // Set DataRate 220Hz ~4.5ms
+    setDataRate(HMC5983_DATARATE_220HZ);
+    // Set number of samples to average
+    setSampleAverages(HMC5983_SAMPLEAVERAGE_2);
+    // Set Mode
+    setMeasurementMode(HMC5983_CONTINOUS);
+    setCalibrationMatrices(initial_m, initial_b);
+    H[0] = 0;
+    H[1] = 0;
+    H[2] = 0;
+    // Setup DRDY int
 //  if (ISR_callback != NULL) {
 //    pinMode(3, INPUT_PULLUP);
 //    attachInterrupt(digitalPinToInterrupt(3), ISR_callback, FALLING);
 //  }
-  return true;
+    return true;
 }
 
 /*
@@ -75,7 +81,8 @@
   Direction (y=0, x>0) = 0.0
 */
 
-void HMC5983::setRange(hmc5983_range_t range) {
+void HMC5983::setRange(hmc5983_range_t range)
+{
 
     writeRegister8(HMC5983_REG_CONFIG_B, range << 5);
 }
@@ -85,7 +92,8 @@
     return (hmc5983_range_t)((readRegister8(HMC5983_REG_CONFIG_B) >> 5));
 }
 
-void HMC5983::setMeasurementMode(hmc5983_mode_t mode) {
+void HMC5983::setMeasurementMode(hmc5983_mode_t mode)
+{
     uint8_t value;
 
     value = readRegister8(HMC5983_REG_MODE);
@@ -95,7 +103,8 @@
     writeRegister8(HMC5983_REG_MODE, value);
 }
 
-hmc5983_mode_t HMC5983::getMeasurementMode(void) {
+hmc5983_mode_t HMC5983::getMeasurementMode(void)
+{
     uint8_t value;
 
     value = readRegister8(HMC5983_REG_MODE);
@@ -104,7 +113,8 @@
     return (hmc5983_mode_t)value;
 }
 
-void HMC5983::setDataRate(hmc5983_dataRate_t dataRate) {
+void HMC5983::setDataRate(hmc5983_dataRate_t dataRate)
+{
     uint8_t value;
 
     value = readRegister8(HMC5983_REG_CONFIG_A);
@@ -114,7 +124,8 @@
     writeRegister8(HMC5983_REG_CONFIG_A, value);
 }
 
-hmc5983_dataRate_t HMC5983::getDataRate(void) {
+hmc5983_dataRate_t HMC5983::getDataRate(void)
+{
     uint8_t value;
 
     value = readRegister8(HMC5983_REG_CONFIG_A);
@@ -124,7 +135,8 @@
     return (hmc5983_dataRate_t)value;
 }
 
-void HMC5983::setSampleAverages(hmc5983_sampleAverages_t sampleAverages) {
+void HMC5983::setSampleAverages(hmc5983_sampleAverages_t sampleAverages)
+{
     uint8_t value;
 
     value = readRegister8(HMC5983_REG_CONFIG_A);
@@ -134,7 +146,8 @@
     writeRegister8(HMC5983_REG_CONFIG_A, value);
 }
 
-hmc5983_sampleAverages_t HMC5983::getSampleAverages(void) {
+hmc5983_sampleAverages_t HMC5983::getSampleAverages(void)
+{
     uint8_t value;
 
     value = readRegister8(HMC5983_REG_CONFIG_A);
@@ -145,32 +158,35 @@
 }
 
 // Write byte to register
-void HMC5983::writeRegister8(uint8_t reg, uint8_t value) {
-  char cmd[2];
-  
-  cmd[0] = reg;
-  cmd[1] = value;
-  i2c_.write(HMC5983_ADDRESS, cmd, 2);
+void HMC5983::writeRegister8(uint8_t reg, uint8_t value)
+{
+    char cmd[2];
+
+    cmd[0] = reg;
+    cmd[1] = value;
+    i2c_.write(HMC5983_ADDRESS, cmd, 2);
 }
 
 // Read byte to register
-uint8_t HMC5983::fastRegister8(uint8_t reg) {
-  uint8_t value;
+uint8_t HMC5983::fastRegister8(uint8_t reg)
+{
+    uint8_t value;
 
-  i2c_.write(HMC5983_ADDRESS, (char *)&reg, 1);
-  i2c_.read(HMC5983_ADDRESS, (char *)&value, 1);
+    i2c_.write(HMC5983_ADDRESS, (char *)&reg, 1);
+    i2c_.read(HMC5983_ADDRESS, (char *)&value, 1);
 
-  return value;
+    return value;
 }
 
 // Read byte from register
-uint8_t HMC5983::readRegister8(uint8_t reg) {
-  uint8_t value;
+uint8_t HMC5983::readRegister8(uint8_t reg)
+{
+    uint8_t value;
 
-  i2c_.write(HMC5983_ADDRESS, (char *)&reg, 1);
-  i2c_.read(HMC5983_ADDRESS, (char *)&value, 1);
-  
-  return value;
+    i2c_.write(HMC5983_ADDRESS, (char *)&reg, 1);
+    i2c_.read(HMC5983_ADDRESS, (char *)&value, 1);
+
+    return value;
 
 //  Wire.requestFrom(HMC5983_ADDRESS, 1);
 //  while(!Wire.available()) {};
@@ -178,59 +194,108 @@
 }
 
 // Read word from register
-int16_t HMC5983::readRegister16(uint8_t reg) {
-  int16_t value;
-  char resp[2];
+int16_t HMC5983::readRegister16(uint8_t reg)
+{
+    int16_t value;
+    char resp[2];
 
-  i2c_.write(HMC5983_ADDRESS, (char *)&reg, 1);
-  i2c_.read(HMC5983_ADDRESS, resp, 2);
+    i2c_.write(HMC5983_ADDRESS, (char *)&reg, 1);
+    i2c_.read(HMC5983_ADDRESS, resp, 2);
 
 //  Wire.requestFrom(HMC5983_ADDRESS, 2);
 //  while(!Wire.available()) {};
 
-  value = (uint8_t)resp[0] << 8 | (uint8_t)resp[1];
+    value = (uint8_t)resp[0] << 8 | (uint8_t)resp[1];
 
-  return value;
+    return value;
 }
 
-void HMC5983::getHeadingRegs() {
-  // the values for X, Y & Z must be read in X, Z & Y order.
-  char data[6];
-  writeRegister8(HMC5983_OUT_X_MSB, 0); // Select MSB X register
-  i2c_.read(HMC5983_ADDRESS, data, 6);
-  uint8_t X_MSB = data[0];
-  uint8_t X_LSB = data[1];
-  uint8_t Z_MSB = data[2];
-  uint8_t Z_LSB = data[3];
-  uint8_t Y_MSB = data[4];
-  uint8_t Y_LSB = data[5];
+void HMC5983::getHeadingRegs()
+{
+    // the values for X, Y & Z must be read in X, Z & Y order.
+    char data[6];
+    writeRegister8(HMC5983_OUT_X_MSB, 0); // Select MSB X register
+    i2c_.read(HMC5983_ADDRESS, data, 6);
+    uint8_t X_MSB = data[0];
+    uint8_t X_LSB = data[1];
+    uint8_t Z_MSB = data[2];
+    uint8_t Z_LSB = data[3];
+    uint8_t Y_MSB = data[4];
+    uint8_t Y_LSB = data[5];
+
+    // compose byte for X, Y, Z's LSB & MSB 8bit registers
+    H[0] = (uint16_t(X_MSB) << 8) | X_LSB;
+    H[1] = (uint16_t(Z_MSB) << 8) | Z_LSB;
+    H[2] = (uint16_t(Y_MSB) << 8) | Y_LSB;
+}
 
-  // compose byte for X, Y, Z's LSB & MSB 8bit registers
-  HX = (uint16_t(X_MSB) << 8) | X_LSB;
-  HZ = (uint16_t(Z_MSB) << 8) | Z_LSB;
-  HY = (uint16_t(Y_MSB) << 8) | Y_LSB;
+void HMC5983::computeCalibratedHeadingVector(float *v)
+{
+    float Hub[3];
+
+    int i, j;
+
+    // remove bias
+    for (i = 0; i < 3; i++) {
+        Hub[i] = H[i] - _b[i];
+        v[i] = 0.0f;
+    }
+
+    // apply matrix transformation
+    for (i = 0; i < 3; i++) {
+        for (j = 0; j < 3; j++) {
+            v[i] += _m[3*i+j]*Hub[j];
+        }
+    }
 }
 
-float HMC5983::readHeading() {
-  // declare the heading variable we'll be returning
-  float H = 0;
+void HMC5983::setCalibrationMatrices(const float *m, const float *b)
+{
+    int i, j;
 
-  getHeadingRegs();
-  // this is the correct way, fixed from original David's work.
-  // corrected following datasheet and his own comments...xD
-  // even corrected from datasheet, the 90-270 angle is a bit confusing, but the 360º are captured.
-  if (HY > 0) H = 90.0f - (float)atan(float(HX) / HY) * 180.0f / M_PI;
-  if (HY < 0) H = 270.0f - (float)atan(float(HX) / HY) * 180.0f / M_PI;
-  if (HY == 0 && HX < 0) H = 180.0f;
-  if (HY == 0 && HX > 0) H = 0.0f;
-
-  return H;
+    for (i = 0; i < 3; i++) {
+        for (j = 0; j < 3; j++) {
+            _m[3*i+j] = m[3*i+j];
+        }
+        _b[i] = b[i];
+    }
 }
 
-void HMC5983::readHeadingVector(int16_t *v) {
+float HMC5983::readHeading(bool calibrate)
+{
+    // declare the heading variable we'll be returning
+    float result = 0;
+    float cH[3];
+    int i;
+
     getHeadingRegs();
-    
-    v[0] = HX;
-    v[1] = HY;
-    v[2] = HZ;
+    for (i = 0; i < 3; i++) {
+        cH[i] = H[i];
+    }
+
+    if (calibrate) {
+        computeCalibratedHeadingVector(cH);
+    }
+    // this is the correct way, fixed from original David's work.
+    // corrected following datasheet and his own comments...xD
+    // even corrected from datasheet, the 90-270 angle is a bit confusing, but the 360º are captured.
+    if (cH[1] > 0.0f) result = 90.0f - (float)atan(cH[0] / cH[1]) * 180.0f / M_PI;
+    if (cH[1] < 0.0f) result = 270.0f - (float)atan(cH[0] / cH[1]) * 180.0f / M_PI;
+    if (cH[1] == 0.0f && cH[0] < 0.0f) result = 180.0f;
+    if (cH[1] == 0.0f && cH[0] > 0.0f) result = 0.0f;
+
+    return result;
+}
+
+void HMC5983::readHeadingVector(float *v, bool calibrate)
+{
+    getHeadingRegs();
+    if (calibrate) {
+        computeCalibratedHeadingVector(v);
+    }
+    else {
+        v[0] = H[0];
+        v[1] = H[1];
+        v[2] = H[2];
+    }
 }
\ No newline at end of file

--- a/HMC5983.h	Thu Jul 05 15:43:14 2018 +0000
+++ b/HMC5983.h	Thu Jul 05 17:57:00 2018 +0000
@@ -121,18 +121,20 @@
     void  setSampleAverages(hmc5983_sampleAverages_t sampleAverages);
     hmc5983_sampleAverages_t getSampleAverages(void);
     
-    float readHeading();
-    void readHeadingVector(int16_t *v);
+    void setCalibrationMatrices(const float *m, const float *b);
+    float readHeading(bool calibrate=true);
+    void readHeadingVector(float *v, bool calibrate=true);
     
   private:
     I2C &i2c_;
     char i2cRaw[sizeof(I2C)];
-    int16_t HX;
-    int16_t HZ;
-    int16_t HY;
+    float _m[9];
+    float _b[3];
+    int16_t H[3];
 
     
     void getHeadingRegs();
+    void computeCalibratedHeadingVector(float *v);
     void writeRegister8(uint8_t reg, uint8_t value);
     uint8_t readRegister8(uint8_t reg);
     uint8_t fastRegister8(uint8_t reg);