Skip to content

Commit

Permalink
Add gov_min_throttle (#175)
Browse files Browse the repository at this point in the history 
* Add gov_min_throttle

* Add min_throttle to MSP_GOVERNOR_PROFILE

Co-authored-by: Petri Mattila <[email protected]>
  • Loading branch information
@rotorflight @pmattila
rotorflight and pmattila authored Oct 16, 2024
1 parent 68f0af0 commit 0198496
Show file tree
Hide file tree
Showing 5 changed files with 98 additions and 35 deletions.
1 change: 1 addition & 0 deletions src/main/cli/settings.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
126 changes: 91 additions & 35 deletions src/main/flight/governor.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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

Expand All @@ -93,6 +103,9 @@ typedef struct {
// Maximum allowed throttle
float maxThrottle;

// Minimum throttle when PID active
float minThrottle;

// Throttle handover level
float maxIdleThrottle;

Expand Down Expand Up @@ -804,30 +817,72 @@ 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
*/

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)
{
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;
Expand All @@ -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;
}
Expand All @@ -853,26 +908,26 @@ 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)
{
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;
Expand All @@ -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;
}
Expand All @@ -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)
Expand All @@ -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;
Expand All @@ -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;
}
Expand Down Expand Up @@ -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);

Expand Down Expand Up @@ -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;
Expand Down
4 changes: 4 additions & 0 deletions src/main/msp/msp.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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;
Expand Down
1 change: 1 addition & 0 deletions src/main/pg/pid.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
);
}

Expand Down
1 change: 1 addition & 0 deletions src/main/pg/pid.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down

0 comments on commit 0198496

Please sign in to comment.