Skip to content

Commit 9d11a22

Browse files
f5sohfilnet
f5soh
authored and
filnet
committed
Merged in f5soh/librepilot/LP-399_GUI_for_Autotune (pull request #451)
LP-399 GUI for Autotune Approved-by: Lalanne Laurent <f5soh@free.fr> Approved-by: Philippe Renon <philippe_renon@yahoo.fr> Approved-by: Vladimir Zidar <mr_w@mindnever.org> Approved-by: Jan NIJS <dr.oblivium@gmail.com> Approved-by: Alessio Morale <alessiomorale@gmail.com>
2 parents d1ab06e + 8da8172 commit 9d11a22

9 files changed

Lines changed: 2158 additions & 747 deletions

File tree

‎flight/modules/AutoTune/autotune.c‎

Lines changed: 69 additions & 67 deletions
Original file line numberDiff line numberDiff line change
@@ -9,7 +9,7 @@
99
* @{
1010
*
1111
* @file AutoTune/autotune.c
12-
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
12+
* @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016-2017.
1313
* dRonin, http://dRonin.org/, Copyright (C) 2015-2016
1414
* Tau Labs, http://taulabs.org, Copyright (C) 2013-2014
1515
* The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
@@ -69,43 +69,39 @@
6969
#undef STACK_SIZE_BYTES
7070
// Pull Request version tested on Sparky2. 292 bytes of stack left when configured with 1340
7171
// Beware that Nano needs 156 bytes more stack than Sparky2
72-
#define STACK_SIZE_BYTES 1340
73-
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
72+
#define STACK_SIZE_BYTES 1340
73+
#define TASK_PRIORITY (tskIDLE_PRIORITY + 1)
7474

75-
#define AF_NUMX 13
76-
#define AF_NUMP 43
75+
#define AF_NUMX 13
76+
#define AF_NUMP 43
7777

7878
#if !defined(AT_QUEUE_NUMELEM)
79-
#define AT_QUEUE_NUMELEM 18
79+
#define AT_QUEUE_NUMELEM 18
8080
#endif
8181

82-
#define TASK_STARTUP_DELAY_MS 250 /* delay task startup this much, waiting on accessory valid */
83-
#define NOT_AT_MODE_DELAY_MS 50 /* delay this many ms if not in autotune mode */
84-
#define NOT_AT_MODE_RATE (1000.0f / NOT_AT_MODE_DELAY_MS) /* this many loops per second if not in autotune mode */
85-
#define SMOOTH_QUICK_FLUSH_DELAY 0.5f /* wait this long after last change to flush to permanent storage */
86-
#define SMOOTH_QUICK_FLUSH_TICKS (SMOOTH_QUICK_FLUSH_DELAY * NOT_AT_MODE_RATE) /* this many ticks after last change to flush to permanent storage */
82+
#define TASK_STARTUP_DELAY_MS 250 /* delay task startup this much, waiting on accessory valid */
83+
#define NOT_AT_MODE_DELAY_MS 50 /* delay this many ms if not in autotune mode */
84+
#define NOT_AT_MODE_RATE (1000.0f / NOT_AT_MODE_DELAY_MS) /* this many loops per second if not in autotune mode */
85+
#define SMOOTH_QUICK_FLUSH_DELAY 0.5f /* wait this long after last change to flush to permanent storage */
86+
#define SMOOTH_QUICK_FLUSH_TICKS (SMOOTH_QUICK_FLUSH_DELAY * NOT_AT_MODE_RATE) /* this many ticks after last change to flush to permanent storage */
8787

88-
#define MAX_PTS_PER_CYCLE 4 /* max gyro updates to process per loop see YIELD_MS and consider gyro rate */
89-
#define INIT_TIME_DELAY_MS 100 /* delay to allow stab bank, etc. to be populated after flight mode switch change detection */
90-
#define SYSTEMIDENT_TIME_DELAY_MS 2000 /* delay before starting systemident (shaking) flight mode */
91-
#define INIT_TIME_DELAY2_MS 2500 /* delay before starting to capture data */
92-
#define YIELD_MS 2 /* delay this long between processing sessions see MAX_PTS_PER_CYCLE and consider gyro rate */
88+
#define MAX_PTS_PER_CYCLE 4 /* max gyro updates to process per loop see YIELD_MS and consider gyro rate */
89+
#define INIT_TIME_DELAY_MS 100 /* delay to allow stab bank, etc. to be populated after flight mode switch change detection */
90+
#define SYSTEMIDENT_TIME_DELAY_MS 2000 /* delay before starting systemident (shaking) flight mode */
91+
#define INIT_TIME_DELAY2_MS 2500 /* delay before starting to capture data */
92+
#define YIELD_MS 2 /* delay this long between processing sessions see MAX_PTS_PER_CYCLE and consider gyro rate */
9393

9494
// CheckSettings() returned error bits
95-
#define TAU_NAN 1
96-
#define BETA_NAN 2
97-
#define ROLL_BETA_LOW 4
98-
#define PITCH_BETA_LOW 8
99-
#define YAW_BETA_LOW 16
100-
#define TAU_TOO_LONG 32
101-
#define TAU_TOO_SHORT 64
102-
#define CPU_TOO_SLOW 128
103-
104-
// smooth-quick modes
105-
#define SMOOTH_QUICK_DISABLED 0
106-
#define SMOOTH_QUICK_ACCESSORY_BASE 10
107-
#define SMOOTH_QUICK_TOGGLE_BASE 20
95+
#define TAU_NAN 1
96+
#define BETA_NAN 2
97+
#define ROLL_BETA_LOW 4
98+
#define PITCH_BETA_LOW 8
99+
#define YAW_BETA_LOW 16
100+
#define TAU_TOO_LONG 32
101+
#define TAU_TOO_SHORT 64
102+
#define CPU_TOO_SLOW 128
108103

104+
#define FMS_TOGGLE_STEP_DISABLED 0.0f
109105

110106
// Private types
111107
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;
136132
static float gyroReadTimeAverageAlphaAlpha;
137133
static float alpha;
138134
static float smoothQuickValue;
135+
static float flightModeSwitchToggleStepValue;
139136
static volatile uint32_t atPointsSpilled;
140137
static uint32_t throttleAccumulator;
141138
static uint8_t rollMax, pitchMax;
142139
static int8_t accessoryToUse;
143-
static int8_t flightModeSwitchTogglePosition;
144140
static bool moduleEnabled;
145141

146142

@@ -288,13 +284,13 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
288284
savePidNeeded = false;
289285
// Save PIDs to permanent settings
290286
switch (systemIdentSettings.DestinationPidBank) {
291-
case 1:
287+
case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK1:
292288
UAVObjSave(StabilizationSettingsBank1Handle(), 0);
293289
break;
294-
case 2:
290+
case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK2:
295291
UAVObjSave(StabilizationSettingsBank2Handle(), 0);
296292
break;
297-
case 3:
293+
case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK3:
298294
UAVObjSave(StabilizationSettingsBank3Handle(), 0);
299295
break;
300296
}
@@ -306,13 +302,13 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
306302
// and the autotune data gathering is complete
307303
// and the autotune data gathered is good
308304
// note: CheckFlightModeSwitchForPidRequest(mode) only returns true if current mode is not autotune
309-
if (flightModeSwitchTogglePosition != -1 && CheckFlightModeSwitchForPidRequest(flightStatus.FlightMode)
305+
if (flightModeSwitchToggleStepValue > FMS_TOGGLE_STEP_DISABLED && CheckFlightModeSwitchForPidRequest(flightStatus.FlightMode)
310306
&& systemIdentSettings.Complete && !CheckSettings()) {
311307
if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) {
312308
// if user toggled while armed set PID's to next in sequence
313309
// if you assume that smoothest is -1 and quickest is +1
314310
// this corresponds to 0,+.50,+1.00,-1.00,-.50 (for 5 position toggle)
315-
smoothQuickValue += 1.0f / (float)flightModeSwitchTogglePosition;
311+
smoothQuickValue += flightModeSwitchToggleStepValue;
316312
if (smoothQuickValue > 1.001f) {
317313
smoothQuickValue = -1.0f;
318314
}
@@ -746,39 +742,45 @@ static void InitSystemIdent(bool loadDefaults)
746742
// Update SmoothQuickSource to be used
747743
static void UpdateSmoothQuickSource(uint8_t smoothQuickSource, bool loadDefaults)
748744
{
745+
// disable PID changing with accessory0-3 and flight mode switch toggle
746+
accessoryToUse = -1;
747+
flightModeSwitchToggleStepValue = FMS_TOGGLE_STEP_DISABLED;
748+
749749
switch (smoothQuickSource) {
750-
case SMOOTH_QUICK_ACCESSORY_BASE + 0: // use accessory0
751-
case SMOOTH_QUICK_ACCESSORY_BASE + 1: // use accessory1
752-
case SMOOTH_QUICK_ACCESSORY_BASE + 2: // use accessory2
753-
case SMOOTH_QUICK_ACCESSORY_BASE + 3: // use accessory3
754-
// leave smoothQuickValue alone since it is always controlled by knob
755-
// disable PID changing with flight mode switch
756-
flightModeSwitchTogglePosition = -1;
757-
// enable PID changing with accessory0-3
758-
accessoryToUse = smoothQuickSource - SMOOTH_QUICK_ACCESSORY_BASE;
750+
case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_ACCESSORY0:
751+
accessoryToUse = 0;
759752
break;
760-
case SMOOTH_QUICK_TOGGLE_BASE + 3: // use flight mode switch toggle with 3 points
761-
case SMOOTH_QUICK_TOGGLE_BASE + 5: // use flight mode switch toggle with 5 points
762-
case SMOOTH_QUICK_TOGGLE_BASE + 7: // use flight mode switch toggle with 7 points
763-
// don't allow init of current toggle position in the middle of 3x fms toggle
764-
if (loadDefaults) {
765-
// set toggle to middle of range
766-
smoothQuickValue = 0.0f;
767-
}
768-
// enable PID changing with flight mode switch
769-
flightModeSwitchTogglePosition = (smoothQuickSource - 1 - SMOOTH_QUICK_TOGGLE_BASE) / 2;
770-
// disable PID changing with accessory0-3
771-
accessoryToUse = -1;
753+
case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_ACCESSORY1:
754+
accessoryToUse = 1;
755+
break;
756+
case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_ACCESSORY2:
757+
accessoryToUse = 2;
772758
break;
773-
case SMOOTH_QUICK_DISABLED:
759+
case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_ACCESSORY3:
760+
accessoryToUse = 3;
761+
break;
762+
// enable PID changing with flight mode switch
763+
// -1 to +1 give a range = 2, define step value for desired positions: 3, 5, 7
764+
case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_FMSTOGGLE3POS:
765+
flightModeSwitchToggleStepValue = 1.0f;
766+
break;
767+
case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_FMSTOGGLE5POS:
768+
flightModeSwitchToggleStepValue = 0.5f;
769+
break;
770+
case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_FMSTOGGLE7POS:
771+
flightModeSwitchToggleStepValue = 0.33f;
772+
break;
773+
case SYSTEMIDENTSETTINGS_SMOOTHQUICKSOURCE_DISABLED:
774774
default:
775-
// leave smoothQuickValue alone so user can set it to a different value and have it stay that value
776-
// disable PID changing with flight mode switch
777-
flightModeSwitchTogglePosition = -1;
778-
// disable PID changing with accessory0-3
779775
accessoryToUse = -1;
776+
flightModeSwitchToggleStepValue = FMS_TOGGLE_STEP_DISABLED;
780777
break;
781778
}
779+
// don't allow init of current toggle position in the middle of 3x fms toggle
780+
if (loadDefaults && (flightModeSwitchToggleStepValue > FMS_TOGGLE_STEP_DISABLED)) {
781+
// set toggle to middle of range
782+
smoothQuickValue = 0.0f;
783+
}
782784
}
783785

784786

@@ -928,13 +930,13 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
928930
_Static_assert(sizeof(StabilizationSettingsBank1Data) == sizeof(StabilizationBankData), "sizeof(StabilizationSettingsBank1Data) != sizeof(StabilizationBankData)");
929931
StabilizationBankData volatile stabSettingsBank;
930932
switch (systemIdentSettings.DestinationPidBank) {
931-
case 1:
933+
case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK1:
932934
StabilizationSettingsBank1Get((void *)&stabSettingsBank);
933935
break;
934-
case 2:
936+
case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK2:
935937
StabilizationSettingsBank2Get((void *)&stabSettingsBank);
936938
break;
937-
case 3:
939+
case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK3:
938940
StabilizationSettingsBank3Get((void *)&stabSettingsBank);
939941
break;
940942
}
@@ -1161,13 +1163,13 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float
11611163

11621164
// Save PIDs to UAVO RAM (not permanently yet)
11631165
switch (systemIdentSettings.DestinationPidBank) {
1164-
case 1:
1166+
case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK1:
11651167
StabilizationSettingsBank1Set((void *)&stabSettingsBank);
11661168
break;
1167-
case 2:
1169+
case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK2:
11681170
StabilizationSettingsBank2Set((void *)&stabSettingsBank);
11691171
break;
1170-
case 3:
1172+
case SYSTEMIDENTSETTINGS_DESTINATIONPIDBANK_BANK3:
11711173
StabilizationSettingsBank3Set((void *)&stabSettingsBank);
11721174
break;
11731175
}

0 commit comments

Comments
 (0)