From a53b1c061947b2624e58df5e29847163b252f89c Mon Sep 17 00:00:00 2001 From: Andi Kanzler Date: Sat, 8 Aug 2020 22:42:03 +0200 Subject: [PATCH] Add FF to inflight adjustments --- docs/Inflight Adjustments.md | 58 +++++++++++++++++----------------- src/main/fc/rc_adjustments.c | 24 +++++++------- src/main/fc/rc_adjustments.h | 8 ++--- src/main/flight/pid.c | 9 ++++++ src/main/flight/pid.h | 1 + src/main/flight/pid_autotune.c | 6 ++-- src/main/io/osd.c | 8 ++--- 7 files changed, 62 insertions(+), 52 deletions(-) diff --git a/docs/Inflight Adjustments.md b/docs/Inflight Adjustments.md index 7182d9dc38..fee4046954 100644 --- a/docs/Inflight Adjustments.md +++ b/docs/Inflight Adjustments.md @@ -113,45 +113,45 @@ this reason ensure that you define enough ranges to cover the range channel's us | 8 | PITCH_ROLL_D | | 9 | YAW_P | | 10 | YAW_I | -| 11 | YAW_D | +| 11 | YAW_D_FF | | 12 | RATE_PROFILE | Switch between 3 rate profiles using a 3 position switch. | | 13 | PITCH_RATE | | 14 | ROLL_RATE | | 15 | PITCH_P | | 16 | PITCH_I | -| 17 | PITCH_D | +| 17 | PITCH_D_FF | | 18 | ROLL_P | | 19 | ROLL_I | -| 20 | RC_YAW_EXPO | -| 21 | MANUAL_RC_EXPO | -| 22 | MANUAL_RC_YAW_EXPO | -| 23 | MANUAL_PITCH_ROLL_RATE | -| 24 | MANUAL_ROLL_RATE | -| 25 | MANUAL_PITCH_RATE | -| 26 | MANUAL_YAW_RATE | -| 27 | NAV_FW_CRUISE_THROTTLE | -| 28 | NAV_FW_PITCH2THR | -| 29 | ROLL_BOARD_ALIGNMENT | -| 30 | PITCH_BOARD_ALIGNMENT | -| 31 | LEVEL_P | -| 32 | LEVEL_I | -| 33 | LEVEL_D | -| 34 | POS_XY_P | -| 35 | POS_XY_I | -| 36 | POS_XY_D | -| 37 | POS_Z_P | -| 38 | POS_Z_I | -| 39 | POS_Z_D | -| 40 | HEADING_P | -| 41 | VEL_XY_P | -| 42 | VEL_XY_I | -| 43 | VEL_XY_D | -| 44 | VEL_Z_P | +| 20 | ROLL_D_FF | +| 21 | RC_YAW_EXPO | +| 22 | MANUAL_RC_EXPO | +| 23 | MANUAL_RC_YAW_EXPO | +| 24 | MANUAL_PITCH_ROLL_RATE | +| 25 | MANUAL_ROLL_RATE | +| 26 | MANUAL_PITCH_RATE | +| 27 | MANUAL_YAW_RATE | +| 28 | NAV_FW_CRUISE_THROTTLE | +| 29 | NAV_FW_PITCH2THR | +| 30 | ROLL_BOARD_ALIGNMENT | +| 31 | PITCH_BOARD_ALIGNMENT | +| 32 | LEVEL_P | +| 33 | LEVEL_I | +| 34 | LEVEL_D | +| 35 | POS_XY_P | +| 36 | POS_XY_I | +| 37 | POS_XY_D | +| 38 | POS_Z_P | +| 39 | POS_Z_I | +| 40 | POS_Z_D | +| 41 | HEADING_P | +| 42 | VEL_XY_P | +| 43 | VEL_XY_I | +| 44 | VEL_XY_D | | 45 | VEL_Z_P | | 46 | VEL_Z_I | | 47 | VEL_Z_D | | 48 | FW_MIN_THROTTLE_DOWN_PITCH_ANGLE | -| 49 | PROFILE | Switch between 3 rate profiles using a 3 position switch. | +| 49 | ADJUSTMENT_VTX_POWER_LEVEL | ## Examples @@ -263,4 +263,4 @@ The following examples shows __incorrect__ configurations - the entire usable ra In the following example, the incorrect configuraton (above) has been corrected by adding a range that makes 'No changes'. -![Configurator example 7](Screenshots/adjustments-incorrect-config-2-corrected.png) +![Configurator example 7](Screenshots/adjustments-incorrect-config-2-corrected.png) \ No newline at end of file diff --git a/src/main/fc/rc_adjustments.c b/src/main/fc/rc_adjustments.c index bf07c3d6f3..182653a506 100644 --- a/src/main/fc/rc_adjustments.c +++ b/src/main/fc/rc_adjustments.c @@ -101,7 +101,7 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D, + .adjustmentFunction = ADJUSTMENT_PITCH_ROLL_D_FF, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { @@ -113,7 +113,7 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_YAW_D, + .adjustmentFunction = ADJUSTMENT_YAW_D_FF, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { @@ -137,7 +137,7 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_PITCH_D, + .adjustmentFunction = ADJUSTMENT_PITCH_D_FF, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { @@ -149,7 +149,7 @@ static const adjustmentConfig_t defaultAdjustmentConfigs[ADJUSTMENT_FUNCTION_COU .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { - .adjustmentFunction = ADJUSTMENT_ROLL_D, + .adjustmentFunction = ADJUSTMENT_ROLL_D_FF, .mode = ADJUSTMENT_MODE_STEP, .data = { .stepConfig = { .step = 1 }} }, { @@ -441,18 +441,18 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t applyAdjustmentPID(ADJUSTMENT_ROLL_I, &pidBankMutable()->pid[PID_ROLL].I, delta); schedulePidGainsUpdate(); break; - case ADJUSTMENT_PITCH_ROLL_D: - case ADJUSTMENT_PITCH_D: - applyAdjustmentPID(ADJUSTMENT_PITCH_D, &pidBankMutable()->pid[PID_PITCH].D, delta); - if (adjustmentFunction == ADJUSTMENT_PITCH_D) { + case ADJUSTMENT_PITCH_ROLL_D_FF: + case ADJUSTMENT_PITCH_D_FF: + applyAdjustmentPID(ADJUSTMENT_PITCH_D_FF, getD_FFRefByBank(pidBankMutable(), PID_PITCH), delta); + if (adjustmentFunction == ADJUSTMENT_PITCH_D_FF) { schedulePidGainsUpdate(); break; } // follow though for combined ADJUSTMENT_PITCH_ROLL_D FALLTHROUGH; - case ADJUSTMENT_ROLL_D: - applyAdjustmentPID(ADJUSTMENT_ROLL_D, &pidBankMutable()->pid[PID_ROLL].D, delta); + case ADJUSTMENT_ROLL_D_FF: + applyAdjustmentPID(ADJUSTMENT_ROLL_D_FF, getD_FFRefByBank(pidBankMutable(), PID_ROLL), delta); schedulePidGainsUpdate(); break; case ADJUSTMENT_YAW_P: @@ -463,8 +463,8 @@ static void applyStepAdjustment(controlRateConfig_t *controlRateConfig, uint8_t applyAdjustmentPID(ADJUSTMENT_YAW_I, &pidBankMutable()->pid[PID_YAW].I, delta); schedulePidGainsUpdate(); break; - case ADJUSTMENT_YAW_D: - applyAdjustmentPID(ADJUSTMENT_YAW_D, &pidBankMutable()->pid[PID_YAW].D, delta); + case ADJUSTMENT_YAW_D_FF: + applyAdjustmentPID(ADJUSTMENT_YAW_D_FF, getD_FFRefByBank(pidBankMutable(), PID_YAW), delta); schedulePidGainsUpdate(); break; case ADJUSTMENT_NAV_FW_CRUISE_THR: diff --git a/src/main/fc/rc_adjustments.h b/src/main/fc/rc_adjustments.h index c629086c13..b51cf390d6 100644 --- a/src/main/fc/rc_adjustments.h +++ b/src/main/fc/rc_adjustments.h @@ -33,19 +33,19 @@ typedef enum { ADJUSTMENT_YAW_RATE = 5, ADJUSTMENT_PITCH_ROLL_P = 6, ADJUSTMENT_PITCH_ROLL_I = 7, - ADJUSTMENT_PITCH_ROLL_D = 8, + ADJUSTMENT_PITCH_ROLL_D_FF = 8, ADJUSTMENT_YAW_P = 9, ADJUSTMENT_YAW_I = 10, - ADJUSTMENT_YAW_D = 11, + ADJUSTMENT_YAW_D_FF = 11, ADJUSTMENT_RATE_PROFILE = 12, // Unused, placeholder for compatibility ADJUSTMENT_PITCH_RATE = 13, ADJUSTMENT_ROLL_RATE = 14, ADJUSTMENT_PITCH_P = 15, ADJUSTMENT_PITCH_I = 16, - ADJUSTMENT_PITCH_D = 17, + ADJUSTMENT_PITCH_D_FF = 17, ADJUSTMENT_ROLL_P = 18, ADJUSTMENT_ROLL_I = 19, - ADJUSTMENT_ROLL_D = 20, + ADJUSTMENT_ROLL_D_FF = 20, ADJUSTMENT_RC_YAW_EXPO = 21, ADJUSTMENT_MANUAL_RC_EXPO = 22, ADJUSTMENT_MANUAL_RC_YAW_EXPO = 23, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index c76b36cfec..f58ab3b7de 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1114,3 +1114,12 @@ const pidBank_t * pidBank(void) { pidBank_t * pidBankMutable(void) { return usedPidControllerType == PID_TYPE_PIFF ? &pidProfileMutable()->bank_fw : &pidProfileMutable()->bank_mc; } + +uint8_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex) +{ + if (pidIndexGetType(pidIndex) == PID_TYPE_PIFF) { + return &pidBank->pid[pidIndex].FF; + } else { + return &pidBank->pid[pidIndex].D; + } +} diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index c9849800af..c325190923 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -198,3 +198,4 @@ void autotuneUpdateState(void); void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput); pidType_e pidIndexGetType(pidIndex_e pidIndex); +uint8_t * getD_FFRefByBank(pidBank_t *pidBank, pidIndex_e pidIndex); diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c index e66bf0a939..9db1f15284 100755 --- a/src/main/flight/pid_autotune.c +++ b/src/main/flight/pid_autotune.c @@ -230,15 +230,15 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa switch (axis) { case FD_ROLL: - blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_D, tuneCurrent[axis].gainFF); + blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_D_FF, tuneCurrent[axis].gainFF); break; case FD_PITCH: - blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_D, tuneCurrent[axis].gainFF); + blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_D_FF, tuneCurrent[axis].gainFF); break; case FD_YAW: - blackboxLogAutotuneEvent(ADJUSTMENT_YAW_D, tuneCurrent[axis].gainFF); + blackboxLogAutotuneEvent(ADJUSTMENT_YAW_D_FF, tuneCurrent[axis].gainFF); break; } } diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 29bdaedd90..dccfb514f1 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1181,7 +1181,7 @@ static void osdDisplayPIDValues(uint8_t elemPosX, uint8_t elemPosY, const char * elemAttr = TEXT_ATTRIBUTES_NONE; tfp_sprintf(buff, "%3d", pidType == PID_TYPE_PIFF ? pid->FF : pid->D); - if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D) || (adjFuncD == ADJUSTMENT_PITCH_D)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D)))) + if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D_FF) || (adjFuncD == ADJUSTMENT_PITCH_D_FF)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D_FF)))) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr); } @@ -1777,15 +1777,15 @@ static bool osdDrawSingleElement(uint8_t item) #endif case OSD_ROLL_PIDS: - osdDisplayPIDValues(elemPosX, elemPosY, "ROL", PID_ROLL, ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D); + osdDisplayPIDValues(elemPosX, elemPosY, "ROL", PID_ROLL, ADJUSTMENT_ROLL_P, ADJUSTMENT_ROLL_I, ADJUSTMENT_ROLL_D_FF); return true; case OSD_PITCH_PIDS: - osdDisplayPIDValues(elemPosX, elemPosY, "PIT", PID_PITCH, ADJUSTMENT_PITCH_P, ADJUSTMENT_PITCH_I, ADJUSTMENT_PITCH_D); + osdDisplayPIDValues(elemPosX, elemPosY, "PIT", PID_PITCH, ADJUSTMENT_PITCH_P, ADJUSTMENT_PITCH_I, ADJUSTMENT_PITCH_D_FF); return true; case OSD_YAW_PIDS: - osdDisplayPIDValues(elemPosX, elemPosY, "YAW", PID_YAW, ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D); + osdDisplayPIDValues(elemPosX, elemPosY, "YAW", PID_YAW, ADJUSTMENT_YAW_P, ADJUSTMENT_YAW_I, ADJUSTMENT_YAW_D_FF); return true; case OSD_LEVEL_PIDS: