Merge pull request #539 from Ralim/lets-fix-qc

Fixing QC 12V

2 files changed, 94 insertions, 81 deletions

diff --git a/workspace/TS100/Core/Src/hardware.cpp b/workspace/TS100/Core/Src/hardware.cpp
index 080705b2..3dd306cb 100644
--- a/workspace/TS100/Core/Src/hardware.cpp
+++ b/workspace/TS100/Core/Src/hardware.cpp
@@ -49,7 +49,7 @@ uint16_t getTipInstantTemperature() {
 }
 
 //2 second filter (ADC is PID_TIM_HZ Hz)
-history<uint16_t, PID_TIM_HZ > rawTempFilter = { { 0 }, 0, 0 };
+history<uint16_t, PID_TIM_HZ> rawTempFilter = { { 0 }, 0, 0 };
 
 uint16_t getTipRawTemp(uint8_t refresh) {
 	if (refresh) {
@@ -66,7 +66,12 @@ uint16_t getInputVoltageX10(uint16_t divisor, uint8_t sample) {
 	// Therefore we can divide down from there
 	// Multiplying ADC max by 4 for additional calibration options,
 	// ideal term is 467
+#ifdef MODEL_TS100
 #define BATTFILTERDEPTH 32
+#else
+#define BATTFILTERDEPTH 8
+
+#endif
 	static uint8_t preFillneeded = 10;
 	static uint32_t samples[BATTFILTERDEPTH];
 	static uint8_t index = 0;
@@ -88,6 +93,51 @@ uint16_t getInputVoltageX10(uint16_t divisor, uint8_t sample) {
 	return sum * 4 / divisor;
 }
 #ifdef MODEL_TS80
+inline void DPlusZero_Six() {
+	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_RESET);		// pull down D+
+}
+inline void DNegZero_Six() {
+	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_SET);
+	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_RESET);
+}
+inline void DPlusThree_Three() {
+	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_SET);			// pull up D+
+}
+inline void DNegThree_Three() {
+	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_SET);
+	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_SET);
+}
+
+inline void QC_Seek9V() {
+	DNegZero_Six();
+	DPlusThree_Three();
+}
+inline void QC_Seek12V() {
+	DNegZero_Six();
+	DPlusZero_Six();
+}
+inline void QC_Seek20V() {
+	DNegThree_Three();
+	DPlusThree_Three();
+}
+inline void QC_SeekContMode() {
+	DNegThree_Three();
+	DPlusZero_Six();
+}
+inline void QC_SeekContPlus() {
+	QC_SeekContMode();
+	vTaskDelay(3);
+	QC_Seek20V();
+	vTaskDelay(1);
+	QC_SeekContMode();
+}
+inline void QC_SeekContNeg() {
+	QC_SeekContMode();
+	vTaskDelay(3);
+	QC_Seek12V();
+	vTaskDelay(1);
+	QC_SeekContMode();
+}
 uint8_t QCMode = 0;
 uint8_t QCTries = 0;
 void seekQC(int16_t Vx10, uint16_t divisor) {
@@ -98,142 +148,108 @@ void seekQC(int16_t Vx10, uint16_t divisor) {
 
 	if (Vx10 < 45)
 		return;
+	if (xTaskGetTickCount() < 100)
+		return;
 	if (Vx10 > 130)
 		Vx10 = 130;  //Cap max value at 13V
 	// Seek the QC to the Voltage given if this adapter supports continuous mode
 	// try and step towards the wanted value
 
 	// 1. Measure current voltage
-	int16_t vStart = getInputVoltageX10(divisor, 0);
+	int16_t vStart = getInputVoltageX10(divisor, 1);
 	int difference = Vx10 - vStart;
 
 	// 2. calculate ideal steps (0.2V changes)
 
 	int steps = difference / 2;
 	if (QCMode == 3) {
+		if (steps > -2 && steps < 2)
+			return; // dont bother with small steps
 		while (steps < 0) {
-			HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_RESET);	//D+0.6
-			HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_SET);	//D-3.3V
-			HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_SET);	// D-3.3Vs
+			QC_SeekContNeg();
 			vTaskDelay(3);
-			HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_RESET);	//-0.6V
-			HAL_Delay(1);
-			HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_SET);
-
-			HAL_Delay(1);
 			steps++;
 		}
 		while (steps > 0) {
-			// step once up
-			HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_RESET);
-			HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_SET);
-			HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_SET);
+			QC_SeekContPlus();
 			vTaskDelay(3);
-
-			HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_SET);
-			HAL_Delay(1);
-			HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_RESET);
-
-			HAL_Delay(1);
 			steps--;
 		}
+		vTaskDelay(10);
 	}
+#ifdef ENABLE_QC2
 	// Re-measure
 	/* Disabled due to nothing to test and code space of around 1k*/
-#ifdef QC2_ROUND_DOWN
-	steps = vStart - getInputVoltageX10(195);
-	if (steps < 0) steps = -steps;
-	if (steps > (difference / 2)) {
+	steps = vStart - getInputVoltageX10(divisor, 1);
+	if (steps < 0)
+		steps = -steps;
+	if (steps > 4) {
 		// No continuous mode, so QC2
 		QCMode = 2;
 		// Goto nearest
-		if (Vx10 > 10.5) {
+		if (Vx10 > 110) {
 			// request 12V
 			// D- = 0.6V, D+ = 0.6V
 			// Clamp PB3
-			HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_RESET);// pull down D+
-			HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_SET);
-			HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_RESET);
+			QC_Seek12V();
 
 		} else {
 			// request 9V
-			HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_SET);
-			HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_SET);
-			HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_RESET);
+			QC_Seek9V();
 		}
 	}
 #endif
 }
-
 // Must be called after FreeRToS Starts
 void startQC(uint16_t divisor) {
 	// Pre check that the input could be >5V already, and if so, dont both
 	// negotiating as someone is feeding in hv
 	uint16_t vin = getInputVoltageX10(divisor, 1);
 	if (vin > 100) {
-		QCMode = 1;  // ALready at ~12V
+		QCMode = 1;  // Already at 12V, user has probably over-ridden this
 		return;
 	}
 	GPIO_InitTypeDef GPIO_InitStruct;
-
-	// Tries to negotiate QC for 9V
-	// This is a multiple step process.
-	// 1. Set around 0.6V on D+ for 1.25 Seconds or so
-	// 2. After this It should un-short D+->D- and instead add a 20k pulldown on
-	// D-
-	// 3. Now set D+ to 3.3V and D- to 0.6V to request 9V
-	// OR both at 0.6V for 12V request (if the adapter can do it).
-	// If 12V is implimented then should fallback to 9V after validation
-	// Step 1. We want to pull D+ to 0.6V
-	// Pull PB3 donwn to ground
-	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_RESET);// pull low to put 0.6V on D+
-	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_RESET);
-	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_RESET);
 	GPIO_InitStruct.Pin = GPIO_PIN_3;
 	GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
 	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_HIGH;
 	HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
-	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_RESET);// pull low to put 0.6V on D+
-	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_RESET);
-	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_RESET);
-
+	GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
+	GPIO_InitStruct.Pull = GPIO_NOPULL;
+	GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_10;
+	HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
+	//Turn off output mode on pins that we can
 	GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
 	GPIO_InitStruct.Pull = GPIO_NOPULL;
 	GPIO_InitStruct.Pin = GPIO_PIN_11 | GPIO_PIN_12 | GPIO_PIN_14 | GPIO_PIN_13;
 	HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
 
+	// Tries to negotiate QC for 9V
+	// This is a multiple step process.
+	// 1. Set around 0.6V on D+ for 1.25 Seconds or so
+	// 2. After this It should un-short D+->D- and instead add a 20k pulldown on
+	// D-
+	DPlusZero_Six();
+
 	// Delay 1.25 seconds
 	uint8_t enteredQC = 0;
-	for (uint16_t i = 0; i < 130 && enteredQC == 0; i++) {
-		//		HAL_Delay(10);
-		vTaskDelay(1);
-
-	}
+	vTaskDelay(125);
 	// Check if D- is low to spot a QC charger
 	if (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_11) == GPIO_PIN_RESET)
 		enteredQC = 1;
 	if (enteredQC) {
 		// We have a QC capable charger
-		HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_SET);
-		HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_SET);
-		HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_RESET);
-
+		QC_Seek9V();
+		GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_10;
 		GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
-		GPIO_InitStruct.Pin = GPIO_PIN_10 | GPIO_PIN_8;
 		HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
-		HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_SET);
-		HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_SET);
-		HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_RESET);
-
+		QC_Seek9V();
 		// Wait for frontend ADC to stabilise
 		QCMode = 4;
 		for (uint8_t i = 0; i < 10; i++) {
 			if (getInputVoltageX10(divisor, 1) > 80) {
 				// yay we have at least QC2.0 or QC3.0
 				QCMode = 3;	// We have at least QC2, pray for 3
-				HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_RESET);
-				HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_SET);
-				HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_SET);
 				return;
 			}
 			vTaskDelay(10);  // 100mS
@@ -245,7 +261,6 @@ void startQC(uint16_t divisor) {
 	} else {
 		// no QC
 		QCMode = 0;
-
 	}
 	if (QCTries > 10)
 		QCMode = 0;
diff --git a/workspace/TS100/Core/Src/main.cpp b/workspace/TS100/Core/Src/main.cpp
index 44e1a5a3..9bbbfec8 100644
--- a/workspace/TS100/Core/Src/main.cpp
+++ b/workspace/TS100/Core/Src/main.cpp
@@ -16,7 +16,7 @@ uint8_t PCBVersion = 0;
 // File local variables
 uint32_t currentTempTargetDegC = 0; // Current temperature target in C
 uint32_t lastMovementTime = 0;
-int16_t idealQCVoltage = 0;
+
 bool settingsWereReset = false;
 // FreeRTOS variables
 
@@ -112,9 +112,6 @@ void startPIDTask(void const *argument __unused) {
 	 * control PWM.
 	 */
 	setTipX10Watts(0); // disable the output driver if the output is set to be off
-#ifdef MODEL_TS80
-	idealQCVoltage = calculateMaxVoltage(systemSettings.cutoutSetting);
-#endif
 
 #ifdef MODEL_TS80
 	//Set power management code to the tip resistance in ohms * 10
@@ -182,12 +179,13 @@ void startPIDTask(void const *argument __unused) {
 			}
 #ifdef MODEL_TS80
 			//If its a TS80, we want to have the option of using an occasional pulse to keep the power bank on
-			if (((xTaskGetTickCount() - lastPowerPulse) > maxPowerIdleTicks) &&
-			    (x10WattsOut < x10PowerPulseWatts)) {
+			if (((xTaskGetTickCount() - lastPowerPulse) > maxPowerIdleTicks)
+					&& (x10WattsOut < x10PowerPulseWatts)) {
 				x10WattsOut = x10PowerPulseWatts;
 			}
-			if (((xTaskGetTickCount() - lastPowerPulse) > (maxPowerIdleTicks + powerPulseTicks)) &&
-			    (x10WattsOut >= x10PowerPulseWatts)) {
+			if (((xTaskGetTickCount() - lastPowerPulse)
+					> (maxPowerIdleTicks + powerPulseTicks))
+					&& (x10WattsOut >= x10PowerPulseWatts)) {
 				lastPowerPulse = xTaskGetTickCount();
 			}
 #endif
@@ -212,7 +210,8 @@ void startMOVTask(void const *argument __unused) {
 	while (pidTaskNotification == 0)
 		osDelay(30);  // To ensure we return after idealQCVoltage/tip resistance
 
-	seekQC(idealQCVoltage, systemSettings.voltageDiv); // this will move the QC output to the preferred voltage to start with
+	seekQC((systemSettings.cutoutSetting) ? 120 : 90,
+			systemSettings.voltageDiv); // this will move the QC output to the preferred voltage to start with
 
 #else
 	osDelay(250);  // wait for accelerometer to stabilize
@@ -275,9 +274,8 @@ void startMOVTask(void const *argument __unused) {
 
 		osDelay(100);  // Slow down update rate
 #ifdef MODEL_TS80
-//		if (currentlyActiveTemperatureTarget) {
-//			seekQC(idealQCVoltage, systemSettings.voltageDiv); // Run the QC seek again to try and compensate for cable V drop
-//		}
+		seekQC((systemSettings.cutoutSetting) ? 120 : 90,
+				systemSettings.voltageDiv); // Run the QC seek again if we have drifted too much
 #endif
 	}
 }