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
|
#ifdef BARO
|
||||||
{"BaroAlt", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_BARO},
|
{"BaroAlt", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_BARO},
|
||||||
#endif
|
#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 */
|
/* 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)},
|
{"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:
|
case FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC:
|
||||||
return feature(FEATURE_CURRENT_METER) && masterConfig.batteryConfig.currentMeterType == CURRENT_SENSOR_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:
|
case FLIGHT_LOG_FIELD_CONDITION_NOT_LOGGING_EVERY_FRAME:
|
||||||
return masterConfig.blackbox_rate_num < masterConfig.blackbox_rate_denom;
|
return masterConfig.blackbox_rate_num < masterConfig.blackbox_rate_denom;
|
||||||
|
|
||||||
|
@ -468,6 +478,12 @@ static void writeIntraframe(void)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifdef SONAR
|
||||||
|
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SONAR)) {
|
||||||
|
blackboxWriteSignedVB(blackboxCurrent->sonarRaw);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
for (x = 0; x < XYZ_AXIS_COUNT; x++) {
|
for (x = 0; x < XYZ_AXIS_COUNT; x++) {
|
||||||
blackboxWriteSignedVB(blackboxCurrent->gyroData[x]);
|
blackboxWriteSignedVB(blackboxCurrent->gyroData[x]);
|
||||||
}
|
}
|
||||||
|
@ -550,7 +566,7 @@ static void writeInterframe(void)
|
||||||
|
|
||||||
blackboxWriteTag8_4S16(deltas);
|
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;
|
int optionalFieldCount = 0;
|
||||||
|
|
||||||
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
|
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) {
|
||||||
|
@ -574,6 +590,13 @@ static void writeInterframe(void)
|
||||||
deltas[optionalFieldCount++] = blackboxCurrent->BaroAlt - blackboxLast->BaroAlt;
|
deltas[optionalFieldCount++] = blackboxCurrent->BaroAlt - blackboxLast->BaroAlt;
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifdef SONAR
|
||||||
|
if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_SONAR)) {
|
||||||
|
deltas[optionalFieldCount++] = blackboxCurrent->sonarRaw - blackboxLast->sonarRaw;
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
blackboxWriteTag8_8SVB(deltas, optionalFieldCount);
|
blackboxWriteTag8_8SVB(deltas, optionalFieldCount);
|
||||||
|
|
||||||
//Since gyros, accs and motors are noisy, base the prediction on the average of the history:
|
//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;
|
blackboxCurrent->BaroAlt = BaroAlt;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifdef SONAR
|
||||||
|
// Store the raw sonar value without applying tilt correction
|
||||||
|
blackboxCurrent->sonarRaw = sonarRead();
|
||||||
|
#endif
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
#ifdef USE_SERVOS
|
||||||
//Tail servo for tricopters
|
//Tail servo for tricopters
|
||||||
blackboxCurrent->servo[5] = servo[5];
|
blackboxCurrent->servo[5] = servo[5];
|
||||||
|
|
|
@ -43,6 +43,9 @@ typedef struct blackboxValues_t {
|
||||||
#ifdef MAG
|
#ifdef MAG
|
||||||
int16_t magADC[XYZ_AXIS_COUNT];
|
int16_t magADC[XYZ_AXIS_COUNT];
|
||||||
#endif
|
#endif
|
||||||
|
#ifdef SONAR
|
||||||
|
int32_t sonarRaw;
|
||||||
|
#endif
|
||||||
} blackboxValues_t;
|
} blackboxValues_t;
|
||||||
|
|
||||||
void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data);
|
void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data);
|
||||||
|
|
|
@ -33,6 +33,7 @@ typedef enum FlightLogFieldCondition {
|
||||||
FLIGHT_LOG_FIELD_CONDITION_BARO,
|
FLIGHT_LOG_FIELD_CONDITION_BARO,
|
||||||
FLIGHT_LOG_FIELD_CONDITION_VBAT,
|
FLIGHT_LOG_FIELD_CONDITION_VBAT,
|
||||||
FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC,
|
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_0,
|
||||||
FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1,
|
FLIGHT_LOG_FIELD_CONDITION_NONZERO_PID_D_1,
|
||||||
|
|
|
@ -145,6 +145,10 @@ void hcsr04_start_reading(void)
|
||||||
digitalLo(GPIOB, sonarHardware->trigger_pin);
|
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)
|
int32_t hcsr04_get_distance(void)
|
||||||
{
|
{
|
||||||
// The speed of sound is 340 m/s or approx. 29 microseconds per centimeter.
|
// The speed of sound is 340 m/s or approx. 29 microseconds per centimeter.
|
||||||
|
|
|
@ -94,11 +94,20 @@ void sonarUpdate(void)
|
||||||
hcsr04_start_reading();
|
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)
|
int32_t sonarRead(void)
|
||||||
{
|
{
|
||||||
return hcsr04_get_distance();
|
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)
|
int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle)
|
||||||
{
|
{
|
||||||
// calculate sonar altitude only if the sonar is facing downwards(<25deg)
|
// 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;
|
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)
|
int32_t sonarGetLatestAltitude(void)
|
||||||
{
|
{
|
||||||
return calculatedAltitude;
|
return calculatedAltitude;
|
||||||
|
|
Loading…
Reference in New Issue