Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-17 05:15:25 +03:00
Code Issues Wiki Activity

Airmode Plus with Insane Acrobility Factor

Browse source
This commit is contained in:
borisbstyle 2016-01-06 17:18:12 +01:00
parent 42dfba8218
commit d2122e0674
4 changed files with 81 additions and 12 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/config/config.c
View file

@ -179,6 +179,7 @@ static void resetPidProfile(pidProfile_t *pidProfile)
pidProfile->gyro_soft_lpf = 1; // filtering ON by default
pidProfile->dterm_cut_hz = 0;
pidProfile->delta_from_gyro_error = 0;
pidProfile->airModeInsaneAcrobilityFactor = 0;
pidProfile->P_f[ROLL] = 1.5f; // new PID with preliminary defaults test carefully
pidProfile->I_f[ROLL] = 0.3f;

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

@ -22,6 +22,7 @@
#include <platform.h>
#include "build_config.h"
#include "debug.h"
#include "common/axis.h"
#include "common/maths.h"
@ -96,10 +97,48 @@ void setPidDeltaSamples(uint8_t filter) {
}
}
void airModePlus(airModePlus_t *axisState, int axis, pidProfile_t *pidProfile, float referenceTerm) {
float rcCommandReflection = (float)rcCommand[axis] / 500.0f;
//Ki scaler
axisState->iTermScaler = constrainf(1.0f - (1.5f * ABS(rcCommandReflection)), 0.0f, 1.0f);
//dynamic Ki handler
if (axisState->isCurrentlyAtZero) {
if (axisState->previousReferenceIsPositive ^ IS_POSITIVE(referenceTerm)) {
axisState->isCurrentlyAtZero = false;
} else {
axisState->iTermScaler = 0;
errorGyroIf[axis] = 0;
errorGyroI[axis] = 0;
}
}
if (!axisState->iTermScaler) {
if (!axisState->isCurrentlyAtZero) {
if (IS_POSITIVE(referenceTerm)) {
axisState->previousReferenceIsPositive = true;
} else {
axisState->previousReferenceIsPositive = false;
}
} else {
axisState->isCurrentlyAtZero = true;
}
}
if (axis != YAW && pidProfile->airModeInsaneAcrobilityFactor) {
axisState->wowFactor = 1.0f - (ABS(rcCommandReflection) * ((float)pidProfile->airModeInsaneAcrobilityFactor / 100.0f)); //0-1f
axisState->factor = (axisState->wowFactor * rcCommandReflection) * 1000;
} else {
axisState->wowFactor = 1;
axisState->factor = 0;
}
}
const angle_index_t rcAliasToAngleIndexMap[] = { AI_ROLL, AI_PITCH };
static filterStatePt1_t DTermState[3];
//static filterStatePt1_t yawPTermState;
static airModePlus_t airModePlusAxisState[3];
static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig,
uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig)
@ -173,7 +212,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100;
// -----calculate I component.
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + 0.5f * (lastError[axis] + RateError) * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10, -250.0f, 250.0f);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
airModePlus(&airModePlusAxisState[axis], axis, pidProfile, PTerm);
errorGyroIf[axis] *= airModePlusAxisState[axis].iTermScaler;
}
if (allowITermShrinkOnly || motorLimitReached) {
if (ABS(errorGyroIf[axis]) < ABS(previousErrorGyroIf[axis])) {
@ -190,12 +234,12 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
ITerm = errorGyroIf[axis];
//-----calculate D-term
if (!pidProfile->delta_from_gyro_error) {
delta = RateError - lastError[axis];
lastError[axis] = RateError;
} else {
if (pidProfile->delta_from_gyro_error || axis == YAW) {
delta = -(gyroRate - lastGyroRate[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyroRate[axis] = gyroRate;
} else {
delta = RateError - lastError[axis];
lastError[axis] = RateError;
}
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
@ -221,6 +265,10 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa
// -----calculate total PID output
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
axisPID[axis] = lrintf(airModePlusAxisState[axis].factor + airModePlusAxisState[axis].wowFactor * axisPID[axis]);
}
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);
@ -310,12 +358,18 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// Precision is critical, as I prevents from long-time drift. Thus, 32 bits integrator is used.
// Time correction (to avoid different I scaling for different builds based on average cycle time)
// is normalized to cycle time = 2048.
errorGyroI[axis] = errorGyroI[axis] + ((((lastError[axis] + RateError) / 2) * (uint16_t)targetLooptime) >> 11) * pidProfile->I8[axis];
errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetLooptime) >> 11) * pidProfile->I8[axis];
// limit maximum integrator value to prevent WindUp - accumulating extreme values when system is saturated.
// I coefficient (I8) moved before integration to make limiting independent from PID settings
errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13);
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
airModePlus(&airModePlusAxisState[axis], axis, pidProfile, (float) PTerm);
errorGyroI[axis] *= airModePlusAxisState[axis].iTermScaler;
}
ITerm = errorGyroI[axis] >> 13;
if (allowITermShrinkOnly || motorLimitReached) {
@ -329,12 +383,12 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
}
//-----calculate D-term
if (!pidProfile->delta_from_gyro_error) { // quick and dirty solution for testing
delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastError[axis] = RateError;
} else {
if (pidProfile->delta_from_gyro_error || axis == YAW) { // quick and dirty solution for testing
delta = -(gyroRate - lastGyroRate[axis]); // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastGyroRate[axis] = gyroRate;
} else {
delta = RateError - lastError[axis]; // 16 bits is ok here, the dif between 2 consecutive gyro reads is limited to 800
lastError[axis] = RateError;
}
// Correct difference by cycle time. Cycle time is jittery (can be different 2 times), so calculated difference
@ -359,6 +413,10 @@ static void pidRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRat
// -----calculate total PID output
axisPID[axis] = PTerm + ITerm + DTerm;
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
axisPID[axis] = lrintf(airModePlusAxisState[axis].factor + airModePlusAxisState[axis].wowFactor * axisPID[axis]);
}
#ifdef GTUNE
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
calculate_Gtune(axis);

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

@ -20,6 +20,7 @@
#define GYRO_I_MAX 256 // Gyro I limiter
#define YAW_P_LIMIT_MIN 100 // Maximum value for yaw P limiter
#define YAW_P_LIMIT_MAX 500 // Maximum value for yaw P limiter
#define IS_POSITIVE(x) ((x > 0) ? true : false)
typedef enum {
PIDROLL,
@ -57,7 +58,7 @@ typedef struct pidProfile_s {
float H_level;
uint8_t H_sensitivity;
uint16_t yaw_p_limit; // set P term limit (fixed value was 300)
uint16_t airModeInsaneAcrobilityFactor; // Air mode acrobility factor
uint8_t dterm_cut_hz; // (default 17Hz, Range 1-50Hz) Used for PT1 element in PID1, PID2 and PID5
uint8_t delta_from_gyro_error; // Used for filering Pterm noise on noisy frames
uint8_t gyro_soft_lpf; // Gyro FIR filter
@ -71,6 +72,14 @@ typedef struct pidProfile_s {
#endif
} pidProfile_t;
typedef struct airModePlus {
float factor;
float wowFactor;
float iTermScaler;
bool isCurrentlyAtZero;
bool previousReferenceIsPositive;
} airModePlus_t;
extern int16_t axisPID[XYZ_AXIS_COUNT];
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];

1
src/main/io/serial_cli.c
View file

@ -658,6 +658,7 @@ const clivalue_t valueTable[] = {
{ "gyro_soft_lpf", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.gyro_soft_lpf, .config.lookup = { TABLE_OFF_ON } },
{ "dterm_cut_hz", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_cut_hz, .config.minmax = {0, 200 } },
{ "delta_from_gyro_error", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.delta_from_gyro_error, .config.minmax = {0, 1} },
{ "insane_acrobility_factor", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.airModeInsaneAcrobilityFactor, .config.minmax = {0, 30 } },
#ifdef BLACKBOX
{ "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, .config.minmax = { 1, 32 } },