Don't fail HMC5883L initialisation for a single saturated reading (#563)

* Don't fail HMC5883L initialisation for a single saturated reading

* Added log message for HMC55883L valid reads in initialisation

* Changed delay between reads to 70

Author: martinbudden

src/main/drivers/compass_hmc5883l.c

@@ -185,14 +185,12 @@ bool hmc5883lDetect(mag_t* mag, const hmc5883Config_t *hmc5883ConfigToUse)
     return true;
 }
 
-#define INIT_MAX_FAILURES 5
+#define INITIALISATION_MAX_READ_FAILURES 5
 bool hmc5883lInit(void)
 {
     int16_t magADC[3];
     int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow.
     bool bret = true;           // Error indicator
-    bool error_read = false;
-    bool error_saturation = false;
 
     delay(50);
     i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS);   // Reg A DOR = 0x010 + MS1, MS0 set to pos bias
@@ -202,68 +200,64 @@ bool hmc5883lInit(void)
     delay(100);
     hmc5883lRead(magADC);
 
-    int validSamples = 0;
-    int failedSamples = 0;
-    while (validSamples < 10 && failedSamples < INIT_MAX_FAILURES) { // Collect 10 samples
+    int validSamples1 = 0;
+    int failedSamples1 = 0;
+    int saturatedSamples1 = 0;
+    while (validSamples1 < 10 && failedSamples1 < INITIALISATION_MAX_READ_FAILURES) { // Collect 10 samples
         i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1);
-        delay(50);
+        delay(70);
         if (hmc5883lRead(magADC)) { // Get the raw values in case the scales have already been changed.
-            ++validSamples;
-            // Since the measurements are noisy, they should be averaged rather than taking the max.
-            xyz_total[X] += magADC[X];
-            xyz_total[Y] += magADC[Y];
-            xyz_total[Z] += magADC[Z];
             // Detect saturation.
             if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) {
-                error_saturation = true;
-                bret = false;
-                break;              // Breaks out of the for loop.  No sense in continuing if we saturated.
+                ++saturatedSamples1;
+                ++failedSamples1;
+            } else {
+                ++validSamples1;
+                // Since the measurements are noisy, they should be averaged rather than taking the max.
+                xyz_total[X] += magADC[X];
+                xyz_total[Y] += magADC[Y];
+                xyz_total[Z] += magADC[Z];
+
             }
         } else {
-            ++failedSamples;
+            ++failedSamples1;
         }
         LED1_TOGGLE;
     }
 
-    if (failedSamples >= INIT_MAX_FAILURES) {
-        bret = false;
-    }
-
-    if (failedSamples > 0) {
-        error_read = true;
-    }
-
     // Apply the negative bias. (Same gain)
     i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS);   // Reg A DOR = 0x010 + MS1, MS0 set to negative bias.
-    validSamples = 0;
-    failedSamples = 0;
-    while (validSamples < 10 && failedSamples < INIT_MAX_FAILURES) { // Collect 10 samples
+    int validSamples2 = 0;
+    int failedSamples2 = 0;
+    int saturatedSamples2 = 0;
+    while (validSamples2 < 10 && failedSamples2 < INITIALISATION_MAX_READ_FAILURES) { // Collect 10 samples
         i2cWrite(MAG_I2C_INSTANCE, MAG_ADDRESS, HMC58X3_R_MODE, 1);
-        delay(50);
+        delay(70);
         if (hmc5883lRead(magADC)) { // Get the raw values in case the scales have already been changed.
-            ++validSamples;
-            // Since the measurements are noisy, they should be averaged.
-            xyz_total[X] -= magADC[X];
-            xyz_total[Y] -= magADC[Y];
-            xyz_total[Z] -= magADC[Z];
             // Detect saturation.
             if (-4096 >= MIN(magADC[X], MIN(magADC[Y], magADC[Z]))) {
-                error_saturation = true;
-                bret = false;
-                break;              // Breaks out of the for loop.  No sense in continuing if we saturated.
+                ++saturatedSamples2;
+                ++failedSamples2;
+            } else {
+                ++validSamples2;
+                // Since the measurements are noisy, they should be averaged.
+                xyz_total[X] -= magADC[X];
+                xyz_total[Y] -= magADC[Y];
+                xyz_total[Z] -= magADC[Z];
             }
         } else {
-            ++failedSamples;
+            ++failedSamples2;
         }
         LED1_TOGGLE;
     }
 
-    if (failedSamples >= INIT_MAX_FAILURES) {
+    if (failedSamples1 >= INITIALISATION_MAX_READ_FAILURES || failedSamples2 >= INITIALISATION_MAX_READ_FAILURES) {
+        addBootlogEvent4(BOOT_EVENT_HMC5883L_READ_OK_COUNT, BOOT_EVENT_FLAGS_NONE, validSamples1, validSamples2);
+        addBootlogEvent4(BOOT_EVENT_HMC5883L_READ_FAILED, BOOT_EVENT_FLAGS_WARNING, failedSamples1, failedSamples2);
         bret = false;
     }
-
-    if (failedSamples > 0) {
-        error_read = true;
+    if (saturatedSamples1 > 0 || saturatedSamples2 > 0) {
+        addBootlogEvent4(BOOT_EVENT_HMC5883L_SATURATION, BOOT_EVENT_FLAGS_WARNING, saturatedSamples1, saturatedSamples2);
     }
 
     if (bret) {
@@ -285,14 +279,6 @@ bool hmc5883lInit(void)
 
     hmc5883lConfigureDataReadyInterruptHandling();
 
-    if (error_read) {
-        addBootlogEvent2(BOOT_EVENT_HMC5883L_READ_FAILED, BOOT_EVENT_FLAGS_WARNING);
-    }
-
-    if (error_saturation) {
-        addBootlogEvent2(BOOT_EVENT_HMC5883L_SATURATION, BOOT_EVENT_FLAGS_ERROR);
-    }
-
     return bret;
 }
 

src/main/drivers/logging.c

@@ -51,6 +51,7 @@ static const char *     eventDescription[BOOT_EVENT_CODE_COUNT] = {
     [BOOT_EVENT_MAG_DETECTION]              = "MAG_DETECTION",
     [BOOT_EVENT_RANGEFINDER_DETECTION]      = "RANGEFINDER_DETECTION",
     [BOOT_EVENT_MAG_INIT_FAILED]            = "MAG_INIT_FAILED",
+    [BOOT_EVENT_HMC5883L_READ_OK_COUNT]     = "HMC5883L_READ_OK_COUNT",
     [BOOT_EVENT_HMC5883L_READ_FAILED]       = "HMC5883L_READ_FAILED",
     [BOOT_EVENT_HMC5883L_SATURATION]        = "HMC5883L_SATURATION",
     [BOOT_EVENT_TIMER_CH_SKIPPED]           = "TIMER_CHANNEL_SKIPPED",

src/main/drivers/logging_codes.h

@@ -42,10 +42,11 @@ typedef enum {
     BOOT_EVENT_MAG_DETECTION            = 12,
     BOOT_EVENT_RANGEFINDER_DETECTION    = 13,
     BOOT_EVENT_MAG_INIT_FAILED          = 14,
-    BOOT_EVENT_HMC5883L_READ_FAILED     = 15,
-    BOOT_EVENT_HMC5883L_SATURATION      = 16,
-    BOOT_EVENT_TIMER_CH_SKIPPED         = 17,   // 1 - MAX_MOTORS exceeded, 2 - MAX_SERVOS exceeded, 3 - feature clash
-    BOOT_EVENT_TIMER_CH_MAPPED          = 18,   // 0 - PPM, 1 - PWM, 2 - MOTOR, 3 - SERVO
+    BOOT_EVENT_HMC5883L_READ_OK_COUNT   = 15,
+    BOOT_EVENT_HMC5883L_READ_FAILED     = 16,
+    BOOT_EVENT_HMC5883L_SATURATION      = 17,
+    BOOT_EVENT_TIMER_CH_SKIPPED         = 18,   // 1 - MAX_MOTORS exceeded, 2 - MAX_SERVOS exceeded, 3 - feature clash
+    BOOT_EVENT_TIMER_CH_MAPPED          = 19,   // 0 - PPM, 1 - PWM, 2 - MOTOR, 3 - SERVO
 
     BOOT_EVENT_CODE_COUNT
 } bootLogEventCode_e;