From e6a40d732d8f303599b56f2848f3d262d3a7218b Mon Sep 17 00:00:00 2001 From: Nicholas Sherlock Date: Tue, 12 May 2015 18:37:56 +1200 Subject: [PATCH] Add logging for raw sonar data --- src/main/blackbox/blackbox.c | 30 +++++++++++++++++++++++++- src/main/blackbox/blackbox.h | 3 +++ src/main/blackbox/blackbox_fielddefs.h | 1 + src/main/drivers/sonar_hcsr04.c | 4 ++++ src/main/sensors/sonar.c | 13 +++++++++++ 5 files changed, 50 insertions(+), 1 deletion(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 4dbf02686..6e7153cd8 100644 --- a/src/main/blackbox/blackbox.c +++ b/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]; diff --git a/src/main/blackbox/blackbox.h b/src/main/blackbox/blackbox.h index 59085249e..fe6954713 100644 --- a/src/main/blackbox/blackbox.h +++ b/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); diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 23f563c6a..9b1ad46eb 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/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, diff --git a/src/main/drivers/sonar_hcsr04.c b/src/main/drivers/sonar_hcsr04.c index 2ab0c001d..13216ee20 100644 --- a/src/main/drivers/sonar_hcsr04.c +++ b/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. diff --git a/src/main/sensors/sonar.c b/src/main/sensors/sonar.c index 8660bcc1c..cb7643224 100644 --- a/src/main/sensors/sonar.c +++ b/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;