From 0198496f0c01a54a7f139f84725c719e6b147bb6 Mon Sep 17 00:00:00 2001 From: Rotorflight Date: Wed, 16 Oct 2024 22:21:32 +0100 Subject: [PATCH] Add gov_min_throttle (#175) * Add gov_min_throttle * Add min_throttle to MSP_GOVERNOR_PROFILE Co-authored-by: Petri Mattila --- src/main/cli/settings.c | 1 + src/main/flight/governor.c | 126 ++++++++++++++++++++++++++----------- src/main/msp/msp.c | 4 ++ src/main/pg/pid.c | 1 + src/main/pg/pid.h | 1 + 5 files changed, 98 insertions(+), 35 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index a5d3c26b0d..561485e7d9 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1182,6 +1182,7 @@ const clivalue_t valueTable[] = { { "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_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 af846d2f12..b1b2a872ac 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; @@ -804,6 +817,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 */ @@ -811,13 +866,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) @@ -825,9 +880,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; @@ -836,11 +891,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; } @@ -853,15 +908,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) @@ -869,10 +924,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; @@ -881,11 +936,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; } @@ -904,15 +959,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) @@ -923,10 +978,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; @@ -935,11 +990,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; } @@ -1001,6 +1056,7 @@ void governorInitProfile(const pidProfile_t *pidProfile) gov.collectiveWeight = pidProfile->governor.collective_ff_weight / 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); @@ -1049,22 +1105,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/msp/msp.c b/src/main/msp/msp.c index dd7af4cb45..c076b41de8 100644 --- a/src/main/msp/msp.c +++ b/src/main/msp/msp.c @@ -1868,6 +1868,7 @@ static bool mspProcessOutCommand(int16_t cmdMSP, sbuf_t *dst) sbufWriteU8(dst, currentPidProfile->governor.cyclic_ff_weight); sbufWriteU8(dst, currentPidProfile->governor.collective_ff_weight); sbufWriteU8(dst, currentPidProfile->governor.max_throttle); + sbufWriteU8(dst, currentPidProfile->governor.min_throttle); break; case MSP_SENSOR_CONFIG: @@ -2626,6 +2627,9 @@ static mspResult_e mspProcessInCommand(mspDescriptor_t srcDesc, int16_t cmdMSP, currentPidProfile->governor.cyclic_ff_weight = sbufReadU8(src); currentPidProfile->governor.collective_ff_weight = sbufReadU8(src); currentPidProfile->governor.max_throttle = sbufReadU8(src); + if (sbufBytesRemaining(src) >= 1) { + currentPidProfile->governor.min_throttle = sbufReadU8(src); + } /* Load new values */ governorInitProfile(currentPidProfile); break; diff --git a/src/main/pg/pid.c b/src/main/pg/pid.c index 963c0db98c..c4be76ef44 100644 --- a/src/main/pg/pid.c +++ b/src/main/pg/pid.c @@ -125,6 +125,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .governor.cyclic_ff_weight = 10, .governor.collective_ff_weight = 100, .governor.max_throttle = 100, + .governor.min_throttle = 10, ); } diff --git a/src/main/pg/pid.h b/src/main/pg/pid.h index 07f785c83a..8b7e6ae6e9 100644 --- a/src/main/pg/pid.h +++ b/src/main/pg/pid.h @@ -89,6 +89,7 @@ typedef struct { uint8_t cyclic_ff_weight; uint8_t collective_ff_weight; uint8_t max_throttle; + uint8_t min_throttle; } governorProfile_t; typedef struct {