diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 211e25911c..49677e062a 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -354,6 +354,12 @@ static void writeSignedVB(int32_t value) writeUnsignedVB((uint32_t)((value << 1) ^ (value >> 31))); } +static void writeS16(int16_t value) +{ + blackboxWrite(value & 0xFF); + blackboxWrite((value >> 8) & 0xFF); +} + /** * Write a 2 bit tag followed by 3 signed fields of 2, 4, 6 or 32 bits */ @@ -1270,17 +1276,24 @@ void blackboxLogEvent(FlightLogEvent event, flightLogEventData_t *data) break; case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START: blackboxWrite(data->autotuneCycleStart.phase); - blackboxWrite(data->autotuneCycleStart.cycle); + blackboxWrite(data->autotuneCycleStart.cycle | (data->autotuneCycleStart.rising ? 0x80 : 0)); blackboxWrite(data->autotuneCycleStart.p); blackboxWrite(data->autotuneCycleStart.i); blackboxWrite(data->autotuneCycleStart.d); break; case FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT: - blackboxWrite(data->autotuneCycleResult.overshot); + blackboxWrite(data->autotuneCycleResult.flags); blackboxWrite(data->autotuneCycleStart.p); blackboxWrite(data->autotuneCycleStart.i); blackboxWrite(data->autotuneCycleStart.d); break; + case FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS: + writeS16(data->autotuneTargets.currentAngle); + blackboxWrite((uint8_t) data->autotuneTargets.targetAngle); + blackboxWrite((uint8_t) data->autotuneTargets.targetAngleAtPeak); + writeS16(data->autotuneTargets.firstPeakAngle); + writeS16(data->autotuneTargets.secondPeakAngle); + break; case FLIGHT_LOG_EVENT_LOG_END: blackboxPrint("End of log"); blackboxWrite(0); diff --git a/src/main/blackbox/blackbox_fielddefs.h b/src/main/blackbox/blackbox_fielddefs.h index 26d862268f..d328e71e2c 100644 --- a/src/main/blackbox/blackbox_fielddefs.h +++ b/src/main/blackbox/blackbox_fielddefs.h @@ -101,6 +101,7 @@ typedef enum FlightLogEvent { FLIGHT_LOG_EVENT_SYNC_BEEP = 0, FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START = 10, FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_RESULT = 11, + FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS = 12, FLIGHT_LOG_EVENT_LOG_END = 255 } FlightLogEvent; @@ -114,21 +115,31 @@ typedef struct flightLogEvent_autotuneCycleStart_t { uint8_t p; uint8_t i; uint8_t d; + uint8_t rising; } flightLogEvent_autotuneCycleStart_t; +#define FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT 1 +#define FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_TIMEDOUT 2 + typedef struct flightLogEvent_autotuneCycleResult_t { - uint8_t overshot; + uint8_t flags; uint8_t p; uint8_t i; uint8_t d; } flightLogEvent_autotuneCycleResult_t; +typedef struct flightLogEvent_autotuneTargets_t { + uint16_t currentAngle; + int8_t targetAngle, targetAngleAtPeak; + uint16_t firstPeakAngle, secondPeakAngle; +} flightLogEvent_autotuneTargets_t; + typedef union flightLogEventData_t { flightLogEvent_syncBeep_t syncBeep; flightLogEvent_autotuneCycleStart_t autotuneCycleStart; flightLogEvent_autotuneCycleResult_t autotuneCycleResult; - + flightLogEvent_autotuneTargets_t autotuneTargets; } flightLogEventData_t; typedef struct flightLogEvent_t diff --git a/src/main/flight/autotune.c b/src/main/flight/autotune.c index b36168e6bf..04d8e6ad53 100644 --- a/src/main/flight/autotune.c +++ b/src/main/flight/autotune.c @@ -161,11 +161,32 @@ static void autotuneLogCycleStart() eventData.p = pid.p * MULTIWII_P_MULTIPLIER; eventData.i = pid.i * MULTIWII_I_MULTIPLIER; eventData.d = pid.d; + eventData.rising = rising ? 1 : 0; blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_CYCLE_START, (flightLogEventData_t*)&eventData); } } +static void autotuneLogAngleTargets(float currentAngle) +{ + if (feature(FEATURE_BLACKBOX)) { + flightLogEvent_autotuneTargets_t eventData; + + // targetAngle is always just -AUTOTUNE_TARGET_ANGLE or +AUTOTUNE_TARGET_ANGLE so no need for float precision: + eventData.targetAngle = (int) targetAngle; + // and targetAngleAtPeak is set to targetAngle so it has the same small precision requirement: + eventData.targetAngleAtPeak = (int) targetAngleAtPeak; + + // currentAngle is integer decidegrees divided by 10, so just reverse that process to get an integer again: + eventData.currentAngle = round(currentAngle * 10); + // the peak angles are only ever set to currentAngle, so they get the same treatment: + eventData.firstPeakAngle = round(firstPeakAngle * 10); + eventData.secondPeakAngle = round(secondPeakAngle * 10); + + blackboxLogEvent(FLIGHT_LOG_EVENT_AUTOTUNE_TARGETS, (flightLogEventData_t*)&eventData); + } +} + #endif static void startNewCycle(void) @@ -199,6 +220,7 @@ static void updateTargetAngle(void) float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclination, float errorAngle) { float currentAngle; + bool overshot; if (!(phase == PHASE_TUNE_ROLL || phase == PHASE_TUNE_PITCH) || autoTuneAngleIndex != angleIndex) { return errorAngle; @@ -229,6 +251,10 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin debug[2] = DEGREES_TO_DECIDEGREES(targetAngle); #endif +#ifdef BLACKBOX + autotuneLogAngleTargets(currentAngle); +#endif + if (secondPeakAngle == 0) { // The peak will be when our angular velocity is negative. To be sure we are in the right place, // we also check to make sure our angle position is greater than zero. @@ -244,20 +270,22 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin switch (cycle) { case CYCLE_TUNE_I: // when checking the I value, we would like to overshoot the target position by half of the max oscillation. - if (currentAngle - targetAngle < AUTOTUNE_MAX_OSCILLATION_ANGLE / 2) { - pid.i *= AUTOTUNE_INCREASE_MULTIPLIER; - } else { + overshot = currentAngle - targetAngle >= AUTOTUNE_MAX_OSCILLATION_ANGLE / 2; + + if (overshot) { pid.i *= AUTOTUNE_DECREASE_MULTIPLIER; if (pid.i < AUTOTUNE_MINIMUM_I_VALUE) { pid.i = AUTOTUNE_MINIMUM_I_VALUE; } + } else { + pid.i *= AUTOTUNE_INCREASE_MULTIPLIER; } #ifdef BLACKBOX if (feature(FEATURE_BLACKBOX)) { flightLogEvent_autotuneCycleResult_t eventData; - eventData.overshot = currentAngle - targetAngle < AUTOTUNE_MAX_OSCILLATION_ANGLE / 2 ? 0 : 1; + eventData.flags = overshot ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT: 0; eventData.p = pidProfile->P8[pidIndex]; eventData.i = pidProfile->I8[pidIndex]; eventData.d = pidProfile->D8[pidIndex]; @@ -300,7 +328,7 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin // analyze the data // Our goal is to have zero overshoot and to have AUTOTUNE_MAX_OSCILLATION_ANGLE amplitude - bool overshot = firstPeakAngle > targetAngleAtPeak; + overshot = firstPeakAngle > targetAngleAtPeak; if (overshot) { #ifdef DEBUG_AUTOTUNE debug[0] = 1; @@ -338,7 +366,7 @@ float autotune(angle_index_t angleIndex, const rollAndPitchInclination_t *inclin if (feature(FEATURE_BLACKBOX)) { flightLogEvent_autotuneCycleResult_t eventData; - eventData.overshot = overshot; + eventData.flags = (overshot ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_OVERSHOT : 0) | (timedOut ? FLIGHT_LOG_EVENT_AUTOTUNE_FLAG_TIMEDOUT : 0); eventData.p = pidProfile->P8[pidIndex]; eventData.i = pidProfile->I8[pidIndex]; eventData.d = pidProfile->D8[pidIndex];