From cdf28ee2913d2e7dfc0ce8dccd75e5e1409537ec Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 27 Aug 2021 09:01:55 +0200 Subject: [PATCH] Add FF component to BB logs --- src/main/blackbox/blackbox.c | 8 ++++++++ src/main/build/debug.h | 2 -- src/main/fc/config.c | 2 +- src/main/fc/settings.yaml | 2 +- src/main/flight/pid.c | 10 +++++++--- src/main/flight/pid.h | 2 +- 6 files changed, 18 insertions(+), 8 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index b90580294b..7a4b11870b 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -214,6 +214,9 @@ static const blackboxDeltaFieldDefinition_t blackboxMainFields[] = { {"axisD", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_0)}, {"axisD", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_1)}, {"axisD", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(NONZERO_PID_D_2)}, + {"axisF", 0, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ALWAYS}, + {"axisF", 1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ALWAYS}, + {"axisF", 2, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), FLIGHT_LOG_FIELD_CONDITION_ALWAYS}, {"fwAltP", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)}, {"fwAltI", -1, SIGNED, .Ipredict = PREDICT(0), .Iencode = ENCODING(SIGNED_VB), .Ppredict = PREDICT(PREVIOUS), .Pencode = ENCODING(SIGNED_VB), CONDITION(FIXED_WING_NAV)}, @@ -435,6 +438,7 @@ typedef struct blackboxMainState_s { int32_t axisPID_P[XYZ_AXIS_COUNT]; int32_t axisPID_I[XYZ_AXIS_COUNT]; int32_t axisPID_D[XYZ_AXIS_COUNT]; + int32_t axisPID_F[XYZ_AXIS_COUNT]; int32_t axisPID_Setpoint[XYZ_AXIS_COUNT]; int32_t mcPosAxisP[XYZ_AXIS_COUNT]; @@ -757,6 +761,7 @@ static void writeIntraframe(void) blackboxWriteSignedVB(blackboxCurrent->axisPID_D[x]); } } + blackboxWriteSignedVBArray(blackboxCurrent->axisPID_F, XYZ_AXIS_COUNT); if (testBlackboxCondition(CONDITION(FIXED_WING_NAV))) { blackboxWriteSignedVBArray(blackboxCurrent->fwAltPID, 3); @@ -978,6 +983,8 @@ static void writeInterframe(void) } } + blackboxWriteSignedVBArray(blackboxCurrent->axisPID_F, XYZ_AXIS_COUNT); + if (testBlackboxCondition(CONDITION(FIXED_WING_NAV))) { arraySubInt32(deltas, blackboxCurrent->fwAltPID, blackboxLast->fwAltPID, 3); @@ -1445,6 +1452,7 @@ static void loadMainState(timeUs_t currentTimeUs) blackboxCurrent->axisPID_P[i] = axisPID_P[i]; blackboxCurrent->axisPID_I[i] = axisPID_I[i]; blackboxCurrent->axisPID_D[i] = axisPID_D[i]; + blackboxCurrent->axisPID_F[i] = axisPID_F[i]; blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]); blackboxCurrent->accADC[i] = lrintf(acc.accADCf[i] * acc.dev.acc_1G); #ifdef USE_MAG diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 197532337d..9fd78f251d 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -72,7 +72,6 @@ typedef enum { DEBUG_DYNAMIC_FILTER, DEBUG_DYNAMIC_FILTER_FREQUENCY, DEBUG_IRLOCK, - DEBUG_CD, DEBUG_KALMAN_GAIN, DEBUG_PID_MEASUREMENT, DEBUG_SPM_CELLS, // Smartport master FLVSS @@ -81,7 +80,6 @@ typedef enum { DEBUG_PCF8574, DEBUG_DYNAMIC_GYRO_LPF, DEBUG_AUTOLEVEL, - DEBUG_FW_D, DEBUG_IMU2, DEBUG_ALTITUDE, DEBUG_GYRO_ALPHA_BETA_GAMMA, diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 2bea10ec0f..a7e9bd6eb3 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -107,7 +107,7 @@ PG_RESET_TEMPLATE(featureConfig_t, featureConfig, .enabledFeatures = DEFAULT_FEATURES | COMMON_DEFAULT_FEATURES ); -PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 3); +PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 4); PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .current_profile_index = 0, diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index b0ee6f16c8..479f2022bc 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -93,7 +93,7 @@ tables: "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY", - "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "FW_D", "IMU2", "ALTITUDE", + "IRLOCK", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "AUTOLEVEL", "IMU2", "ALTITUDE", "GYRO_ALPHA_BETA_GAMMA", "SMITH_PREDICTOR", "AUTOTRIM", "AUTOTUNE", "RATE_DYNAMICS"] - name: async_mode values: ["NONE", "GYRO", "ALL"] diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3a26bbac00..a32de2ce07 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -123,7 +123,11 @@ STATIC_FASTRAM bool pidGainsUpdateRequired; FASTRAM int16_t axisPID[FLIGHT_DYNAMICS_INDEX_COUNT]; #ifdef USE_BLACKBOX -int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_I[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_D[FLIGHT_DYNAMICS_INDEX_COUNT], axisPID_Setpoint[FLIGHT_DYNAMICS_INDEX_COUNT]; +int32_t axisPID_P[FLIGHT_DYNAMICS_INDEX_COUNT]; +int32_t axisPID_I[FLIGHT_DYNAMICS_INDEX_COUNT]; +int32_t axisPID_D[FLIGHT_DYNAMICS_INDEX_COUNT]; +int32_t axisPID_F[FLIGHT_DYNAMICS_INDEX_COUNT]; +int32_t axisPID_Setpoint[FLIGHT_DYNAMICS_INDEX_COUNT]; #endif static EXTENDED_FASTRAM pidState_t pidState[FLIGHT_DYNAMICS_INDEX_COUNT]; @@ -747,7 +751,6 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh const float newDTerm = dTermProcess(pidState, dT); const float newFFTerm = pidState->rateTarget * pidState->kFF; - DEBUG_SET(DEBUG_FW_D, axis, newDTerm); /* * Integral should be updated only if axis Iterm is not frozen */ @@ -773,6 +776,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh axisPID_P[axis] = newPTerm; axisPID_I[axis] = pidState->errorGyroIf; axisPID_D[axis] = newDTerm; + axisPID_F[axis] = newFFTerm; axisPID_Setpoint[axis] = pidState->rateTarget; #endif @@ -809,7 +813,6 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid * Compute Control Derivative */ const float newCDTerm = rateTargetDeltaFiltered * (pidState->kCD / dT); - DEBUG_SET(DEBUG_CD, axis, newCDTerm); // TODO: Get feedback from mixer on available correction range for each axis const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm; @@ -833,6 +836,7 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid axisPID_P[axis] = newPTerm; axisPID_I[axis] = pidState->errorGyroIf; axisPID_D[axis] = newDTerm; + axisPID_F[axis] = newCDTerm; axisPID_Setpoint[axis] = pidState->rateTarget; #endif diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index f547bb5626..8520ff5d0d 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -189,7 +189,7 @@ const pidBank_t * pidBank(void); pidBank_t * pidBankMutable(void); extern int16_t axisPID[]; -extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_Setpoint[]; +extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_F[], axisPID_Setpoint[]; void pidInit(void); bool pidInitFilters(void);