Add logging for raw sonar data

src/main/blackbox/blackbox.c

@@ -197,6 +197,9 @@ static const blackboxMainFieldDefinition_t blackboxMainFields[] = {
 #ifdef BARO
     {"BaroAlt",    -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_BARO},
 #endif
+#ifdef SONAR
+    {"sonarRaw",   -1, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(PREVIOUS),      .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_SONAR},
+#endif
 
     /* Gyros and accelerometers base their P-predictions on the average of the previous 2 frames to reduce noise impact */
     {"gyroData",   0, SIGNED,   .Ipredict = PREDICT(0),       .Iencode = ENCODING(SIGNED_VB),   .Ppredict = PREDICT(AVERAGE_2),     .Pencode = ENCODING(SIGNED_VB), CONDITION(ALWAYS)},
@@ -349,6 +352,13 @@ static bool testBlackboxConditionUncached(FlightLogFieldCondition condition)
         case FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC:
             return feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_ADC;
 
+        case FLIGHT_LOG_FIELD_CONDITION_SONAR:
+#ifdef SONAR
+            return feature(FEATURE_SONAR);
+#else
+            return false;
+#endif
+
         case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME:
             return masterConfig.blackbox_rate_num < masterConfig.blackbox_rate_denom;
 
@@ -468,6 +478,12 @@ static void writeIntraframe(void)
         }
 #endif
 
+#ifdef SONAR
+        if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SONAR)) {
+            blackboxWriteSignedVB(blackboxCurrent->sonarRaw);
+        }
+#endif
+
     for (x = 0; x < XYZ_AXIS_COUNT; x++) {
         blackboxWriteSignedVB(blackboxCurrent->gyroData[x]);
     }
@@ -550,7 +566,7 @@ static void writeInterframe(void)
 
     blackboxWriteTag8_4S16(deltas);
 
-    //Check for sensors that are updated periodically (so deltas are normally zero) VBAT, Amperage, MAG, BARO
+    //Check for sensors that are updated periodically (so deltas are normally zero)
     int optionalFieldCount = 0;
 
     if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
@@ -574,6 +590,13 @@ static void writeInterframe(void)
         deltas[optionalFieldCount++] = blackboxCurrent->BaroAlt - blackboxLast->BaroAlt;
     }
 #endif
+
+#ifdef SONAR
+    if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SONAR)) {
+        deltas[optionalFieldCount++] = blackboxCurrent->sonarRaw - blackboxLast->sonarRaw;
+    }
+#endif
+
     blackboxWriteTag8_8SVB(deltas, optionalFieldCount);
 
     //Since gyros, accs and motors are noisy, base the prediction on the average of the history:
@@ -775,6 +798,11 @@ static void loadBlackboxState(void)
     blackboxCurrent->BaroAlt = BaroAlt;
 #endif
 
+#ifdef SONAR
+    // Store the raw sonar value without applying tilt correction
+    blackboxCurrent->sonarRaw = sonarRead();
+#endif
+
 #ifdef USE_SERVOS
     //Tail servo for tricopters
     blackboxCurrent->servo[5] = servo[5];

src/main/blackbox/blackbox.h

@@ -43,6 +43,9 @@ typedef struct blackboxValues_t {
 #ifdef MAG
     int16_t magADC[XYZ_AXIS_COUNT];
 #endif
+#ifdef SONAR
+    int32_t sonarRaw;
+#endif
 } blackboxValues_t;
 
 void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data);

src/main/blackbox/blackbox_fielddefs.h

@@ -33,6 +33,7 @@ typedef enum FlightLogFieldCondition {
     FLIGHT_LOG_FIELD_CONDITION_BARO,
     FLIGHT_LOG_FIELD_CONDITION_VBAT,
     FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC,
+    FLIGHT_LOG_FIELD_CONDITION_SONAR,
 
     FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_0,
     FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1,

src/main/drivers/sonar_hcsr04.c

@@ -145,6 +145,10 @@ void hcsr04_start_reading(void)
     digitalLo(GPIOB, sonarHardware->trigger_pin);
 }
 
+/**
+ * Get the distance that was measured by the last pulse, in centimeters. When the ground is too far away to be
+ * reliably read by the sonar, -1 is returned instead.
+ */
 int32_t hcsr04_get_distance(void)
 {
     // The speed of sound is 340 m/s or approx. 29 microseconds per centimeter.

src/main/sensors/sonar.c

@@ -94,11 +94,20 @@ void sonarUpdate(void)
     hcsr04_start_reading();
 }
 
+/**
+ * Get the last distance measured by the sonar in centimeters. When the ground is too far away, -1 is returned instead.
+ */
 int32_t sonarRead(void)
 {
     return hcsr04_get_distance();
 }
 
+/**
+ * Apply tilt correction to the given raw sonar reading in order to compensate for the tilt of the craft when estimating
+ * the altitude. Returns the computed altitude in centimeters.
+ *
+ * When the ground is too far away or the tilt is too strong, -1 is returned instead.
+ */
 int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle)
 {
     // calculate sonar altitude only if the sonar is facing downwards(<25deg)
@@ -110,6 +119,10 @@ int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle)
     return calculatedAltitude;
 }
 
+/**
+ * Get the latest altitude that was computed by a call to sonarCalculateAltitude(), or -1 if sonarCalculateAltitude
+ * has never been called.
+ */
 int32_t sonarGetLatestAltitude(void)
 {
     return calculatedAltitude;