etb autotune (#1274)

* Astrom-Hagglund

* enums

* comments

* dead

* fix

* changed enum

* ts guard

* safety

* etb autotune debug channels

Co-authored-by: Matthew Kennedy <makenne@microsoft.com>

firmware/config/boards/kinetis/config/controllers/algo/auto_generated_enums.cpp

@@ -393,8 +393,8 @@ case DBG_START_STOP:
   return "DBG_START_STOP";
 case DBG_LAUNCH:
   return "DBG_LAUNCH";
-case DBG_39:
-  return "DBG_39";
+case DBG_ETB_AUTOTUNE:
+  return "DBG_ETB_AUTOTUNE";
 case DBG_40:
   return "DBG_40";
 case DBG_ALTERNATOR_PID:

firmware/controllers/actuators/electronic_throttle.cpp

@@ -89,10 +89,6 @@
 #define ETB_MAX_COUNT 2
 #endif /* ETB_MAX_COUNT */
 
-static pid_s tuneWorkingPidSettings;
-static Pid tuneWorkingPid(&tuneWorkingPidSettings);
-static PID_AutoTune autoTune;
-
 static LoggingWithStorage logger("ETB");
 static pedal2tps_t pedal2tpsMap("Pedal2Tps", 1);
 
@@ -193,27 +189,6 @@ void EtbController::PeriodicTask() {
 		return;
 	}
 
-	if (engine->etbAutoTune) {
-		autoTune.input = actualThrottlePosition.Value;
-		bool result = autoTune.Runtime(&logger);
-
-		tuneWorkingPid.updateFactors(autoTune.output, 0, 0);
-
-		float value = tuneWorkingPid.getOutput(50, actualThrottlePosition.Value);
-		scheduleMsg(&logger, "AT input=%f output=%f PID=%f", autoTune.input,
-				autoTune.output,
-				value);
-		scheduleMsg(&logger, "AT PID=%f", value);
-		m_motor->set(ETB_PERCENT_TO_DUTY(value));
-
-		if (result) {
-			scheduleMsg(&logger, "GREAT NEWS! %f/%f/%f", autoTune.GetKp(), autoTune.GetKi(), autoTune.GetKd());
-		}
-
-		return;
-	}
-
-
 	float rpm = GET_RPM();
 	engine->engineState.targetFromTable = pedal2tpsMap.getValue(rpm / RPM_1_BYTE_PACKING_MULT, pedalPosition.Value);
 	percent_t etbIdleAddition = CONFIG(useETBforIdleControl) ? engine->engineState.idle.etbIdleAddition : 0;
@@ -236,8 +211,80 @@ void EtbController::PeriodicTask() {
 	m_pid.iTermMin = engineConfiguration->etb_iTermMin;
 	m_pid.iTermMax = engineConfiguration->etb_iTermMax;
 
-	currentEtbDuty = engine->engineState.etbFeedForward +
-			m_pid.getOutput(targetPosition, actualThrottlePosition.Value);
+	float closedLoop;
+
+	// Only allow autotune with stopped engine
+	if (rpm == 0 && engine->etbAutoTune) {
+		bool isPositive = actualThrottlePosition.Value > targetPosition;
+
+		float autotuneAmplitude = 15;
+
+		// Bang-bang control the output to induce oscillation
+		closedLoop = autotuneAmplitude * (isPositive ? -1 : 1);
+
+		// End of cycle - record & reset
+		if (!isPositive && m_lastIsPositive) {
+			efitick_t now = getTimeNowNt();
+
+			// Determine period
+			efitick_t cycleTime = now - m_cycleStartTime;
+			m_cycleStartTime = now;
+
+			// Determine amplitude
+			float a = m_maxCycleTps - m_minCycleTps;
+
+			// Reset bounds
+			m_minCycleTps = 100;
+			m_maxCycleTps = 0;
+
+			// Math is for Åström–Hägglund (relay) auto tuning
+			// https://warwick.ac.uk/fac/cross_fac/iatl/reinvention/archive/volume5issue2/hornsey
+
+			// Publish to TS state
+#if EFI_TUNER_STUDIO
+			if (engineConfiguration->debugMode == DBG_ETB_AUTOTUNE) {
+				// a - amplitude of output (TPS %)
+				tsOutputChannels.debugFloatField1 = a;
+				float b = 2 * autotuneAmplitude;
+				// b - amplitude of input (Duty cycle %)
+				tsOutputChannels.debugFloatField2 = b;
+				// Tu - oscillation period (seconds)
+				float tu = NT2US((float)cycleTime) / 1e6;
+				tsOutputChannels.debugFloatField3 = tu;
+
+				// Ultimate gain per A-H relay tuning rule
+				// Ku
+				float ku = 4 * b / (3.14159f * a);
+				tsOutputChannels.debugFloatField4 = ku;
+
+				// The multipliers below are somewhere near the "no overshoot" 
+				// and "some overshoot" flavors of the Ziegler-Nichols method
+				// Kp
+				tsOutputChannels.debugFloatField5 = 0.35f * ku;
+				// Ki
+				tsOutputChannels.debugFloatField6 = 0.25f * ku / tu;
+				// Kd
+				tsOutputChannels.debugFloatField7 = 0.08f * ku * tu;
+			}
+#endif
+		}
+
+		m_lastIsPositive = isPositive;
+
+		// Find the min/max of each cycle
+		if (actualThrottlePosition.Value < m_minCycleTps) {
+			m_minCycleTps = actualThrottlePosition.Value;
+		}
+
+		if (actualThrottlePosition.Value > m_maxCycleTps) {
+			m_maxCycleTps = actualThrottlePosition.Value;
+		}
+	} else {
+		// Normal case - use PID to compute closed loop part
+		closedLoop = m_pid.getOutput(targetPosition, actualThrottlePosition.Value);
+	}
+
+	currentEtbDuty = engine->engineState.etbFeedForward + closedLoop;
 
 	m_motor->enable();
 	m_motor->set(ETB_PERCENT_TO_DUTY(currentEtbDuty));
@@ -477,15 +524,6 @@ static void setAutoStep(float value) {
 	autoTune.SetOutputStep(value);
 }
 
-static void setAutoPeriod(int period) {
-	tuneWorkingPidSettings.periodMs = period;
-	autoTune.reset();
-}
-
-static void setAutoOffset(int offset) {
-	tuneWorkingPidSettings.offset = offset;
-	autoTune.reset();
-}
 #endif /* EFI_PROD_CODE */
 
 static const float defaultBiasBins[] = {
@@ -532,11 +570,6 @@ void doInitElectronicThrottle(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 
 	pedal2tpsMap.init(config->pedalToTpsTable, config->pedalToTpsPedalBins, config->pedalToTpsRpmBins);
 
-#if 0
-	// not alive code
-	autoTune.SetOutputStep(0.1);
-#endif
-
 #if 0 && ! EFI_UNIT_TEST
 	percent_t startupThrottlePosition = getTPS(PASS_ENGINE_PARAMETER_SIGNATURE);
 	if (absF(startupThrottlePosition - engineConfiguration->etbNeutralPosition) > STARTUP_NEUTRAL_POSITION_ERROR_THRESHOLD) {
@@ -569,23 +602,6 @@ void doInitElectronicThrottle(DECLARE_ENGINE_PARAMETER_SIGNATURE) {
 	addConsoleActionNANF(CMD_ETB_DUTY, setThrottleDutyCycle);
 #endif /* EFI_PROD_CODE */
 
-#if EFI_PROD_CODE && 0
-	tuneWorkingPidSettings.pFactor = 1;
-	tuneWorkingPidSettings.iFactor = 0;
-	tuneWorkingPidSettings.dFactor = 0;
-//	tuneWorkingPidSettings.offset = 10; // todo: not hard-coded value
-	//todo tuneWorkingPidSettings.periodMs = 10;
-	tuneWorkingPidSettings.minValue = 0;
-	tuneWorkingPidSettings.maxValue = 100;
-	tuneWorkingPidSettings.periodMs = 100;
-
-	// this is useful once you do "enable etb_auto"
-	addConsoleActionF("set_etbat_output", setTempOutput);
-	addConsoleActionF("set_etbat_step", setAutoStep);
-	addConsoleActionI("set_etbat_period", setAutoPeriod);
-	addConsoleActionI("set_etbat_offset", setAutoOffset);
-#endif /* EFI_PROD_CODE */
-
 	etbPidReset(PASS_ENGINE_PARAMETER_SIGNATURE);
 
 	for (int i = 0 ; i < engine->etbActualCount; i++) {

firmware/controllers/actuators/electronic_throttle.h

@@ -48,6 +48,12 @@ class EtbController final : public IEtbController {
 	DcMotor *m_motor;
 	Pid m_pid;
 	bool m_shouldResetPid = false;
+
+	// Autotune helpers
+	bool m_lastIsPositive = false;
+	efitick_t m_cycleStartTime = 0;
+	float m_minCycleTps = 0;
+	float m_maxCycleTps = 0;
 };
 
 void initElectronicThrottle(DECLARE_ENGINE_PARAMETER_SIGNATURE);

firmware/controllers/algo/auto_generated_enums.cpp

@@ -535,8 +535,8 @@ case DBG_START_STOP:
   return "DBG_START_STOP";
 case DBG_LAUNCH:
   return "DBG_LAUNCH";
-case DBG_39:
-  return "DBG_39";
+case DBG_ETB_AUTOTUNE:
+  return "DBG_ETB_AUTOTUNE";
 case DBG_40:
   return "DBG_40";
 case DBG_ALTERNATOR_PID:

firmware/controllers/algo/rusefi_enums.h

@@ -675,7 +675,7 @@ typedef enum {
 	DBG_BOOST = 36,
 	DBG_START_STOP = 37,
 	DBG_LAUNCH = 38,
-	DBG_39 = 39,
+	DBG_ETB_AUTOTUNE = 39,
 	DBG_40 = 40,
 
 	Force_4_bytes_size_debug_mode_e = ENUM_32_BITS,

firmware/integration/rusefi_config.txt

@@ -202,7 +202,7 @@ float baseFuel;+Base duration of the fuel injection during cranking, this is mod
 int16_t rpm;+This sets the RPM limit below which the ECU will use cranking fuel and ignition logic, typically this is around 350-450rpm. \nset cranking_rpm X;"RPM",      1,     0,    0, 3000,      0
 end_struct
 
-#define debug_mode_e_enum "Alternator PID", "TPS acceleration enrichment", "INVALID", "Idle Control", "Engine Load accl enrich", "Trigger Counters", "FSIO_ADC", "AUX_PID_1", "VVT input", "Cranking", "Timing", "Closed-loop fuel corr PID", "VSS", "SD card", "sr5", "Knock", "mode16", "Electronic Throttle", "Executor", "Bench Test / TS commands", "Aux Valves", "Analog inputs #1", "INSTANT_RPM", "FSIO_EXPRESSION", "Status", "CJ125", "CAN", "MAP", "Metrics", "ETB#2", "Ion Sense", "TLE8888", "Analog inputs #2", "Dwell Metric", "INVALID", "ETB Logic", "Boost Control", "Start/Stop", "Launch", "Mode39", "Mode40"
+#define debug_mode_e_enum "Alternator PID", "TPS acceleration enrichment", "INVALID", "Idle Control", "Engine Load accl enrich", "Trigger Counters", "FSIO_ADC", "AUX_PID_1", "VVT input", "Cranking", "Timing", "Closed-loop fuel corr PID", "VSS", "SD card", "sr5", "Knock", "mode16", "Electronic Throttle", "Executor", "Bench Test / TS commands", "Aux Valves", "Analog inputs #1", "INSTANT_RPM", "FSIO_EXPRESSION", "Status", "CJ125", "CAN", "MAP", "Metrics", "ETB#2", "Ion Sense", "TLE8888", "Analog inputs #2", "Dwell Metric", "INVALID", "ETB Logic", "Boost Control", "Start/Stop", "Launch", "ETB Autotune", "Mode40"
 custom debug_mode_e 4 bits,    U32,   @OFFSET@, [0:7], @@debug_mode_e_enum@@
 
 #define vvt_mode_e_enum "First half", "Second half", "2GZ", "Miata NB2", "mode4", "mode5", "mode6", "mode7"