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-76 auto module enable, more sanity checks, SmoothQuick tweaks

Browse Source
This commit is contained in:
Cliff Geerdes 2016-05-14 00:03:26 -04:00
parent 03b06daef6
commit 68c3d5030a
3 changed files with 110 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
flight/libraries/sanitycheck.c
View File

@ -167,6 +167,9 @@ int32_t configuration_check()
#if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) #if !defined(PIOS_EXCLUDE_ADVANCED_FEATURES)
case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE: case FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE:
ADDSEVERITY(!gps_assisted); ADDSEVERITY(!gps_assisted);
// it would be fun to try autotune on a fixed wing
// but that should only be attempted by devs at first
ADDSEVERITY(multirotor);
break; break;
#endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */ #endif /* !defined(PIOS_EXCLUDE_ADVANCED_FEATURES) */
default: default:

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

@ -39,6 +39,7 @@
#include <flightstatus.h> #include <flightstatus.h>
#include <manualcontrolcommand.h> #include <manualcontrolcommand.h>
#include <manualcontrolsettings.h> #include <manualcontrolsettings.h>
#include <flightmodesettings.h>
#include <gyrostate.h> #include <gyrostate.h>
#include <actuatordesired.h> #include <actuatordesired.h>
#include <stabilizationdesired.h> #include <stabilizationdesired.h>
@ -145,7 +146,7 @@ static void UpdateSystemIdentState(const float *X, const float *noise, float dT_
static void UpdateStabilizationDesired(bool doingIdent); static void UpdateStabilizationDesired(bool doingIdent);
static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode); static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode);
static void InitSystemIdent(bool loadDefaults); static void InitSystemIdent(bool loadDefaults);
static void InitSmoothQuick(); static void InitSmoothQuick(bool loadToggle);
/** /**
@ -161,11 +162,26 @@ int32_t AutoTuneInitialize(void)
HwSettingsOptionalModulesData optionalModules; HwSettingsOptionalModulesData optionalModules;
HwSettingsOptionalModulesGet(&optionalModules); HwSettingsOptionalModulesGet(&optionalModules);
if (optionalModules.AutoTune == HWSETTINGS_OPTIONALMODULES_ENABLED) { if (optionalModules.AutoTune == HWSETTINGS_OPTIONALMODULES_ENABLED) {
// even though the AutoTune module is automatically enabled
// (below, when the flight mode switch is configured to use autotune)
// there are use cases where the user may even want it enabled without being on the FMS
// that allows PIDs to be adjusted in flight
moduleEnabled = true; moduleEnabled = true;
} else { } else {
// if the user did not enable the autotune module
// do it for them if they have autotune on their flight mode switch
FlightModeSettingsFlightModePositionOptions fms[FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM];
moduleEnabled = false; moduleEnabled = false;
FlightModeSettingsInitialize();
FlightModeSettingsFlightModePositionGet(fms);
for (uint8_t i = 0; i < FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_NUMELEM; ++i) {
if (fms[i] == FLIGHTMODESETTINGS_FLIGHTMODEPOSITION_AUTOTUNE) {
moduleEnabled = true;
break;
}
}
} }
#endif #endif /* ifdef MODULE_AutoTune_BUILTIN */
if (moduleEnabled) { if (moduleEnabled) {
SystemIdentSettingsInitialize(); SystemIdentSettingsInitialize();
@ -226,7 +242,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
// based on what is in SystemIdent // based on what is in SystemIdent
// so that the user can use the PID smooth->quick slider in following flights // so that the user can use the PID smooth->quick slider in following flights
InitSystemIdent(false); InitSystemIdent(false);
InitSmoothQuick(); InitSmoothQuick(true);
while (1) { while (1) {
uint32_t diffTime; uint32_t diffTime;
@ -287,14 +303,27 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
savePidNeeded = true; savePidNeeded = true;
} }
//////////////////////////////////////////////////////////////////////////////////////
// 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)
// then the smooth-quick slider is always active (when not actually in autotune mode)
//
// when the slider is active it will immediately change the PIDs
// and it will schedule the PIDs to be written to permanent storage
//
// if the FC is disarmed, the perm write will happen on next loop
// but if the FC is armed, the perm write will only occur when the FC goes disarmed
//////////////////////////////////////////////////////////////////////////////////////
// we don't want it saving to permanent storage many times
// while the user is moving the knob once, so wait till the knob stops moving
static uint8_t savePidDelay;
// any time we are not in AutoTune mode: // any time we are not in AutoTune mode:
// - the user may be using the accessory0-3 knob/slider to request PID changes // - the user may be using the accessory0-3 knob/slider to request PID changes
// - the state machine needs to be reset // - the state machine needs to be reset
// - the local version of Attitude mode gets skipped // - the local version of Attitude mode gets skipped
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) { if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_AUTOTUNE) {
// we don't want it saving to permanent storage many times static bool savePidActive = false;
// while the user is moving the knob once, so wait till the knob stops moving
static uint8_t savePidDelay;
// if accessory0-3 is configured as a PID changing slider/knob over the smooth to quick range // if accessory0-3 is configured as a PID changing slider/knob over the smooth to quick range
// and FC is not currently running autotune // and FC is not currently running autotune
// and accessory0-3 changed by at least 1/160 of full range (2) // and accessory0-3 changed by at least 1/160 of full range (2)
@ -309,16 +338,27 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
// some old PPM receivers use a low resolution chip which only allows about 180 steps // some old PPM receivers use a low resolution chip which only allows about 180 steps
// what we are doing here does not need any higher precision than that // what we are doing here does not need any higher precision than that
if (fabsf(accessoryValueOld.AccessoryVal - accessoryValue.AccessoryVal) > (2.0f / 160.0f)) { if (fabsf(accessoryValueOld.AccessoryVal - accessoryValue.AccessoryVal) > (2.0f / 160.0f)) {
accessoryValueOld = accessoryValue; accessoryValueOld = accessoryValue;
// this copies the PIDs to memory and makes them active // this copies the PIDs to memory and makes them active
// but does not write them to permanent storage // but does not write them to permanent storage
ProportionPidsSmoothToQuick(-1.0f, accessoryValue.AccessoryVal, 1.0f); ProportionPidsSmoothToQuick(-1.0f, accessoryValue.AccessoryVal, 1.0f);
// this schedules the first possible write of the PIDs to occur later (a fraction of a second) // don't save PID to perm storage the first time
savePidDelay = SMOOTH_QUICK_FLUSH_TICKS; // that usually means at power up
//
// that keeps it from writing the same value at each boot
// but means that it won't be permanent if they move the slider before FC power on
// (note that the PIDs will be changed, just not saved permanently)
if (savePidActive) {
// this schedules the first possible write of the PIDs to occur a fraction of a second or so from now
// and moves the scheduled time if it is already scheduled
savePidDelay = SMOOTH_QUICK_FLUSH_TICKS;
} else {
savePidActive = true;
}
} else if (savePidDelay && --savePidDelay == 0) { } else if (savePidDelay && --savePidDelay == 0) {
// this flags that the PIDs can be written to permanent storage right now // this flags that the PIDs can be written to permanent storage right now
// but they will only be written when the FC is disarmed // but they will only be written when the FC is disarmed
// so this means immediate or wait till FC is disarmed // so this means immediate (after NOT_AT_MODE_DELAY_MS) or wait till FC is disarmed
savePidNeeded = true; savePidNeeded = true;
} }
} else { } else {
@ -327,6 +367,8 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
state = AT_INIT; state = AT_INIT;
vTaskDelay(NOT_AT_MODE_DELAY_MS / portTICK_RATE_MS); vTaskDelay(NOT_AT_MODE_DELAY_MS / portTICK_RATE_MS);
continue; continue;
} else {
savePidDelay = 0;
} }
switch (state) { switch (state) {
@ -355,9 +397,15 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
// load SystemIdentSettings so that they can change it // load SystemIdentSettings so that they can change it
// and do smooth-quick on changed values // and do smooth-quick on changed values
InitSystemIdent(false); InitSystemIdent(false);
// no InitSmoothQuick() here or the toggle switch gets reset even in a "quick toggle" that should increment it InitSmoothQuick(false);
state = AT_INIT_DELAY2; // wait for FC to arm in case they are doing this without a flight mode switch
lastUpdateTime = xTaskGetTickCount(); // that causes the 2+ second delay that follows to happen after arming
// which gives them a chance to take off before the shakes start
// the FC must be armed and if we check here it also allows switchless setup to use autotune
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
state = AT_INIT_DELAY2;
lastUpdateTime = xTaskGetTickCount();
}
} }
break; break;
@ -373,21 +421,18 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
doingIdent = true; doingIdent = true;
// after an additional .5 seconds start capturing data // after an additional .5 seconds start capturing data
if (diffTime > INIT_TIME_DELAY2_MS) { if (diffTime > INIT_TIME_DELAY2_MS) {
// Only start when armed and flying // Reset save status
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { // save SI data even if partial or bad, aids in diagnostics
// Reset save status saveSiNeeded = true;
// save SI data even if partial or bad, aids in diagnostics // don't save PIDs until data gathering is complete
saveSiNeeded = true; // and the complete data has been sanity checked
// don't save PIDs until data gathering is complete savePidNeeded = false;
// and the complete data has been sanity checked InitSystemIdent(true);
savePidNeeded = false; InitSmoothQuick(true);
InitSystemIdent(true); AfInit(gX, gP);
InitSmoothQuick(); UpdateSystemIdentState(gX, NULL, 0.0f, 0, 0, 0.0f);
AfInit(gX, gP); measureTime = (uint32_t)systemIdentSettings.TuningDuration * (uint32_t)1000;
UpdateSystemIdentState(gX, NULL, 0.0f, 0, 0, 0.0f); state = AT_START;
measureTime = (uint32_t)systemIdentSettings.TuningDuration * (uint32_t)1000;
state = AT_START;
}
} }
} }
break; break;
@ -587,13 +632,13 @@ static void InitSystemIdent(bool loadDefaults)
// so that if they are changed there (mainly for future code changes), they will be changed here too // so that if they are changed there (mainly for future code changes), they will be changed here too
SystemIdentStateSetDefaults(SystemIdentStateHandle(), 0); SystemIdentStateSetDefaults(SystemIdentStateHandle(), 0);
SystemIdentStateGet(&systemIdentState); SystemIdentStateGet(&systemIdentState);
// Tau Beta and the Complete flag get default values // Tau, Beta, and the Complete flag get default values
// in preparation for running AutoTune // in preparation for running AutoTune
systemIdentSettings.Tau = systemIdentState.Tau; systemIdentSettings.Tau = systemIdentState.Tau;
memcpy(&systemIdentSettings.Beta, &systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData)); memcpy(&systemIdentSettings.Beta, &systemIdentState.Beta, sizeof(SystemIdentSettingsBetaData));
systemIdentSettings.Complete = systemIdentState.Complete; systemIdentSettings.Complete = systemIdentState.Complete;
} else { } else {
// Tau Beta and the Complete flag get stored values // Tau, Beta, and the Complete flag get stored values
// so the user can fly another battery to select and test PIDs with the slider/knob // so the user can fly another battery to select and test PIDs with the slider/knob
systemIdentState.Tau = systemIdentSettings.Tau; systemIdentState.Tau = systemIdentSettings.Tau;
memcpy(&systemIdentState.Beta, &systemIdentSettings.Beta, sizeof(SystemIdentStateBetaData)); memcpy(&systemIdentState.Beta, &systemIdentSettings.Beta, sizeof(SystemIdentStateBetaData));
@ -602,29 +647,42 @@ static void InitSystemIdent(bool loadDefaults)
} }
static void InitSmoothQuick() static void InitSmoothQuick(bool loadToggle)
{ {
uint8_t SmoothQuickSource = systemIdentSettings.SmoothQuickSource; uint8_t SmoothQuickSource = systemIdentSettings.SmoothQuickSource;
// default to disable PID changing with flight mode switch and accessory0-3
accessoryToUse = -1;
flightModeSwitchTogglePosition = -1;
systemIdentSettings.SmoothQuickSource = SMOOTH_QUICK_DISABLED;
switch (SmoothQuickSource) { 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
case SMOOTH_QUICK_ACCESSORY_BASE + 3: // use accessory3 case SMOOTH_QUICK_ACCESSORY_BASE + 3: // use accessory3
// disable PID changing with flight mode switch
// ignore loadToggle if user is also switching to use knob as source
flightModeSwitchTogglePosition = -1;
accessoryToUse = SmoothQuickSource - SMOOTH_QUICK_ACCESSORY_BASE; accessoryToUse = SmoothQuickSource - SMOOTH_QUICK_ACCESSORY_BASE;
systemIdentSettings.SmoothQuickSource = SmoothQuickSource; systemIdentSettings.SmoothQuickSource = SmoothQuickSource;
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
case SMOOTH_QUICK_TOGGLE_BASE + 7: // use flight mode switch toggle with 7 points case SMOOTH_QUICK_TOGGLE_BASE + 7: // use flight mode switch toggle with 7 points
// first test PID is in the middle of the smooth -> quick range // disable PID changing with accessory0-3
flightModeSwitchTogglePosition = (SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2; accessoryToUse = -1;
// don't allow init of current toggle position in the middle of 3x fms toggle
if (loadToggle) {
// first test PID is in the middle of the smooth -> quick range
flightModeSwitchTogglePosition = (SmoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2;
}
systemIdentSettings.SmoothQuickSource = SmoothQuickSource; systemIdentSettings.SmoothQuickSource = SmoothQuickSource;
break; break;
case SMOOTH_QUICK_DISABLED:
default:
// disable PID changing with flight mode switch
// ignore loadToggle since user is disabling toggle
flightModeSwitchTogglePosition = -1;
// disable PID changing with accessory0-3
accessoryToUse = -1;
systemIdentSettings.SmoothQuickSource = SMOOTH_QUICK_DISABLED;
break;
} }
} }
@ -642,7 +700,7 @@ static void UpdateSystemIdentState(const float *X, const float *noise,
systemIdentState.Bias.Pitch = X[11]; systemIdentState.Bias.Pitch = X[11];
systemIdentState.Bias.Yaw = X[12]; systemIdentState.Bias.Yaw = X[12];
systemIdentState.Tau = X[9]; systemIdentState.Tau = X[9];
// 'settings' beta and tau have same value as state versions // 'settings' beta and tau have same value as 'state' versions
// the state version produces a GCS log // the state version produces a GCS log
// the settings version is remembered after power off/on // the settings version is remembered after power off/on
systemIdentSettings.Tau = systemIdentState.Tau; systemIdentSettings.Tau = systemIdentState.Tau;

18
shared/uavobjectdefinition/systemidentsettings.xml
View File

@ -36,14 +36,18 @@
<!-- <field name="YawPIDRatioFunction" units="" type="enum" elements="1" options="Disable,Limit" defaultvalue="Limit"/> --> <!-- <field name="YawPIDRatioFunction" units="" type="enum" elements="1" options="Disable,Limit" defaultvalue="Limit"/> -->
<field name="DestinationPidBank" units="bank#" type="uint8" elements="1" defaultvalue="2"/> <field name="DestinationPidBank" units="bank#" type="uint8" elements="1" defaultvalue="2"/>
<field name="TuningDuration" units="sec" type="uint8" elements="1" defaultvalue="60"/> <field name="TuningDuration" units="sec" type="uint8" elements="1" defaultvalue="60"/>
<!-- smooth vs. quick PID selector: --> <!-- SmoothQuickSource: the smooth vs. quick PID selector -->
<!-- 0 = disabled --> <!-- 0 = disabled -->
<!-- 10 thru 13 correspond to accessory0 -> accessory3 transmitter knobs --> <!-- 10 thru 13 correspond to accessory0 -> accessory3 transmitter knobs -->
<!-- 23, 25, and 27 are discrete 3, 5, and 7 stop rount robin selectors incremented by quickly toggling the fms 3 times --> <!-- an accessory knob works as expected, with full left being smoothest and full right being quickest -->
<!-- 23 (3 stops) means stops at 0%, 100%, -100% (repeat as you toggle) --> <!-- 23, 25, and 27 are discrete 3, 5, and 7 position rount robin selectors -->
<!-- 25 (5 stops) means stops at 0%, 50%, 100%, -100%, -50% (repeat as you toggle) --> <!-- incremented by quickly double toggling the fms 3 times (starting outside autotune mode -->
<!-- 27 (7 stops) means stops at 0%, 33%, 67%, 100%, -100%, -67%, -33% (repeat as you toggle) --> <!-- with each double toggle going into and back out of autotune) to go to the next position -->
<!-- 25 is special in that the 3 middle values (-50, 0, 50) are exactly those that are recommended for smooth, normal, and quick responses --> <!-- think of the positions on a scale from 0 to 100, with smoothest being 0 and quickest being 100, and you start out at 50 -->
<!-- 23 (3 stops) means stops at 50, then 100, then 0 then back to 50 -->
<!-- 25 (5 stops) means stops at 50, 75, 100, 0, 25 (repeat as you toggle) -->
<!-- 27 (7 stops) means stops at 50, 67, 83, 100, 0, 17, 33 (repeat as you toggle) -->
<!-- 25 is special in that the 3 middle values (25, 50, 75) are exactly those that are recommended for smooth, normal, and quick responses -->
<field name="SmoothQuickSource" units="" type="uint8" elements="1" defaultvalue="25"/> <field name="SmoothQuickSource" units="" type="uint8" elements="1" defaultvalue="25"/>
<field name="DisableSanityChecks" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/> <field name="DisableSanityChecks" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/>
<field name="Complete" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/> <field name="Complete" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False"/>