Use RTC backup register to save Stepper IAC position (#471)

firmware/controllers/engine_controller.cpp

@@ -91,13 +91,6 @@ Engine _engine CCM_OPTIONAL;
 Engine * engine = &_engine;
 #endif /* EFI_PROD_CODE */
 
-/**
- * I am not sure if this needs to be configurable.
- *
- * Also technically the whole feature might be implemented as cranking fuel coefficient curve by TPS.
- */
-#define CLEANUP_MODE_TPS 90
-
 static msg_t csThread(void) {
 	chRegSetThreadName("status");
 #if EFI_SHAFT_POSITION_INPUT || defined(__DOXYGEN__)

firmware/controllers/engine_controller.h

@@ -14,6 +14,14 @@
 #include "engine_configuration.h"
 #include "engine.h"
 
+/**
+ * I am not sure if this needs to be configurable.
+ *
+ * Also technically the whole feature might be implemented as cranking fuel coefficient curve by TPS.
+ */
+#define CLEANUP_MODE_TPS 90
+#define STEPPER_PARKING_TPS CLEANUP_MODE_TPS
+
 char * getPinNameByAdcChannel(const char *msg, adc_channel_e hwChannel, char *buffer);
 void initPeriodicEvents(DECLARE_ENGINE_PARAMETER_SIGNATURE);
 void initEngineContoller(Logging *sharedLogger DECLARE_ENGINE_PARAMETER_SUFFIX);

firmware/controllers/idle_thread.cpp

@@ -335,7 +335,7 @@ static void initIdleHardware() {
 	if (boardConfiguration->useStepperIdle) {
 		iacMotor.initialize(boardConfiguration->idle.stepperStepPin, boardConfiguration->idle.stepperDirectionPin, 
 				engineConfiguration->stepperDirectionPinMode, engineConfiguration->idleStepperReactionTime, 
-				engineConfiguration->idleStepperTotalSteps, engineConfiguration->stepperEnablePin);
+				engineConfiguration->idleStepperTotalSteps, engineConfiguration->stepperEnablePin, logger);
 	} else {
 		/**
 		 * Start PWM for idleValvePin

firmware/hw_layer/adc_inputs.cpp

@@ -70,7 +70,7 @@ AdcDevice::AdcDevice(ADCConversionGroup* hwConfig) {
 // is there a reason to have this configurable?
 #define ADC_FAST_DEVICE ADCD2
 
-static int slowAdcCounter = 0;
+static volatile int slowAdcCounter = 0;
 static LoggingWithStorage logger("ADC");
 
 // todo: move this flag to Engine god object
@@ -546,6 +546,14 @@ static void setAdcDebugReporting(int value) {
 	scheduleMsg(&logger, "adcDebug=%d", adcDebugReporting);
 }
 
+void waitForSlowAdc() {
+	// we use slowAdcCounter instead of slowAdc.conversionCount because we need ADC_COMPLETE state
+	// todo: use sync.objects?
+	while (slowAdcCounter < 1) {
+		chThdSleepMilliseconds(1);
+	}
+}
+
 static void adc_callback_slow(ADCDriver *adcp, adcsample_t *buffer, size_t n) {
 	(void) buffer;
 	(void) n;

firmware/hw_layer/adc_inputs.h

@@ -27,6 +27,9 @@ ioportid_t getAdcChannelPort(const char *msg, adc_channel_e hwChannel);
 adc_channel_e getAdcChannel(brain_pin_e pin);
 brain_pin_e getAdcChannelBrainPin(const char *msg, adc_channel_e hwChannel);
 
+// wait until at least 1 slowADC sampling is complete
+void waitForSlowAdc();
+
 int getAdcHardwareIndexByInternalIndex(int index);
 
 void printFullAdcReportIfNeeded(Logging *log);

firmware/hw_layer/stepper.cpp

@@ -10,22 +10,54 @@
 #include "stepper.h"
 #include "pin_repository.h"
 #include "engine.h"
+#include "tps.h"
+#include "engine_controller.h"
+#include "adc_inputs.h"
 
 EXTERN_ENGINE;
 
+static Logging *logger;
+
+static void saveStepperPos(int pos) {
+	// use backup-power RTC registers to store the data
+	RTCD1.rtc->BKP0R = (pos + 1);
+}
+
+static int loadStepperPos() {
+	return (int)RTCD1.rtc->BKP0R - 1;
+}
+
 static msg_t stThread(StepperMotor *motor) {
 	chRegSetThreadName("stepper");
 
 	motor->directionPin.setValue(false);
 
-	/**
+	// try to get saved stepper position (-1 for no data)
-	 * let's park the motor in a known position to begin with
+	motor->currentPosition = loadStepperPos();
-	 *
+
-	 * I believe it's safer to retract the valve for parking - at least on a bench I've seen valves
+	// first wait until at least 1 slowADC sampling is complete
-	 * disassembling themselves while pushing too far out.
+	waitForSlowAdc();
-	 */
+	// now check if stepper motor re-initialization is requested - if the throttle pedal is pressed at startup
-	for (int i = 0; i < motor->totalSteps; i++) {
+	bool forceStepperParking = !engine->rpmCalculator.isRunning(PASS_ENGINE_PARAMETER_SIGNATURE) && getTPS(PASS_ENGINE_PARAMETER_SIGNATURE) > STEPPER_PARKING_TPS;
-		motor->pulse();
+	scheduleMsg(logger, "Stepper: savedStepperPos=%d forceStepperParking=%d (tps=%f)", motor->currentPosition, (forceStepperParking ? 1 : 0), getTPS(PASS_ENGINE_PARAMETER_SIGNATURE));
+
+	if (motor->currentPosition < 0 || forceStepperParking) {
+		// reset saved value
+		saveStepperPos(-1);
+		
+		/**
+		 * let's park the motor in a known position to begin with
+		 *
+		 * I believe it's safer to retract the valve for parking - at least on a bench I've seen valves
+		 * disassembling themselves while pushing too far out.
+		 */
+		for (int i = 0; i < motor->totalSteps; i++) {
+			motor->pulse();
+		}
+
+		// set & save zero stepper position after the parking completion
+		motor->currentPosition = 0;
+		saveStepperPos(motor->currentPosition);
 	}
 
 	while (true) {
@@ -44,6 +76,8 @@ static msg_t stThread(StepperMotor *motor) {
 			motor->currentPosition--;
 		}
 		motor->pulse();
+		// save position to backup RTC register
+		saveStepperPos(motor->currentPosition);
 	}
 
 	// let's part the motor in a known position to begin with
@@ -85,9 +119,12 @@ void StepperMotor::pulse() {
 }
 
 void StepperMotor::initialize(brain_pin_e stepPin, brain_pin_e directionPin, pin_output_mode_e directionPinMode,
-		float reactionTime, int totalSteps, brain_pin_e enablePin) {
+		float reactionTime, int totalSteps, brain_pin_e enablePin, Logging *sharedLogger) {
 	this->reactionTime = maxF(1, reactionTime);
 	this->totalSteps = maxI(3, totalSteps);
+	
+	logger = sharedLogger;
+
 	if (stepPin == GPIO_UNASSIGNED || directionPin == GPIO_UNASSIGNED) {
 		return;
 	}

firmware/hw_layer/stepper.h

@@ -14,7 +14,7 @@ class StepperMotor {
 public:
 	StepperMotor();
 	void initialize(brain_pin_e stepPin, brain_pin_e directionPin, pin_output_mode_e directionPinMode, float reactionTime, int totalSteps,
-			brain_pin_e enablePin);
+			brain_pin_e enablePin, Logging *sharedLogger);
 	void pulse();
 	void setTargetPosition(int targetPosition);
 	int getTargetPosition();