From faf2fffb5ab686fa625d844584384d7b722a4e44 Mon Sep 17 00:00:00 2001 From: Cliff Geerdes Date: Fri, 4 Mar 2016 23:21:23 -0500 Subject: [PATCH] LP-76 smooth quick PIDs via accessory0-3 and mode switch toggle --- flight/modules/AutoTune/autotune.c | 232 +++++++++++++++++++++++++---- 1 file changed, 199 insertions(+), 33 deletions(-) diff --git a/flight/modules/AutoTune/autotune.c b/flight/modules/AutoTune/autotune.c index a30611cba..548c2f2e0 100644 --- a/flight/modules/AutoTune/autotune.c +++ b/flight/modules/AutoTune/autotune.c @@ -317,7 +317,7 @@ static void AtNewGyroData(UAVObjEvent * ev); static void UpdateSystemIdent(const float *X, const float *noise, float dT_s, uint32_t predicts, uint32_t spills, float hover_throttle); static void UpdateStabilizationDesired(bool doingIdent); static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode); -static void InitSystemIdent(); +static void InitSystemIdent(bool loadDefaults); /** @@ -397,7 +397,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // correctly set accessoryToUse and flightModeSwitchTogglePosition // based on what is in SystemIdent // so that the user can use the PID smooth->quick slider in a following flight - InitSystemIdent(); + InitSystemIdent(false); while (1) { static uint32_t updateCounter = 0; @@ -415,14 +415,13 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) if (flightStatus.Armed == FLIGHTSTATUS_ARMED_DISARMED) { if (saveSiNeeded) { + saveSiNeeded = false; // Save SystemIdent to permanent settings UAVObjSave(SystemIdentHandle(), 0); - saveSiNeeded = false; //so how to restart if it failed and both saves are false } if (savePidNeeded) { - // Save SystemIdent to permanent settings - UAVObjSave(SystemIdentHandle(), 0); + savePidNeeded = false; // Save PIDs to permanent settings switch (systemIdentData.DestinationPidBank) { case 1: @@ -435,8 +434,8 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) UAVObjSave(StabilizationSettingsBank3Handle(), 0); break; } - savePidNeeded = false; } +// can't set to AT_INIT because when we land and disarm it will jump to init and clear things out after 2 seconds //state = AT_INIT; } @@ -444,7 +443,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // and user toggled into and back out of AutoTune // three times in the last two seconds // CheckFlightModeSwitchForPidRequest(mode) only returns true if mode is not autotune - if (flightModeSwitchTogglePosition!=-1 && CheckFlightModeSwitchForPidRequest(flightStatus.FlightMode)) { + if (flightModeSwitchTogglePosition!=-1 && CheckFlightModeSwitchForPidRequest(flightStatus.FlightMode) && systemIdentData.Complete && !CheckSettings()) { if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { // if user toggled while armed set PID's to next in sequence 3,4,5,1,2 or 2,3,1 ++flightModeSwitchTogglePosition; @@ -466,7 +465,7 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // don't bother checking to see if the requested accessory# is configured properly // if it isn't, the value will be 0 which is the center of [-1,1] anyway // if (accessoryToUse!=-1 && CheckAccessoryForPidRequest(accessoryToUse)) { - if (accessoryToUse != -1) { + if (accessoryToUse != -1 && systemIdentData.Complete && !CheckSettings()) { static AccessoryDesiredData accessoryValueOld = { 0.0f }; // static float accessoryValueOld = 0.0f; AccessoryDesiredData accessoryValue; @@ -496,11 +495,12 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) // Reset save status; only save if this tune completes. saveSiNeeded = false; savePidNeeded = false; +// systemIdentData.Complete = false; lastUpdateTime = xTaskGetTickCount(); // Only start when armed and flying if (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED) { - InitSystemIdent(); + InitSystemIdent(true); AfInit(gX, gP); UpdateSystemIdent(gX, NULL, 0.0f, 0, 0, 0.0f); measureTime = (uint32_t)systemIdentData.TuningDuration * (uint32_t)1000; @@ -581,7 +581,6 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) } if (diffTime > measureTime) { // Move on to next state // permanent flag that AT is complete and PIDs can be calculated - systemIdentData.Complete = true; state = AT_FINISHED; lastUpdateTime = xTaskGetTickCount(); } @@ -599,20 +598,20 @@ static void AutoTuneTask(__attribute__((unused)) void *parameters) if (!failureBits) { ComputeStabilizationAndSetPids(); savePidNeeded = true; + systemIdentData.Complete = true; } else { //is this right // default to disable PID changing with flight mode switch and accessory0-3 accessoryToUse = -1; flightModeSwitchTogglePosition = -1; +// systemIdentData.Complete = false; // raise a warning ExtendedAlarmsSet(SYSTEMALARMS_ALARM_SYSTEMCONFIGURATION, SYSTEMALARMS_ALARM_WARNING, SYSTEMALARMS_EXTENDEDALARMSTATUS_AUTOTUNE, failureBits); } } state = AT_WAITING; -// break; -// fall through in the unlikely case that it is disarmed at AT_FINISHED -// and armed before it ever gets to AT_WAITING the first time + break; case AT_WAITING: default: @@ -713,26 +712,42 @@ static bool CheckFlightModeSwitchForPidRequest(uint8_t flightMode) { } -static void InitSystemIdent() { +static void InitSystemIdent(bool loadDefaults) { SystemIdentGet(&systemIdentData); - // these are values that could be changed by the user - // save them through the following xSetDefaults() call - uint8_t dampRate = systemIdentData.DampRate; - uint8_t noiseRate = systemIdentData.NoiseRate; - bool calcYaw = systemIdentData.CalculateYaw; - uint8_t destBank = systemIdentData.DestinationPidBank; uint8_t smoothQuick = systemIdentData.SmoothQuick; - // get these 10.0 10.0 7.0 -4.0 from default values of SystemIdent (.Beta and .Tau) - // so that if they are changed there (mainly for future code changes), they will be changed here too - SystemIdentSetDefaults(SystemIdentHandle(), 0); - SystemIdentGet(&systemIdentData); + if (loadDefaults) { + // these are values that could be changed by the user + // save them through the following xSetDefaults() call + uint8_t dampMin = systemIdentData.DampMin; + uint8_t dampRate = systemIdentData.DampRate; + uint8_t dampMax = systemIdentData.DampMax; + uint8_t noiseMin = systemIdentData.NoiseMin; + uint8_t noiseRate = systemIdentData.NoiseRate; + uint8_t noiseMax = systemIdentData.NoiseMax; + bool calcYaw = systemIdentData.CalculateYaw; + uint8_t destBank = systemIdentData.DestinationPidBank; + uint8_t tuningDuration = systemIdentData.TuningDuration; +// bool complete = systemIdentData.Complete; + + // get these 10.0 10.0 7.0 -4.0 from default values of SystemIdent (.Beta and .Tau) + // so that if they are changed there (mainly for future code changes), they will be changed here too + SystemIdentSetDefaults(SystemIdentHandle(), 0); + SystemIdentGet(&systemIdentData); + + // restore the user changeable values + systemIdentData.DampMin = dampMin; + systemIdentData.DampRate = dampRate; + systemIdentData.DampMax = dampMax; + systemIdentData.NoiseMin = noiseMin; + systemIdentData.NoiseRate = noiseRate; + systemIdentData.NoiseMax = noiseMax; + systemIdentData.CalculateYaw = calcYaw; + systemIdentData.DestinationPidBank = destBank; + systemIdentData.TuningDuration = tuningDuration; +// systemIdentData.Complete = complete; + } - // restore the user changeable values - systemIdentData.DampRate = dampRate; - systemIdentData.NoiseRate = noiseRate; - systemIdentData.CalculateYaw = calcYaw; - systemIdentData.DestinationPidBank = destBank; // default to disable PID changing with flight mode switch and accessory0-3 accessoryToUse = -1; flightModeSwitchTogglePosition = -1; @@ -791,6 +806,7 @@ static void UpdateStabilizationDesired(bool doingIdent) { stabDesired.Roll = manual_control_command.Roll * rollMax; stabDesired.Pitch = manual_control_command.Pitch * pitchMax; stabDesired.Yaw = manual_control_command.Yaw * manualRate.Yaw; + stabDesired.Thrust = (airframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP) ? manual_control_command.Collective : manual_control_command.Throttle; if (doingIdent) { stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT; @@ -801,8 +817,8 @@ static void UpdateStabilizationDesired(bool doingIdent) { stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; } + stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL; - stabDesired.Thrust = (airframeType == SYSTEMSETTINGS_AIRFRAMETYPE_HELICP) ? manual_control_command.Collective : manual_control_command.Throttle; // is this a race // control feels very sluggish too StabilizationDesiredSet(&stabDesired); @@ -836,6 +852,7 @@ static uint8_t CheckSettings() } +#if 0 static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float noiseRate) { StabilizationSettingsBank1Data stabSettingsBank; @@ -863,6 +880,7 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float const double damp = (double) dampRate / 100.0d; double tau = exp(systemIdentData.Tau); +#if 0 double beta_roll = systemIdentData.Beta.Roll; double beta_pitch = systemIdentData.Beta.Pitch; @@ -871,7 +889,16 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float for (int i = 0; i < 30; i++) { double tau_d_roll = (2.0d*damp*tau*wn - 1.0d)/(4.0d*tau*damp*damp*wn*wn - 2.0d*damp*wn - tau*wn*wn + exp(beta_roll)*ghf); double tau_d_pitch = (2.0d*damp*tau*wn - 1.0d)/(4.0d*tau*damp*damp*wn*wn - 2.0d*damp*wn - tau*wn*wn + exp(beta_pitch)*ghf); +#else + double exp_beta_roll_times_ghf = exp(systemIdentData.Beta.Roll)*ghf; + double exp_beta_pitch_times_ghf = exp(systemIdentData.Beta.Pitch)*ghf; + double wn = 1.0d/tau; + double tau_d = 0.0d; + for (int i = 0; i < 30; i++) { + double tau_d_roll = (2.0d*damp*tau*wn - 1.0d)/(4.0d*tau*damp*damp*wn*wn - 2.0d*damp*wn - tau*wn*wn + exp_beta_roll_times_ghf); + double tau_d_pitch = (2.0d*damp*tau*wn - 1.0d)/(4.0d*tau*damp*damp*wn*wn - 2.0d*damp*wn - tau*wn*wn + exp_beta_pitch_times_ghf); +#endif // Select the slowest filter property tau_d = (tau_d_roll > tau_d_pitch) ? tau_d_roll : tau_d_pitch; wn = (tau + tau_d) / (tau*tau_d) / (2.0d * damp + 2.0d); @@ -938,6 +965,145 @@ static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float break; } } +#else +static void ComputeStabilizationAndSetPidsFromDampAndNoise(float dampRate, float noiseRate) +{ + StabilizationSettingsBank1Data stabSettingsBank; + +#if 0 +systemIdentData.Bias.Roll = dampRate; +systemIdentData.Bias.Pitch = noiseRate; +SystemIdentSet(&systemIdentData); +#endif + + switch (systemIdentData.DestinationPidBank) { + case 1: + StabilizationSettingsBank1Get((void *)&stabSettingsBank); + break; + case 2: + StabilizationSettingsBank2Get((void *)&stabSettingsBank); + break; + case 3: + StabilizationSettingsBank3Get((void *)&stabSettingsBank); + break; + } + + // These three parameters define the desired response properties + // - rate scale in the fraction of the natural speed of the system + // to strive for. + // - damp is the amount of damping in the system. higher values + // make oscillations less likely + // - ghf is the amount of high frequency gain and limits the influence + // of noise + const double ghf = (double) noiseRate / 1000.0d; + const double damp = (double) dampRate / 100.0d; + + double tau = exp(systemIdentData.Tau); + { + double exp_beta_roll_times_ghf = exp(systemIdentData.Beta.Roll)*ghf; + double exp_beta_pitch_times_ghf = exp(systemIdentData.Beta.Pitch)*ghf; + + double wn = 1.0d/tau; + double tau_d = 0.0d; + for (int i = 0; i < 30; i++) { + double tau_d_roll = (2.0d*damp*tau*wn - 1.0d)/(4.0d*tau*damp*damp*wn*wn - 2.0d*damp*wn - tau*wn*wn + exp_beta_roll_times_ghf); + double tau_d_pitch = (2.0d*damp*tau*wn - 1.0d)/(4.0d*tau*damp*damp*wn*wn - 2.0d*damp*wn - tau*wn*wn + exp_beta_pitch_times_ghf); + // Select the slowest filter property + tau_d = (tau_d_roll > tau_d_pitch) ? tau_d_roll : tau_d_pitch; + wn = (tau + tau_d) / (tau*tau_d) / (2.0d * damp + 2.0d); + } + + // Set the real pole position. The first pole is quite slow, which + // prevents the integral being too snappy and driving too much + // overshoot. + const double a = ((tau+tau_d) / tau / tau_d - 2.0d * damp * wn) / 20.0d; + const double b = ((tau+tau_d) / tau / tau_d - 2.0d * damp * wn - a); + + // Calculate the gain for the outer loop by approximating the + // inner loop as a single order lpf. Set the outer loop to be + // critically damped; + const double zeta_o = 1.3d; + const double kp_o = 1.0d / 4.0d / (zeta_o * zeta_o) / (1.0d/wn); + + for (int i = 0; i < 2; i++) { + double beta = exp(SystemIdentBetaToArray(systemIdentData.Beta)[i]); + + double ki = a * b * wn * wn * tau * tau_d / beta; + double kp = tau * tau_d * ((a+b)*wn*wn + 2.0d*a*b*damp*wn) / beta - ki*tau_d; + double kd = (tau * tau_d * (a*b + wn*wn + (a+b)*2.0d*damp*wn) - 1.0d) / beta - kp * tau_d; + + switch(i) { + case 0: // Roll + stabSettingsBank.RollRatePID.Kp = kp; + stabSettingsBank.RollRatePID.Ki = ki; + stabSettingsBank.RollRatePID.Kd = kd; + stabSettingsBank.RollPI.Kp = kp_o; + stabSettingsBank.RollPI.Ki = 0; + break; + case 1: // Pitch + stabSettingsBank.PitchRatePID.Kp = kp; + stabSettingsBank.PitchRatePID.Ki = ki; + stabSettingsBank.PitchRatePID.Kd = kd; + stabSettingsBank.PitchPI.Kp = kp_o; + stabSettingsBank.PitchPI.Ki = 0; + break; + } + } + } + + // do yaw if requested + if (systemIdentData.CalculateYaw) { + double exp_beta_yaw_times_ghf = exp(systemIdentData.Beta.Yaw)*ghf; + + double wn = 1.0d/tau; + double tau_d = 0.0d; + for (int i = 0; i < 30; i++) { + tau_d = (2.0d*damp*tau*wn - 1.0d)/(4.0d*tau*damp*damp*wn*wn - 2.0d*damp*wn - tau*wn*wn + exp_beta_yaw_times_ghf); + wn = (tau + tau_d) / (tau*tau_d) / (2.0d * damp + 2.0d); + } + + // Set the real pole position. The first pole is quite slow, which + // prevents the integral being too snappy and driving too much + // overshoot. + const double a = ((tau+tau_d) / tau / tau_d - 2.0d * damp * wn) / 20.0d; + const double b = ((tau+tau_d) / tau / tau_d - 2.0d * damp * wn - a); + + // Calculate the gain for the outer loop by approximating the + // inner loop as a single order lpf. Set the outer loop to be + // critically damped; + const double zeta_o = 1.3d; + const double kp_o = 1.0d / 4.0d / (zeta_o * zeta_o) / (1.0d/wn); + + double beta = exp(systemIdentData.Beta.Yaw); + + double ki = a * b * wn * wn * tau * tau_d / beta; + double kp = tau * tau_d * ((a+b)*wn*wn + 2.0d*a*b*damp*wn) / beta - ki*tau_d; + double kd = (tau * tau_d * (a*b + wn*wn + (a+b)*2.0d*damp*wn) - 1.0d) / beta - kp * tau_d; + + // optional Yaw + stabSettingsBank.YawRatePID.Kp = kp; + stabSettingsBank.YawRatePID.Ki = ki; + stabSettingsBank.YawRatePID.Kd = kd; + stabSettingsBank.YawPI.Kp = kp_o; + stabSettingsBank.YawPI.Ki = 0; + } + + //stabSettingsBank.DerivativeCutoff = 1.0d / (2.0d*M_PI*tau_d); + + // Save PIDs to permanent settings + switch (systemIdentData.DestinationPidBank) { + case 1: + StabilizationSettingsBank1Set((void *)&stabSettingsBank); + break; + case 2: + StabilizationSettingsBank2Set((void *)&stabSettingsBank); + break; + case 3: + StabilizationSettingsBank3Set((void *)&stabSettingsBank); + break; + } +} +#endif static void ComputeStabilizationAndSetPids() @@ -960,9 +1126,9 @@ static void ComputeStabilizationAndSetPids() #define dampMin systemIdentData.DampMin #define dampDefault systemIdentData.DampRate #define dampMax systemIdentData.DampMax -#define noiseMin systemIdentData.DampMin -#define noiseDefault systemIdentData.DampRate -#define noiseMax systemIdentData.DampMax +#define noiseMin systemIdentData.NoiseMin +#define noiseDefault systemIdentData.NoiseRate +#define noiseMax systemIdentData.NoiseMax static void ProportionPidsSmoothToQuick(float min, float val, float max) { float ratio, damp, noise;