rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00
Code Issues Releases Activity

LP-399 Prepare Autotune GUI : Define some settings as enums

Browse Source
This commit is contained in:
Laurent Lalanne 2017-07-02 01:15:35 +02:00
parent d031352cdd
commit 1abe34ce7e
2 changed files with 77 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -9,7 +9,7 @@
* @{
*
* @file AutoTune/autotune.c
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016-2017.
* dRonin, http://dRonin.org/, Copyright (C) 2015-2016
* Tau Labs, http://taulabs.org, Copyright (C) 2013-2014
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
@ -69,43 +69,39 @@
#undef STACK_SIZE_BYTES
// Pull Request version tested on Sparky2. 292 bytes of stack left when configured with 1340
// Beware that Nano needs 156 bytes more stack than Sparky2
#define STACK_SIZE_BYTES 1340
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
#define STACK_SIZE_BYTES 1340
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
#define AF_NUMX 13
#define AF_NUMP 43
#define AF_NUMX 13
#define AF_NUMP 43
#if !defined(AT_QUEUE_NUMELEM)
#define AT_QUEUE_NUMELEM 18
#define AT_QUEUE_NUMELEM 18
#endif
#define TASK_STARTUP_DELAY_MS 250 /* delay task startup this much, waiting on accessory valid */
#define NOT_AT_MODE_DELAY_MS 50 /* delay this many ms if not in autotune mode */
#define NOT_AT_MODE_RATE (1000.0f / NOT_AT_MODE_DELAY_MS) /* this many loops per second if not in autotune mode */
#define SMOOTH_QUICK_FLUSH_DELAY 0.5f /* wait this long after last change to flush to permanent storage */
#define SMOOTH_QUICK_FLUSH_TICKS (SMOOTH_QUICK_FLUSH_DELAY * NOT_AT_MODE_RATE) /* this many ticks after last change to flush to permanent storage */
#define TASK_STARTUP_DELAY_MS 250 /* delay task startup this much, waiting on accessory valid */
#define NOT_AT_MODE_DELAY_MS 50 /* delay this many ms if not in autotune mode */
#define NOT_AT_MODE_RATE (1000.0f / NOT_AT_MODE_DELAY_MS) /* this many loops per second if not in autotune mode */
#define SMOOTH_QUICK_FLUSH_DELAY 0.5f /* wait this long after last change to flush to permanent storage */
#define SMOOTH_QUICK_FLUSH_TICKS (SMOOTH_QUICK_FLUSH_DELAY * NOT_AT_MODE_RATE) /* this many ticks after last change to flush to permanent storage */
#define MAX_PTS_PER_CYCLE 4 /* max gyro updates to process per loop see YIELD_MS and consider gyro rate */
#define INIT_TIME_DELAY_MS 100 /* delay to allow stab bank, etc. to be populated after flight mode switch change detection */
#define SYSTEMIDENT_TIME_DELAY_MS 2000 /* delay before starting systemident (shaking) flight mode */
#define INIT_TIME_DELAY2_MS 2500 /* delay before starting to capture data */
#define YIELD_MS 2 /* delay this long between processing sessions see MAX_PTS_PER_CYCLE and consider gyro rate */
#define MAX_PTS_PER_CYCLE 4 /* max gyro updates to process per loop see YIELD_MS and consider gyro rate */
#define INIT_TIME_DELAY_MS 100 /* delay to allow stab bank, etc. to be populated after flight mode switch change detection */
#define SYSTEMIDENT_TIME_DELAY_MS 2000 /* delay before starting systemident (shaking) flight mode */
#define INIT_TIME_DELAY2_MS 2500 /* delay before starting to capture data */
#define YIELD_MS 2 /* delay this long between processing sessions see MAX_PTS_PER_CYCLE and consider gyro rate */
// CheckSettings() returned error bits
#define TAU_NAN 1
#define BETA_NAN 2
#define ROLL_BETA_LOW 4
#define PITCH_BETA_LOW 8
#define YAW_BETA_LOW 16
#define TAU_TOO_LONG 32
#define TAU_TOO_SHORT 64
#define CPU_TOO_SLOW 128
// smooth-quick modes
#define SMOOTH_QUICK_DISABLED 0
#define SMOOTH_QUICK_ACCESSORY_BASE 10
#define SMOOTH_QUICK_TOGGLE_BASE 20
#define TAU_NAN 1
#define BETA_NAN 2
#define ROLL_BETA_LOW 4
#define PITCH_BETA_LOW 8
#define YAW_BETA_LOW 16
#define TAU_TOO_LONG 32
#define TAU_TOO_SHORT 64
#define CPU_TOO_SLOW 128
#define FMS_TOGGLE_STEP_DISABLED 0.0f
// Private types
enum AUTOTUNE_STATE { AT_INIT, AT_INIT_DELAY, AT_INIT_DELAY2, AT_START, AT_RUN, AT_FINISHED, AT_WAITING };
@ -136,11 +132,11 @@ static float gyroReadTimeAverageAlpha;
static float gyroReadTimeAverageAlphaAlpha;
static float alpha;
static float smoothQuickValue;
static float flightModeSwitchToggleStepValue;
static volatile uint32_t atPointsSpilled;
static uint32_t throttleAccumulator;
static uint8_t rollMax, pitchMax;
static int8_t accessoryToUse;
static int8_t flightModeSwitchTogglePosition;
static bool moduleEnabled;
@ -288,13 +284,13 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
savePidNeeded = false;
// Save PIDs to permanent settings
switch (systemIdentSettings.DestinationPidBank) {
case 1:
case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK1:
UAVObjSave(StabilizationSettingsBank1Handle(), 0);
break;
case 2:
case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK2:
UAVObjSave(StabilizationSettingsBank2Handle(), 0);
break;
case 3:
case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK3:
UAVObjSave(StabilizationSettingsBank3Handle(), 0);
break;
}
@ -306,13 +302,13 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
// and the autotune data gathering is complete
// and the autotune data gathered is good
// note: CheckFlightModeSwitchForPidRequest(mode) only returns true if current mode is not autotune
if (flightModeSwitchTogglePosition != -1 && CheckFlightModeSwitchForPidRequest(flightStatus.FlightMode)
if (flightModeSwitchToggleStepValue > FMS_TOGGLE_STEP_DISABLED && CheckFlightModeSwitchForPidRequest(flightStatus.FlightMode)
&& systemIdentSettings.Complete && !CheckSettings()) {
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
// if user toggled while armed set PID's to next in sequence
// if you assume that smoothest is -1 and quickest is +1
// this corresponds to 0,+.50,+1.00,-1.00,-.50 (for 5 position toggle)
smoothQuickValue += 1.0f / (float)flightModeSwitchTogglePosition;
smoothQuickValue += flightModeSwitchToggleStepValue;
if (smoothQuickValue > 1.001f) {
smoothQuickValue = -1.0f;
}
@ -746,39 +742,45 @@ static void InitSystemIdent(bool loadDefaults)
// Update SmoothQuickSource to be used
static void UpdateSmoothQuickSource(uint8_t smoothQuickSource, bool loadDefaults)
{
// disable PID changing with accessory0-3 and flight mode switch toggle
accessoryToUse = -1;
flightModeSwitchToggleStepValue = FMS_TOGGLE_STEP_DISABLED;
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
case SMOOTH_QUICK_ACCESSORY_BASE + 3: // use accessory3
// leave smoothQuickValue alone since it is always controlled by knob
// disable PID changing with flight mode switch
flightModeSwitchTogglePosition = -1;
// enable PID changing with accessory0-3
accessoryToUse = smoothQuickSource - SMOOTH_QUICK_ACCESSORY_BASE;
case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_ACCESSORY0:
accessoryToUse = 0;
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
case SMOOTH_QUICK_TOGGLE_BASE + 7: // use flight mode switch toggle with 7 points
// don't allow init of current toggle position in the middle of 3x fms toggle
if (loadDefaults) {
// set toggle to middle of range
smoothQuickValue = 0.0f;
}
// enable PID changing with flight mode switch
flightModeSwitchTogglePosition = (smoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2;
// disable PID changing with accessory0-3
accessoryToUse = -1;
case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_ACCESSORY1:
accessoryToUse = 1;
break;
case SMOOTH_QUICK_DISABLED:
case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_ACCESSORY2:
accessoryToUse = 2;
break;
case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_ACCESSORY3:
accessoryToUse = 3;
break;
// enable PID changing with flight mode switch
// -1 to +1 give a range = 2, define step value for desired positions: 3, 5, 7
case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_FMSTOGGLE3POS:
flightModeSwitchToggleStepValue = 1.0f;
break;
case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_FMSTOGGLE5POS:
flightModeSwitchToggleStepValue = 0.5f;
break;
case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_FMSTOGGLE7POS:
flightModeSwitchToggleStepValue = 0.33f;
break;
case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_DISABLED:
default:
// leave smoothQuickValue alone so user can set it to a different value and have it stay that value
// disable PID changing with flight mode switch
flightModeSwitchTogglePosition = -1;
// disable PID changing with accessory0-3
accessoryToUse = -1;
flightModeSwitchToggleStepValue = FMS_TOGGLE_STEP_DISABLED;
break;
}
// don't allow init of current toggle position in the middle of 3x fms toggle
if (loadDefaults && (flightModeSwitchToggleStepValue > FMS_TOGGLE_STEP_DISABLED)) {
// set toggle to middle of range
smoothQuickValue = 0.0f;
}
}
@ -928,13 +930,13 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
_Static_assert(sizeof(StabilizationSettingsBank1Data) == sizeof(StabilizationBankData), "sizeof(StabilizationSettingsBank1Data) != sizeof(StabilizationBankData)");
StabilizationBankData volatile stabSettingsBank;
switch (systemIdentSettings.DestinationPidBank) {
case 1:
case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK1:
StabilizationSettingsBank1Get((void *)&stabSettingsBank);
break;
case 2:
case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK2:
StabilizationSettingsBank2Get((void *)&stabSettingsBank);
break;
case 3:
case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK3:
StabilizationSettingsBank3Get((void *)&stabSettingsBank);
break;
}
@ -1161,13 +1163,13 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
// Save PIDs to UAVO RAM (not permanently yet)
switch (systemIdentSettings.DestinationPidBank) {
case 1:
case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK1:
StabilizationSettingsBank1Set((void *)&stabSettingsBank);
break;
case 2:
case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK2:
StabilizationSettingsBank2Set((void *)&stabSettingsBank);
break;
case 3:
case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK3:
StabilizationSettingsBank3Set((void *)&stabSettingsBank);
break;
}

20
shared/uavobjectdefinition/systemidentsettings.xml
View File

@ -56,23 +56,19 @@
<field name="YawToRollPitchPIDRatioMax" units="" type="float" elements="1" defaultvalue="2.5" description="Setting: Yaw PID will be at most this times Pitch PID (if enabled)"/>
<field name="DerivativeFactor" units="" type="float" elements="1" defaultvalue="1.0" limits="%BE:0:1" description="Setting: Multiplicative factor. If you have D term oscillations, reduce it toward zero and it will reduce PID D terms."/>
<field name="OuterLoopKpSoftClamp" units="" type="float" elements="1" defaultvalue="6.5" limits="%BE:0:100" description="Setting: Change this to change the outer loop Kp and Ki limiting"/>
<field name="DestinationPidBank" units="bank#" type="uint8" elements="1" defaultvalue="3" limits="%BE:1:3" description="Setting: Which bank the calculated PIDs will be stored in after tuning"/>
<field name="DestinationPidBank" units="bank#" type="enum" elements="1" options="Bank1,Bank2,Bank3" defaultvalue="Bank3" description="Setting: Which bank the calculated PIDs will be stored in after tuning"/>
<field name="TuningDuration" units="s" type="uint8" elements="1" defaultvalue="60" limits="%BI:0" description="Setting: Duration of the tuning motions (expert)"/>
<field name="ThrustControl" units="" type="enum" elements="1" options="Manual,AltitudeVario" defaultvalue="Manual" description="Setting: Manual is direct control, AltitudeVario helps to maintain altitude"/>
<!-- SmoothQuickSource: the smooth vs. quick PID selector -->
<!-- 0 = disabled -->
<!-- 10 thru 13 correspond to accessory0 -> accessory3 transmitter knobs -->
<!-- an accessory knob works as expected, with full left being smoothest and full right being quickest -->
<!-- 23, 25, and 27 are discrete 3, 5, and 7 position rount robin selectors -->
<!-- accessory0 -> accessory3 transmitter knobs works as expected, with full left being smoothest (-1) and full right being quickest (+1) -->
<!-- FMSToggle 3, 5, and 7 positions rount robin selectors -->
<!-- incremented by quickly double toggling the fms 3 times (starting outside autotune mode -->
<!-- with each double toggle going into and back out of autotune) to go to the next position -->
<!-- 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" description="Setting: 10-13 to use one of accessory0-3, 23 25 27 for FMS 3x toggle method with 3 5 7 positions"/>
<field name="SmoothQuickValue" units="" type="float" elements="1" defaultvalue="0.0" description="Remembers the value of the FMS 3x toggle"/>
<!-- 3 stops means stops at 0, then +1, then -1 then back to 0 -->
<!-- 5 stops means stops at 0, +0.5, +1, -1, -0.5 the 3 middle values (-0.5, 0, +0.5) are exactly those that are recommended for smooth, normal, and quick responses-->
<!-- 7 stops means stops at 0, +0.33, +0.66, +1, -1, -0.66, -0.33 -->
<field name="SmoothQuickSource" units="" type="enum" elements="1" options="Disabled,Accessory0,Accessory1,Accessory2,Accessory3,FMSToggle3pos,FMSToggle5pos,FMSToggle7pos" defaultvalue="FMSToggle5pos" description="Setting: Choose Accessory or FMS 3x toggle method with 3, 5, 7 positions"/>
<field name="SmoothQuickValue" units="" type="float" elements="1" defaultvalue="0.0" description="Remembers the value of the FMS 3x toggle (-1 to +1 range)"/>
<field name="DisableSanityChecks" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False" description="Setting: Debugging tool (expert)"/>
<field name="GyroReadTimeAverage" units="s" type="float" elements="1" defaultvalue="0.001" description="Measured delay from gyro read to inner loop"/>
<field name="Complete" units="bool" type="enum" elements="1" options="False,True" defaultvalue="False" description="Automatically set True for a good complete tune or False for incomplete or bad"/>