From 43bcfe490c66ec635d4952675bf28b00da14f9b4 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Tue, 7 May 2024 09:41:03 +0100 Subject: [PATCH 1/7] Add yaw_precomp_exp --- src/main/cli/settings.c | 1 + src/main/flight/pid.c | 5 +++-- src/main/flight/pid.h | 2 ++ src/main/pg/pid.c | 1 + src/main/pg/pid.h | 1 + 5 files changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 72efab8ba4..1400455662 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1100,6 +1100,7 @@ const clivalue_t valueTable[] = { { "yaw_cw_stop_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 25, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_cw_stop_gain) }, { "yaw_ccw_stop_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 25, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_ccw_stop_gain) }, + { "yaw_precomp_exp", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 25, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_precomp_exp) }, { "yaw_precomp_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_precomp_cutoff) }, { "yaw_precomp_filter_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LPF_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_precomp_filter_type) }, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b347ee7b1d..01558dedca 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -225,6 +225,7 @@ void INIT_CODE pidInitProfile(const pidProfile_t *pidProfile) pt1FilterInit(&pid.precomp.collDynamicFilter, 100.0f / constrainf(pidProfile->yaw_collective_dynamic_decay, 1, 250), pid.freq); // Tail/yaw precomp + pid.precomp.exponent = pidProfile->yaw_precomp_exp / 100.0f; pid.precomp.yawCyclicFFGain = pidProfile->yaw_cyclic_ff_gain / 100.0f; pid.precomp.yawCollectiveFFGain = pidProfile->yaw_collective_ff_gain / 100.0f; pid.precomp.yawCollectiveDynamicGain = pidProfile->yaw_collective_dynamic_gain / 100.0f; @@ -406,11 +407,11 @@ static void pidApplyPrecomp(void) //// Collective-to-Yaw Precomp // Collective components - const float yawCollectiveFF = fabsf(collectiveDeflection) * pid.precomp.yawCollectiveFFGain; + const float yawCollectiveFF = pow_approx(fabsf(collectiveDeflection), pid.precomp.exponent) * pid.precomp.yawCollectiveFFGain; const float yawCollectiveHF = fabsf(collectiveHF) * pid.precomp.yawCollectiveDynamicGain; // Cyclic component - float yawCyclicFF = fabsf(cyclicDeflection) * pid.precomp.yawCyclicFFGain; + const float yawCyclicFF = pow_approx(fabsf(cyclicDeflection), pid.precomp.exponent) * pid.precomp.yawCyclicFFGain; // Calculate total precompensation float yawPrecomp = (yawCollectiveFF + yawCollectiveHF + yawCyclicFF) * masterGain; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 875e9863ba..625f813ced 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -85,6 +85,8 @@ typedef struct { pt1Filter_t collDynamicFilter; + float exponent; + float yawCyclicFFGain; float yawCollectiveFFGain; float yawCollectiveDynamicGain; diff --git a/src/main/pg/pid.c b/src/main/pg/pid.c index 23fe0bae46..ce1346e3e8 100644 --- a/src/main/pg/pid.c +++ b/src/main/pg/pid.c @@ -77,6 +77,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .gyro_filter_type = LPF_1ST_ORDER, .yaw_cw_stop_gain = 120, .yaw_ccw_stop_gain = 80, + .yaw_precomp_exp = 100, .yaw_precomp_cutoff = 5, .yaw_precomp_filter_type = LPF_1ST_ORDER, .yaw_cyclic_ff_gain = 0, diff --git a/src/main/pg/pid.h b/src/main/pg/pid.h index 07f785c83a..58cd24e6ed 100644 --- a/src/main/pg/pid.h +++ b/src/main/pg/pid.h @@ -160,6 +160,7 @@ typedef struct pidProfile_s { uint8_t yaw_cw_stop_gain; uint8_t yaw_ccw_stop_gain; + uint8_t yaw_precomp_exp; uint8_t yaw_precomp_cutoff; uint8_t yaw_precomp_filter_type; From 899ba9eb610efddc9d15f4232ae09c3ad9e90d42 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Thu, 12 Sep 2024 19:04:38 +0100 Subject: [PATCH 2/7] Add yaw_precomp_type --- src/main/cli/settings.c | 6 ++++++ src/main/cli/settings.h | 1 + src/main/flight/pid.c | 24 ++++++++++++++++++++---- src/main/flight/pid.h | 1 + src/main/pg/pid.h | 1 + 5 files changed, 29 insertions(+), 4 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 1400455662..e4c2318ab9 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -505,6 +505,10 @@ const char * const lookupTableParamType[] = { "NONE", "TIMER1", "TIMER2", "TIMER3", "GV1", "GV2", "GV3", "GV4", "GV5", "GV6", "GV7", "GV8", "GV9" }; +const char * const lookupTablePrecompType[] = { + "DEFLECTION", "SETPOINT", +}; + #define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) } const lookupTableEntry_t lookupTables[] = { @@ -619,6 +623,7 @@ const lookupTableEntry_t lookupTables[] = { LOOKUP_TABLE_ENTRY(lookupTablePullMode), LOOKUP_TABLE_ENTRY(lookupTableEdgeMode), LOOKUP_TABLE_ENTRY(lookupTableParamType), + LOOKUP_TABLE_ENTRY(lookupTablePrecompType), }; #undef LOOKUP_TABLE_ENTRY @@ -1100,6 +1105,7 @@ const clivalue_t valueTable[] = { { "yaw_cw_stop_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 25, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_cw_stop_gain) }, { "yaw_ccw_stop_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 25, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_ccw_stop_gain) }, + { "yaw_precomp_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_PRECOMP_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_precomp_type) }, { "yaw_precomp_exp", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 25, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_precomp_exp) }, { "yaw_precomp_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_precomp_cutoff) }, { "yaw_precomp_filter_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LPF_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_precomp_filter_type) }, diff --git a/src/main/cli/settings.h b/src/main/cli/settings.h index 43da0bb477..954dacb560 100644 --- a/src/main/cli/settings.h +++ b/src/main/cli/settings.h @@ -132,6 +132,7 @@ typedef enum { TABLE_INPUT_PULL_MODE, TABLE_INPUT_EDGE_MODE, TABLE_PARAM_TYPE, + TABLE_PRECOMP_TYPE, LOOKUP_TABLE_COUNT } lookupTableIndex_e; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 01558dedca..36c997b722 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -225,6 +225,7 @@ void INIT_CODE pidInitProfile(const pidProfile_t *pidProfile) pt1FilterInit(&pid.precomp.collDynamicFilter, 100.0f / constrainf(pidProfile->yaw_collective_dynamic_decay, 1, 250), pid.freq); // Tail/yaw precomp + pid.precomp.yawPrecompType = pidProfile->yaw_precomp_type; pid.precomp.exponent = pidProfile->yaw_precomp_exp / 100.0f; pid.precomp.yawCyclicFFGain = pidProfile->yaw_cyclic_ff_gain / 100.0f; pid.precomp.yawCollectiveFFGain = pidProfile->yaw_collective_ff_gain / 100.0f; @@ -388,13 +389,28 @@ static void pidApplyCollective(void) static void pidApplyPrecomp(void) { + float collectiveDeflection, pitchDeflection, rollDeflection; + // Yaw precompensation direction and ratio const float masterGain = mixerRotationSign() * getSpoolUpRatio(); - // Get actual control deflections (from previous cycle) - const float collectiveDeflection = filterApply(&pid.precomp.collDeflectionFilter, mixerGetInput(MIXER_IN_STABILIZED_COLLECTIVE)); - const float pitchDeflection = filterApply(&pid.precomp.pitchDeflectionFilter, mixerGetInput(MIXER_IN_STABILIZED_PITCH)); - const float rollDeflection = filterApply(&pid.precomp.rollDeflectionFilter, mixerGetInput(MIXER_IN_STABILIZED_ROLL)); + if (pid.precomp.yawPrecompType) { + // Get values from PID inputs + collectiveDeflection = pid.collective; + pitchDeflection = pid.data[PITCH].setPoint / 300.0f; + rollDeflection = pid.data[ROLL].setPoint / 300.0f; + } + else { + // Get actual control deflections (from previous cycle) + collectiveDeflection = mixerGetInput(MIXER_IN_STABILIZED_COLLECTIVE); + pitchDeflection = mixerGetInput(MIXER_IN_STABILIZED_PITCH); + rollDeflection = mixerGetInput(MIXER_IN_STABILIZED_ROLL); + } + + // Apply filters + collectiveDeflection = filterApply(&pid.precomp.collDeflectionFilter, collectiveDeflection); + pitchDeflection = filterApply(&pid.precomp.pitchDeflectionFilter, pitchDeflection); + rollDeflection = filterApply(&pid.precomp.rollDeflectionFilter, rollDeflection); // Calculate cyclic deflection from the filtered controls const float cyclicDeflection = sqrtf(sq(pitchDeflection) + sq(rollDeflection)); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 625f813ced..8786b0a01c 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -86,6 +86,7 @@ typedef struct { pt1Filter_t collDynamicFilter; float exponent; + float yawPrecompType; float yawCyclicFFGain; float yawCollectiveFFGain; diff --git a/src/main/pg/pid.h b/src/main/pg/pid.h index 58cd24e6ed..a5033b0831 100644 --- a/src/main/pg/pid.h +++ b/src/main/pg/pid.h @@ -160,6 +160,7 @@ typedef struct pidProfile_s { uint8_t yaw_cw_stop_gain; uint8_t yaw_ccw_stop_gain; + uint8_t yaw_precomp_type; uint8_t yaw_precomp_exp; uint8_t yaw_precomp_cutoff; uint8_t yaw_precomp_filter_type; From 95a41a949b0f7be3fd3da0efa084e8003f94c791 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Tue, 6 Aug 2024 10:51:00 +0100 Subject: [PATCH 3/7] Add gov_ff_exponent 100 = angle^1.0 150 = angle^1.5 --- src/main/cli/settings.c | 1 + src/main/flight/governor.c | 5 ++++- src/main/pg/pid.c | 1 + src/main/pg/pid.h | 1 + 4 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index e4c2318ab9..d3ea2d62b2 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1188,6 +1188,7 @@ const clivalue_t valueTable[] = { { "gov_yaw_ff_weight", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, governor.yaw_ff_weight) }, { "gov_cyclic_ff_weight", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, governor.cyclic_ff_weight) }, { "gov_collective_ff_weight", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, governor.collective_ff_weight) }, + { "gov_ff_exponent", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, governor.ff_exponent) }, { "gov_max_throttle", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, governor.max_throttle) }, // PG_TELEMETRY_CONFIG diff --git a/src/main/flight/governor.c b/src/main/flight/governor.c index 7bd3c9bcb1..c6ea6aa1fa 100644 --- a/src/main/flight/governor.c +++ b/src/main/flight/governor.c @@ -153,6 +153,7 @@ typedef struct { float yawWeight; float cyclicWeight; float collectiveWeight; + float FFExponent; filter_t FFFilter; // Tail Torque Assist @@ -336,7 +337,7 @@ static inline float idleMap(float throttle) static inline float angleDrag(float angle) { - return angle * sqrtf(angle); // angle ^ 1.5 + return pow_approx(angle, gov.FFExponent); } static inline void govChangeState(govState_e futureState) @@ -1005,6 +1006,8 @@ void governorInitProfile(const pidProfile_t *pidProfile) gov.cyclicWeight = pidProfile->governor.cyclic_ff_weight / 100.0f; gov.collectiveWeight = pidProfile->governor.collective_ff_weight / 100.0f; + gov.FFExponent = pidProfile->governor.ff_exponent / 100.0f; + gov.maxThrottle = pidProfile->governor.max_throttle / 100.0f; gov.fullHeadSpeed = constrainf(pidProfile->governor.headspeed, 100, 50000); diff --git a/src/main/pg/pid.c b/src/main/pg/pid.c index ce1346e3e8..8d9e98f973 100644 --- a/src/main/pg/pid.c +++ b/src/main/pg/pid.c @@ -125,6 +125,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .governor.tta_limit = 20, .governor.cyclic_ff_weight = 10, .governor.collective_ff_weight = 100, + .governor.ff_exponent = 150, .governor.max_throttle = 100, ); } diff --git a/src/main/pg/pid.h b/src/main/pg/pid.h index a5033b0831..c529a25669 100644 --- a/src/main/pg/pid.h +++ b/src/main/pg/pid.h @@ -88,6 +88,7 @@ typedef struct { uint8_t yaw_ff_weight; uint8_t cyclic_ff_weight; uint8_t collective_ff_weight; + uint8_t ff_exponent; uint8_t max_throttle; } governorProfile_t; From d280c4a1f09dccfaf1642edaff0ffdd34341a185 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Tue, 7 May 2024 09:48:31 +0100 Subject: [PATCH 4/7] Add gov_ff_filter_type --- src/main/cli/settings.c | 1 + src/main/flight/governor.c | 2 +- src/main/pg/governor.c | 3 +++ src/main/pg/governor.h | 1 + 4 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index d3ea2d62b2..b42b3c97aa 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -940,6 +940,7 @@ const clivalue_t valueTable[] = { { "gov_rpm_filter", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_GOVERNOR_CONFIG, offsetof(governorConfig_t, gov_rpm_filter) }, { "gov_tta_filter", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_GOVERNOR_CONFIG, offsetof(governorConfig_t, gov_tta_filter) }, { "gov_ff_filter", VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_GOVERNOR_CONFIG, offsetof(governorConfig_t, gov_ff_filter) }, + { "gov_ff_filter_type", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_LPF_TYPE }, PG_GOVERNOR_CONFIG, offsetof(governorConfig_t, gov_ff_filter_type) }, // PG_CONTROLRATE_PROFILES #ifdef USE_PROFILE_NAMES diff --git a/src/main/flight/governor.c b/src/main/flight/governor.c index c6ea6aa1fa..6b5f5ea2b2 100644 --- a/src/main/flight/governor.c +++ b/src/main/flight/governor.c @@ -1112,7 +1112,7 @@ void governorInit(const pidProfile_t *pidProfile) lowpassFilterInit(&gov.motorCurrentFilter, LPF_DAMPED, governorConfig()->gov_pwr_filter, gyro.targetRateHz, 0); lowpassFilterInit(&gov.motorRPMFilter, LPF_DAMPED, governorConfig()->gov_rpm_filter, gyro.targetRateHz, 0); lowpassFilterInit(&gov.TTAFilter, LPF_DAMPED, governorConfig()->gov_tta_filter, gyro.targetRateHz, 0); - lowpassFilterInit(&gov.FFFilter, LPF_DAMPED, governorConfig()->gov_ff_filter, gyro.targetRateHz, 0); + lowpassFilterInit(&gov.FFFilter, governorConfig()->gov_ff_filter_type, governorConfig()->gov_ff_filter, gyro.targetRateHz, 0); governorInitProfile(pidProfile); } diff --git a/src/main/pg/governor.c b/src/main/pg/governor.c index d8903931bc..a6506c15b2 100644 --- a/src/main/pg/governor.c +++ b/src/main/pg/governor.c @@ -19,6 +19,8 @@ #include "config/config_reset.h" +#include "common/filter.h" + #include "pg/pg.h" #include "pg/pg_ids.h" @@ -43,5 +45,6 @@ PG_RESET_TEMPLATE(governorConfig_t, governorConfig, .gov_rpm_filter = 10, .gov_tta_filter = 0, .gov_ff_filter = 10, + .gov_ff_filter_type = LPF_DAMPED, ); diff --git a/src/main/pg/governor.h b/src/main/pg/governor.h index 802e81b342..5fdeca196b 100644 --- a/src/main/pg/governor.h +++ b/src/main/pg/governor.h @@ -38,6 +38,7 @@ typedef struct governorConfig_s { uint8_t gov_rpm_filter; uint8_t gov_tta_filter; uint8_t gov_ff_filter; + uint8_t gov_ff_filter_type; } governorConfig_t; PG_DECLARE(governorConfig_t, governorConfig); From 1df1688f60a0ad0b123e3b687ecfb982337e7339 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Fri, 13 Sep 2024 09:21:08 +0100 Subject: [PATCH 5/7] Add gov_ff_type --- src/main/cli/settings.c | 1 + src/main/flight/governor.c | 21 ++++++++++++++++----- src/main/pg/pid.h | 1 + 3 files changed, 18 insertions(+), 5 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index b42b3c97aa..0db630e9b8 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1189,6 +1189,7 @@ const clivalue_t valueTable[] = { { "gov_yaw_ff_weight", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, governor.yaw_ff_weight) }, { "gov_cyclic_ff_weight", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, governor.cyclic_ff_weight) }, { "gov_collective_ff_weight", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, governor.collective_ff_weight) }, + { "gov_ff_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_PRECOMP_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, governor.ff_type) }, { "gov_ff_exponent", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, governor.ff_exponent) }, { "gov_max_throttle", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, governor.max_throttle) }, diff --git a/src/main/flight/governor.c b/src/main/flight/governor.c index 6b5f5ea2b2..f09d08cf64 100644 --- a/src/main/flight/governor.c +++ b/src/main/flight/governor.c @@ -155,6 +155,7 @@ typedef struct { float collectiveWeight; float FFExponent; filter_t FFFilter; + uint8_t FFType; // Tail Torque Assist float TTAAdd; @@ -428,14 +429,23 @@ static void govUpdateData(void) gov.requestRatio = 0; } - // Calculate feedforward from collective deflection - float collectiveFF = gov.collectiveWeight * getCollectiveDeflectionAbs(); + float collectiveFF, cyclicFF, yawFF; - // Calculate feedforward from cyclic deflection - float cyclicFF = gov.cyclicWeight * getCyclicDeflection(); + if (gov.FFType) { + const pidAxisData_t * data = pidGetAxisData(); + const float roll = data[ROLL].setPoint / 300.0; + const float pitch = data[PITCH].setPoint / 300.0; + const float cyclic = sqrtf(sq(roll) + sq(pitch)); + cyclicFF = gov.cyclicWeight * cyclic; + collectiveFF = gov.collectiveWeight * pidGetCollective(); + } + else { + cyclicFF = gov.cyclicWeight * getCyclicDeflection(); + collectiveFF = gov.collectiveWeight * getCollectiveDeflectionAbs(); + } // Calculate feedforward from yaw deflection - float yawFF = gov.yawWeight * getYawDeflectionAbs(); + yawFF = gov.yawWeight * getYawDeflectionAbs(); // Angle-of-attack vs. FeedForward curve float totalFF = angleDrag(collectiveFF + cyclicFF) + angleDrag(yawFF); @@ -1006,6 +1016,7 @@ void governorInitProfile(const pidProfile_t *pidProfile) gov.cyclicWeight = pidProfile->governor.cyclic_ff_weight / 100.0f; gov.collectiveWeight = pidProfile->governor.collective_ff_weight / 100.0f; + gov.FFType = pidProfile->governor.ff_type; gov.FFExponent = pidProfile->governor.ff_exponent / 100.0f; gov.maxThrottle = pidProfile->governor.max_throttle / 100.0f; diff --git a/src/main/pg/pid.h b/src/main/pg/pid.h index c529a25669..38aa121989 100644 --- a/src/main/pg/pid.h +++ b/src/main/pg/pid.h @@ -88,6 +88,7 @@ typedef struct { uint8_t yaw_ff_weight; uint8_t cyclic_ff_weight; uint8_t collective_ff_weight; + uint8_t ff_type; uint8_t ff_exponent; uint8_t max_throttle; } governorProfile_t; From 0f3361426718ececf4e08f2c706a80c3aa659363 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Tue, 6 Aug 2024 10:40:00 +0100 Subject: [PATCH 6/7] Add gov_min_pid_throttle --- src/main/cli/settings.c | 1 + src/main/flight/governor.c | 126 ++++++++++++++++++++++++++----------- src/main/pg/pid.c | 1 + src/main/pg/pid.h | 1 + 4 files changed, 94 insertions(+), 35 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 0db630e9b8..17196996f0 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1192,6 +1192,7 @@ const clivalue_t valueTable[] = { { "gov_ff_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_PRECOMP_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, governor.ff_type) }, { "gov_ff_exponent", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, governor.ff_exponent) }, { "gov_max_throttle", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, governor.max_throttle) }, + { "gov_min_throttle", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, governor.min_throttle) }, // PG_TELEMETRY_CONFIG #ifdef USE_TELEMETRY diff --git a/src/main/flight/governor.c b/src/main/flight/governor.c index f09d08cf64..b3e6f92e0c 100644 --- a/src/main/flight/governor.c +++ b/src/main/flight/governor.c @@ -50,8 +50,9 @@ // Throttle mapping in IDLE state #define GOV_THROTTLE_OFF_LIMIT 0.05f -// Minimum throttle output from PI algorithms -#define GOV_MIN_THROTTLE_OUTPUT 0.05f +// Throttle limits for spoolup +#define GOV_MIN_SPOOLUP_THROTTLE 0.05f +#define GOV_MAX_SPOOLUP_THROTTLE 0.95f // Headspeed quality levels #define GOV_HS_DETECT_DELAY 200 @@ -68,6 +69,15 @@ // Nominal battery cell voltage #define GOV_NOMINAL_CELL_VOLTAGE 3.70f +// PID term limits +#define GOV_P_TERM_LIMIT 0.20f +#define GOV_I_TERM_LIMIT 0.95f +#define GOV_D_TERM_LIMIT 0.20f +#define GOV_F_TERM_LIMIT 0.50f + +#define SPOOLUP_P_TERM_LIMIT 0.10f +#define SPOOLUP_I_TERM_LIMIT 0.95f + //// Internal Data @@ -93,6 +103,9 @@ typedef struct { // Maximum allowed throttle float maxThrottle; + // Minimum throttle when PID active + float minThrottle; + // Throttle handover level float maxIdleThrottle; @@ -820,6 +833,48 @@ static void governorUpdateState(void) } +/* + * Spoolup PID controller + */ + +static void govSpoolUpInit(void) +{ + // PID limits + gov.P = constrainf(gov.P, -SPOOLUP_P_TERM_LIMIT, SPOOLUP_P_TERM_LIMIT); + + // Use gov.I to reach the target + gov.I = gov.throttle - gov.P; + + // Limited range + gov.I = constrainf(gov.I, 0, SPOOLUP_I_TERM_LIMIT); +} + +static float govSpoolUpControl(void) +{ + float output; + + // PID limits + gov.P = constrainf(gov.P, -SPOOLUP_P_TERM_LIMIT, SPOOLUP_P_TERM_LIMIT); + gov.I = constrainf(gov.I, 0, SPOOLUP_I_TERM_LIMIT); + gov.D = 0; + + // Governor PI sum + gov.pidSum = gov.P + gov.I + gov.C; + + // Generate throttle signal + output = gov.pidSum; + + // Apply gov.C if output not saturated + if (!((output > GOV_MAX_SPOOLUP_THROTTLE && gov.C > 0) || (output < GOV_MIN_SPOOLUP_THROTTLE && gov.C < 0))) + gov.I += gov.C; + + // Limit output + output = constrainf(output, GOV_MIN_SPOOLUP_THROTTLE, GOV_MAX_SPOOLUP_THROTTLE); + + return output; +} + + /* * Standard PID controller */ @@ -827,13 +882,13 @@ static void governorUpdateState(void) static void govPIDInit(void) { // PID limits - gov.P = constrainf(gov.P, -0.25f, 0.25f); + gov.P = constrainf(gov.P, -GOV_P_TERM_LIMIT, GOV_P_TERM_LIMIT); // Use gov.I to reach the target gov.I = gov.throttle - gov.P; // Limited range - gov.I = constrainf(gov.I, 0, 0.95f); + gov.I = constrainf(gov.I, 0, GOV_I_TERM_LIMIT); } static float govPIDControl(void) @@ -841,9 +896,9 @@ static float govPIDControl(void) float output; // PID limits - gov.P = constrainf(gov.P, -0.25f, 0.25f); - gov.I = constrainf(gov.I, 0, 0.95f); - gov.D = constrainf(gov.D, -0.25f, 0.25f); + gov.P = constrainf(gov.P, -GOV_P_TERM_LIMIT, GOV_P_TERM_LIMIT); + gov.I = constrainf(gov.I, 0, GOV_I_TERM_LIMIT); + gov.D = constrainf(gov.D, -GOV_D_TERM_LIMIT, GOV_D_TERM_LIMIT); // Governor PID sum gov.pidSum = gov.P + gov.I + gov.D + gov.C; @@ -852,11 +907,11 @@ static float govPIDControl(void) output = gov.pidSum; // Apply gov.C if output not saturated - if (!((output > gov.maxThrottle && gov.C > 0) || (output < GOV_MIN_THROTTLE_OUTPUT && gov.C < 0))) + if (!((output > gov.maxThrottle && gov.C > 0) || (output < gov.minThrottle && gov.C < 0))) gov.I += gov.C; // Limit output - output = constrainf(output, GOV_MIN_THROTTLE_OUTPUT, gov.maxThrottle); + output = constrainf(output, gov.minThrottle, gov.maxThrottle); return output; } @@ -869,15 +924,15 @@ static float govPIDControl(void) static void govMode1Init(void) { // PID limits - gov.P = constrainf(gov.P, -0.25f, 0.25f); - gov.D = constrainf(gov.D, -0.25f, 0.25f); - gov.F = constrainf(gov.F, 0, 0.50f); + gov.P = constrainf(gov.P, -GOV_P_TERM_LIMIT, GOV_P_TERM_LIMIT); + gov.D = constrainf(gov.D, -GOV_D_TERM_LIMIT, GOV_D_TERM_LIMIT); + gov.F = constrainf(gov.F, 0, GOV_F_TERM_LIMIT); // Use gov.I to reach the target gov.I = gov.throttle - (gov.P + gov.D + gov.F); // Limited range - gov.I = constrainf(gov.I, 0, 0.95f); + gov.I = constrainf(gov.I, 0, GOV_I_TERM_LIMIT); } static float govMode1Control(void) @@ -885,10 +940,10 @@ static float govMode1Control(void) float output; // PID limits - gov.P = constrainf(gov.P, -0.25f, 0.25f); - gov.I = constrainf(gov.I, 0, 0.95f); - gov.D = constrainf(gov.D, -0.25f, 0.25f); - gov.F = constrainf(gov.F, 0, 0.50f); + gov.P = constrainf(gov.P, -GOV_P_TERM_LIMIT, GOV_P_TERM_LIMIT); + gov.I = constrainf(gov.I, 0, GOV_I_TERM_LIMIT); + gov.D = constrainf(gov.D, -GOV_D_TERM_LIMIT, GOV_D_TERM_LIMIT); + gov.F = constrainf(gov.F, 0, GOV_F_TERM_LIMIT); // Governor PIDF sum gov.pidSum = gov.P + gov.I + gov.C + gov.D + gov.F; @@ -897,11 +952,11 @@ static float govMode1Control(void) output = gov.pidSum; // Apply gov.C if output not saturated - if (!((output > gov.maxThrottle && gov.C > 0) || (output < GOV_MIN_THROTTLE_OUTPUT && gov.C < 0))) + if (!((output > gov.maxThrottle && gov.C > 0) || (output < gov.minThrottle && gov.C < 0))) gov.I += gov.C; // Limit output - output = constrainf(output, GOV_MIN_THROTTLE_OUTPUT, gov.maxThrottle); + output = constrainf(output, gov.minThrottle, gov.maxThrottle); return output; } @@ -920,15 +975,15 @@ static void govMode2Init(void) float pidTarget = gov.throttle / pidGain; // PID limits - gov.P = constrainf(gov.P, -0.25f, 0.25f); - gov.D = constrainf(gov.D, -0.25f, 0.25f); - gov.F = constrainf(gov.F, 0, 0.50f); + gov.P = constrainf(gov.P, -GOV_P_TERM_LIMIT, GOV_P_TERM_LIMIT); + gov.D = constrainf(gov.D, -GOV_D_TERM_LIMIT, GOV_D_TERM_LIMIT); + gov.F = constrainf(gov.F, 0, GOV_F_TERM_LIMIT); // Use gov.I to reach the target gov.I = pidTarget - (gov.P + gov.D + gov.F); // Limited range - gov.I = constrainf(gov.I, 0, 0.95f); + gov.I = constrainf(gov.I, 0, GOV_I_TERM_LIMIT); } static float govMode2Control(void) @@ -939,10 +994,10 @@ static float govMode2Control(void) float pidGain = gov.nominalVoltage / gov.motorVoltage; // PID limits - gov.P = constrainf(gov.P, -0.25f, 0.25f); - gov.I = constrainf(gov.I, 0, 0.95f); - gov.D = constrainf(gov.D, -0.25f, 0.25f); - gov.F = constrainf(gov.F, 0, 0.50f); + gov.P = constrainf(gov.P, -GOV_P_TERM_LIMIT, GOV_P_TERM_LIMIT); + gov.I = constrainf(gov.I, 0, GOV_I_TERM_LIMIT); + gov.D = constrainf(gov.D, -GOV_D_TERM_LIMIT, GOV_D_TERM_LIMIT); + gov.F = constrainf(gov.F, 0, GOV_F_TERM_LIMIT); // Governor PIDF sum gov.pidSum = gov.P + gov.I + gov.C + gov.D + gov.F; @@ -951,11 +1006,11 @@ static float govMode2Control(void) output = gov.pidSum * pidGain; // Apply gov.C if output not saturated - if (!((output > gov.maxThrottle && gov.C > 0) || (output < GOV_MIN_THROTTLE_OUTPUT && gov.C < 0))) + if (!((output > gov.maxThrottle && gov.C > 0) || (output < gov.minThrottle && gov.C < 0))) gov.I += gov.C; // Limit output - output = constrainf(output, GOV_MIN_THROTTLE_OUTPUT, gov.maxThrottle); + output = constrainf(output, gov.minThrottle, gov.maxThrottle); return output; } @@ -1020,6 +1075,7 @@ void governorInitProfile(const pidProfile_t *pidProfile) gov.FFExponent = pidProfile->governor.ff_exponent / 100.0f; gov.maxThrottle = pidProfile->governor.max_throttle / 100.0f; + gov.minThrottle = pidProfile->governor.min_throttle / 100.0f; gov.fullHeadSpeed = constrainf(pidProfile->governor.headspeed, 100, 50000); @@ -1068,22 +1124,22 @@ void governorInit(const pidProfile_t *pidProfile) break; case GM_STANDARD: govStateUpdate = governorUpdateState; - govSpoolupInit = govPIDInit; - govSpoolupCalc = govPIDControl; + govSpoolupInit = govSpoolUpInit; + govSpoolupCalc = govSpoolUpControl; govActiveInit = govPIDInit; govActiveCalc = govPIDControl; break; case GM_MODE1: govStateUpdate = governorUpdateState; - govSpoolupInit = govPIDInit; - govSpoolupCalc = govPIDControl; + govSpoolupInit = govSpoolUpInit; + govSpoolupCalc = govSpoolUpControl; govActiveInit = govMode1Init; govActiveCalc = govMode1Control; break; case GM_MODE2: govStateUpdate = governorUpdateState; - govSpoolupInit = govPIDInit; - govSpoolupCalc = govPIDControl; + govSpoolupInit = govSpoolUpInit; + govSpoolupCalc = govSpoolUpControl; govActiveInit = govMode2Init; govActiveCalc = govMode2Control; break; diff --git a/src/main/pg/pid.c b/src/main/pg/pid.c index 8d9e98f973..af9c43a4ff 100644 --- a/src/main/pg/pid.c +++ b/src/main/pg/pid.c @@ -127,6 +127,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .governor.collective_ff_weight = 100, .governor.ff_exponent = 150, .governor.max_throttle = 100, + .governor.min_throttle = 10, ); } diff --git a/src/main/pg/pid.h b/src/main/pg/pid.h index 38aa121989..646ff50008 100644 --- a/src/main/pg/pid.h +++ b/src/main/pg/pid.h @@ -91,6 +91,7 @@ typedef struct { uint8_t ff_type; uint8_t ff_exponent; uint8_t max_throttle; + uint8_t min_throttle; } governorProfile_t; typedef struct { From 8becefb93271fe8bba078f57bd7e776494bbcdf0 Mon Sep 17 00:00:00 2001 From: Petri Mattila Date: Fri, 13 Sep 2024 10:08:34 +0100 Subject: [PATCH 7/7] Add setpoint boost --- src/main/cli/settings.c | 2 ++ src/main/flight/pid.c | 9 +++++++++ src/main/flight/pid.h | 3 +++ src/main/pg/pid.h | 2 ++ 4 files changed, 16 insertions(+) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 17196996f0..fd9575b996 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1103,6 +1103,8 @@ const clivalue_t valueTable[] = { { "yaw_gyro_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, gyro_cutoff[PID_YAW]) }, { "yaw_error_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, error_cutoff[PID_YAW]) }, + { "setpoint_boost", VAR_UINT16 | PROFILE_VALUE | MODE_ARRAY, .config.array.length = 3, PG_PID_PROFILE, offsetof(pidProfile_t, setpoint_boost) }, + { "yaw_cw_stop_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 25, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_cw_stop_gain) }, { "yaw_ccw_stop_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 25, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, yaw_ccw_stop_gain) }, diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 36c997b722..080a4f9c92 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -160,6 +160,11 @@ void INIT_CODE pidInitProfile(const pidProfile_t *pidProfile) pid.coef[PID_YAW].Kf = YAW_F_TERM_SCALE * pidProfile->pid[PID_YAW].F; pid.coef[PID_YAW].Kb = YAW_B_TERM_SCALE * pidProfile->pid[PID_YAW].B; + // Setpoint boost + pid.boostGain[PID_ROLL] = ROLL_B_TERM_SCALE * pidProfile->setpoint_boost[PID_ROLL]; + pid.boostGain[PID_PITCH] = PITCH_B_TERM_SCALE * pidProfile->setpoint_boost[PID_PITCH]; + pid.boostGain[PID_YAW] = YAW_B_TERM_SCALE * pidProfile->setpoint_boost[PID_YAW]; + // Bleed conversion for pitch if (pidProfile->pid[PID_PITCH].O > 0 && pidProfile->pid[PID_PITCH].I > 0) pid.coef[PID_PITCH].Kc = pid.coef[PID_PITCH].Ko / pid.coef[PID_PITCH].Ki; @@ -196,6 +201,7 @@ void INIT_CODE pidInitProfile(const pidProfile_t *pidProfile) lowpassFilterInit(&pid.errorFilter[i], LPF_ORDER1, pidProfile->error_cutoff[i], pid.freq, 0); difFilterInit(&pid.dtermFilter[i], pidProfile->dterm_cutoff[i], pid.freq); difFilterInit(&pid.btermFilter[i], pidProfile->bterm_cutoff[i], pid.freq); + difFilterInit(&pid.boostFilter[i], pidProfile->bterm_cutoff[i], pid.freq); } // Error relax @@ -357,6 +363,9 @@ static float pidApplySetpoint(uint8_t axis) setpoint = rescueApply(axis, setpoint); #endif + // Apply boost + setpoint += difFilterApply(&pid.boostFilter[axis], setpoint) * pid.boostGain[axis]; + // Save setpoint pid.data[axis].setPoint = setpoint; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 8786b0a01c..e0d1aad6e3 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -121,6 +121,8 @@ typedef struct pid_s { float yawCWStopGain; float yawCCWStopGain; + float boostGain[PID_AXIS_COUNT]; + float cyclicCrossCouplingGain[XY_AXIS_COUNT]; float collective; @@ -137,6 +139,7 @@ typedef struct pid_s { difFilter_t dtermFilter[PID_AXIS_COUNT]; difFilter_t btermFilter[PID_AXIS_COUNT]; + difFilter_t boostFilter[PID_AXIS_COUNT]; order1Filter_t crossCouplingFilter[XY_AXIS_COUNT]; diff --git a/src/main/pg/pid.h b/src/main/pg/pid.h index 646ff50008..e5b4a9c4ed 100644 --- a/src/main/pg/pid.h +++ b/src/main/pg/pid.h @@ -179,6 +179,8 @@ typedef struct pidProfile_s { uint8_t cyclic_cross_coupling_ratio; uint8_t cyclic_cross_coupling_cutoff; + uint16_t setpoint_boost[3]; + pidAngleMode_t angle; pidHorizonMode_t horizon; pidTrainerMode_t trainer;