diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index 6630ea953..a5cf0ecc1 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -156,6 +156,7 @@ static uint8_t CheckSettingsRaw(); static void ComputeStabilizationAndSetPidsFromDampAndNoise(float damp, float noise); static void FlightModeSettingsUpdatedCb(UAVObjEvent *ev); static void InitSystemIdent(bool loadDefaults); +static void UpdateSmoothQuickSource(uint8_t smoothQuickSource, bool loadDefaults); static void ProportionPidsSmoothToQuick(); static void UpdateSystemIdentState(const float *X, const float *noise, float dT_s, uint32_t predicts, uint32_t spills, float hover_throttle); static void UpdateStabilizationDesired(bool doingIdent); @@ -249,6 +250,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) uint32_t measureTime = 0; uint32_t updateCounter = 0; enum AUTOTUNE_STATE state = AT_INIT; + uint8_t currentSmoothQuickSource = 0; bool saveSiNeeded = false; bool savePidNeeded = false; @@ -327,6 +329,15 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) saveSiNeeded = true; } + // Check if the SmoothQuickSource changed, + // allow config changes without reboot or reinit + uint8_t smoothQuickSource; + SystemIdentSettingsSmoothQuickSourceGet(&smoothQuickSource); + if (smoothQuickSource != currentSmoothQuickSource) { + UpdateSmoothQuickSource(smoothQuickSource, true); + currentSmoothQuickSource = smoothQuickSource; + } + ////////////////////////////////////////////////////////////////////////////////////// // if configured to use a slider for smooth-quick and the autotune module is running // (note that the module can be automatically or manually enabled) @@ -355,12 +366,9 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) if (accessoryToUse != -1 && systemIdentSettings.Complete && !CheckSettings()) { AccessoryDesiredData accessoryValue; AccessoryDesiredInstGet(accessoryToUse, &accessoryValue); - // if the accessory changed more than some percent of total range - // some old PPM receivers use a low resolution chip which only allows about 180 steps out of a range of 2.0 - // a test Taranis transmitter knob has about 0.0233 slop out of a range of 2.0 - // what we are doing here does not need any higher precision than that - // user must move the knob more than 1/85th of the total range (of 2.0) for it to register as changed - if (fabsf(smoothQuickValue - accessoryValue.AccessoryVal) > (2.0f / 85.0f)) { + // if the accessory changed more than 2 percent of total range (~20µs) + // the smoothQuickValue will be changed + if (fabsf(smoothQuickValue - accessoryValue.AccessoryVal) > 0.02f) { smoothQuickValue = accessoryValue.AccessoryVal; // calculate PIDs based on new smoothQuickValue and save to the PID bank ProportionPidsSmoothToQuick(); @@ -727,8 +735,14 @@ static void InitSystemIdent(bool loadDefaults) gyroReadTimeAverageAlpha = 0.99999988f; gyroReadTimeAverage = u.systemIdentState.GyroReadTimeAverage; - uint8_t SmoothQuickSource = systemIdentSettings.SmoothQuickSource; - switch (SmoothQuickSource) { + UpdateSmoothQuickSource(systemIdentSettings.SmoothQuickSource, loadDefaults); +} + + +// Update SmoothQuickSource to be used +static void UpdateSmoothQuickSource(uint8_t smoothQuickSource, bool loadDefaults) +{ + switch (smoothQuickSource) { case SMOOTH_QUICK_ACCESSORY_BASE + 0: // use accessory0 case SMOOTH_QUICK_ACCESSORY_BASE + 1: // use accessory1 case SMOOTH_QUICK_ACCESSORY_BASE + 2: // use accessory2 @@ -737,7 +751,7 @@ static void InitSystemIdent(bool loadDefaults) // disable PID changing with flight mode switch flightModeSwitchTogglePosition = -1; // enable PID changing with accessory0-3 - accessoryToUse = SmoothQuickSource - SMOOTH_QUICK_ACCESSORY_BASE; + accessoryToUse = smoothQuickSource - SMOOTH_QUICK_ACCESSORY_BASE; break; case SMOOTH_QUICK_TOGGLE_BASE + 3: // use flight mode switch toggle with 3 points case SMOOTH_QUICK_TOGGLE_BASE + 5: // use flight mode switch toggle with 5 points @@ -748,7 +762,7 @@ static void InitSystemIdent(bool loadDefaults) smoothQuickValue = 0.0f; } // enable PID changing with flight mode switch - flightModeSwitchTogglePosition = (SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2; + flightModeSwitchTogglePosition = (smoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2; // disable PID changing with accessory0-3 accessoryToUse = -1; break;