Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Etocii testing #134

Closed
wants to merge 7 commits into from
+178 −48
Closed

Etocii testing #134

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
43bcfe4
Add yaw_precomp_exp
pmattila May 7, 2024
899ba9e
Add yaw_precomp_type
pmattila Sep 12, 2024
95a41a9
Add gov_ff_exponent
pmattila Aug 6, 2024
d280c4a
Add gov_ff_filter_type
pmattila May 7, 2024
1df1688
Add gov_ff_type
pmattila Sep 13, 2024
0f33614
Add gov_min_pid_throttle
pmattila Aug 6, 2024
8becefb
Add setpoint boost
pmattila Sep 13, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
13 changes: 13 additions & 0 deletions src/main/cli/settings.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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[] = {
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -935,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
Expand Down Expand Up @@ -1097,9 +1103,13 @@ 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) },

{ "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) },

Expand Down Expand Up @@ -1181,7 +1191,10 @@ 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) },
{ "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
1 change: 1 addition & 0 deletions src/main/cli/settings.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
154 changes: 112 additions & 42 deletions src/main/flight/governor.c
View file
Open in desktop
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 @@ -153,7 +166,9 @@ typedef struct {
float yawWeight;
float cyclicWeight;
float collectiveWeight;
float FFExponent;
filter_t FFFilter;
uint8_t FFType;

// Tail Torque Assist
float TTAAdd;
Expand Down Expand Up @@ -336,7 +351,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)
Expand Down Expand Up @@ -427,14 +442,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);
Expand Down Expand Up @@ -809,30 +833,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 @@ -841,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;
}
Expand All @@ -858,26 +924,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 @@ -886,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;
}
Expand All @@ -909,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)
Expand All @@ -928,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;
Expand All @@ -940,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;
}
Expand Down Expand Up @@ -1005,7 +1071,11 @@ 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;
gov.minThrottle = pidProfile->governor.min_throttle / 100.0f;

gov.fullHeadSpeed = constrainf(pidProfile->governor.headspeed, 100, 50000);

Expand Down Expand Up @@ -1054,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;
Expand Down Expand Up @@ -1109,7 +1179,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);
}
Expand Down
Loading