Iterm reset rework // Airmode Iterm Protection Configurable // Dynamic Ki
New defaults
This commit is contained in:
parent
8149508352
commit
ff5c320b4a
|
@ -1280,24 +1280,24 @@ static bool blackboxWriteSysinfo()
|
|||
masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_average_count);
|
||||
break;
|
||||
case 36:
|
||||
blackboxPrintfHeaderLine("dynamic_pterm:%d",
|
||||
masterConfig.profile[masterConfig.current_profile_index].pidProfile.dynamic_pterm);
|
||||
blackboxPrintfHeaderLine("dynamic_pid:%d",
|
||||
masterConfig.profile[masterConfig.current_profile_index].pidProfile.dynamic_pid);
|
||||
break;
|
||||
case 37:
|
||||
blackboxPrintfHeaderLine("rollPitchItermResetRate:%d",
|
||||
masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermResetRate);
|
||||
masterConfig.profile[masterConfig.current_profile_index].pidProfile.rollPitchItermIgnoreRate);
|
||||
break;
|
||||
case 38:
|
||||
blackboxPrintfHeaderLine("yawItermResetRate:%d",
|
||||
masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermResetRate);
|
||||
masterConfig.profile[masterConfig.current_profile_index].pidProfile.yawItermIgnoreRate);
|
||||
break;
|
||||
case 39:
|
||||
blackboxPrintfHeaderLine("dterm_lpf_hz:%d",
|
||||
(int)(masterConfig.profile[masterConfig.current_profile_index].pidProfile.dterm_lpf_hz * 100.0f));
|
||||
break;
|
||||
case 40:
|
||||
blackboxPrintfHeaderLine("iterm_reset_offset:%d",
|
||||
masterConfig.profile[masterConfig.current_profile_index].pidProfile.itermResetOffset);
|
||||
blackboxPrintfHeaderLine("airmode_activate_throttle:%d",
|
||||
masterConfig.rxConfig.airModeActivateThreshold);
|
||||
break;
|
||||
case 41:
|
||||
blackboxPrintfHeaderLine("deadband:%d", masterConfig.rcControlsConfig.deadband);
|
||||
|
|
|
@ -140,7 +140,7 @@ static uint32_t activeFeaturesLatch = 0;
|
|||
static uint8_t currentControlRateProfileIndex = 0;
|
||||
controlRateConfig_t *currentControlRateProfile;
|
||||
|
||||
static const uint8_t EEPROM_CONF_VERSION = 135;
|
||||
static const uint8_t EEPROM_CONF_VERSION = 136;
|
||||
|
||||
static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
|
||||
{
|
||||
|
@ -154,13 +154,13 @@ static void resetPidProfile(pidProfile_t *pidProfile)
|
|||
pidProfile->pidController = 1;
|
||||
|
||||
pidProfile->P8[ROLL] = 45;
|
||||
pidProfile->I8[ROLL] = 35;
|
||||
pidProfile->I8[ROLL] = 55;
|
||||
pidProfile->D8[ROLL] = 18;
|
||||
pidProfile->P8[PITCH] = 45;
|
||||
pidProfile->I8[PITCH] = 35;
|
||||
pidProfile->I8[PITCH] = 55;
|
||||
pidProfile->D8[PITCH] = 18;
|
||||
pidProfile->P8[YAW] = 90;
|
||||
pidProfile->I8[YAW] = 40;
|
||||
pidProfile->I8[YAW] = 50;
|
||||
pidProfile->D8[YAW] = 0;
|
||||
pidProfile->P8[PIDALT] = 50;
|
||||
pidProfile->I8[PIDALT] = 0;
|
||||
|
@ -184,12 +184,10 @@ static void resetPidProfile(pidProfile_t *pidProfile)
|
|||
|
||||
pidProfile->yaw_p_limit = YAW_P_LIMIT_MAX;
|
||||
pidProfile->yaw_lpf_hz = 80;
|
||||
pidProfile->rollPitchItermResetRate = 200;
|
||||
pidProfile->rollPitchItermResetAlways = 0;
|
||||
pidProfile->yawItermResetRate = 50;
|
||||
pidProfile->itermResetOffset = 15;
|
||||
pidProfile->rollPitchItermIgnoreRate = 200;
|
||||
pidProfile->yawItermIgnoreRate = 50;;
|
||||
pidProfile->dterm_lpf_hz = 110; // filtering ON by default
|
||||
pidProfile->dynamic_pterm = 1;
|
||||
pidProfile->dynamic_pid = 1;
|
||||
|
||||
#ifdef GTUNE
|
||||
pidProfile->gtune_lolimP[ROLL] = 10; // [0..200] Lower limit of ROLL P during G tune.
|
||||
|
@ -462,6 +460,7 @@ static void resetConf(void)
|
|||
masterConfig.rxConfig.fpvCamAngleDegrees = 0;
|
||||
masterConfig.rxConfig.max_aux_channel = 6;
|
||||
masterConfig.rxConfig.superExpoFactor = 30;
|
||||
masterConfig.rxConfig.airModeActivateThreshold = 1350;
|
||||
masterConfig.rxConfig.superExpoFactorYaw = 30;
|
||||
masterConfig.rxConfig.superExpoYawMode = 0;
|
||||
|
||||
|
|
|
@ -822,7 +822,6 @@ void mixTable(void)
|
|||
throttleRange = throttleMax - throttleMin;
|
||||
|
||||
if (rollPitchYawMixRange > throttleRange) {
|
||||
motorLimitReached = true;
|
||||
mixReduction = qConstruct(throttleRange, rollPitchYawMixRange);
|
||||
|
||||
for (i = 0; i < motorCount; i++) {
|
||||
|
@ -833,7 +832,6 @@ void mixTable(void)
|
|||
|
||||
if (debugMode == DEBUG_AIRMODE && i < 3) debug[1] = rollPitchYawMixRange;
|
||||
} else {
|
||||
motorLimitReached = false;
|
||||
throttleMin = throttleMin + (rollPitchYawMixRange / 2);
|
||||
throttleMax = throttleMax - (rollPitchYawMixRange / 2);
|
||||
}
|
||||
|
|
|
@ -189,7 +189,6 @@ extern int16_t servo[MAX_SUPPORTED_SERVOS];
|
|||
bool isMixerUsingServos(void);
|
||||
void writeServos(void);
|
||||
void filterServos(void);
|
||||
bool motorLimitReached;
|
||||
#endif
|
||||
|
||||
extern int16_t motor[MAX_SUPPORTED_MOTORS];
|
||||
|
|
|
@ -49,7 +49,6 @@
|
|||
#include "config/runtime_config.h"
|
||||
|
||||
extern uint8_t motorCount;
|
||||
extern bool motorLimitReached;
|
||||
uint32_t targetPidLooptime;
|
||||
|
||||
int16_t axisPID[3];
|
||||
|
@ -61,13 +60,11 @@ int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
|
|||
// PIDweight is a scale factor for PIDs which is derived from the throttle and TPA setting, and 100 = 100% scale means no PID reduction
|
||||
uint8_t PIDweight[3];
|
||||
|
||||
static int32_t errorGyroI[3], errorGyroILimit[3];
|
||||
static int32_t errorGyroI[3];
|
||||
#ifndef SKIP_PID_LUXFLOAT
|
||||
static float errorGyroIf[3], errorGyroIfLimit[3];
|
||||
static float errorGyroIf[3];
|
||||
#endif
|
||||
|
||||
static bool lowThrottlePidReduction;
|
||||
|
||||
static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRateConfig_t *controlRateConfig,
|
||||
uint16_t max_angle_inclination, const rollAndPitchTrims_t *angleTrim, const rxConfig_t *rxConfig);
|
||||
|
||||
|
@ -101,23 +98,29 @@ uint16_t getDynamicKp(int axis, const pidProfile_t *pidProfile) {
|
|||
return dynamicKp;
|
||||
}
|
||||
|
||||
void pidResetErrorGyroState(uint8_t resetOption)
|
||||
uint16_t getDynamicKi(int axis, const pidProfile_t *pidProfile) {
|
||||
uint16_t dynamicKi;
|
||||
uint16_t resetRate;
|
||||
|
||||
resetRate = (axis == YAW) ? pidProfile->yawItermIgnoreRate : pidProfile->rollPitchItermIgnoreRate;
|
||||
|
||||
uint32_t dynamicFactor = (1 << 8) - constrain(ABS(gyroADC[axis] << 8) / resetRate, 0, 1 << 8);
|
||||
|
||||
dynamicKi = (pidProfile->I8[axis] * dynamicFactor) >> 8;
|
||||
|
||||
return dynamicKi;
|
||||
}
|
||||
|
||||
void pidResetErrorGyroState(void)
|
||||
{
|
||||
if (resetOption >= RESET_ITERM) {
|
||||
int axis;
|
||||
|
||||
for (axis = 0; axis < 3; axis++) {
|
||||
errorGyroI[axis] = 0;
|
||||
#ifndef SKIP_PID_LUXFLOAT
|
||||
errorGyroIf[axis] = 0.0f;
|
||||
#endif
|
||||
}
|
||||
}
|
||||
|
||||
if (resetOption == RESET_ITERM_AND_REDUCE_PID) {
|
||||
lowThrottlePidReduction = true;
|
||||
} else {
|
||||
lowThrottlePidReduction = false;
|
||||
}
|
||||
}
|
||||
|
||||
float getdT (void) {
|
||||
|
@ -202,7 +205,7 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_
|
|||
// multiplication of rcCommand corresponds to changing the sticks scaling here
|
||||
RateError = AngleRate - gyroRate;
|
||||
|
||||
uint16_t kP = (pidProfile->dynamic_pterm) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis];
|
||||
uint16_t kP = (pidProfile->dynamic_pid) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis];
|
||||
|
||||
// -----calculate P component
|
||||
if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) {
|
||||
|
@ -217,21 +220,9 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_
|
|||
}
|
||||
|
||||
// -----calculate I component.
|
||||
errorGyroIf[axis] = constrainf(errorGyroIf[axis] + luxITermScale * RateError * getdT() * pidProfile->I8[axis], -250.0f, 250.0f);
|
||||
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile) : pidProfile->I8[axis];
|
||||
|
||||
if ((pidProfile->rollPitchItermResetAlways || IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) && axis != YAW) {
|
||||
if (ABS(gyroRate / 4.1f) >= pidProfile->rollPitchItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -pidProfile->itermResetOffset, pidProfile->itermResetOffset);
|
||||
}
|
||||
|
||||
if (axis == YAW) {
|
||||
if (ABS(gyroRate / 4.1f) >= pidProfile->yawItermResetRate) errorGyroIf[axis] = constrainf(errorGyroIf[axis], -YAW_ITERM_RESET_OFFSET, YAW_ITERM_RESET_OFFSET);
|
||||
}
|
||||
|
||||
if (antiWindupProtection || motorLimitReached) {
|
||||
errorGyroIf[axis] = constrainf(errorGyroIf[axis], -errorGyroIfLimit[axis], errorGyroIfLimit[axis]);
|
||||
} else {
|
||||
errorGyroIfLimit[axis] = ABS(errorGyroIf[axis]);
|
||||
}
|
||||
errorGyroIf[axis] = constrainf(kI + luxITermScale * RateError * getdT() * pidProfile->I8[axis], -250.0f, 250.0f);
|
||||
|
||||
// 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
|
||||
|
@ -257,8 +248,6 @@ static void pidLuxFloat(const pidProfile_t *pidProfile, const controlRateConfig_
|
|||
// -----calculate total PID output
|
||||
axisPID[axis] = constrain(lrintf(PTerm + ITerm + DTerm), -1000, 1000);
|
||||
|
||||
if (lowThrottlePidReduction) axisPID[axis] /= 3;
|
||||
|
||||
#ifdef GTUNE
|
||||
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
|
||||
calculate_Gtune(axis);
|
||||
|
@ -333,7 +322,7 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
|
|||
gyroRate = gyroADC[axis] / 4;
|
||||
RateError = AngleRateTmp - gyroRate;
|
||||
|
||||
uint16_t kP = (pidProfile->dynamic_pterm) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis];
|
||||
uint16_t kP = (pidProfile->dynamic_pid) ? getDynamicKp(axis, pidProfile) : pidProfile->P8[axis];
|
||||
|
||||
// -----calculate P component
|
||||
if ((IS_RC_MODE_ACTIVE(BOXSUPEREXPO) && axis != YAW) || (axis == YAW && rxConfig->superExpoYawMode == SUPEREXPO_YAW_ALWAYS)) {
|
||||
|
@ -352,26 +341,14 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
|
|||
// 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] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * pidProfile->I8[axis];
|
||||
uint16_t kI = (pidProfile->dynamic_pid) ? getDynamicKi(axis, pidProfile) : pidProfile->I8[axis];
|
||||
|
||||
errorGyroI[axis] = errorGyroI[axis] + ((RateError * (uint16_t)targetPidLooptime) >> 11) * kI;
|
||||
|
||||
// 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 ((pidProfile->rollPitchItermResetAlways || IS_RC_MODE_ACTIVE(BOXSUPEREXPO)) && axis != YAW) {
|
||||
if (ABS(gyroRate *10 / 41) >= pidProfile->rollPitchItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) -pidProfile->itermResetOffset << 13, (int32_t) + pidProfile->itermResetOffset << 13);
|
||||
}
|
||||
|
||||
if (axis == YAW) {
|
||||
if (ABS(gyroRate * 10 / 41) >= pidProfile->yawItermResetRate) errorGyroI[axis] = constrain(errorGyroI[axis], (int32_t) -YAW_ITERM_RESET_OFFSET << 13, (int32_t) + YAW_ITERM_RESET_OFFSET << 13);
|
||||
}
|
||||
|
||||
if (antiWindupProtection || motorLimitReached) {
|
||||
errorGyroI[axis] = constrain(errorGyroI[axis], -errorGyroILimit[axis], errorGyroILimit[axis]);
|
||||
} else {
|
||||
errorGyroILimit[axis] = ABS(errorGyroI[axis]);
|
||||
}
|
||||
|
||||
ITerm = errorGyroI[axis] >> 13;
|
||||
|
||||
//-----calculate D-term
|
||||
|
@ -394,8 +371,6 @@ static void pidMultiWiiRewrite(const pidProfile_t *pidProfile, const controlRate
|
|||
// -----calculate total PID output
|
||||
axisPID[axis] = PTerm + ITerm + DTerm;
|
||||
|
||||
if (lowThrottlePidReduction) axisPID[axis] /= 3;
|
||||
|
||||
#ifdef GTUNE
|
||||
if (FLIGHT_MODE(GTUNE_MODE) && ARMING_FLAG(ARMED)) {
|
||||
calculate_Gtune(axis);
|
||||
|
|
|
@ -22,7 +22,6 @@
|
|||
#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 YAW_ITERM_RESET_OFFSET 15 // May be made configurable in the future, but not really needed for yaw
|
||||
#define DYNAMIC_PTERM_STICK_THRESHOLD 400
|
||||
|
||||
typedef enum {
|
||||
|
@ -50,12 +49,6 @@ typedef enum {
|
|||
DELTA_FROM_MEASUREMENT
|
||||
} pidDeltaType_e;
|
||||
|
||||
typedef enum {
|
||||
RESET_DISABLE = 0,
|
||||
RESET_ITERM,
|
||||
RESET_ITERM_AND_REDUCE_PID
|
||||
} pidErrorResetOption_e;
|
||||
|
||||
typedef enum {
|
||||
SUPEREXPO_YAW_OFF = 0,
|
||||
SUPEREXPO_YAW_ON,
|
||||
|
@ -71,15 +64,13 @@ typedef struct pidProfile_s {
|
|||
uint8_t I8[PID_ITEM_COUNT];
|
||||
uint8_t D8[PID_ITEM_COUNT];
|
||||
|
||||
uint8_t itermResetOffset; // Reset offset for Iterm
|
||||
uint16_t dterm_lpf_hz; // Delta Filter in hz
|
||||
uint16_t yaw_lpf_hz; // Additional yaw filter when yaw axis too noisy
|
||||
uint16_t rollPitchItermResetRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates
|
||||
uint8_t rollPitchItermResetAlways; // Reset Iterm also without SUPER EXPO
|
||||
uint16_t yawItermResetRate; // Experimental threshold for resetting iterm for yaw on certain rates
|
||||
uint16_t rollPitchItermIgnoreRate; // Experimental threshold for resetting iterm for pitch and roll on certain rates
|
||||
uint16_t yawItermIgnoreRate; // Experimental threshold for resetting iterm for yaw on certain rates
|
||||
uint16_t yaw_p_limit;
|
||||
uint8_t dterm_average_count; // Configurable delta count for dterm
|
||||
uint8_t dynamic_pterm;
|
||||
uint8_t dynamic_pid; // Dynamic PID implementation (currently only P and I)
|
||||
|
||||
#ifdef GTUNE
|
||||
uint8_t gtune_lolimP[3]; // [0..200] Lower limit of P during G tune
|
||||
|
@ -98,10 +89,10 @@ typedef void (*pidControllerFuncPtr)(const pidProfile_t *pidProfile, const struc
|
|||
|
||||
extern int16_t axisPID[XYZ_AXIS_COUNT];
|
||||
extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
|
||||
bool antiWindupProtection;
|
||||
bool airmodeWasActivated;
|
||||
extern uint32_t targetPidLooptime;
|
||||
|
||||
void pidSetController(pidControllerType_e type);
|
||||
void pidResetErrorGyroState(uint8_t resetOption);
|
||||
void pidResetErrorGyroState(void);
|
||||
void setTargetPidLooptime(uint8_t pidProcessDenom);
|
||||
|
||||
|
|
|
@ -124,15 +124,6 @@ throttleStatus_e calculateThrottleStatus(rxConfig_t *rxConfig, uint16_t deadband
|
|||
return THROTTLE_HIGH;
|
||||
}
|
||||
|
||||
rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig)
|
||||
{
|
||||
if (((rcData[PITCH] < (rxConfig->midrc + AIRMODEDEADBAND)) && (rcData[PITCH] > (rxConfig->midrc -AIRMODEDEADBAND)))
|
||||
&& ((rcData[ROLL] < (rxConfig->midrc + AIRMODEDEADBAND)) && (rcData[ROLL] > (rxConfig->midrc -AIRMODEDEADBAND))))
|
||||
return CENTERED;
|
||||
|
||||
return NOT_CENTERED;
|
||||
}
|
||||
|
||||
void processRcStickPositions(rxConfig_t *rxConfig, throttleStatus_e throttleStatus, bool disarm_kill_switch)
|
||||
{
|
||||
static uint8_t rcDelayCommand; // this indicates the number of time (multiple of RC measurement at 50Hz) the sticks must be maintained to run or switch off motors
|
||||
|
|
|
@ -252,4 +252,3 @@ bool isUsingSticksForArming(void);
|
|||
|
||||
int32_t getRcStickDeflection(int32_t axis, uint16_t midrc);
|
||||
bool isModeActivationConditionPresent(modeActivationCondition_t *modeActivationConditions, boxId_e modeId);
|
||||
rollPitchStatus_e calculateRollPitchCenterStatus(rxConfig_t *rxConfig);
|
||||
|
|
|
@ -698,6 +698,7 @@ const clivalue_t valueTable[] = {
|
|||
{ "super_expo_factor", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.superExpoFactor, .config.minmax = {1, 100 } },
|
||||
{ "super_expo_factor_yaw", VAR_UINT8 | MASTER_VALUE, &masterConfig.rxConfig.superExpoFactorYaw, .config.minmax = {1, 100 } },
|
||||
{ "super_expo_yaw", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.rxConfig.superExpoYawMode, .config.lookup = { TABLE_SUPEREXPO_YAW } },
|
||||
{ "airmode_activate_throttle", VAR_UINT16 | MASTER_VALUE, &masterConfig.rxConfig.airModeActivateThreshold, .config.minmax = {1000, 2000 } },
|
||||
|
||||
{ "failsafe_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_delay, .config.minmax = { 0, 200 } },
|
||||
{ "failsafe_off_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_off_delay, .config.minmax = { 0, 200 } },
|
||||
|
@ -730,11 +731,9 @@ const clivalue_t valueTable[] = {
|
|||
{ "mag_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mag_hardware, .config.lookup = { TABLE_MAG_HARDWARE } },
|
||||
{ "mag_declination", VAR_INT16 | MASTER_VALUE, &masterConfig.mag_declination, .config.minmax = { -18000, 18000 } },
|
||||
{ "dterm_lowpass", VAR_INT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.dterm_lpf_hz, .config.minmax = {0, 500 } },
|
||||
{ "dynamic_pterm", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dynamic_pterm, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "iterm_always_reset", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.rollPitchItermResetAlways, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermResetRate, .config.minmax = {50, 1000 } },
|
||||
{ "yaw_iterm_reset_degrees", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermResetRate, .config.minmax = {25, 1000 } },
|
||||
{ "iterm_reset_offset", VAR_UINT8 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.itermResetOffset, .config.minmax = { 0, 100 } },
|
||||
{ "dynamic_pid", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, &masterConfig.profile[0].pidProfile.dynamic_pid, .config.lookup = { TABLE_OFF_ON } },
|
||||
{ "iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.rollPitchItermIgnoreRate, .config.minmax = {50, 1000 } },
|
||||
{ "yaw_iterm_ignore_threshold", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yawItermIgnoreRate, .config.minmax = {25, 1000 } },
|
||||
{ "yaw_lowpass", VAR_UINT16 | PROFILE_VALUE, &masterConfig.profile[0].pidProfile.yaw_lpf_hz, .config.minmax = {0, 500 } },
|
||||
{ "pid_process_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.pid_process_denom, .config.minmax = { 1, 8 } },
|
||||
|
||||
|
|
|
@ -101,6 +101,8 @@ enum {
|
|||
|
||||
#define GYRO_WATCHDOG_DELAY 80 // delay for gyro sync
|
||||
|
||||
#define AIRMODE_THOTTLE_THRESHOLD 1350 // Make configurable in the future. ~35% throttle should be fine
|
||||
|
||||
uint16_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
|
||||
|
||||
int16_t magHold;
|
||||
|
@ -113,7 +115,6 @@ static uint32_t disarmAt; // Time of automatic disarm when "Don't spin the m
|
|||
|
||||
extern uint32_t currentTime;
|
||||
extern uint8_t PIDweight[3];
|
||||
extern bool antiWindupProtection;
|
||||
|
||||
uint16_t filteredCycleTime;
|
||||
static bool isRXDataNew;
|
||||
|
@ -453,6 +454,7 @@ void updateMagHold(void)
|
|||
void processRx(void)
|
||||
{
|
||||
static bool armedBeeperOn = false;
|
||||
static bool airmodeIsActivated;
|
||||
|
||||
calculateRxChannelsAndUpdateFailsafe(currentTime);
|
||||
|
||||
|
@ -474,27 +476,17 @@ void processRx(void)
|
|||
}
|
||||
|
||||
throttleStatus_e throttleStatus = calculateThrottleStatus(&masterConfig.rxConfig, masterConfig.flight3DConfig.deadband3d_throttle);
|
||||
rollPitchStatus_e rollPitchStatus = calculateRollPitchCenterStatus(&masterConfig.rxConfig);
|
||||
|
||||
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) && ARMING_FLAG(ARMED)) {
|
||||
if (rcCommand[THROTTLE] >= masterConfig.rxConfig.airModeActivateThreshold) airmodeIsActivated = true; // Prevent Iterm from being reset
|
||||
} else {
|
||||
airmodeIsActivated = false;
|
||||
}
|
||||
|
||||
/* In airmode Iterm should be prevented to grow when Low thottle and Roll + Pitch Centered.
|
||||
This is needed to prevent Iterm winding on the ground, but keep full stabilisation on 0 throttle while in air */
|
||||
if (throttleStatus == THROTTLE_LOW) {
|
||||
if (IS_RC_MODE_ACTIVE(BOXAIRMODE) && !failsafeIsActive() && ARMING_FLAG(ARMED)) {
|
||||
if (rollPitchStatus == CENTERED) {
|
||||
antiWindupProtection = true;
|
||||
} else {
|
||||
antiWindupProtection = false;
|
||||
}
|
||||
} else {
|
||||
if (IS_RC_MODE_ACTIVE(BOXAIRMODE)) {
|
||||
pidResetErrorGyroState(RESET_ITERM);
|
||||
} else {
|
||||
pidResetErrorGyroState(RESET_ITERM_AND_REDUCE_PID);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
pidResetErrorGyroState(RESET_DISABLE);
|
||||
antiWindupProtection = false;
|
||||
if (throttleStatus == THROTTLE_LOW && !airmodeIsActivated) {
|
||||
pidResetErrorGyroState();
|
||||
}
|
||||
|
||||
// When armed and motors aren't spinning, do beeps and then disarm
|
||||
|
|
|
@ -127,6 +127,7 @@ typedef struct rxConfig_s {
|
|||
uint8_t superExpoFactor; // Super Expo Factor
|
||||
uint8_t superExpoFactorYaw; // Super Expo Factor Yaw
|
||||
uint8_t superExpoYawMode; // Seperate Super expo for yaw
|
||||
uint16_t airModeActivateThreshold; // Throttle setpoint where airmode gets activated
|
||||
|
||||
uint16_t rx_min_usec;
|
||||
uint16_t rx_max_usec;
|
||||
|
|
|
@ -17,7 +17,7 @@
|
|||
|
||||
#define FC_VERSION_MAJOR 2 // increment when a major release is made (big new feature, etc)
|
||||
#define FC_VERSION_MINOR 7 // increment when a minor release is made (small new feature, change etc)
|
||||
#define FC_VERSION_PATCH_LEVEL 1 // increment when a bug is fixed
|
||||
#define FC_VERSION_PATCH_LEVEL 2 // increment when a bug is fixed
|
||||
|
||||
#define STR_HELPER(x) #x
|
||||
#define STR(x) STR_HELPER(x)
|
||||
|
|
Loading…
Reference in New Issue