rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #6220 from ctzsnooze/AG_New 

Improved anti-gravity
This commit is contained in:
Michael Keller 2018-07-22 02:06:31 +12:00 committed by GitHub
parent 039c74c896 4439852984
commit 625b23915e
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
18 changed files with 100 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
src/main/blackbox/blackbox.c
View File

@ -1283,6 +1283,7 @@ static bool blackboxWriteSysinfo(void)
BLACKBOX_PRINT_HEADER_LINE("pidAtMinThrottle", "%d", currentPidProfile->pidAtMinThrottle);
// Betaflight PID controller parameters
BLACKBOX_PRINT_HEADER_LINE("anti_gravity_mode", "%d", currentPidProfile->antiGravityMode);
BLACKBOX_PRINT_HEADER_LINE("anti_gravity_threshold", "%d", currentPidProfile->itermThrottleThreshold);
BLACKBOX_PRINT_HEADER_LINE("anti_gravity_gain", "%d", currentPidProfile->itermAcceleratorGain);
BLACKBOX_PRINT_HEADER_LINE("setpoint_relaxation_ratio", "%d", currentPidProfile->setpointRelaxRatio);

1
src/main/build/debug.c
View File

@ -74,4 +74,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
"RC_SMOOTHING",
"RX_SIGNAL_LOSS",
"RC_SMOOTHING_RATE",
"ANTI_GRAVITY",
};

1
src/main/build/debug.h
View File

@ -92,6 +92,7 @@ typedef enum {
DEBUG_RC_SMOOTHING,
DEBUG_RX_SIGNAL_LOSS,
DEBUG_RC_SMOOTHING_RATE,
DEBUG_ANTI_GRAVITY,
DEBUG_COUNT
} debugType_e;

3
src/main/fc/fc_core.c
View File

@ -837,6 +837,9 @@ bool processRx(timeUs_t currentTimeUs)
beeper(BEEPER_RC_SMOOTHING_INIT_FAIL);
}
#endif
pidSetAntiGravityState(IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY));
return true;
}

12
src/main/fc/fc_rc.c
View File

@ -212,10 +212,12 @@ static void checkForThrottleErrorResetState(uint16_t rxRefreshRate)
const int16_t rcCommandSpeed = rcCommand[THROTTLE] - rcCommandThrottlePrevious[index];
if (ABS(rcCommandSpeed) > throttleVelocityThreshold) {
pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain));
} else {
pidSetItermAccelerator(1.0f);
if (currentPidProfile->antiGravityMode == ANTI_GRAVITY_STEP) {
if (ABS(rcCommandSpeed) > throttleVelocityThreshold) {
pidSetItermAccelerator(CONVERT_PARAMETER_TO_FLOAT(currentPidProfile->itermAcceleratorGain));
} else {
pidSetItermAccelerator(1.0f);
}
}
}
@ -536,7 +538,7 @@ FAST_CODE void processRcCommand(void)
{
uint8_t updatedChannel;
if (isRXDataNew && isAntiGravityModeActive()) {
if (isRXDataNew && pidAntiGravityEnabled()) {
checkForThrottleErrorResetState(currentRxRefreshRate);
}

4
src/main/fc/rc_modes.c
View File

@ -62,10 +62,6 @@ bool isAirmodeActive(void) {
return (IS_RC_MODE_ACTIVE(BOXAIRMODE) || feature(FEATURE_AIRMODE));
}
bool isAntiGravityModeActive(void) {
return (IS_RC_MODE_ACTIVE(BOXANTIGRAVITY) || feature(FEATURE_ANTI_GRAVITY));
}
bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range) {
if (!IS_RANGE_USABLE(range)) {
return false;

1
src/main/fc/rc_modes.h
View File

@ -126,7 +126,6 @@ bool IS_RC_MODE_ACTIVE(boxId_e boxId);
void rcModeUpdate(boxBitmask_t *newState);
bool isAirmodeActive(void);
bool isAntiGravityModeActive(void);
bool isRangeActive(uint8_t auxChannelIndex, const channelRange_t *range);
void updateActivatedModes(void);

6
src/main/flight/mixer.c
View File

@ -806,10 +806,12 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs, uint8_t vbatPidCompensa
motorMix[i] = mix;
}
pidUpdateAntiGravityThrottleFilter(throttle);
#if defined(USE_THROTTLE_BOOST)
if (throttleBoost > 0.0f) {
float throttlehpf = throttle - pt1FilterApply(&throttleLpf, throttle);
throttle = constrainf(throttle + throttleBoost * throttlehpf, 0.0f, 1.0f);
const float throttleHpf = throttle - pt1FilterApply(&throttleLpf, throttle);
throttle = constrainf(throttle + throttleBoost * throttleHpf, 0.0f, 1.0f);
}
#endif

1
src/main/flight/mixer.h
View File

@ -136,3 +136,4 @@ uint16_t convertMotorToExternal(float motorValue);
bool mixerIsTricopter(void);
void mixerSetThrottleAngleCorrection(int correctionValue);

58
src/main/flight/pid.c
View File

@ -68,6 +68,12 @@ static FAST_RAM_ZERO_INIT bool inCrashRecoveryMode = false;
static FAST_RAM_ZERO_INIT float dT;
static FAST_RAM_ZERO_INIT float pidFrequency;
static FAST_RAM_ZERO_INIT uint8_t antiGravityMode;
static FAST_RAM_ZERO_INIT float antiGravityThrottleHpf;
static FAST_RAM_ZERO_INIT uint16_t itermAcceleratorGain;
static FAST_RAM float antiGravityOsdCutoff = 1.0f;
static FAST_RAM_ZERO_INIT bool antiGravityEnabled;
PG_REGISTER_WITH_RESET_TEMPLATE(pidConfig_t, pidConfig, PG_PID_CONFIG, 2);
#ifdef STM32F10X
@ -96,6 +102,8 @@ PG_RESET_TEMPLATE(pidConfig_t, pidConfig,
#define ACRO_TRAINER_SETPOINT_LIMIT 1000.0f // Limit the correcting setpoint
#endif // USE_ACRO_TRAINER
#define ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF 15 // The anti gravity throttle highpass filter cutoff
PG_REGISTER_ARRAY_WITH_RESET_FN(pidProfile_t, MAX_PROFILE_COUNT, pidProfiles, PG_PID_PROFILE, 4);
void resetPidProfile(pidProfile_t *pidProfile)
@ -130,7 +138,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.dtermSetpointWeight = 60,
.yawRateAccelLimit = 100,
.rateAccelLimit = 0,
.itermThrottleThreshold = 350,
.itermThrottleThreshold = 250,
.itermAcceleratorGain = 5000,
.crash_time = 500, // ms
.crash_delay = 0, // ms
@ -158,6 +166,7 @@ void resetPidProfile(pidProfile_t *pidProfile)
.abs_control_gain = 0,
.abs_control_limit = 90,
.abs_control_error_limit = 20,
.antiGravityMode = ANTI_GRAVITY_SMOOTH,
);
}
@ -182,9 +191,9 @@ void pidSetItermAccelerator(float newItermAccelerator)
itermAccelerator = newItermAccelerator;
}
float pidItermAccelerator(void)
bool pidOsdAntiGravityActive(void)
{
return itermAccelerator;
return (itermAccelerator > antiGravityOsdCutoff);
}
void pidStabilisationState(pidStabilisationState_e pidControllerState)
@ -222,6 +231,8 @@ static FAST_RAM_ZERO_INIT uint8_t rcSmoothingDebugAxis;
static FAST_RAM_ZERO_INIT uint8_t rcSmoothingFilterType;
#endif // USE_RC_SMOOTHING_FILTER
static FAST_RAM_ZERO_INIT pt1Filter_t antiGravityThrottleLpf;
void pidInitFilters(const pidProfile_t *pidProfile)
{
BUILD_BUG_ON(FD_YAW != 2); // only setting up Dterm filters on roll and pitch axes, so ensure yaw axis is 2
@ -306,6 +317,8 @@ void pidInitFilters(const pidProfile_t *pidProfile)
}
}
#endif
pt1FilterInit(&antiGravityThrottleLpf, pt1FilterGain(ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, dT));
}
#ifdef USE_RC_SMOOTHING_FILTER
@ -402,6 +415,13 @@ static FAST_RAM_ZERO_INIT int acroTrainerAxisState[2]; // only need roll and pi
static FAST_RAM_ZERO_INIT float acroTrainerGain;
#endif // USE_ACRO_TRAINER
void pidUpdateAntiGravityThrottleFilter(float throttle)
{
if (antiGravityMode) {
antiGravityThrottleHpf = throttle - pt1FilterApply(&antiGravityThrottleLpf, throttle);
}
}
void pidInitConfig(const pidProfile_t *pidProfile)
{
for (int axis = FD_ROLL; axis <= FD_YAW; axis++) {
@ -426,6 +446,7 @@ void pidInitConfig(const pidProfile_t *pidProfile)
maxVelocity[FD_YAW] = pidProfile->yawRateAccelLimit * 100 * dT;
const float ITermWindupPoint = (float)pidProfile->itermWindupPointPercent / 100.0f;
ITermWindupPointInv = 1.0f / (1.0f - ITermWindupPoint);
itermAcceleratorGain = pidProfile->itermAcceleratorGain;
crashTimeLimitUs = pidProfile->crash_time * 1000;
crashTimeDelayUs = pidProfile->crash_delay * 1000;
crashRecoveryAngleDeciDegrees = pidProfile->crash_recovery_angle * 10;
@ -439,6 +460,18 @@ void pidInitConfig(const pidProfile_t *pidProfile)
throttleBoost = pidProfile->throttle_boost * 0.1f;
#endif
itermRotation = pidProfile->iterm_rotation;
antiGravityMode = pidProfile->antiGravityMode;
// Calculate the anti-gravity value that will trigger the OSD display.
// For classic AG it's either 1.0 for off and > 1.0 for on.
// For the new AG it's a continuous floating value so we want to trigger the OSD
// display when it exceeds 25% of its possible range. This gives a useful indication
// of AG activity without excessive display.
antiGravityOsdCutoff = 1.0f;
if (antiGravityMode == ANTI_GRAVITY_SMOOTH) {
antiGravityOsdCutoff += ((itermAcceleratorGain - 1000) / 1000.0f) * 0.25f;
}
#if defined(USE_SMART_FEEDFORWARD)
smartFeedforward = pidProfile->smart_feedforward;
#endif
@ -779,6 +812,11 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, const rollAndPitchT
#endif
// Dynamic i component,
if ((antiGravityMode == ANTI_GRAVITY_SMOOTH) && antiGravityEnabled) {
itermAccelerator = 1 + fabsf(antiGravityThrottleHpf) * 0.01f * (itermAcceleratorGain - 1000);
DEBUG_SET(DEBUG_ANTI_GRAVITY, 1, lrintf(antiGravityThrottleHpf * 1000));
}
DEBUG_SET(DEBUG_ANTI_GRAVITY, 0, lrintf(itermAccelerator * 1000));
// gradually scale back integration when above windup point
const float dynCi = MIN((1.0f - motorMixRange) * ITermWindupPointInv, 1.0f) * dT * itermAccelerator;
@ -1028,3 +1066,17 @@ void pidSetAcroTrainerState(bool newState)
}
}
#endif // USE_ACRO_TRAINER
void pidSetAntiGravityState(bool newState)
{
if (newState != antiGravityEnabled) {
// reset the accelerator on state changes
itermAccelerator = 1.0f;
}
antiGravityEnabled = newState;
}
bool pidAntiGravityEnabled(void)
{
return antiGravityEnabled;
}

16
src/main/flight/pid.h
View File

@ -76,6 +76,11 @@ typedef struct pid8_s {
uint8_t D;
} pid8_t;
typedef enum {
ANTI_GRAVITY_SMOOTH,
ANTI_GRAVITY_STEP
} antiGravityMode_e;
typedef enum {
ITERM_RELAX_OFF,
ITERM_RELAX_RP,
@ -108,6 +113,7 @@ typedef struct pidProfile_s {
uint8_t horizon_tilt_expert_mode; // OFF or ON
// Betaflight PID controller parameters
uint8_t antiGravityMode; // type of anti gravity method
uint16_t itermThrottleThreshold; // max allowed throttle delta before iterm accelerated in ms
uint16_t itermAcceleratorGain; // Iterm Accelerator Gain when itermThrottlethreshold is hit
uint16_t dtermSetpointWeight; // Setpoint weight for Dterm (0= measurement, 1= full error, 1 > aggressive derivative)
@ -128,8 +134,8 @@ typedef struct pidProfile_s {
uint8_t crash_recovery; // off, on, on and beeps when it is in crash recovery mode
uint8_t throttle_boost; // how much should throttle be boosted during transient changes 0-100, 100 adds 10x hpf filtered throttle
uint8_t throttle_boost_cutoff; // Which cutoff frequency to use for throttle boost. higher cutoffs keep the boost on for shorter. Specified in hz.
uint8_t iterm_rotation; // rotates iterm to translate world errors to local coordinate system
uint8_t smart_feedforward; // takes only the larger of P and the D weight feed forward term if they have the same sign.
uint8_t iterm_rotation; // rotates iterm to translate world errors to local coordinate system
uint8_t smart_feedforward; // takes only the larger of P and the D weight feed forward term if they have the same sign.
uint8_t iterm_relax_type; // Specifies type of relax algorithm
uint8_t iterm_relax_cutoff; // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint
uint8_t iterm_relax; // Enable iterm suppression during stick input
@ -176,7 +182,6 @@ extern pt1Filter_t throttleLpf;
void pidResetITerm(void);
void pidStabilisationState(pidStabilisationState_e pidControllerState);
void pidSetItermAccelerator(float newItermAccelerator);
float pidItermAccelerator(void);
void pidInitFilters(const pidProfile_t *pidProfile);
void pidInitConfig(const pidProfile_t *pidProfile);
void pidInit(const pidProfile_t *pidProfile);
@ -186,3 +191,8 @@ void pidAcroTrainerInit(void);
void pidSetAcroTrainerState(bool newState);
void pidInitSetpointDerivativeLpf(uint16_t filterCutoff, uint8_t debugAxis, uint8_t filterType);
void pidUpdateSetpointDerivativeLpf(uint16_t filterCutoff);
void pidUpdateAntiGravityThrottleFilter(float throttle);
bool pidOsdAntiGravityActive(void);
bool pidOsdAntiGravityMode(void);
void pidSetAntiGravityState(bool newState);
bool pidAntiGravityEnabled(void);

8
src/main/interface/settings.c
View File

@ -273,6 +273,10 @@ static const char * const lookupTableDtermLowpassType[] = {
"BIQUAD",
};
static const char * const lookupTableAntiGravityMode[] = {
"SMOOTH",
"STEP",
};
static const char * const lookupTableFailsafe[] = {
"AUTO-LAND", "DROP", "GPS-RESCUE"
@ -413,6 +417,7 @@ const lookupTableEntry_t lookupTables[] = {
LOOKUP_TABLE_ENTRY(lookupTableRcInterpolationChannels),
LOOKUP_TABLE_ENTRY(lookupTableLowpassType),
LOOKUP_TABLE_ENTRY(lookupTableDtermLowpassType),
LOOKUP_TABLE_ENTRY(lookupTableAntiGravityMode),
LOOKUP_TABLE_ENTRY(lookupTableFailsafe),
LOOKUP_TABLE_ENTRY(lookupTableFailsafeSwitchMode),
LOOKUP_TABLE_ENTRY(lookupTableCrashRecovery),
@ -784,6 +789,7 @@ const clivalue_t valueTable[] = {
{ "dterm_notch_cutoff", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 16000 }, PG_PID_PROFILE, offsetof(pidProfile_t, dterm_notch_cutoff) },
{ "vbat_pid_gain", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, vbatPidCompensation) },
{ "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, pidAtMinThrottle) },
{ "anti_gravity_mode", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ANTI_GRAVITY_MODE }, PG_PID_PROFILE, offsetof(pidProfile_t, antiGravityMode) },
{ "anti_gravity_threshold", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 20, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermThrottleThreshold) },
{ "anti_gravity_gain", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 1000, 30000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermAcceleratorGain) },
{ "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, setpointRelaxRatio) },
@ -807,7 +813,7 @@ const clivalue_t valueTable[] = {
#if defined(USE_ITERM_RELAX)
{ "iterm_relax", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ITERM_RELAX }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax) },
{ "iterm_relax_type", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ITERM_RELAX_TYPE }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_type) },
{ "iterm_relax_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff) },
{ "iterm_relax_cutoff", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, iterm_relax_cutoff) },
#endif
{ "iterm_windup", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 30, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermWindupPointPercent) },
{ "iterm_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermLimit) },

1
src/main/interface/settings.h
View File

@ -68,6 +68,7 @@ typedef enum {
TABLE_RC_INTERPOLATION_CHANNELS,
TABLE_LOWPASS_TYPE,
TABLE_DTERM_LOWPASS_TYPE,
TABLE_ANTI_GRAVITY_MODE,
TABLE_FAILSAFE,
TABLE_FAILSAFE_SWITCH_MODE,
TABLE_CRASH_RECOVERY,

2
src/main/io/osd.c
View File

@ -599,7 +599,7 @@ static bool osdDrawSingleElement(uint8_t item)
case OSD_ANTI_GRAVITY:
{
if (pidItermAccelerator() > 1.0f) {
if (pidOsdAntiGravityActive()) {
strcpy(buff, "AG");
}

1
src/test/unit/arming_prevention_unittest.cc
View File

@ -850,4 +850,5 @@ extern "C" {
void rescheduleTask(cfTaskId_e, uint32_t) {}
bool usbCableIsInserted(void) { return false; }
bool usbVcpIsConnected(void) { return false; }
void pidSetAntiGravityState(bool) {}
}

2
src/test/unit/osd_unittest.cc
View File

@ -1036,5 +1036,5 @@ extern "C" {
return false;
}
float pidItermAccelerator(void) { return 1.0; }
bool pidOsdAntiGravityActive(void) { return false; }
}

3
src/test/unit/pid_unittest.cc
View File

@ -105,7 +105,8 @@ void setDefaultTestSettings(void) {
pidProfile->dtermSetpointWeight = 0;
pidProfile->yawRateAccelLimit = 100;
pidProfile->rateAccelLimit = 0;
pidProfile->itermThrottleThreshold = 350;
pidProfile->antiGravityMode = ANTI_GRAVITY_SMOOTH;
pidProfile->itermThrottleThreshold = 250;
pidProfile->itermAcceleratorGain = 1000;
pidProfile->crash_time = 500;
pidProfile->crash_delay = 0;

1
src/test/unit/vtx_unittest.cc
View File

@ -172,4 +172,5 @@ extern "C" {
void rescheduleTask(cfTaskId_e, uint32_t) {}
bool usbCableIsInserted(void) { return false; }
bool usbVcpIsConnected(void) { return false; }
void pidSetAntiGravityState(bool newState) { UNUSED(newState); }
}