From 3e9b2d0e1f87f4b6990e4a16f81b1b6c9ca4e2a7 Mon Sep 17 00:00:00 2001
From: andreika-git <andrei.home@gmail.com>
Date: Fri, 10 Nov 2023 20:12:01 +0200
Subject: [PATCH] Refactor AlternatorController to use ClosedLoopController<>

---
 .../actuators/alternator_controller.cpp       | 102 +++++++++++-------
 .../actuators/alternator_controller.h         |  18 +++-
 2 files changed, 79 insertions(+), 41 deletions(-)

diff --git a/firmware/controllers/actuators/alternator_controller.cpp b/firmware/controllers/actuators/alternator_controller.cpp
index 7aaa1d5f61..75bf26e455 100644
--- a/firmware/controllers/actuators/alternator_controller.cpp
+++ b/firmware/controllers/actuators/alternator_controller.cpp
@@ -20,27 +20,41 @@
 #endif /* HAS_OS_ACCESS */
 
 static SimplePwm alternatorControl("alt");
-static Pid alternatorPid(&persistentState.persistentConfiguration.engineConfiguration.alternatorControl);
 
 static percent_t currentAltDuty;
 
 static bool shouldResetPid = false;
 
-static void pidReset() {
-	alternatorPid.reset();
+void AlternatorController::AlternatorController() {
+	alternatorPid.initPidClass(&engineConfiguration->alternatorControl);
 }
 
-void AlternatorController::onFastCallback() {
-	if (!isBrainPinValid(engineConfiguration->alternatorControlPin)) {
-		return;
+expected<float> AlternatorController::observePlant() {
+	auto vBatt = Sensor::get(SensorType::BatteryVoltage);
+	// if somehow battery voltage isn't valid
+	if (!vBatt)
+		return unexpected;
+	return vBatt.Value;
+}
+
+expected<float> AlternatorController::getSetpoint() {
+	// check if the engine is not running
+	bool alternatorShouldBeEnabledAtCurrentRpm = Sensor::getOrZero(SensorType::Rpm) > engineConfiguration->cranking.rpm;
+
+	if (!engineConfiguration->isAlternatorControlEnabled || !alternatorShouldBeEnabledAtCurrentRpm) {
+		return unexpected;
 	}
 
-#if ! EFI_UNIT_TEST
-	if (shouldResetPid) {
-		pidReset();
-		shouldResetPid = false;
-	}
-#endif
+	return engineConfiguration->targetVBatt;
+}
+
+expected<percent_t> AlternatorController::getOpenLoop(float target) {
+	UNUSED(target);
+	return 0;
+}
+
+expected<percent_t> AlternatorController::getClosedLoop(float targetVoltage, float vBattVoltage) {
+	percent_t altDuty = alternatorPid.getOutput(targetVoltage, vBattVoltage, FAST_CALLBACK_PERIOD_MS / 1000.0f);
 
 	// this block could be executed even in on/off alternator control mode
 	// but at least we would reflect latest state
@@ -48,39 +62,47 @@ void AlternatorController::onFastCallback() {
 	alternatorPid.postState(engine->outputChannels.alternatorStatus);
 #endif /* EFI_TUNER_STUDIO */
 
-	bool alternatorShouldBeEnabledAtCurrentRpm = Sensor::getOrZero(SensorType::Rpm) > engineConfiguration->cranking.rpm;
-
-	if (!engineConfiguration->isAlternatorControlEnabled || !alternatorShouldBeEnabledAtCurrentRpm) {
-		// we need to avoid accumulating iTerm while engine is not running
-		pidReset();
+   	// see "idle air Bump for AC" comment
+	int acDutyBump = engine->module<AcController>().unmock().acButtonState ? engineConfiguration->acRelayAlternatorDutyAdder : 0;
+	altDuty += acDutyBump;
+	if (engineConfiguration->isVerboseAlternator) {
+		efiPrintf("alt duty: %.2f/vbatt=%.2f/p=%.2f/i=%.2f/d=%.2f int=%.2f", altDuty, vBattVoltage,
+				alternatorPid.getP(), alternatorPid.getI(), alternatorPid.getD(), alternatorPid.getIntegration());
+	}
+	return altDuty;
+}
 
+void AlternatorController::setOutput(expected<percent_t> outputValue) {
+	if (outputValue) {
+		currentAltDuty = outputValue.Value;
+		alternatorControl.setSimplePwmDutyCycle(PERCENT_TO_DUTY(outputValue.Value));
+	} else {
 		// Shut off output if not needed
 		alternatorControl.setSimplePwmDutyCycle(0);
-
-		return;
-	}
-
-	auto vBatt = Sensor::get(SensorType::BatteryVoltage);
-	float targetVoltage = engineConfiguration->targetVBatt;
-
-	if (!vBatt) {
-		// Somehow battery voltage isn't valid, disable alternator control
-		alternatorPid.reset();
-		alternatorControl.setSimplePwmDutyCycle(0);
-	} else {
-		currentAltDuty = alternatorPid.getOutput(targetVoltage, vBatt.Value, FAST_CALLBACK_PERIOD_MS / 1000.0f);
-    	// see "idle air Bump for AC" comment
-		int acDutyBump = engine->module<AcController>().unmock().acButtonState ? engineConfiguration->acRelayAlternatorDutyAdder : 0;
-		currentAltDuty += acDutyBump;
-		if (engineConfiguration->isVerboseAlternator) {
-			efiPrintf("alt duty: %.2f/vbatt=%.2f/p=%.2f/i=%.2f/d=%.2f int=%.2f", currentAltDuty, vBatt.Value,
-					alternatorPid.getP(), alternatorPid.getI(), alternatorPid.getD(), alternatorPid.getIntegration());
-		}
-
-		alternatorControl.setSimplePwmDutyCycle(PERCENT_TO_DUTY(currentAltDuty));
+		// we need to avoid accumulating iTerm while the alternator is not working
+		pidReset();
 	}
 }
 
+void AlternatorController::pidReset() {
+	alternatorPid.reset();
+}
+
+void AlternatorController::onFastCallback() {
+	if (!isBrainPinValid(engineConfiguration->alternatorControlPin)) {
+		return;
+	}
+#if ! EFI_UNIT_TEST
+	if (shouldResetPid) {
+		pidReset();
+		shouldResetPid = false;
+	}
+#endif
+
+	ClosedLoopController::update();
+}
+
+
 void showAltInfo(void) {
 	efiPrintf("alt=%s @%s t=%dms", boolToString(engineConfiguration->isAlternatorControlEnabled),
 			hwPortname(engineConfiguration->alternatorControlPin),
@@ -94,7 +116,7 @@ void showAltInfo(void) {
 void setAltPFactor(float p) {
 	engineConfiguration->alternatorControl.pFactor = p;
 	efiPrintf("setAltPid: %.2f", p);
-	pidReset();
+	engine->module<AlternatorController>()->pidReset();
 	showAltInfo();
 }
 
diff --git a/firmware/controllers/actuators/alternator_controller.h b/firmware/controllers/actuators/alternator_controller.h
index 72c677787b..def4a68ea6 100644
--- a/firmware/controllers/actuators/alternator_controller.h
+++ b/firmware/controllers/actuators/alternator_controller.h
@@ -17,8 +17,24 @@ void setAltIFactor(float p);
 void setAltDFactor(float p);
 void showAltInfo(void);
 
-class AlternatorController : public EngineModule {
+class AlternatorController : public EngineModule, public ClosedLoopController<float, percent_t> {
 public:
+	AlternatorController();
+
+	void pidReset();
+
+	// EngineModule implementation
 	void onFastCallback() override;
+	void onConfigurationChange(engine_configuration_s const * previousConfiguration) override;
+
+	// ClosedLoopController implementation
+	expected<float> getSetpoint() override;
+	expected<float> observePlant() override;
+	expected<percent_t> getOpenLoop(float setpoint) override;
+	expected<percent_t> getClosedLoop(float targetVoltage, float vBattVoltage) override;
+	void setOutput(expected<percent_t> outputValue) override;
+
+private:
+	Pid alternatorPid;
 };