Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 12:55:19 +03:00
Code Issues Wiki Activity

Requested changes, cleanup and control logic updates

Browse source 
Also added a new parameter acro_trainer_gain to allow adjustments to the limiting strength.
This commit is contained in:
Bruce Luckcuck 2018-05-26 16:16:35 -04:00
parent 9b43839052
commit 2384088855
4 changed files with 46 additions and 39 deletions
Download patch file Download diff file Expand all files Collapse all files

81
src/main/flight/pid.c
View file

@ -89,8 +89,8 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
#endif #endif
#ifdef USE_ACRO_TRAINER #ifdef USE_ACRO_TRAINER
#define ACRO_TRAINER_GAIN 5.0f // matches default angle mode strength of 50 #define ACRO_TRAINER_LOOKAHEAD_MIN_TIME 0.01f
#define ACRO_TRAINER_LOOKAHEAD_MIN 10000 // 10ms #define ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT 500.0f // Max gyro rate for lookahead time scaling
#endif // USE_ACRO_TRAINER #endif // USE_ACRO_TRAINER
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 3); PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 3);
@ -150,7 +150,8 @@ void resetPidProfile(pidProfile_t *pidProfile)
.iterm_relax_cutoff_high = 15, .iterm_relax_cutoff_high = 15,
.acro_trainer_angle_limit = 20, .acro_trainer_angle_limit = 20,
.acro_trainer_lookahead_ms = 50, .acro_trainer_lookahead_ms = 50,
.acro_trainer_debug_axis = FD_ROLL .acro_trainer_debug_axis = FD_ROLL,
.acro_trainer_gain = 75
); );
} }
@ -321,11 +322,12 @@ static FAST_RAM_ZERO_INIT bool itermRotation;
static FAST_RAM_ZERO_INIT bool smartFeedforward; static FAST_RAM_ZERO_INIT bool smartFeedforward;
#ifdef USE_ACRO_TRAINER #ifdef USE_ACRO_TRAINER
static FAST_RAM_ZERO_INIT float acro_trainer_angle_limit; static FAST_RAM_ZERO_INIT float acroTrainerAngleLimit;
static FAST_RAM_ZERO_INIT timeUs_t acro_trainer_lookahead_time; static FAST_RAM_ZERO_INIT float acroTrainerLookaheadTime;
static FAST_RAM_ZERO_INIT uint8_t acro_trainer_debug_axis; static FAST_RAM_ZERO_INIT uint8_t acroTrainerDebugAxis;
static FAST_RAM_ZERO_INIT bool acroTrainerActive; static FAST_RAM_ZERO_INIT bool acroTrainerActive;
static FAST_RAM_ZERO_INIT int8_t acroTrainerAxisState[2]; // only need roll and pitch static FAST_RAM_ZERO_INIT int acroTrainerAxisState[2]; // only need roll and pitch
static FAST_RAM_ZERO_INIT float acroTrainerGain;
#endif // USE_ACRO_TRAINER #endif // USE_ACRO_TRAINER
void pidInitConfig(const pidProfile_t *pidProfile) void pidInitConfig(const pidProfile_t *pidProfile)
@ -369,9 +371,10 @@ void pidInitConfig(const pidProfile_t *pidProfile)
itermRelaxCutoffHigh = pidProfile->iterm_relax_cutoff_high; itermRelaxCutoffHigh = pidProfile->iterm_relax_cutoff_high;
#ifdef USE_ACRO_TRAINER #ifdef USE_ACRO_TRAINER
acro_trainer_angle_limit = pidProfile->acro_trainer_angle_limit; acroTrainerAngleLimit = pidProfile->acro_trainer_angle_limit;
acro_trainer_lookahead_time = pidProfile->acro_trainer_lookahead_ms * 1000; acroTrainerLookaheadTime = (float)pidProfile->acro_trainer_lookahead_ms / 1000.0f;
acro_trainer_debug_axis = pidProfile->acro_trainer_debug_axis; acroTrainerDebugAxis = pidProfile->acro_trainer_debug_axis;
acroTrainerGain = (float)pidProfile->acro_trainer_gain / 10.0f;
#endif // USE_ACRO_TRAINER #endif // USE_ACRO_TRAINER
} }
@ -576,9 +579,9 @@ static void handleItermRotation()
#ifdef USE_ACRO_TRAINER #ifdef USE_ACRO_TRAINER
int8_t acroTrainerSign(float x) int acroTrainerSign(float x)
{ {
return (x > 0.0f) - (x < 0.0f); return x > 0 ? 1 : -1;
} }
// Acro Trainer - Manipulate the setPoint to limit axis angle while in acro mode // Acro Trainer - Manipulate the setPoint to limit axis angle while in acro mode
@ -589,59 +592,61 @@ int8_t acroTrainerSign(float x)
// Manage the setPoint to control the gyro rate as the actual angle approaches the limit (try to prevent overshoot) // Manage the setPoint to control the gyro rate as the actual angle approaches the limit (try to prevent overshoot)
// 3. If no potential overflow is detected, then return the original setPoint // 3. If no potential overflow is detected, then return the original setPoint
static float applyAcroTrainer(int axis, const rollAndPitchTrims_t *angleTrim, float setPoint) // Use the FAST_CODE_NOINLINE directive to avoid this code from being inlined into ITCM RAM. We accept the
// performance decrease when Acro Trainer mode is active under the assumption that user is unlikely to be
// expecting ultimate flight performance at very high loop rates when in this mode.
static FAST_CODE_NOINLINE float applyAcroTrainer(int axis, const rollAndPitchTrims_t *angleTrim, float setPoint)
{ {
float ret = setPoint; float ret = setPoint;
#ifndef SIMULATOR_BUILD
if (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE)) { if (!FLIGHT_MODE(ANGLE_MODE) && !FLIGHT_MODE(HORIZON_MODE)) {
bool resetIterm = false; bool resetIterm = false;
float projectedAngle; float projectedAngle = 0;
const int8_t setpointSign = acroTrainerSign(setPoint); const int setpointSign = acroTrainerSign(setPoint);
const float currentAngle = (attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f; const float currentAngle = (attitude.raw[axis] - angleTrim->raw[axis]) / 10.0f;
const int8_t angleSign = acroTrainerSign(currentAngle); const int angleSign = acroTrainerSign(currentAngle);
if ((acroTrainerAxisState[axis] != 0) && (acroTrainerAxisState[axis] != setpointSign)) { // stick has reversed - stop limiting
acroTrainerAxisState[axis] = 0;
}
// Limit and correct the angle when it exceeds the limit // Limit and correct the angle when it exceeds the limit
if ((fabsf(currentAngle) > acro_trainer_angle_limit) || (acroTrainerAxisState[axis] != 0)) { if ((fabsf(currentAngle) > acroTrainerAngleLimit) && (acroTrainerAxisState[axis] == 0)) {
if (angleSign == -setpointSign) { // stick has reversed - stop limiting if (angleSign == setpointSign) {
acroTrainerAxisState[axis] = 0; acroTrainerAxisState[axis] = angleSign;
} else { resetIterm = true;
if (acroTrainerAxisState[axis] != angleSign) {
acroTrainerAxisState[axis] = angleSign;
resetIterm = true;
}
ret = ((acro_trainer_angle_limit * angleSign) - currentAngle) * ACRO_TRAINER_GAIN;
} }
} }
if (acroTrainerAxisState[axis] != 0) {
ret = ((acroTrainerAngleLimit * angleSign) - currentAngle) * acroTrainerGain;
} else {
// Not currently over the limit so project the angle based on current angle and // Not currently over the limit so project the angle based on current angle and
// gyro angular rate using a sliding window based on gyro rate (faster rotation means larger window. // gyro angular rate using a sliding window based on gyro rate (faster rotation means larger window.
// If the projected angle exceeds the limit then apply limiting to minimize overshoot. // If the projected angle exceeds the limit then apply limiting to minimize overshoot.
if (acroTrainerAxisState[axis] == 0) { // Calculate the lookahead window by scaling proportionally with gyro rate from 0-500dps
const timeUs_t checkInterval = MAX(lrintf(constrainf(fabsf(gyro.gyroADCf[axis]) / 500.0f, 0.0f, 1.0f) * acro_trainer_lookahead_time), ACRO_TRAINER_LOOKAHEAD_MIN); float checkInterval = constrainf(fabsf(gyro.gyroADCf[axis]) / ACRO_TRAINER_LOOKAHEAD_RATE_LIMIT, 0.0f, 1.0f) * acroTrainerLookaheadTime;
projectedAngle = (gyro.gyroADCf[axis] * checkInterval * 0.000001f) + currentAngle; checkInterval = MAX(checkInterval, ACRO_TRAINER_LOOKAHEAD_MIN_TIME);
const int8_t projectedAngleSign = acroTrainerSign(projectedAngle); projectedAngle = (gyro.gyroADCf[axis] * checkInterval) + currentAngle;
if ((fabsf(projectedAngle) > acro_trainer_angle_limit) && (projectedAngleSign == setpointSign)) { const int projectedAngleSign = acroTrainerSign(projectedAngle);
ret = ((acro_trainer_angle_limit * projectedAngleSign) - projectedAngle) * ACRO_TRAINER_GAIN; if ((fabsf(projectedAngle) > acroTrainerAngleLimit) && (projectedAngleSign == setpointSign)) {
ret = ((acroTrainerAngleLimit * projectedAngleSign) - projectedAngle) * acroTrainerGain;
resetIterm = true; resetIterm = true;
} }
} }
if (resetIterm) { if (resetIterm) {
pidData[axis].I = 0.0f; pidData[axis].I = 0;
} }
if (axis == acro_trainer_debug_axis) { if (axis == acroTrainerDebugAxis) {
DEBUG_SET(DEBUG_ACRO_TRAINER, 0, lrintf(currentAngle * 10.0f)); DEBUG_SET(DEBUG_ACRO_TRAINER, 0, lrintf(currentAngle * 10.0f));
DEBUG_SET(DEBUG_ACRO_TRAINER, 1, acroTrainerAxisState[axis]); DEBUG_SET(DEBUG_ACRO_TRAINER, 1, acroTrainerAxisState[axis]);
DEBUG_SET(DEBUG_ACRO_TRAINER, 2, lrintf(ret)); DEBUG_SET(DEBUG_ACRO_TRAINER, 2, lrintf(ret));
DEBUG_SET(DEBUG_ACRO_TRAINER, 3, lrintf(projectedAngle * 10.0f)); DEBUG_SET(DEBUG_ACRO_TRAINER, 3, lrintf(projectedAngle * 10.0f));
} }
} }
#else
UNUSED(axis);
UNUSED(angleTrim);
#endif // SIMULATOR_BUILD
return ret; return ret;
} }

1
src/main/flight/pid.h
View file

@ -131,6 +131,7 @@ typedef struct pidProfile_s {
uint8_t acro_trainer_angle_limit; // Acro trainer roll/pitch angle limit in degrees uint8_t acro_trainer_angle_limit; // Acro trainer roll/pitch angle limit in degrees
uint16_t acro_trainer_lookahead_ms; // The lookahead window in milliseconds used to reduce overshoot uint16_t acro_trainer_lookahead_ms; // The lookahead window in milliseconds used to reduce overshoot
uint8_t acro_trainer_debug_axis; // The axis for which record debugging values are captured 0=roll, 1=pitch uint8_t acro_trainer_debug_axis; // The axis for which record debugging values are captured 0=roll, 1=pitch
uint8_t acro_trainer_gain; // The strength of the limiting. Raising may reduce overshoot but also lead to oscillation around the angle limit
} pidProfile_t; } pidProfile_t;
#ifndef USE_OSD_SLAVE #ifndef USE_OSD_SLAVE

2
src/main/interface/msp_box.c
View file

@ -94,7 +94,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = {
{ BOXPIDAUDIO, "PID AUDIO", 44 }, { BOXPIDAUDIO, "PID AUDIO", 44 },
{ BOXPARALYZE, "PARALYZE", 45 }, { BOXPARALYZE, "PARALYZE", 45 },
{ BOXGPSRESCUE, "GPS RESCUE", 46 }, { BOXGPSRESCUE, "GPS RESCUE", 46 },
{ BOXACROTRAINER, "ACRO TRAINER", 47}, { BOXACROTRAINER, "ACRO TRAINER", 47 },
}; };
// mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index // mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index

1
src/main/interface/settings.c
View file

@ -771,6 +771,7 @@ const clivalue_t valueTable[] = {
{ "acro_trainer_angle_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 80 }, PG_PID_PROFILE, offsetof(pidProfile_t, acro_trainer_angle_limit) }, { "acro_trainer_angle_limit", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 10, 80 }, PG_PID_PROFILE, offsetof(pidProfile_t, acro_trainer_angle_limit) },
{ "acro_trainer_lookahead_ms", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 10, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, acro_trainer_lookahead_ms) }, { "acro_trainer_lookahead_ms", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 10, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, acro_trainer_lookahead_ms) },
{ "acro_trainer_debug_axis", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ACRO_TRAINER_DEBUG }, PG_PID_PROFILE, offsetof(pidProfile_t, acro_trainer_debug_axis) }, { "acro_trainer_debug_axis", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ACRO_TRAINER_DEBUG }, PG_PID_PROFILE, offsetof(pidProfile_t, acro_trainer_debug_axis) },
{ "acro_trainer_gain", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 25, 255 }, PG_PID_PROFILE, offsetof(pidProfile_t, acro_trainer_gain) },
#endif // USE_ACRO_TRAINER #endif // USE_ACRO_TRAINER
{ "p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].P) }, { "p_pitch", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, pid[PID_PITCH].P) },