Simplify HMC5883 Compass (#5289)

Remove the mag gain calculation. After some tests the maximun difference
is 6 degrees, so is not important.

src/main/drivers/compass/compass_hmc5883l.c

@@ -198,11 +198,6 @@ static void hmc5883SpiInit(busDevice_t *busdev)
 }
 #endif
 
-static int16_t parseMag(uint8_t *raw, int16_t gain) {
-  int ret = (int16_t)(raw[0] << 8 | raw[1]) * gain / 256;
-  return constrain(ret, INT16_MIN, INT16_MAX);
-}
-
 static bool hmc5883lRead(magDev_t *mag, int16_t *magData)
 {
     uint8_t buf[6];
@@ -214,94 +209,27 @@ static bool hmc5883lRead(magDev_t *mag, int16_t *magData)
     if (!ack) {
         return false;
     }
-    // During calibration, magGain is 1.0, so the read returns normal non-calibrated values.
-    // After calibration is done, magGain is set to calculated gain values.
 
-    magData[X] = parseMag(buf + 0, mag->magGain[X]);
-    magData[Z] = parseMag(buf + 2, mag->magGain[Z]);
-    magData[Y] = parseMag(buf + 4, mag->magGain[Y]);
+    magData[X] = (int16_t)(buf[0] << 8 | buf[1]);
+    magData[Z] = (int16_t)(buf[2] << 8 | buf[3]);
+    magData[Y] = (int16_t)(buf[4] << 8 | buf[5]);
 
     return true;
 }
 
 static bool hmc5883lInit(magDev_t *mag)
 {
-    enum {
-        polPos,
-        polNeg
-    };
 
     busDevice_t *busdev = &mag->busdev;
 
-    int16_t magADC[3];
-    int i;
-    int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow.
-    bool bret = true;           // Error indicator
-
-    mag->magGain[X] = 256;
-    mag->magGain[Y] = 256;
-    mag->magGain[Z] = 256;
-
-    delay(50);
-
-    busWriteRegister(busdev, HMC58X3_REG_CONFA, HMC_CONFA_DOR_15HZ | HMC_CONFA_POS_BIAS);   // Reg A DOR = 0x010 + MS1, MS0 set to pos bias
-
-    // Note that the  very first measurement after a gain change maintains the same gain as the previous setting.
-    // The new gain setting is effective from the second measurement and on.
-
-    busWriteRegister(busdev, HMC58X3_REG_CONFB, HMC_CONFB_GAIN_2_5GA); // Set the Gain to 2.5Ga (7:5->011)
-
-    delay(100);
-
-    hmc5883lRead(mag, magADC);
-
-    for (int polarity = polPos; polarity <= polNeg; polarity++) {
-        switch(polarity) {
-        case polPos:
-            busWriteRegister(busdev, HMC58X3_REG_CONFA, HMC_CONFA_DOR_15HZ | HMC_CONFA_POS_BIAS);   // Reg A DOR = 0x010 + MS1, MS0 set to pos bias
-            break;
-        case polNeg:
-            busWriteRegister(busdev, HMC58X3_REG_CONFA, HMC_CONFA_DOR_15HZ | HMC_CONFA_NEG_BIAS);   // Reg A DOR = 0x010 + MS1, MS0 set to negative bias.
-            break;
-        }
-        for (i = 0; i < 10; i++) {  // Collect 10 samples
-            busWriteRegister(busdev, HMC58X3_REG_MODE, HMC_MODE_SINGLE);
-            delay(20);
-            hmc5883lRead(mag, magADC);       // Get the raw values in case the scales have already been changed.
-
-            // Since the measurements are noisy, they should be averaged rather than taking the max.
-
-            xyz_total[X] += ((polarity == polPos) ? 1 : -1) * magADC[X];
-            xyz_total[Y] += ((polarity == polPos) ? 1 : -1) * magADC[Y];
-            xyz_total[Z] += ((polarity == polPos) ? 1 : -1) * magADC[Z];
-
-            // Detect saturation.
-            if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) {
-                bret = false;
-                break;              // Breaks out of the for loop.  No sense in continuing if we saturated.
-            }
-            LED1_TOGGLE;
-        }
-    }
-
-    mag->magGain[X] = (int)(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[X];
-    mag->magGain[Y] = (int)(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[Y];
-    mag->magGain[Z] = (int)(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f * 256.0f) / xyz_total[Z];
 
     // leave test mode
-
     busWriteRegister(busdev, HMC58X3_REG_CONFA, HMC_CONFA_8_SAMLES | HMC_CONFA_DOR_15HZ | HMC_CONFA_NORMAL);    // Configuration Register A  -- 0 11 100 00  num samples: 8 ; output rate: 15Hz ; normal measurement mode
     busWriteRegister(busdev, HMC58X3_REG_CONFB, HMC_CONFB_GAIN_1_3GA);                                          // Configuration Register B  -- 001 00000    configuration gain 1.3Ga
     busWriteRegister(busdev, HMC58X3_REG_MODE, HMC_MODE_CONTINOUS);                                             // Mode register             -- 000000 00    continuous Conversion Mode
 
     delay(100);
 
-    if (!bret) {                // Something went wrong so get a best guess
-        mag->magGain[X] = 256;
-        mag->magGain[Y] = 256;
-        mag->magGain[Z] = 256;
-    }
-
     hmc5883lConfigureDataReadyInterruptHandling(mag);
     return true;
 }

src/main/sensors/compass.c

@@ -58,7 +58,7 @@ mag_t mag;                   // mag access functions
 #define COMPASS_INTERRUPT_TAG   IO_TAG_NONE
 #endif
 
-PG_REGISTER_WITH_RESET_FN(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 0);
+PG_REGISTER_WITH_RESET_FN(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 1);
 
 void pgResetFn_compassConfig(compassConfig_t *compassConfig)
 {