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

Discontinuity fix (#5509)

Browse source 
* use continuous feed-forward

* use continuous feed-forward

* formatting
This commit is contained in:
joelucid 2018-03-23 02:13:32 +01:00 • committed by Michael Keller
parent a0add440f0
commit 98a77dcd96
3 changed files with 60 additions and 43 deletions
Download patch file Download diff file Expand all files Collapse all files

2
src/main/cms/cms_menu_imu.c
View file

@ -283,7 +283,7 @@ static OSD_Entry cmsx_menuProfileOtherEntries[] = {
{ "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString, 0 }, { "-- OTHER PP --", OME_Label, NULL, pidProfileIndexString, 0 },
{ "D SETPT WT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_dtermSetpointWeight, 0, 255, 1, 10 }, 0 }, { "D SETPT WT", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_dtermSetpointWeight, 0, 255, 1, 10 }, 0 },
{ "SETPT TRS", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_setpointRelaxRatio, 1, 100, 1, 10 }, 0 }, { "SETPT TRS", OME_FLOAT, NULL, &(OSD_FLOAT_t) { &cmsx_setpointRelaxRatio, 0, 100, 1, 10 }, 0 },
{ "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleStrength, 0, 200, 1 } , 0 }, { "ANGLE STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_angleStrength, 0, 200, 1 } , 0 },
{ "HORZN STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonStrength, 0, 200, 1 } , 0 }, { "HORZN STR", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonStrength, 0, 200, 1 } , 0 },
{ "HORZN TRS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonTransition, 0, 200, 1 } , 0 }, { "HORZN TRS", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_horizonTransition, 0, 200, 1 } , 0 },

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

@ -293,7 +293,11 @@ void pidInitConfig(const pidProfile_t *pidProfile)
Kd[axis] = DTERM_SCALE * pidProfile->pid[axis].D; Kd[axis] = DTERM_SCALE * pidProfile->pid[axis].D;
} }
dtermSetpointWeight = pidProfile->dtermSetpointWeight / 127.0f; dtermSetpointWeight = pidProfile->dtermSetpointWeight / 127.0f;
relaxFactor = 1.0f / (pidProfile->setpointRelaxRatio / 100.0f); if (pidProfile->setpointRelaxRatio == 0) {
relaxFactor = 0;
} else {
relaxFactor = 100.0f / pidProfile->setpointRelaxRatio;
}
levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f; levelGain = pidProfile->pid[PID_LEVEL].P / 10.0f;
horizonGain = pidProfile->pid[PID_LEVEL].I / 10.0f; horizonGain = pidProfile->pid[PID_LEVEL].I / 10.0f;
horizonTransition = (float)pidProfile->pid[PID_LEVEL].D; horizonTransition = (float)pidProfile->pid[PID_LEVEL].D;
@ -426,7 +430,8 @@ static float accelerationLimit(int axis, float currentPidSetpoint)
// Based on 2DOF reference design (matlab) // Based on 2DOF reference design (matlab)
void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs) void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *angleTrim, timeUs_t currentTimeUs)
{ {
static float previousRateError[2]; static float previousGyroRateFiltered[2];
static float previousPidSetpoint[2];
const float tpaFactor = getThrottlePIDAttenuation(); const float tpaFactor = getThrottlePIDAttenuation();
const float motorMixRange = getMotorMixRange(); const float motorMixRange = getMotorMixRange();
static timeUs_t crashDetectedAtUs; static timeUs_t crashDetectedAtUs;
@ -522,11 +527,19 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
gyroRateFiltered = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateFiltered); gyroRateFiltered = dtermLowpassApplyFn((filter_t *) &dtermLowpass[axis], gyroRateFiltered);
gyroRateFiltered = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateFiltered); gyroRateFiltered = dtermLowpass2ApplyFn((filter_t *) &dtermLowpass2[axis], gyroRateFiltered);
const float rD = dynCd * MIN(getRcDeflectionAbs(axis) * relaxFactor, 1.0f) * currentPidSetpoint - gyroRateFiltered; // cr - y // no transition if relaxFactor == 0
float transition = 1;
if (relaxFactor > 0) {
transition = getRcDeflectionAbs(axis) * relaxFactor;
}
// Divide rate change by deltaT to get differential (ie dr/dt) // Divide rate change by deltaT to get differential (ie dr/dt)
float delta = (rD - previousRateError[axis]) / deltaT; const float delta = (
dynCd * transition * (currentPidSetpoint - previousPidSetpoint[axis]) -
(gyroRateFiltered - previousGyroRateFiltered[axis])) / deltaT;
previousPidSetpoint[axis] = currentPidSetpoint;
previousGyroRateFiltered[axis] = gyroRateFiltered;
previousRateError[axis] = rD;
// if crash recovery is on and accelerometer enabled and there is no gyro overflow, then check for a crash // if crash recovery is on and accelerometer enabled and there is no gyro overflow, then check for a crash
// no point in trying to recover if the crash is so severe that the gyro overflows // no point in trying to recover if the crash is so severe that the gyro overflows

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

@ -319,73 +319,77 @@ static const char * const lookupOverclock[] = {
}; };
#endif #endif
#define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
const lookupTableEntry_t lookupTables[] = { const lookupTableEntry_t lookupTables[] = {
{ lookupTableOffOn, sizeof(lookupTableOffOn) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableOffOn),
{ lookupTableUnit, sizeof(lookupTableUnit) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableUnit),
{ lookupTableAlignment, sizeof(lookupTableAlignment) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableAlignment),
#ifdef USE_GPS #ifdef USE_GPS
{ lookupTableGPSProvider, sizeof(lookupTableGPSProvider) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableGPSProvider),
{ lookupTableGPSSBASMode, sizeof(lookupTableGPSSBASMode) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableGPSSBASMode),
#endif #endif
#ifdef USE_BLACKBOX #ifdef USE_BLACKBOX
{ lookupTableBlackboxDevice, sizeof(lookupTableBlackboxDevice) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableBlackboxDevice),
{ lookupTableBlackboxMode, sizeof(lookupTableBlackboxMode) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableBlackboxMode),
#endif #endif
{ currentMeterSourceNames, sizeof(currentMeterSourceNames) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(currentMeterSourceNames),
{ voltageMeterSourceNames, sizeof(voltageMeterSourceNames) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(voltageMeterSourceNames),
#ifdef USE_SERVOS #ifdef USE_SERVOS
{ lookupTableGimbalMode, sizeof(lookupTableGimbalMode) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableGimbalMode),
#endif #endif
#ifdef USE_SERIAL_RX #ifdef USE_SERIAL_RX
{ lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableSerialRX),
#endif #endif
#ifdef USE_RX_SPI #ifdef USE_RX_SPI
{ lookupTableRxSpi, sizeof(lookupTableRxSpi) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableRxSpi),
#endif #endif
{ lookupTableGyroHardwareLpf, sizeof(lookupTableGyroHardwareLpf) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableGyroHardwareLpf),
#if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689) #if defined(USE_GYRO_SPI_MPU6500) || defined(USE_GYRO_SPI_MPU9250) || defined(USE_GYRO_SPI_ICM20689)
{ lookupTableGyro32khzHardwareLpf, sizeof(lookupTableGyro32khzHardwareLpf) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableGyro32khzHardwareLpf),
#endif #endif
{ lookupTableAccHardware, sizeof(lookupTableAccHardware) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableAccHardware),
#ifdef USE_BARO #ifdef USE_BARO
{ lookupTableBaroHardware, sizeof(lookupTableBaroHardware) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableBaroHardware),
#endif #endif
#ifdef USE_MAG #ifdef USE_MAG
{ lookupTableMagHardware, sizeof(lookupTableMagHardware) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableMagHardware),
#endif #endif
{ debugModeNames, sizeof(debugModeNames) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(debugModeNames),
{ lookupTablePwmProtocol, sizeof(lookupTablePwmProtocol) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTablePwmProtocol),
{ lookupTableRcInterpolation, sizeof(lookupTableRcInterpolation) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableRcInterpolation),
{ lookupTableRcInterpolationChannels, sizeof(lookupTableRcInterpolationChannels) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableRcInterpolationChannels),
{ lookupTableLowpassType, sizeof(lookupTableLowpassType) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableLowpassType),
{ lookupTableDtermLowpassType, sizeof(lookupTableDtermLowpassType) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableDtermLowpassType),
{ lookupTableFailsafe, sizeof(lookupTableFailsafe) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableFailsafe),
{ lookupTableCrashRecovery, sizeof(lookupTableCrashRecovery) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableCrashRecovery),
#ifdef USE_CAMERA_CONTROL #ifdef USE_CAMERA_CONTROL
{ lookupTableCameraControlMode, sizeof(lookupTableCameraControlMode) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableCameraControlMode),
#endif #endif
{ lookupTableBusType, sizeof(lookupTableBusType) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableBusType),
#ifdef USE_MAX7456 #ifdef USE_MAX7456
{ lookupTableMax7456Clock, sizeof(lookupTableMax7456Clock) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableMax7456Clock),
#endif #endif
#ifdef USE_RANGEFINDER #ifdef USE_RANGEFINDER
{ lookupTableRangefinderHardware, sizeof(lookupTableRangefinderHardware) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableRangefinderHardware),
#endif #endif
#ifdef USE_GYRO_OVERFLOW_CHECK #ifdef USE_GYRO_OVERFLOW_CHECK
{ lookupTableGyroOverflowCheck, sizeof(lookupTableGyroOverflowCheck) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableGyroOverflowCheck),
#endif #endif
{ lookupTableRatesType, sizeof(lookupTableRatesType) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableRatesType),
#ifdef USE_OVERCLOCK #ifdef USE_OVERCLOCK
{ lookupOverclock, sizeof(lookupOverclock) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupOverclock),
#endif #endif
#ifdef USE_LED_STRIP #ifdef USE_LED_STRIP
{ lookupLedStripFormatRGB, sizeof(lookupLedStripFormatRGB) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupLedStripFormatRGB),
#endif #endif
#ifdef USE_DUAL_GYRO #ifdef USE_DUAL_GYRO
{ lookupTableGyro, sizeof(lookupTableGyro) / sizeof(char *) }, LOOKUP_TABLE_ENTRY(lookupTableGyro),
#endif #endif
}; };
#undef LOOKUP_TABLE_ENTRY
const clivalue_t valueTable[] = { const clivalue_t valueTable[] = {
// PG_GYRO_CONFIG // PG_GYRO_CONFIG
{ "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_align) }, { "align_gyro", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_ALIGNMENT }, PG_GYRO_CONFIG, offsetof(gyroConfig_t, gyro_align) },
@ -690,7 +694,7 @@ const clivalue_t valueTable[] = {
{ "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, pidAtMinThrottle) }, { "pid_at_min_throttle", VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_PID_PROFILE, offsetof(pidProfile_t, pidAtMinThrottle) },
{ "anti_gravity_threshold", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 20, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, itermThrottleThreshold) }, { "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) }, { "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 = { 1, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, setpointRelaxRatio) }, { "setpoint_relax_ratio", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, setpointRelaxRatio) },
{ "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 254 }, PG_PID_PROFILE, offsetof(pidProfile_t, dtermSetpointWeight) }, { "dterm_setpoint_weight", VAR_UINT8 | PROFILE_VALUE, .config.minmax = { 0, 254 }, PG_PID_PROFILE, offsetof(pidProfile_t, dtermSetpointWeight) },
{ "acc_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawRateAccelLimit) }, { "acc_limit_yaw", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawRateAccelLimit) },
{ "acc_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, rateAccelLimit) }, { "acc_limit", VAR_UINT16 | PROFILE_VALUE, .config.minmax = { 0, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, rateAccelLimit) },