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

Discontinuity fix (#5509) 

* 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
Show Stats 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 },
{ "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 },
{ "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 },

27
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;
}
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;
horizonGain = pidProfile->pid[PID_LEVEL].I / 10.0f;
horizonTransition = (float)pidProfile->pid[PID_LEVEL].D;
@ -426,7 +430,8 @@ static float accelerationLimit(int axis, float currentPidSetpoint)
// Based on 2DOF reference design (matlab)
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 motorMixRange = getMotorMixRange();
static timeUs_t crashDetectedAtUs;
@ -522,12 +527,20 @@ void pidController(const pidProfile_t *pidProfile, const rollAndPitchTrims_t *an
gyroRateFiltered = dtermLowpassApplyFn((filter_t *) &dtermLowpass[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)
float delta = (rD - previousRateError[axis]) / deltaT;
previousRateError[axis] = rD;
const float delta = (
dynCd * transition * (currentPidSetpoint - previousPidSetpoint[axis]) -
(gyroRateFiltered - previousGyroRateFiltered[axis])) / deltaT;
previousPidSetpoint[axis] = currentPidSetpoint;
previousGyroRateFiltered[axis] = gyroRateFiltered;
// 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
if (pidProfile->crash_recovery && !gyroOverflowDetected()) {

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

@ -319,73 +319,77 @@ static const char * const lookupOverclock[] = {
};
#endif
#define LOOKUP_TABLE_ENTRY(name) { name, ARRAYLEN(name) }
const lookupTableEntry_t lookupTables[] = {
{ lookupTableOffOn, sizeof(lookupTableOffOn) / sizeof(char *) },
{ lookupTableUnit, sizeof(lookupTableUnit) / sizeof(char *) },
{ lookupTableAlignment, sizeof(lookupTableAlignment) / sizeof(char *) },
LOOKUP_TABLE_ENTRY(lookupTableOffOn),
LOOKUP_TABLE_ENTRY(lookupTableUnit),
LOOKUP_TABLE_ENTRY(lookupTableAlignment),
#ifdef USE_GPS
{ lookupTableGPSProvider, sizeof(lookupTableGPSProvider) / sizeof(char *) },
{ lookupTableGPSSBASMode, sizeof(lookupTableGPSSBASMode) / sizeof(char *) },
LOOKUP_TABLE_ENTRY(lookupTableGPSProvider),
LOOKUP_TABLE_ENTRY(lookupTableGPSSBASMode),
#endif
#ifdef USE_BLACKBOX
{ lookupTableBlackboxDevice, sizeof(lookupTableBlackboxDevice) / sizeof(char *) },
{ lookupTableBlackboxMode, sizeof(lookupTableBlackboxMode) / sizeof(char *) },
LOOKUP_TABLE_ENTRY(lookupTableBlackboxDevice),
LOOKUP_TABLE_ENTRY(lookupTableBlackboxMode),
#endif
{ currentMeterSourceNames, sizeof(currentMeterSourceNames) / sizeof(char *) },
{ voltageMeterSourceNames, sizeof(voltageMeterSourceNames) / sizeof(char *) },
LOOKUP_TABLE_ENTRY(currentMeterSourceNames),
LOOKUP_TABLE_ENTRY(voltageMeterSourceNames),
#ifdef USE_SERVOS
{ lookupTableGimbalMode, sizeof(lookupTableGimbalMode) / sizeof(char *) },
LOOKUP_TABLE_ENTRY(lookupTableGimbalMode),
#endif
#ifdef USE_SERIAL_RX
{ lookupTableSerialRX, sizeof(lookupTableSerialRX) / sizeof(char *) },
LOOKUP_TABLE_ENTRY(lookupTableSerialRX),
#endif
#ifdef USE_RX_SPI
{ lookupTableRxSpi, sizeof(lookupTableRxSpi) / sizeof(char *) },
LOOKUP_TABLE_ENTRY(lookupTableRxSpi),
#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)
{ lookupTableGyro32khzHardwareLpf, sizeof(lookupTableGyro32khzHardwareLpf) / sizeof(char *) },
LOOKUP_TABLE_ENTRY(lookupTableGyro32khzHardwareLpf),
#endif
{ lookupTableAccHardware, sizeof(lookupTableAccHardware) / sizeof(char *) },
LOOKUP_TABLE_ENTRY(lookupTableAccHardware),
#ifdef USE_BARO
{ lookupTableBaroHardware, sizeof(lookupTableBaroHardware) / sizeof(char *) },
LOOKUP_TABLE_ENTRY(lookupTableBaroHardware),
#endif
#ifdef USE_MAG
{ lookupTableMagHardware, sizeof(lookupTableMagHardware) / sizeof(char *) },
LOOKUP_TABLE_ENTRY(lookupTableMagHardware),
#endif
{ debugModeNames, sizeof(debugModeNames) / sizeof(char *) },
{ lookupTablePwmProtocol, sizeof(lookupTablePwmProtocol) / sizeof(char *) },
{ lookupTableRcInterpolation, sizeof(lookupTableRcInterpolation) / sizeof(char *) },
{ lookupTableRcInterpolationChannels, sizeof(lookupTableRcInterpolationChannels) / sizeof(char *) },
{ lookupTableLowpassType, sizeof(lookupTableLowpassType) / sizeof(char *) },
{ lookupTableDtermLowpassType, sizeof(lookupTableDtermLowpassType) / sizeof(char *) },
{ lookupTableFailsafe, sizeof(lookupTableFailsafe) / sizeof(char *) },
{ lookupTableCrashRecovery, sizeof(lookupTableCrashRecovery) / sizeof(char *) },
LOOKUP_TABLE_ENTRY(debugModeNames),
LOOKUP_TABLE_ENTRY(lookupTablePwmProtocol),
LOOKUP_TABLE_ENTRY(lookupTableRcInterpolation),
LOOKUP_TABLE_ENTRY(lookupTableRcInterpolationChannels),
LOOKUP_TABLE_ENTRY(lookupTableLowpassType),
LOOKUP_TABLE_ENTRY(lookupTableDtermLowpassType),
LOOKUP_TABLE_ENTRY(lookupTableFailsafe),
LOOKUP_TABLE_ENTRY(lookupTableCrashRecovery),
#ifdef USE_CAMERA_CONTROL
{ lookupTableCameraControlMode, sizeof(lookupTableCameraControlMode) / sizeof(char *) },
LOOKUP_TABLE_ENTRY(lookupTableCameraControlMode),
#endif
{ lookupTableBusType, sizeof(lookupTableBusType) / sizeof(char *) },
LOOKUP_TABLE_ENTRY(lookupTableBusType),
#ifdef USE_MAX7456
{ lookupTableMax7456Clock, sizeof(lookupTableMax7456Clock) / sizeof(char *) },
LOOKUP_TABLE_ENTRY(lookupTableMax7456Clock),
#endif
#ifdef USE_RANGEFINDER
{ lookupTableRangefinderHardware, sizeof(lookupTableRangefinderHardware) / sizeof(char *) },
LOOKUP_TABLE_ENTRY(lookupTableRangefinderHardware),
#endif
#ifdef USE_GYRO_OVERFLOW_CHECK
{ lookupTableGyroOverflowCheck, sizeof(lookupTableGyroOverflowCheck) / sizeof(char *) },
LOOKUP_TABLE_ENTRY(lookupTableGyroOverflowCheck),
#endif
{ lookupTableRatesType, sizeof(lookupTableRatesType) / sizeof(char *) },
LOOKUP_TABLE_ENTRY(lookupTableRatesType),
#ifdef USE_OVERCLOCK
{ lookupOverclock, sizeof(lookupOverclock) / sizeof(char *) },
LOOKUP_TABLE_ENTRY(lookupOverclock),
#endif
#ifdef USE_LED_STRIP
{ lookupLedStripFormatRGB, sizeof(lookupLedStripFormatRGB) / sizeof(char *) },
LOOKUP_TABLE_ENTRY(lookupLedStripFormatRGB),
#endif
#ifdef USE_DUAL_GYRO
{ lookupTableGyro, sizeof(lookupTableGyro) / sizeof(char *) },
LOOKUP_TABLE_ENTRY(lookupTableGyro),
#endif
};
#undef LOOKUP_TABLE_ENTRY
const clivalue_t valueTable[] = {
// PG_GYRO_CONFIG
{ "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) },
{ "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 = { 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) },
{ "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) },