From 99685c21c62fdf7ce23ac3d9ce6dea995cbadef4 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Tue, 22 Jan 2019 16:21:52 -0500 Subject: [PATCH] Add setpoint to blackbox logging Currently only rcCommand values are included in the log data and the configurator calculates the actual setpoint values based on rates values added to the blackbox header. The problem with this is that the rates information is only written at arming so if the rates change during the log (rateprofile change, in-flight adjustments, etc.) then the calculated setpoints will be incorrect. There's no way to tell from the log that this happened. This often causes confusion because it will suddenly make it appear in the log that the PID controller is not acheiving the requested rates when it's just a presentation error. Also the rates will be incorrectly calculated when the user selects Raceflight style rates as the rates type is not supplied in the log header (and the viewer doesn't have the forumla for them anyway). This change adds the actual setpoint values for each axis as used by the PID controller, removing the necessity for the viewer to perform any calculations. In addition to showing any rate changes, it will also show any cases where other flight features have modified the setpoints from the user's input. These were invisible previously (examples include level modes, Acro Trainer, GPS Rescue, yaw spin recovery, etc.). Also the throttle value used in the mixer is included in the throttle axis. This allow visualization of things that affect the commanded throttle like throttle boost, throttle limit, GPS Rescue, angle level strength, etc. --- src/main/blackbox/blackbox.c | 21 +++++++++++++++++++++ src/main/flight/mixer.c | 7 +++++++ src/main/flight/mixer.h | 1 + src/main/flight/pid.c | 8 +++++++- src/main/flight/pid.h | 2 ++ 5 files changed, 38 insertions(+), 1 deletion(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 2ac8a360eb..9109cbfbff 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -192,6 +192,12 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { /* Throttle is always in the range [minthrottle..maxthrottle]: */ {"rcCommand", 3, UNSIGNED, .Ipredict = PREDICT(MINTHROTTLE), .Iencode = ENCODING(UNSIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)}, + // setpoint - define 4 fields like rcCommand to use the same encoding. setpoint[4] contains the mixer throttle + {"setpoint", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)}, + {"setpoint", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)}, + {"setpoint", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)}, + {"setpoint", 3, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_4S16), CONDITION(ALWAYS)}, + {"vbatLatest", -1, UNSIGNED, .Ipredict = PREDICT(VBATREF), .Iencode = ENCODING(NEG_14BIT), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_VBAT}, {"amperageLatest",-1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(TAG8_8SVB), FLIGHT_LOG_FIELD_CONDITION_AMPERAGE_ADC}, @@ -291,6 +297,7 @@ typedef struct blackboxMainState_s { int32_t axisPID_F[XYZ_AXIS_COUNT]; int16_t rcCommand[4]; + int16_t setpoint[4]; int16_t gyroADC[XYZ_AXIS_COUNT]; int16_t accADC[XYZ_AXIS_COUNT]; int16_t debug[DEBUG16_VALUE_COUNT]; @@ -546,6 +553,9 @@ static void writeIntraframe(void) */ blackboxWriteUnsignedVB(blackboxCurrent->rcCommand[THROTTLE] - motorConfig()->minthrottle); + // Write setpoint roll, pitch, yaw, and throttle + blackboxWriteSigned16VBArray(blackboxCurrent->setpoint, 4); + if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { /* * Our voltage is expected to decrease over the course of the flight, so store our difference from @@ -648,6 +658,8 @@ static void writeInterframe(void) blackboxWriteSignedVB((int32_t) (blackboxHistory[0]->time - 2 * blackboxHistory[1]->time + blackboxHistory[2]->time)); int32_t deltas[8]; + int32_t setpointDeltas[4]; + arraySubInt32(deltas, blackboxCurrent->axisPID_P, blackboxLast->axisPID_P, XYZ_AXIS_COUNT); blackboxWriteSignedVBArray(deltas, XYZ_AXIS_COUNT); @@ -677,9 +689,11 @@ static void writeInterframe(void) */ for (int x = 0; x < 4; x++) { deltas[x] = blackboxCurrent->rcCommand[x] - blackboxLast->rcCommand[x]; + setpointDeltas[x] = blackboxCurrent->setpoint[x] - blackboxLast->setpoint[x]; } blackboxWriteTag8_4S16(deltas); + blackboxWriteTag8_4S16(setpointDeltas); //Check for sensors that are updated periodically (so deltas are normally zero) int optionalFieldCount = 0; @@ -1013,6 +1027,13 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->rcCommand[i] = rcCommand[i]; } + // log the currentPidSetpoint values applied to the PID controller + for (int i = 0; i < XYZ_AXIS_COUNT; i++) { + blackboxCurrent->setpoint[i] = lrintf(pidGetPreviousSetpoint(i)); + } + // log the final throttle value used in the mixer + blackboxCurrent->setpoint[3] = lrintf(mixerGetLoggingThrottle() * 1000); + for (int i = 0; i < DEBUG16_VALUE_COUNT; i++) { blackboxCurrent->debug[i] = debug[i]; } diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 6571dfd76e..4c820b405b 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -539,6 +539,7 @@ void stopPwmAllMotors(void) } static FAST_RAM_ZERO_INIT float throttle = 0; +static FAST_RAM_ZERO_INIT float loggingThrottle = 0; static FAST_RAM_ZERO_INIT float motorOutputMin; static FAST_RAM_ZERO_INIT float motorRangeMin; static FAST_RAM_ZERO_INIT float motorRangeMax; @@ -915,6 +916,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa } #endif + loggingThrottle = throttle; motorMixRange = motorMixMax - motorMixMin; if (motorMixRange > 1.0f) { for (int i = 0; i < motorCount; i++) { @@ -1007,3 +1009,8 @@ void mixerSetThrottleAngleCorrection(int correctionValue) { throttleAngleCorrection = correctionValue; } + +float mixerGetLoggingThrottle(void) +{ + return loggingThrottle; +} diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index d0acff7158..6cbce457ca 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -131,4 +131,5 @@ uint16_t convertMotorToExternal(float motorValue); bool mixerIsTricopter(void); void mixerSetThrottleAngleCorrection(int correctionValue); +float mixerGetLoggingThrottle(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0025aa757f..613816af47 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -242,6 +242,8 @@ typedef union dtermLowpass_u { biquadFilter_t biquadFilter; } dtermLowpass_t; +static FAST_RAM_ZERO_INIT float previousPidSetpoint[XYZ_AXIS_COUNT]; + static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermNotchApplyFn; static FAST_RAM_ZERO_INIT biquadFilter_t dtermNotch[XYZ_AXIS_COUNT]; static FAST_RAM_ZERO_INIT filterApplyFnPtr dtermLowpassApplyFn; @@ -1151,7 +1153,6 @@ static float applyLaunchControl(int axis, const rollAndPitchTrims_t *angleTrim) void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs) { static float previousGyroRateDterm[XYZ_AXIS_COUNT]; - static float previousPidSetpoint[XYZ_AXIS_COUNT]; static timeUs_t levelModeStartTimeUs = 0; static bool gpsRescuePreviousState = false; @@ -1458,3 +1459,8 @@ void pidSetItermReset(bool enabled) { zeroThrottleItermReset = enabled; } + +float pidGetPreviousSetpoint(int axis) +{ + return previousPidSetpoint[axis]; +} diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index c9881fad22..9335d68ae3 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -233,3 +233,5 @@ float calcHorizonLevelStrength(void); #endif void dynLpfDTermUpdate(float throttle); void pidSetItermReset(bool enabled); +float pidGetPreviousSetpoint(int axis); +