PID rework - use watts

7 files changed, 194 insertions, 138 deletions

diff --git a/workspace/TS100/inc/hardware.h b/workspace/TS100/inc/hardware.h
index 0b70f227..fb129b1f 100644
--- a/workspace/TS100/inc/hardware.h
+++ b/workspace/TS100/inc/hardware.h
@@ -116,9 +116,8 @@ enum TipType {
 #endif
 
 uint16_t getHandleTemperature();
-uint16_t getTipRawTemp(uint8_t instant);
+uint16_t getTipRawTemp(uint8_t refresh);
 uint16_t getInputVoltageX10(uint16_t divisor);
-uint16_t getTipInstantTemperature();
 uint8_t getTipPWM();
 void setTipPWM(uint8_t pulse);
 uint16_t ctoTipMeasurement(uint16_t temp);
@@ -128,8 +127,8 @@ uint16_t tipMeasurementToF(uint16_t raw);
 void seekQC(int16_t Vx10,uint16_t divisor);
 void setCalibrationOffset(int16_t offSet);
 void setTipType(enum TipType tipType, uint8_t manualCalGain);
-uint32_t calculateTipR(uint8_t useFilter);
-int16_t calculateMaxVoltage(uint8_t useFilter, uint8_t useHP);
+uint32_t calculateTipR();
+int16_t calculateMaxVoltage(uint8_t useHP);
 void startQC(uint16_t divisor);  // Tries to negotiate QC for highest voltage, must be run after
                  // RToS
 // This will try for 12V, failing that 9V, failing that 5V
diff --git a/workspace/TS100/inc/history.hpp b/workspace/TS100/inc/history.hpp
new file mode 100644
index 00000000..856f6536
--- /dev/null
+++ b/workspace/TS100/inc/history.hpp
@@ -0,0 +1,41 @@
+/*
+ * history.hpp
+ *
+ *  Created on: 28 Oct, 2018
+ *     Authors: Ben V. Brown, David Hilton
+ */
+
+#ifndef HISTORY_HPP_
+#define HISTORY_HPP_
+
+#include <stdint.h>
+
+// max size = 127
+template <class T=uint16_t, uint8_t SIZE=15>
+struct history {
+    static const uint8_t size = SIZE;
+	T buf[size];
+	int32_t sum;
+	uint8_t loc;
+
+	void update(T const val) {
+		// step backwards so i+1 is the previous value.
+		loc = (size+loc-1) % size;
+
+		sum -= buf[loc];
+		sum += val;
+		buf[loc] = val;
+	}
+
+	T operator[] (uint8_t i) const {
+		// 0 = newest, size-1 = oldest.
+		i = (i+loc) % size;
+		return buf[i];
+	}
+
+	T average() const {
+		return sum / size;
+	}
+};
+
+#endif  /* HISTORY_HPP_ */
diff --git a/workspace/TS100/inc/power.hpp b/workspace/TS100/inc/power.hpp
new file mode 100644
index 00000000..de4c35db
--- /dev/null
+++ b/workspace/TS100/inc/power.hpp
@@ -0,0 +1,21 @@
+/*
+ * Power.hpp
+ *
+ *  Created on: 28 Oct, 2018
+ *     Authors: Ben V. Brown, David Hilton
+ */
+
+#include "stdint.h"
+#include <history.hpp>
+
+#ifndef POWER_HPP_
+#define POWER_HPP_
+
+extern history<uint16_t, 75> milliWattHistory;
+
+int32_t tempToMilliWatts(int32_t rawTemp, uint16_t mass, uint8_t rawC);
+void setTipMilliWatts(int32_t mw);
+uint8_t milliWattsToPWM(int32_t milliWatts, uint8_t divisor);
+int32_t PWMToMilliWatts(uint8_t pwm, uint8_t divisor);
+
+#endif /* POWER_HPP_ */
diff --git a/workspace/TS100/src/gui.cpp b/workspace/TS100/src/gui.cpp
index b57bf243..9bc11555 100644
--- a/workspace/TS100/src/gui.cpp
+++ b/workspace/TS100/src/gui.cpp
@@ -664,20 +664,8 @@ static void settings_displayTipModel(void) {
 #endif
 }
 static void calibration_displaySimpleCal(void) { printShortDescription(18, 5); }
-static void dotDelay() {
-  for (uint8_t i = 0; i < 20; i++) {
-    getTipRawTemp(
-        1);  // cycle through the filter a fair bit to ensure we're stable.
-    OLED::clearScreen();
-    OLED::setCursor(0, 0);
-    for (uint8_t x = 0; x < i / 4; x++) OLED::print(".");
-    OLED::refresh();
-    osDelay(50);
-  }
-}
 static void setTipOffset() {
   setCalibrationOffset(0);  // turn off the current offset
-  dotDelay();
 
   // If the thermocouple at the end of the tip, and the handle are at
   // equalibrium, then the output should be zero, as there is no temperature
@@ -685,8 +673,7 @@ static void setTipOffset() {
 
   int32_t offset = 0;
   for (uint8_t i = 0; i < 15; i++) {
-    offset += getTipRawTemp(
-        1);  // cycle through the filter a fair bit to ensure we're stable.
+    offset += getTipRawTemp(1);
 
     OLED::clearScreen();
     OLED::setCursor(0, 0);
@@ -719,7 +706,6 @@ static void calibration_enterSimpleCal(void) {
     OLED::refresh();
     osDelay(200);
     waitForButtonPress();
-    dotDelay();  // cycle the filter a bit
     // Now take the three hot measurements
     // Assume water is boiling at 100C
     uint32_t RawTipHot = getTipRawTemp(0) * 10;
diff --git a/workspace/TS100/src/hardware.c b/workspace/TS100/src/hardware.c
index 9b281cf2..17ba5273 100644
--- a/workspace/TS100/src/hardware.c
+++ b/workspace/TS100/src/hardware.c
@@ -118,28 +118,16 @@ uint16_t lookupTipDefaultCalValue(enum TipType tipID) {
 #endif
 }
 
-uint16_t getTipRawTemp(uint8_t instant) {
-	static int64_t filterFP = 0;
+uint16_t getTipRawTemp(uint8_t refresh) {
 	static uint16_t lastSample = 0;
-	const uint8_t filterBeta = 7; // higher values smooth out more, but reduce responsiveness
-
-	if (instant == 1) {
-		uint16_t itemp = getTipInstantTemperature();
-		filterFP = (filterFP << filterBeta) - filterFP;
-		filterFP += (itemp << 9);
-		filterFP = filterFP >> filterBeta;
-		uint16_t temp = itemp;
-		itemp += lastSample;
-		itemp /= 2;
-		lastSample = temp;
-		return itemp;
-	} else if (instant == 2) {
-		filterFP = (getTipInstantTemperature() << 8);
-		return filterFP >> 9;
-	} else {
-		return filterFP >> 9;
+
+	if (refresh) {
+		lastSample = getTipInstantTemperature();
 	}
+
+	return lastSample;
 }
+
 uint16_t getInputVoltageX10(uint16_t divisor) {
 	// ADC maximum is 32767 == 3.3V at input == 28.05V at VIN
 	// Therefore we can divide down from there
@@ -333,7 +321,7 @@ void startQC(uint16_t divisor) {
 		QCMode = 0;
 }
 // Get tip resistance in milliohms
-uint32_t calculateTipR(uint8_t useFilter) {
+uint32_t calculateTipR() {
 	// We inject a small current into the front end of the iron,
 	// By measuring the Vdrop over the tip we can calculate the resistance
 	// Turn PA0 into an output and drive high to inject (3.3V-0.6)/(6K8+Rtip)
@@ -347,27 +335,19 @@ uint32_t calculateTipR(uint8_t useFilter) {
 	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_0, GPIO_PIN_RESET);	// Set low first
 	setTipPWM(0);
 	vTaskDelay(1);
-	uint32_t offReading = getTipInstantTemperature();
+	uint32_t offReading = getTipRawTemp(1);
 	for (uint8_t i = 0; i < 24; i++) {
-		if (useFilter == 0) {
-			vTaskDelay(1);  // delay to allow it too stabilize
-			offReading += getTipInstantTemperature();
-		} else {
-			offReading += getTipRawTemp(0);
-		}
+		vTaskDelay(1);  // delay to allow it to stabilize
+		offReading += getTipRawTemp(1);
 	}
 
 	// Turn on
 	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_0, GPIO_PIN_SET);  // Set low first
-	vTaskDelay(1);  // delay to allow it too stabilize
+	vTaskDelay(1);  // delay to allow it to stabilize
 	uint32_t onReading = getTipInstantTemperature();
 	for (uint8_t i = 0; i < 24; i++) {
-		if (useFilter == 0) {
-			vTaskDelay(1);  // delay to allow it too stabilize
-			onReading += getTipInstantTemperature();
-		} else {
-			onReading += getTipRawTemp(0);
-		}
+		vTaskDelay(1);  // delay to allow it to stabilize
+		onReading += getTipRawTemp(1);
 	}
 
 	uint32_t difference = onReading - offReading;
@@ -392,11 +372,11 @@ static unsigned int sqrt32(unsigned long n) {
 		g |= c;
 	}
 }
-int16_t calculateMaxVoltage(uint8_t useFilter, uint8_t useHP) {
+int16_t calculateMaxVoltage(uint8_t useHP) {
 	// This measures the tip resistance, then it calculates the appropriate
 	// voltage To stay under ~18W. Mosfet is "9A", so no issues there
 	// QC3.0 supports up to 18W, which is 2A @9V and 1.5A @12V
-	uint32_t milliOhms = calculateTipR(useFilter);
+	uint32_t milliOhms = calculateTipR();
 	// Check no tip
 	if (milliOhms > 10000)
 		return -1;
diff --git a/workspace/TS100/src/main.cpp b/workspace/TS100/src/main.cpp
index 94599d85..be365ab8 100644
--- a/workspace/TS100/src/main.cpp
+++ b/workspace/TS100/src/main.cpp
@@ -3,6 +3,8 @@
 #include <gui.hpp>
 #include <main.hpp>
 #include "LIS2DH12.hpp"
+#include <history.hpp>
+#include <power.hpp>
 #include "Settings.h"
 #include "Translation.h"
 #include "cmsis_os.h"
@@ -36,7 +38,7 @@ int main(void) {
 	HAL_Init();
 	Setup_HAL();  // Setup all the HAL objects
 	HAL_IWDG_Refresh(&hiwdg);
-	setTipPWM(0);  // force tip off
+	setTipMilliWatts(0);  // force tip off
 	FRToSI2C::init(&hi2c1);
 	OLED::initialize();  // start up the LCD
 	OLED::setFont(0);    // default to bigger font
@@ -87,6 +89,21 @@ int main(void) {
 	while (1) {
 	}
 }
+
+void debugNumber(int32_t val) {
+	if (abs(val) > 99999) {
+		OLED::print(" OoB"); // out of bounds
+		return;
+	}
+	if (val >= 0) {
+		OLED::drawChar(' ');
+		OLED::printNumber(val, 5);
+	} else {
+		OLED::drawChar('-');
+		OLED::printNumber(-val, 5);
+	}
+}
+
 void printVoltage() {
 	OLED::printNumber(getInputVoltageX10(systemSettings.voltageDiv) / 10, 2);
 	OLED::drawChar('.');
@@ -535,8 +552,10 @@ static void gui_solderingMode(uint8_t jumpToSleep) {
 			if (systemSettings.detailedSoldering) {
 				OLED::setFont(1);
 				OLED::print(SolderingAdvancedPowerPrompt);  // Power:
-				OLED::printNumber(getTipPWM(), 3);
-				OLED::print("%");
+				OLED::printNumber(milliWattHistory[0] / 1000, 2);
+				OLED::drawChar('.');
+				OLED::printNumber(milliWattHistory[0] / 100 % 10, 1);
+				OLED::drawChar('W');
 
 				if (systemSettings.sensitivity && systemSettings.SleepTime) {
 					OLED::print(" ");
@@ -723,7 +742,7 @@ void startGUITask(void const *argument __unused) {
 	uint8_t tempWarningState = 0;
 	bool buttonLockout = false;
 	bool tempOnDisplay = false;
-	getTipRawTemp(2);  // reset filter
+	getTipRawTemp(1);  // reset filter
 	OLED::setRotation(systemSettings.OrientationMode & 1);
 	uint32_t ticks = xTaskGetTickCount();
 	ticks += 400;  // 4 seconds from now
@@ -786,8 +805,7 @@ void startGUITask(void const *argument __unused) {
 #ifdef MODEL_TS80
 			//Here we re-check for tip presence
 			if (idealQCVoltage < 90)
-				idealQCVoltage = calculateMaxVoltage(1,
-						systemSettings.cutoutSetting); // 1 means use filtered values rather than do its own
+				idealQCVoltage = calculateMaxVoltage(systemSettings.cutoutSetting);
 			seekQC(idealQCVoltage,systemSettings.voltageDiv);
 #endif
 			gui_solderingMode(0);  // enter soldering mode
@@ -807,7 +825,7 @@ void startGUITask(void const *argument __unused) {
 
 		currentlyActiveTemperatureTarget = 0;  // ensure tip is off
 
-		uint16_t tipTemp = tipMeasurementToC(getTipRawTemp(1)); // This forces a faster update rate on the filtering
+		uint16_t tipTemp = tipMeasurementToC(getTipRawTemp(0));
 
 		if (tipTemp < 50) {
 			if (systemSettings.sensitivity) {
@@ -906,37 +924,15 @@ void startPIDTask(void const *argument __unused) {
 	 * struct
 	 *
 	 */
-	setTipPWM(0);  // disable the output driver if the output is set to be off
-#ifdef MODEL_TS100
-			for (uint8_t i = 0; i < 50; i++) {
-				osDelay(10);
-				getTipRawTemp(1);  // cycle up the tip temp filter
-				HAL_IWDG_Refresh(&hiwdg);
-			}
-#else
-	// On the TS80 we can measure the tip resistance before cycling the filter a
-	// bit
-	idealQCVoltage = 0;
-	idealQCVoltage = calculateMaxVoltage(0, systemSettings.cutoutSetting);
-	// Rapidly cycle the filter to help converge
-	HAL_IWDG_Refresh(&hiwdg);
-	for (uint8_t i = 0; i < 50; i++) {
-		osDelay(11);
-		getTipRawTemp(1);  // cycle up the tip temp filter
-	}
-	HAL_IWDG_Refresh(&hiwdg);
-
+	setTipMilliWatts(0);  // disable the output driver if the output is set to be off
+#ifdef MODEL_TS80
+	idealQCVoltage = calculateMaxVoltage(systemSettings.cutoutSetting);
 #endif
+	int32_t rawC = ctoTipMeasurement(100) - ctoTipMeasurement(101);  // 1*C change in raw.
 	currentlyActiveTemperatureTarget = 0; // Force start with no output (off). If in sleep / soldering this will
-										  // be over-ridded rapidly
-	int32_t integralCount = 0;
-	int32_t derivativeLastValue = 0;
-
-	// REMEBER ^^^^ These constants are backwards
-	// They act as dividers, so to 'increase' a P term, you make the number
-	// smaller.
+										  // be over-ridden rapidly
+	history<int16_t> tempError = {{0}, 0, 0};
 
-	const int32_t itermMax = 100;
 	pidTaskNotification = xTaskGetCurrentTaskHandle();
 	for (;;) {
 		if (ulTaskNotifyTake(pdTRUE, 50)) {
@@ -944,69 +940,54 @@ void startPIDTask(void const *argument __unused) {
 			// This is a call to block this thread until the ADC does its samples
 			uint16_t rawTemp = getTipRawTemp(1);  // get instantaneous reading
 			if (currentlyActiveTemperatureTarget) {
-				// Compute the PID loop in here
-				// Because our values here are quite large for all measurements (0-32k
-				// ~= 66 counts per C) P I & D are divisors, so inverse logic applies
-				// (beware)
-
 				// Cap the max set point to 450C
 				if (currentlyActiveTemperatureTarget > ctoTipMeasurement(450)) {
 					currentlyActiveTemperatureTarget = ctoTipMeasurement(450);
 				}
 
-				int32_t rawTempError = currentlyActiveTemperatureTarget
-						- rawTemp;
-
-				int32_t ierror = (rawTempError
-						/ ((int32_t) systemSettings.PID_I));
-
-				integralCount += ierror;
-
-				if (integralCount > (itermMax / 2))
-					integralCount = itermMax / 2;  // prevent too much lead
-				else if (integralCount < -itermMax)
-					integralCount = itermMax;
-
-				int32_t dInput = (rawTemp - derivativeLastValue);
-
-				/*Compute PID Output*/
-				int32_t output = (rawTempError
-						/ ((int32_t) systemSettings.PID_P));
-				if (((int32_t) systemSettings.PID_I))
-					output += integralCount;
-				if (((int32_t) systemSettings.PID_D))
-					output -= (dInput / ((int32_t) systemSettings.PID_D));
-
-				if (output > 100) {
-					output = 100;  // saturate
-				} else if (output < 0) {
-					output = 0;
-				}
-
-				if (currentlyActiveTemperatureTarget < rawTemp) {
-					output = 0;
-					integralCount = 0;
-					derivativeLastValue = 0;
-				}
-				setTipPWM(output);
-				derivativeLastValue = rawTemp;  // store for next loop
-
+				// As we get close to our target, temp noise causes the system
+				//  to be unstable. Use a rolling average to dampen it.
+				// We overshoot by roughly 1/2 of 1 degree Fahrenheit.
+				//  This helps stabilize the display.
+				tempError.update(currentlyActiveTemperatureTarget - rawTemp + rawC/4);
+
+				// Now for the PID!
+				int32_t milliWattsOut = 0;
+
+				// P term - total power needed to hit target temp next cycle.
+				// thermal mass = 1690 milliJ/*C for my tip.
+				//  = Watts*Seconds to raise Temp from room temp to +100*C, divided by 100*C.
+				// divided by 4 to let I term dominate near set point.
+				const uint16_t mass = 1690 / 4;
+				int32_t milliWattsNeeded = tempToMilliWatts(tempError.average(), mass, rawC);
+				milliWattsOut += milliWattsNeeded;
+
+				// I term - energy needed to compensate for heat loss.
+				// We track energy put into the system over some window.
+				// Assuming the temp is stable, energy in = energy transfered.
+				//  (If it isn't, P will dominate).
+				milliWattsOut += milliWattHistory.average();
+
+				// D term - use sudden temp change to counter fast cooling/heating.
+				//  In practice, this provides an early boost if temp is dropping
+				//  and counters extra power if the iron is no longer losing temp.
+				// basically: temp - lastTemp
+				//  Unfortunately, our temp signal is too noisy to really help.
+
+				setTipMilliWatts(milliWattsOut);
 			} else {
-				setTipPWM(0); // disable the output driver if the output is set to be off
-				integralCount = 0;
-				derivativeLastValue = 0;
+				setTipMilliWatts(0);
 			}
 
 			HAL_IWDG_Refresh(&hiwdg);
 		} else {
 			if (currentlyActiveTemperatureTarget == 0) {
-				setTipPWM(0); // disable the output driver if the output is set to be off
-				integralCount = 0;
-				derivativeLastValue = 0;
+				setTipMilliWatts(0);
 			}
 		}
 	}
 }
+
 #define MOVFilter 8
 void startMOVTask(void const *argument __unused) {
 	OLED::setRotation(false);
diff --git a/workspace/TS100/src/power.cpp b/workspace/TS100/src/power.cpp
new file mode 100644
index 00000000..a249dc14
--- /dev/null
+++ b/workspace/TS100/src/power.cpp
@@ -0,0 +1,48 @@
+/*
+ * power.cpp
+ *
+ *  Created on: 28 Oct, 2018
+ *     Authors: Ben V. Brown, David Hilton
+ */
+
+#include <power.hpp>
+#include <Settings.h>
+#include <hardware.h>
+
+history<uint16_t, 75> milliWattHistory = {{0}, 0, 0};
+
+const uint8_t tipResistance = 87;
+const uint8_t hz = 33;
+const uint8_t maxPWM = 100;
+
+int32_t tempToMilliWatts(int32_t rawTemp, uint16_t mass, uint8_t rawC) {
+	 // mass is in milliJ/*C, rawC is raw per degree C
+	int32_t milliJoules = mass * rawTemp / rawC;
+	return milliJoules * hz;
+}
+
+void setTipMilliWatts(int32_t mw) {
+	int32_t output = milliWattsToPWM(mw, systemSettings.voltageDiv / 10);
+	setTipPWM(output);
+	uint16_t actualMilliWatts = PWMToMilliWatts(output, systemSettings.voltageDiv / 10);
+
+	milliWattHistory.update(actualMilliWatts);
+}
+
+uint8_t milliWattsToPWM(int32_t milliWatts, uint8_t divisor) {
+	int32_t v = getInputVoltageX10(divisor); // 1000 = 10v
+	int32_t availableMilliWatts = v*v / tipResistance;
+	int32_t pwm = maxPWM * milliWatts / availableMilliWatts;
+
+	if (pwm > maxPWM) {
+		pwm = maxPWM;
+	} else if (pwm < 0) {
+		pwm = 0;
+	}
+	return pwm;
+}
+
+int32_t PWMToMilliWatts(uint8_t pwm, uint8_t divisor) {
+	int32_t v = getInputVoltageX10(divisor);
+	return pwm * (v*v / tipResistance) / maxPWM;
+}