mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
Merged in f5soh/librepilot/LP-488_autotune_altitudeVario (pull request #395)
LP-488 Add AltitudeVario to Autotune Approved-by: Philippe Renon <philippe_renon@yahoo.fr> Approved-by: Lalanne Laurent <f5soh@free.fr> Approved-by: Alessio Morale <alessiomorale@gmail.com> Approved-by: Mateusz Kaduk <mateusz.kaduk@gmail.com>
This commit is contained in:
Lalanne Laurent
Philippe Renon
commit
524cba9279
@ -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;
|
||||
|
||||
@ -314,6 +316,10 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters)
|
||||
if (smoothQuickValue > 1.001f) {
|
||||
smoothQuickValue = -1.0f;
|
||||
}
|
||||
// Assume the value is 0
|
||||
if (fabsf(smoothQuickValue) < 0.001f) {
|
||||
smoothQuickValue = 0.0f;
|
||||
}
|
||||
} else {
|
||||
// if they did the 3x FMS toggle while disarmed, set PID's back to the middle of smoothquick
|
||||
smoothQuickValue = 0.0f;
|
||||
@ -327,6 +333,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 +370,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 +739,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 +755,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 +766,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;
|
||||
@ -823,7 +841,12 @@ static void UpdateStabilizationDesired(bool doingIdent)
|
||||
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
|
||||
}
|
||||
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
|
||||
|
||||
if (systemIdentSettings.ThrustControl == SYSTEMIDENTSETTINGS_THRUSTCONTROL_ALTITUDEVARIO) {
|
||||
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_ALTITUDEVARIO;
|
||||
} else {
|
||||
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
|
||||
}
|
||||
|
||||
StabilizationDesiredSet(&stabDesired);
|
||||
}
|
||||
|
||||
@ -148,7 +148,7 @@ static void StabilizationDesiredUpdatedCb(__attribute__((unused)) UAVObjEvent *e
|
||||
if (t <= 1) {
|
||||
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_ATTITUDE;
|
||||
}
|
||||
// else yaw (other modes don't worry about invalid thrust mode either)
|
||||
// yaw
|
||||
else {
|
||||
StabilizationStatusOuterLoopToArray(status.OuterLoop)[t] = STABILIZATIONSTATUS_OUTERLOOP_DIRECT;
|
||||
}
|
||||
|
||||
@ -58,6 +58,7 @@
|
||||
<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="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 -->
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user