Add logging for raw sonar data
This commit is contained in:
parent
948d39a20c
commit
e6a40d732d
|
@ -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];
|
||||
|
|
|
@ -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);
|
||||
|
|
|
@ -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,
|
||||
|
|
|
@ -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.
|
||||
|
|
|
@ -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;
|
||||
|
|
Loading…
Reference in New Issue