rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-06 21:54:15 +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 ComputeStabilizationAndSetPidsFromDampAndNoise(float damp, float noise);
static void FlightModeSettingsUpdatedCb(UAVObjEvent *ev); static void FlightModeSettingsUpdatedCb(UAVObjEvent *ev);
static void InitSystemIdent(bool loadDefaults); static void InitSystemIdent(bool loadDefaults);
static void UpdateSmoothQuickSource(uint8_t smoothQuickSource, bool loadDefaults);
static void ProportionPidsSmoothToQuick(); 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 UpdateSystemIdentState(const float *X, const float *noise, float dT_s, uint32_t predicts, uint32_t spills, float hover_throttle);
static void UpdateStabilizationDesired(bool doingIdent); static void UpdateStabilizationDesired(bool doingIdent);
@ -249,6 +250,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
uint32_t measureTime = 0; uint32_t measureTime = 0;
uint32_t updateCounter = 0; uint32_t updateCounter = 0;
enum AUTOTUNE_STATE state = AT_INIT; enum AUTOTUNE_STATE state = AT_INIT;
uint8_t currentSmoothQuickSource = 0;
bool saveSiNeeded = false; bool saveSiNeeded = false;
bool savePidNeeded = false; bool savePidNeeded = false;
@ -327,6 +329,15 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
saveSiNeeded = true; 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 // 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) // (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()) { if (accessoryToUse != -1 && systemIdentSettings.Complete && !CheckSettings()) {
AccessoryDesiredData accessoryValue; AccessoryDesiredData accessoryValue;
AccessoryDesiredInstGet(accessoryToUse, &accessoryValue); AccessoryDesiredInstGet(accessoryToUse, &accessoryValue);
// if the accessory changed more than some percent of total range // if the accessory changed more than 2 percent of total range (~20µs)
// some old PPM receivers use a low resolution chip which only allows about 180 steps out of a range of 2.0 // the smoothQuickValue will be changed
// a test Taranis transmitter knob has about 0.0233 slop out of a range of 2.0 if (fabsf(smoothQuickValue - accessoryValue.AccessoryVal) > 0.02f) {
// 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)) {
smoothQuickValue = accessoryValue.AccessoryVal; smoothQuickValue = accessoryValue.AccessoryVal;
// calculate PIDs based on new smoothQuickValue and save to the PID bank // calculate PIDs based on new smoothQuickValue and save to the PID bank
ProportionPidsSmoothToQuick(); ProportionPidsSmoothToQuick();
@ -727,8 +735,14 @@ static void InitSystemIdent(bool loadDefaults)
gyroReadTimeAverageAlpha = 0.99999988f; gyroReadTimeAverageAlpha = 0.99999988f;
gyroReadTimeAverage = u.systemIdentState.GyroReadTimeAverage; gyroReadTimeAverage = u.systemIdentState.GyroReadTimeAverage;
uint8_t SmoothQuickSource = systemIdentSettings.SmoothQuickSource; UpdateSmoothQuickSource(systemIdentSettings.SmoothQuickSource, loadDefaults);
switch (SmoothQuickSource) { }
// 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 + 0: // use accessory0
case SMOOTH_QUICK_ACCESSORY_BASE + 1: // use accessory1 case SMOOTH_QUICK_ACCESSORY_BASE + 1: // use accessory1
case SMOOTH_QUICK_ACCESSORY_BASE + 2: // use accessory2 case SMOOTH_QUICK_ACCESSORY_BASE + 2: // use accessory2
@ -737,7 +751,7 @@ static void InitSystemIdent(bool loadDefaults)
// disable PID changing with flight mode switch // disable PID changing with flight mode switch
flightModeSwitchTogglePosition = -1; flightModeSwitchTogglePosition = -1;
// enable PID changing with accessory0-3 // enable PID changing with accessory0-3
accessoryToUse = SmoothQuickSource - SMOOTH_QUICK_ACCESSORY_BASE; accessoryToUse = smoothQuickSource - SMOOTH_QUICK_ACCESSORY_BASE;
break; break;
case SMOOTH_QUICK_TOGGLE_BASE + 3: // use flight mode switch toggle with 3 points 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 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; smoothQuickValue = 0.0f;
} }
// enable PID changing with flight mode switch // 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 // disable PID changing with accessory0-3
accessoryToUse = -1; accessoryToUse = -1;
break; break;