rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-30 15:52:12 +01:00
Code Issues Releases Activity

LP-489 Apply SmoothQuickSource changes

Browse Source
This commit is contained in:
Laurent Lalanne 2017-03-01 16:12:42 +01:00
parent f73dc05029
commit 7207e95447
1 changed files with 24 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

34
flight/modules/AutoTune/autotune.c
View File

@ -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;