From fd700aecb9f629f75d4a2c3b4ecb278728ce7362 Mon Sep 17 00:00:00 2001
From: "Ben V. Brown" <Ralim@Ralimtek.com>
Date: Fri, 29 May 2020 21:39:06 +1000
Subject: [PATCH] Beginnings of the shuffle

---
 workspace/TS100/Core/BSP/Defines.h            |  19 +
 workspace/TS100/Core/BSP/Miniware/BSP.cpp     | 212 ++++++++++
 .../{Inc/hardware.h => BSP/Miniware/Pins.h}   | 231 ++++------
 .../TS100/Core/BSP/Miniware/UnitSettings.h    |  18 +
 workspace/TS100/Core/Inc/Buttons.hpp          |  35 ++
 workspace/TS100/Core/Inc/FRToSI2C.hpp         |  90 ++--
 workspace/TS100/Core/Inc/FreeRTOSHooks.h      |  28 ++
 workspace/TS100/Core/Inc/LIS2DH12.hpp         |   5 +-
 workspace/TS100/Core/Inc/MMA8652FC.hpp        |   2 +-
 workspace/TS100/Core/Inc/OLED.hpp             |   2 +-
 workspace/TS100/Core/Inc/QC3.h                |  21 +
 .../TS100/Core/{Src => Inc}/TipThermoModel.h  |   2 +-
 workspace/TS100/Core/Inc/gui.hpp              |  76 ++--
 workspace/TS100/Core/Inc/main.hpp             |  18 -
 workspace/TS100/Core/Inc/power.hpp            |   2 +-
 workspace/TS100/Core/Src/Buttons.cpp          | 115 +++++
 workspace/TS100/Core/Src/FRToSI2C.cpp         | 331 +++++++--------
 workspace/TS100/Core/Src/FreeRTOSHooks.c      |  24 ++
 workspace/TS100/Core/Src/GUIThread.cpp        | 108 +----
 workspace/TS100/Core/Src/LIS2DH12.cpp         |  30 +-
 workspace/TS100/Core/Src/MMA8652FC.cpp        |   9 +-
 workspace/TS100/Core/Src/OLED.cpp             |   4 -
 workspace/TS100/Core/Src/QC3.c                | 203 +++++++++
 workspace/TS100/Core/Src/TipThermoModel.cpp   |   2 +-
 workspace/TS100/Core/Src/gui.cpp              |   2 +-
 workspace/TS100/Core/Src/hardware.cpp         | 393 ------------------
 workspace/TS100/Core/Src/power.cpp            |   2 +-
 27 files changed, 1005 insertions(+), 979 deletions(-)
 create mode 100644 workspace/TS100/Core/BSP/Defines.h
 create mode 100644 workspace/TS100/Core/BSP/Miniware/BSP.cpp
 rename workspace/TS100/Core/{Inc/hardware.h => BSP/Miniware/Pins.h} (53%)
 create mode 100644 workspace/TS100/Core/BSP/Miniware/UnitSettings.h
 create mode 100644 workspace/TS100/Core/Inc/Buttons.hpp
 create mode 100644 workspace/TS100/Core/Inc/FreeRTOSHooks.h
 create mode 100644 workspace/TS100/Core/Inc/QC3.h
 rename workspace/TS100/Core/{Src => Inc}/TipThermoModel.h (98%)
 create mode 100644 workspace/TS100/Core/Src/Buttons.cpp
 create mode 100644 workspace/TS100/Core/Src/FreeRTOSHooks.c
 create mode 100644 workspace/TS100/Core/Src/QC3.c
 delete mode 100644 workspace/TS100/Core/Src/hardware.cpp

diff --git a/workspace/TS100/Core/BSP/Defines.h b/workspace/TS100/Core/BSP/Defines.h
new file mode 100644
index 00000000..52556f99
--- /dev/null
+++ b/workspace/TS100/Core/BSP/Defines.h
@@ -0,0 +1,19 @@
+/*
+ * Defines.h
+ *
+ *  Created on: 29 May 2020
+ *      Author: Ralim
+ */
+
+#ifndef BSP_DEFINES_H_
+#define BSP_DEFINES_H_
+
+
+enum Orientation {
+	ORIENTATION_LEFT_HAND = 0, ORIENTATION_RIGHT_HAND = 1, ORIENTATION_FLAT = 3
+};
+
+//It is assumed that all hardware implements an 8Hz update period at this time
+#define PID_TIM_HZ (8)
+
+#endif /* BSP_DEFINES_H_ */
diff --git a/workspace/TS100/Core/BSP/Miniware/BSP.cpp b/workspace/TS100/Core/BSP/Miniware/BSP.cpp
new file mode 100644
index 00000000..21eaa3cb
--- /dev/null
+++ b/workspace/TS100/Core/BSP/Miniware/BSP.cpp
@@ -0,0 +1,212 @@
+//BSP mapping functions
+
+#include "BSP.h"
+#include "Setup.h"
+#include "history.hpp"
+#include "Pins.h"
+#include "history.hpp"
+volatile uint16_t PWMSafetyTimer = 0;
+volatile uint8_t pendingPWM = 0;
+
+
+//2 second filter (ADC is PID_TIM_HZ Hz)
+history<uint16_t, PID_TIM_HZ> rawTempFilter = { { 0 }, 0, 0 };
+void resetWatchdog() {
+	HAL_IWDG_Refresh(&hiwdg);
+}
+
+uint16_t getHandleTemperature() {
+	// We return the current handle temperature in X10 C
+	// TMP36 in handle, 0.5V offset and then 10mV per deg C (0.75V @ 25C for
+	// example) STM32 = 4096 count @ 3.3V input -> But We oversample by 32/(2^2) =
+	// 8 times oversampling Therefore 32768 is the 3.3V input, so 0.1007080078125
+	// mV per count So we need to subtract an offset of 0.5V to center on 0C
+	// (4964.8 counts)
+	//
+	int32_t result = getADC(0);
+	result -= 4965;  // remove 0.5V offset
+	// 10mV per C
+	// 99.29 counts per Deg C above 0C
+	result *= 100;
+	result /= 993;
+	return result;
+}
+
+uint16_t getTipInstantTemperature() {
+	uint16_t sum = 0;	// 12 bit readings * 8 -> 15 bits
+	uint16_t readings[8];
+	//Looking to reject the highest outlier readings.
+	//As on some hardware these samples can run into the op-amp recovery time
+	//Once this time is up the signal stabilises quickly, so no need to reject minimums
+	readings[0] = hadc1.Instance->JDR1;
+	readings[1] = hadc1.Instance->JDR2;
+	readings[2] = hadc1.Instance->JDR3;
+	readings[3] = hadc1.Instance->JDR4;
+	readings[4] = hadc2.Instance->JDR1;
+	readings[5] = hadc2.Instance->JDR2;
+	readings[6] = hadc2.Instance->JDR3;
+	readings[7] = hadc2.Instance->JDR4;
+
+	for (int i = 0; i < 8; i++) {
+		sum += readings[i];
+	}
+	return sum;  // 8x over sample
+}
+
+uint16_t getTipRawTemp(uint8_t refresh) {
+	if (refresh) {
+		uint16_t lastSample = getTipInstantTemperature();
+		rawTempFilter.update(lastSample);
+		return lastSample;
+	} else {
+		return rawTempFilter.average();
+	}
+}
+
+uint16_t getInputVoltageX10(uint16_t divisor, uint8_t sample) {
+	// ADC maximum is 32767 == 3.3V at input == 28.05V at VIN
+	// 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;
+	if (preFillneeded) {
+		for (uint8_t i = 0; i < BATTFILTERDEPTH; i++)
+			samples[i] = getADC(1);
+		preFillneeded--;
+	}
+	if (sample) {
+		samples[index] = getADC(1);
+		index = (index + 1) % BATTFILTERDEPTH;
+	}
+	uint32_t sum = 0;
+
+	for (uint8_t i = 0; i < BATTFILTERDEPTH; i++)
+		sum += samples[i];
+
+	sum /= BATTFILTERDEPTH;
+	return sum * 4 / divisor;
+}
+
+void setTipPWM(uint8_t pulse) {
+	PWMSafetyTimer = 10; // This is decremented in the handler for PWM so that the tip pwm is
+						 // disabled if the PID task is not scheduled often enough.
+
+	pendingPWM = pulse;
+}
+
+// These are called by the HAL after the corresponding events from the system
+// timers.
+
+void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
+// Period has elapsed
+	if (htim->Instance == TIM2) {
+		// we want to turn on the output again
+		PWMSafetyTimer--;
+		// We decrement this safety value so that lockups in the
+		// scheduler will not cause the PWM to become locked in an
+		// active driving state.
+		// While we could assume this could never happen, its a small price for
+		// increased safety
+		htim2.Instance->CCR4 = pendingPWM;
+		if (htim2.Instance->CCR4 && PWMSafetyTimer) {
+			HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
+		} else {
+			HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_1);
+		}
+	} else if (htim->Instance == TIM1) {
+		// STM uses this for internal functions as a counter for timeouts
+		HAL_IncTick();
+	}
+}
+
+void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim) {
+// This was a when the PWM for the output has timed out
+	if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_4) {
+		HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_1);
+	}
+}
+void unstick_I2C() {
+	GPIO_InitTypeDef GPIO_InitStruct;
+	int timeout = 100;
+	int timeout_cnt = 0;
+
+	// 1. Clear PE bit.
+	hi2c1.Instance->CR1 &= ~(0x0001);
+	/**I2C1 GPIO Configuration
+	 PB6     ------> I2C1_SCL
+	 PB7     ------> I2C1_SDA
+	 */
+	//  2. Configure the SCL and SDA I/Os as General Purpose Output Open-Drain, High level (Write 1 to GPIOx_ODR).
+	GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
+	GPIO_InitStruct.Pull = GPIO_PULLUP;
+	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+
+	GPIO_InitStruct.Pin = SCL_Pin;
+	HAL_GPIO_Init(SCL_GPIO_Port, &GPIO_InitStruct);
+	HAL_GPIO_WritePin(SCL_GPIO_Port, SCL_Pin, GPIO_PIN_SET);
+
+	GPIO_InitStruct.Pin = SDA_Pin;
+	HAL_GPIO_Init(SDA_GPIO_Port, &GPIO_InitStruct);
+	HAL_GPIO_WritePin(SDA_GPIO_Port, SDA_Pin, GPIO_PIN_SET);
+
+	while (GPIO_PIN_SET != HAL_GPIO_ReadPin(SDA_GPIO_Port, SDA_Pin)) {
+		//Move clock to release I2C
+		HAL_GPIO_WritePin(SCL_GPIO_Port, SCL_Pin, GPIO_PIN_RESET);
+		asm("nop");
+		asm("nop");
+		asm("nop");
+		asm("nop");
+		HAL_GPIO_WritePin(SCL_GPIO_Port, SCL_Pin, GPIO_PIN_SET);
+
+		timeout_cnt++;
+		if (timeout_cnt > timeout)
+			return;
+	}
+
+	// 12. Configure the SCL and SDA I/Os as Alternate function Open-Drain.
+	GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
+	GPIO_InitStruct.Pull = GPIO_PULLUP;
+	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
+
+	GPIO_InitStruct.Pin = SCL_Pin;
+	HAL_GPIO_Init(SCL_GPIO_Port, &GPIO_InitStruct);
+
+	GPIO_InitStruct.Pin = SDA_Pin;
+	HAL_GPIO_Init(SDA_GPIO_Port, &GPIO_InitStruct);
+
+	HAL_GPIO_WritePin(SCL_GPIO_Port, SCL_Pin, GPIO_PIN_SET);
+	HAL_GPIO_WritePin(SDA_GPIO_Port, SDA_Pin, GPIO_PIN_SET);
+
+	// 13. Set SWRST bit in I2Cx_CR1 register.
+	hi2c1.Instance->CR1 |= 0x8000;
+
+	asm("nop");
+
+	// 14. Clear SWRST bit in I2Cx_CR1 register.
+	hi2c1.Instance->CR1 &= ~0x8000;
+
+	asm("nop");
+
+	// 15. Enable the I2C peripheral by setting the PE bit in I2Cx_CR1 register
+	hi2c1.Instance->CR1 |= 0x0001;
+
+	// Call initialization function.
+	HAL_I2C_Init(&hi2c1);
+}
+
+uint8_t getButtonA() {
+	return HAL_GPIO_ReadPin(KEY_A_GPIO_Port, KEY_A_Pin) == GPIO_PIN_RESET ?
+			1 : 0;
+}
+uint8_t getButtonB() {
+	return HAL_GPIO_ReadPin(KEY_B_GPIO_Port, KEY_B_Pin) == GPIO_PIN_RESET ?
+			1 : 0;
+}
diff --git a/workspace/TS100/Core/Inc/hardware.h b/workspace/TS100/Core/BSP/Miniware/Pins.h
similarity index 53%
rename from workspace/TS100/Core/Inc/hardware.h
rename to workspace/TS100/Core/BSP/Miniware/Pins.h
index 87bf69cc..1f0ddfa9 100644
--- a/workspace/TS100/Core/Inc/hardware.h
+++ b/workspace/TS100/Core/BSP/Miniware/Pins.h
@@ -1,148 +1,83 @@
-/*
- * Hardware.h
- *
- *  Created on: 29Aug.,2017
- *      Author: Ben V. Brown
- */
-
-#ifndef HARDWARE_H_
-#define HARDWARE_H_
-#include "Setup.h"
-#include "stm32f1xx_hal.h"
-#include "FreeRTOS.h"
-#include "stm32f1xx_hal.h"
-#include "cmsis_os.h"
-#include "unit.h"
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-enum Orientation {
-	ORIENTATION_LEFT_HAND = 0, ORIENTATION_RIGHT_HAND = 1, ORIENTATION_FLAT = 3
-};
-#define PID_TIM_HZ (8)
-#if defined(MODEL_TS100) + defined(MODEL_TS80) > 1
-#error "Multiple models defined!"
-#elif defined(MODEL_TS100) + defined(MODEL_TS80) == 0
-#error "No model defined!"
-#endif
-
-#ifdef MODEL_TS100
-
-#define KEY_B_Pin GPIO_PIN_6
-#define KEY_B_GPIO_Port GPIOA
-#define TMP36_INPUT_Pin GPIO_PIN_7
-#define TMP36_INPUT_GPIO_Port GPIOA
-#define TMP36_ADC1_CHANNEL ADC_CHANNEL_7
-#define TIP_TEMP_Pin GPIO_PIN_0
-#define TIP_TEMP_GPIO_Port GPIOB
-#define TIP_TEMP_ADC1_CHANNEL ADC_CHANNEL_8
-#define TIP_TEMP_ADC2_CHANNEL ADC_CHANNEL_8
-#define VIN_Pin GPIO_PIN_1
-#define VIN_GPIO_Port GPIOB
-#define VIN_ADC1_CHANNEL ADC_CHANNEL_9
-#define VIN_ADC2_CHANNEL ADC_CHANNEL_9
-#define OLED_RESET_Pin GPIO_PIN_8
-#define OLED_RESET_GPIO_Port GPIOA
-#define KEY_A_Pin GPIO_PIN_9
-#define KEY_A_GPIO_Port GPIOA
-#define INT_Orientation_Pin GPIO_PIN_3
-#define INT_Orientation_GPIO_Port GPIOB
-#define PWM_Out_Pin GPIO_PIN_4
-#define PWM_Out_GPIO_Port GPIOB
-#define PWM_Out_CHANNEL TIM_CHANNEL_1
-#define PWM_Out_CCR
-#define INT_Movement_Pin GPIO_PIN_5
-#define INT_Movement_GPIO_Port GPIOB
-#define SCL_Pin GPIO_PIN_6
-#define SCL_GPIO_Port GPIOB
-#define SDA_Pin GPIO_PIN_7
-#define SDA_GPIO_Port GPIOB
-
-#else
-// TS80 pin map
-#define KEY_B_Pin GPIO_PIN_0
-#define KEY_B_GPIO_Port GPIOB
-#define TMP36_INPUT_Pin GPIO_PIN_4
-#define TMP36_INPUT_GPIO_Port GPIOA
-#define TMP36_ADC1_CHANNEL ADC_CHANNEL_4
-#define TIP_TEMP_Pin GPIO_PIN_3
-#define TIP_TEMP_GPIO_Port GPIOA
-#define TIP_TEMP_ADC1_CHANNEL ADC_CHANNEL_3
-#define TIP_TEMP_ADC2_CHANNEL ADC_CHANNEL_3
-
-#define VIN_Pin GPIO_PIN_2
-#define VIN_GPIO_Port GPIOA
-#define VIN_ADC1_CHANNEL ADC_CHANNEL_2
-#define VIN_ADC2_CHANNEL ADC_CHANNEL_2
-#define OLED_RESET_Pin GPIO_PIN_15
-#define OLED_RESET_GPIO_Port GPIOA
-#define KEY_A_Pin GPIO_PIN_1
-#define KEY_A_GPIO_Port GPIOB
-#define INT_Orientation_Pin GPIO_PIN_4
-#define INT_Orientation_GPIO_Port GPIOB
-#define PWM_Out_Pin GPIO_PIN_6
-#define PWM_Out_GPIO_Port GPIOA
-#define PWM_Out_CHANNEL TIM_CHANNEL_1
-#define INT_Movement_Pin GPIO_PIN_5
-#define INT_Movement_GPIO_Port GPIOB
-#define SCL_Pin GPIO_PIN_6
-#define SCL_GPIO_Port GPIOB
-#define SDA_Pin GPIO_PIN_7
-#define SDA_GPIO_Port GPIOB
-
-#endif
-
-/*
- * Keep in a uint8_t range for the ID's
- */
-#ifdef MODEL_TS100
-enum TipType {
-	TS_B2 = 0,
-	TS_D24 = 1,
-	TS_BC2 = 2,
-	TS_C1 = 3,
-	Tip_MiniWare = 4,
-	HAKKO_BC2 = 4,
-	Tip_Hakko = 5,
-	Tip_Custom = 5,
-};
-#endif
-#ifdef MODEL_TS80
-enum TipType {
-	TS_B02 = 0, TS_D25 = 1, Tip_MiniWare = 2, Tip_Custom = 3,
-};
-#endif
-extern uint16_t tipGainCalValue ;
-
-uint16_t lookupTipDefaultCalValue(enum TipType tipID);
- uint16_t getHandleTemperature();
-uint16_t getTipRawTemp(uint8_t refresh);
-uint16_t getInputVoltageX10(uint16_t divisor,uint8_t sample);
-
-void setTipPWM(uint8_t pulse);
-uint16_t ctoTipMeasurement(uint16_t temp);
-uint16_t tipMeasurementToC(uint16_t raw);
-uint16_t ftoTipMeasurement(uint16_t temp);
-#ifdef ENABLED_FAHRENHEIT_SUPPORT
-uint16_t tipMeasurementToF(uint16_t raw);
-#endif
-void seekQC(int16_t Vx10, uint16_t divisor);
-void setCalibrationOffset(int16_t offSet);
-void setTipType(enum TipType tipType, uint8_t manualCalGain);
-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
-// If input is over 12V returns -1
-// If the input is [5-12] Will return the value.
-void vApplicationGetIdleTaskMemory(StaticTask_t **ppxIdleTaskTCBBuffer,
-		StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize) ;
-void vApplicationIdleHook(void);
-#ifdef __cplusplus
-}
-#endif
-
-#endif /* HARDWARE_H_ */
+/*
+ * Pins.h
+ *
+ *  Created on: 29 May 2020
+ *      Author: Ralim
+ */
+
+#ifndef BSP_MINIWARE_PINS_H_
+#define BSP_MINIWARE_PINS_H_
+
+#if defined(MODEL_TS100) + defined(MODEL_TS80) > 1
+#error "Multiple models defined!"
+#elif defined(MODEL_TS100) + defined(MODEL_TS80) == 0
+#error "No model defined!"
+#endif
+
+#ifdef MODEL_TS100
+
+#define KEY_B_Pin GPIO_PIN_6
+#define KEY_B_GPIO_Port GPIOA
+#define TMP36_INPUT_Pin GPIO_PIN_7
+#define TMP36_INPUT_GPIO_Port GPIOA
+#define TMP36_ADC1_CHANNEL ADC_CHANNEL_7
+#define TIP_TEMP_Pin GPIO_PIN_0
+#define TIP_TEMP_GPIO_Port GPIOB
+#define TIP_TEMP_ADC1_CHANNEL ADC_CHANNEL_8
+#define TIP_TEMP_ADC2_CHANNEL ADC_CHANNEL_8
+#define VIN_Pin GPIO_PIN_1
+#define VIN_GPIO_Port GPIOB
+#define VIN_ADC1_CHANNEL ADC_CHANNEL_9
+#define VIN_ADC2_CHANNEL ADC_CHANNEL_9
+#define OLED_RESET_Pin GPIO_PIN_8
+#define OLED_RESET_GPIO_Port GPIOA
+#define KEY_A_Pin GPIO_PIN_9
+#define KEY_A_GPIO_Port GPIOA
+#define INT_Orientation_Pin GPIO_PIN_3
+#define INT_Orientation_GPIO_Port GPIOB
+#define PWM_Out_Pin GPIO_PIN_4
+#define PWM_Out_GPIO_Port GPIOB
+#define PWM_Out_CHANNEL TIM_CHANNEL_1
+#define PWM_Out_CCR
+#define INT_Movement_Pin GPIO_PIN_5
+#define INT_Movement_GPIO_Port GPIOB
+#define SCL_Pin GPIO_PIN_6
+#define SCL_GPIO_Port GPIOB
+#define SDA_Pin GPIO_PIN_7
+#define SDA_GPIO_Port GPIOB
+
+#else
+// TS80 pin map
+#define KEY_B_Pin GPIO_PIN_0
+#define KEY_B_GPIO_Port GPIOB
+#define TMP36_INPUT_Pin GPIO_PIN_4
+#define TMP36_INPUT_GPIO_Port GPIOA
+#define TMP36_ADC1_CHANNEL ADC_CHANNEL_4
+#define TIP_TEMP_Pin GPIO_PIN_3
+#define TIP_TEMP_GPIO_Port GPIOA
+#define TIP_TEMP_ADC1_CHANNEL ADC_CHANNEL_3
+#define TIP_TEMP_ADC2_CHANNEL ADC_CHANNEL_3
+
+#define VIN_Pin GPIO_PIN_2
+#define VIN_GPIO_Port GPIOA
+#define VIN_ADC1_CHANNEL ADC_CHANNEL_2
+#define VIN_ADC2_CHANNEL ADC_CHANNEL_2
+#define OLED_RESET_Pin GPIO_PIN_15
+#define OLED_RESET_GPIO_Port GPIOA
+#define KEY_A_Pin GPIO_PIN_1
+#define KEY_A_GPIO_Port GPIOB
+#define INT_Orientation_Pin GPIO_PIN_4
+#define INT_Orientation_GPIO_Port GPIOB
+#define PWM_Out_Pin GPIO_PIN_6
+#define PWM_Out_GPIO_Port GPIOA
+#define PWM_Out_CHANNEL TIM_CHANNEL_1
+#define INT_Movement_Pin GPIO_PIN_5
+#define INT_Movement_GPIO_Port GPIOB
+#define SCL_Pin GPIO_PIN_6
+#define SCL_GPIO_Port GPIOB
+#define SDA_Pin GPIO_PIN_7
+#define SDA_GPIO_Port GPIOB
+
+#endif
+
+#endif /* BSP_MINIWARE_PINS_H_ */
diff --git a/workspace/TS100/Core/BSP/Miniware/UnitSettings.h b/workspace/TS100/Core/BSP/Miniware/UnitSettings.h
new file mode 100644
index 00000000..f3588b8b
--- /dev/null
+++ b/workspace/TS100/Core/BSP/Miniware/UnitSettings.h
@@ -0,0 +1,18 @@
+/*
+ * UnitSettings.h
+ *
+ *  Created on: 29 May 2020
+ *      Author: Ralim
+ */
+
+#ifndef BSP_MINIWARE_UNITSETTINGS_H_
+#define BSP_MINIWARE_UNITSETTINGS_H_
+//On the TS80, the LIS accel is mounted backwards
+#ifdef MODEL_TS80
+#define LIS_ORI_FLIP
+#endif
+
+
+
+
+#endif /* BSP_MINIWARE_UNITSETTINGS_H_ */
diff --git a/workspace/TS100/Core/Inc/Buttons.hpp b/workspace/TS100/Core/Inc/Buttons.hpp
new file mode 100644
index 00000000..fb4b1991
--- /dev/null
+++ b/workspace/TS100/Core/Inc/Buttons.hpp
@@ -0,0 +1,35 @@
+/*
+ * Buttons.h
+ *
+ *  Created on: 29 May 2020
+ *      Author: Ralim
+ */
+#include "BSP.h"
+#ifndef INC_BUTTONS_H_
+#define INC_BUTTONS_H_
+
+extern uint32_t lastButtonTime;
+
+enum ButtonState {
+	BUTTON_NONE = 0, /* No buttons pressed / < filter time*/
+	BUTTON_F_SHORT = 1, /* User has pressed the front button*/
+	BUTTON_B_SHORT = 2, /* User has pressed the back  button*/
+	BUTTON_F_LONG = 4, /* User is  holding the front button*/
+	BUTTON_B_LONG = 8, /* User is  holding the back button*/
+	BUTTON_BOTH = 16, /* User has pressed both buttons*/
+
+/*
+ * Note:
+ * Pressed means press + release, we trigger on a full \__/ pulse
+ * holding means it has gone low, and been low for longer than filter time
+ */
+};
+
+//Returns what buttons are pressed (if any)
+ButtonState getButtonState();
+//Helpers
+void waitForButtonPressOrTimeout(uint32_t timeout);
+void waitForButtonPress();
+
+
+#endif /* INC_BUTTONS_H_ */
diff --git a/workspace/TS100/Core/Inc/FRToSI2C.hpp b/workspace/TS100/Core/Inc/FRToSI2C.hpp
index f1515929..efc11d74 100644
--- a/workspace/TS100/Core/Inc/FRToSI2C.hpp
+++ b/workspace/TS100/Core/Inc/FRToSI2C.hpp
@@ -1,44 +1,46 @@
-/*
- * FRToSI2C.hpp
- *
- *  Created on: 14Apr.,2018
- *      Author: Ralim
- */
-
-#ifndef FRTOSI2C_HPP_
-#define FRTOSI2C_HPP_
-#include "stm32f1xx_hal.h"
-#include "cmsis_os.h"
-
-class FRToSI2C {
-public:
-
-	static void init(I2C_HandleTypeDef *i2chandle) {
-		i2c = i2chandle;
-		I2CSemaphore = nullptr;
-	}
-
-	static void FRToSInit() {
-		I2CSemaphore = xSemaphoreCreateBinaryStatic(&xSemaphoreBuffer);
-		xSemaphoreGive(I2CSemaphore);
-	}
-
-	static void CpltCallback(); //Normal Tx Callback
-
-	static void Mem_Read(uint16_t DevAddress, uint16_t MemAddress,
-			uint16_t MemAddSize, uint8_t *pData, uint16_t Size);
-	static void Mem_Write(uint16_t DevAddress, uint16_t MemAddress,
-			uint16_t MemAddSize, uint8_t *pData, uint16_t Size);
-
-	static void Transmit(uint16_t DevAddress, uint8_t *pData, uint16_t Size);
-	static void I2C_RegisterWrite(uint8_t address, uint8_t reg, uint8_t data);
-	static uint8_t I2C_RegisterRead(uint8_t address, uint8_t reg);
-
-private:
-	static I2C_HandleTypeDef *i2c;
-	static void I2C1_ClearBusyFlagErratum();
-	static SemaphoreHandle_t I2CSemaphore;
-	static StaticSemaphore_t xSemaphoreBuffer;
-};
-
-#endif /* FRTOSI2C_HPP_ */
+/*
+ * FRToSI2C.hpp
+ *
+ *  Created on: 14Apr.,2018
+ *      Author: Ralim
+ */
+
+#ifndef FRTOSI2C_HPP_
+#define FRTOSI2C_HPP_
+#include "stm32f1xx_hal.h"
+#include "cmsis_os.h"
+
+class FRToSI2C {
+public:
+
+	static void init(I2C_HandleTypeDef *i2chandle) {
+		i2c = i2chandle;
+		I2CSemaphore = nullptr;
+	}
+
+	static void FRToSInit() {
+		I2CSemaphore = xSemaphoreCreateBinaryStatic(&xSemaphoreBuffer);
+		xSemaphoreGive(I2CSemaphore);
+	}
+
+	static void CpltCallback(); //Normal Tx Callback
+
+	static bool Mem_Read(uint16_t DevAddress, uint16_t MemAddress,
+			uint16_t MemAddSize, uint8_t *pData, uint16_t Size);
+	static void Mem_Write(uint16_t DevAddress, uint16_t MemAddress,
+			uint16_t MemAddSize, uint8_t *pData, uint16_t Size);
+	//Returns true if device ACK's being addressed
+	static bool probe(uint16_t DevAddress);
+
+	static void Transmit(uint16_t DevAddress, uint8_t *pData, uint16_t Size);
+	static void I2C_RegisterWrite(uint8_t address, uint8_t reg, uint8_t data);
+	static uint8_t I2C_RegisterRead(uint8_t address, uint8_t reg);
+
+private:
+	static I2C_HandleTypeDef *i2c;
+	static void I2C1_ClearBusyFlagErratum();
+	static SemaphoreHandle_t I2CSemaphore;
+	static StaticSemaphore_t xSemaphoreBuffer;
+};
+
+#endif /* FRTOSI2C_HPP_ */
diff --git a/workspace/TS100/Core/Inc/FreeRTOSHooks.h b/workspace/TS100/Core/Inc/FreeRTOSHooks.h
new file mode 100644
index 00000000..8dca47e6
--- /dev/null
+++ b/workspace/TS100/Core/Inc/FreeRTOSHooks.h
@@ -0,0 +1,28 @@
+/*
+ * FreeRTOSHooks.h
+ *
+ *  Created on: 29 May 2020
+ *      Author: Ralim
+ */
+
+#ifndef INC_FREERTOSHOOKS_H_
+#define INC_FREERTOSHOOKS_H_
+
+#include "FreeRTOS.h"
+#include "cmsis_os.h"
+#include "unit.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// RToS
+void vApplicationGetIdleTaskMemory(StaticTask_t **ppxIdleTaskTCBBuffer,
+		StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize);
+void vApplicationIdleHook(void);
+#ifdef __cplusplus
+}
+#endif
+
+
+#endif /* INC_FREERTOSHOOKS_H_ */
diff --git a/workspace/TS100/Core/Inc/LIS2DH12.hpp b/workspace/TS100/Core/Inc/LIS2DH12.hpp
index ed9d844a..3400130d 100644
--- a/workspace/TS100/Core/Inc/LIS2DH12.hpp
+++ b/workspace/TS100/Core/Inc/LIS2DH12.hpp
@@ -10,11 +10,8 @@
 #include "stm32f1xx_hal.h"
 #include "FRToSI2C.hpp"
 #include "LIS2DH12_defines.hpp"
-#include "hardware.h"
+#include "BSP.h"
 
-#ifdef MODEL_TS80
-#define LIS_ORI_FLIP
-#endif
 class LIS2DH12 {
 public:
 	static bool detect();
diff --git a/workspace/TS100/Core/Inc/MMA8652FC.hpp b/workspace/TS100/Core/Inc/MMA8652FC.hpp
index 7a3ec037..2654e903 100644
--- a/workspace/TS100/Core/Inc/MMA8652FC.hpp
+++ b/workspace/TS100/Core/Inc/MMA8652FC.hpp
@@ -10,7 +10,7 @@
 #include "stm32f1xx_hal.h"
 #include "MMA8652FC_defines.h"
 #include "FRToSI2C.hpp"
-#include "hardware.h"
+#include "BSP.h"
 
 class MMA8652FC {
 
diff --git a/workspace/TS100/Core/Inc/OLED.hpp b/workspace/TS100/Core/Inc/OLED.hpp
index 5994e9f6..efb5aee2 100644
--- a/workspace/TS100/Core/Inc/OLED.hpp
+++ b/workspace/TS100/Core/Inc/OLED.hpp
@@ -9,7 +9,7 @@
 
 #ifndef OLED_HPP_
 #define OLED_HPP_
-#include <hardware.h>
+#include <BSP.h>
 #include "stm32f1xx_hal.h"
 #include <stdbool.h>
 #include <string.h>
diff --git a/workspace/TS100/Core/Inc/QC3.h b/workspace/TS100/Core/Inc/QC3.h
new file mode 100644
index 00000000..98bfa0f5
--- /dev/null
+++ b/workspace/TS100/Core/Inc/QC3.h
@@ -0,0 +1,21 @@
+/*
+ * QC3.h
+ *
+ *  Created on: 29 May 2020
+ *      Author: Ralim
+ */
+
+#ifndef INC_QC3_H_
+#define INC_QC3_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+void seekQC(int16_t Vx10, uint16_t divisor);
+void startQC(uint16_t divisor); // Tries to negotiate QC for highest voltage, must be run after
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* INC_QC3_H_ */
diff --git a/workspace/TS100/Core/Src/TipThermoModel.h b/workspace/TS100/Core/Inc/TipThermoModel.h
similarity index 98%
rename from workspace/TS100/Core/Src/TipThermoModel.h
rename to workspace/TS100/Core/Inc/TipThermoModel.h
index 6c31f6bd..a4b0b368 100644
--- a/workspace/TS100/Core/Src/TipThermoModel.h
+++ b/workspace/TS100/Core/Inc/TipThermoModel.h
@@ -8,7 +8,7 @@
 #ifndef SRC_TIPTHERMOMODEL_H_
 #define SRC_TIPTHERMOMODEL_H_
 #include "stdint.h"
-#include "hardware.h"
+#include "BSP.h"
 #include "unit.h"
 class TipThermoModel {
 public:
diff --git a/workspace/TS100/Core/Inc/gui.hpp b/workspace/TS100/Core/Inc/gui.hpp
index d79b71dd..f05d59fb 100644
--- a/workspace/TS100/Core/Inc/gui.hpp
+++ b/workspace/TS100/Core/Inc/gui.hpp
@@ -1,38 +1,38 @@
-/*
- * gui.h
- *
- *  Created on: 3Sep.,2017
- *      Author: Ben V. Brown
- */
-
-#ifndef GUI_HPP_
-#define GUI_HPP_
-#include "Translation.h"
-#include "Settings.h"
-#include "hardware.h"
-
-#define PRESS_ACCEL_STEP			3
-#define PRESS_ACCEL_INTERVAL_MIN	10
-#define PRESS_ACCEL_INTERVAL_MAX	30
-
-
-//GUI holds the menu structure and all its methods for the menu itself
-
-//Declarations for all the methods for the settings menu (at end of this file)
-
-//Wrapper for holding a function pointer
-typedef struct state_func_t {
-	void (*func)(void);
-} state_func;
-
-//Struct for holding the function pointers and descriptions
-typedef struct {
-	const char *description;
-	const state_func incrementHandler;
-	const state_func draw;
-} menuitem;
-
-void enterSettingsMenu();
-extern const menuitem rootSettingsMenu[];
-
-#endif /* GUI_HPP_ */
+/*
+ * gui.h
+ *
+ *  Created on: 3Sep.,2017
+ *      Author: Ben V. Brown
+ */
+
+#ifndef GUI_HPP_
+#define GUI_HPP_
+#include "Translation.h"
+#include "Settings.h"
+#include "BSP.h"
+
+#define PRESS_ACCEL_STEP			3
+#define PRESS_ACCEL_INTERVAL_MIN	10
+#define PRESS_ACCEL_INTERVAL_MAX	30
+
+//GUI holds the menu structure and all its methods for the menu itself
+
+//Declarations for all the methods for the settings menu (at end of this file)
+
+//Wrapper for holding a function pointer
+typedef struct state_func_t {
+	void (*func)(void);
+} state_func;
+
+//Struct for holding the function pointers and descriptions
+typedef struct {
+	const char *description;
+	const state_func incrementHandler;
+	const state_func draw;
+} menuitem;
+
+void enterSettingsMenu();
+void GUIDelay();
+extern const menuitem rootSettingsMenu[];
+
+#endif /* GUI_HPP_ */
diff --git a/workspace/TS100/Core/Inc/main.hpp b/workspace/TS100/Core/Inc/main.hpp
index fb1809c7..f2f2e808 100644
--- a/workspace/TS100/Core/Inc/main.hpp
+++ b/workspace/TS100/Core/Inc/main.hpp
@@ -7,25 +7,7 @@
 extern uint8_t PCBVersion;
 extern uint32_t currentTempTargetDegC;
 extern bool settingsWereReset;
-enum ButtonState {
-	BUTTON_NONE = 0, /* No buttons pressed / < filter time*/
-	BUTTON_F_SHORT = 1, /* User has pressed the front button*/
-	BUTTON_B_SHORT = 2, /* User has pressed the back  button*/
-	BUTTON_F_LONG = 4, /* User is  holding the front button*/
-	BUTTON_B_LONG = 8, /* User is  holding the back button*/
-	BUTTON_BOTH = 16, /* User has pressed both buttons*/
 
-/*
- * Note:
- * Pressed means press + release, we trigger on a full \__/ pulse
- * holding means it has gone low, and been low for longer than filter time
- */
-};
-
-ButtonState getButtonState();
-void waitForButtonPressOrTimeout(uint32_t timeout);
-void waitForButtonPress();
-void GUIDelay();
 #ifdef __cplusplus
 extern "C" {
 #endif
diff --git a/workspace/TS100/Core/Inc/power.hpp b/workspace/TS100/Core/Inc/power.hpp
index 9e86752b..c882a99a 100644
--- a/workspace/TS100/Core/Inc/power.hpp
+++ b/workspace/TS100/Core/Inc/power.hpp
@@ -7,7 +7,7 @@
 
 #include "stdint.h"
 #include <history.hpp>
-#include "hardware.h"
+#include "BSP.h"
 #include "expMovingAverage.h"
 #ifndef POWER_HPP_
 #define POWER_HPP_
diff --git a/workspace/TS100/Core/Src/Buttons.cpp b/workspace/TS100/Core/Src/Buttons.cpp
new file mode 100644
index 00000000..beca08b1
--- /dev/null
+++ b/workspace/TS100/Core/Src/Buttons.cpp
@@ -0,0 +1,115 @@
+/*
+ * Buttons.c
+ *
+ *  Created on: 29 May 2020
+ *      Author: Ralim
+ */
+#include <Buttons.hpp>
+#include "FreeRTOS.h"
+#include "task.h"
+#include "gui.hpp"
+uint32_t lastButtonTime = 0;
+
+ButtonState getButtonState() {
+	/*
+	 * Read in the buttons and then determine if a state change needs to occur
+	 */
+
+	/*
+	 * If the previous state was  00 Then we want to latch the new state if
+	 * different & update time
+	 * If the previous state was !00 Then we want to search if we trigger long
+	 * press (buttons still down), or if release we trigger press
+	 * (downtime>filter)
+	 */
+	static uint8_t previousState = 0;
+	static uint32_t previousStateChange = 0;
+	const uint16_t timeout = 40;
+	uint8_t currentState;
+	currentState = (getButtonA()) << 0;
+	currentState |= (getButtonB()) << 1;
+
+	if (currentState)
+		lastButtonTime = xTaskGetTickCount();
+	if (currentState == previousState) {
+		if (currentState == 0)
+			return BUTTON_NONE;
+		if ((xTaskGetTickCount() - previousStateChange) > timeout) {
+			// User has been holding the button down
+			// We want to send a button is held message
+			if (currentState == 0x01)
+				return BUTTON_F_LONG;
+			else if (currentState == 0x02)
+				return BUTTON_B_LONG;
+			else
+				return BUTTON_NONE; // Both being held case, we dont long hold this
+		} else
+			return BUTTON_NONE;
+	} else {
+		// A change in button state has occurred
+		ButtonState retVal = BUTTON_NONE;
+		if (currentState) {
+			// User has pressed a button down (nothing done on down)
+			if (currentState != previousState) {
+				// There has been a change in the button states
+				// If there is a rising edge on one of the buttons from double press we
+				// want to mask that out As users are having issues with not release
+				// both at once
+				if (previousState == 0x03)
+					currentState = 0x03;
+			}
+		} else {
+			// User has released buttons
+			// If they previously had the buttons down we want to check if they were <
+			// long hold and trigger a press
+			if ((xTaskGetTickCount() - previousStateChange) < timeout) {
+				// The user didn't hold the button for long
+				// So we send button press
+
+				if (previousState == 0x01)
+					retVal = BUTTON_F_SHORT;
+				else if (previousState == 0x02)
+					retVal = BUTTON_B_SHORT;
+				else
+					retVal = BUTTON_BOTH;  // Both being held case
+			}
+		}
+		previousState = currentState;
+		previousStateChange = xTaskGetTickCount();
+		return retVal;
+	}
+	return BUTTON_NONE;
+}
+
+void waitForButtonPress() {
+	// we are just lazy and sleep until user confirms button press
+	// This also eats the button press event!
+	ButtonState buttons = getButtonState();
+	while (buttons) {
+		buttons = getButtonState();
+		GUIDelay();
+	}
+	while (!buttons) {
+		buttons = getButtonState();
+		GUIDelay();
+	}
+}
+
+void waitForButtonPressOrTimeout(uint32_t timeout) {
+	timeout += xTaskGetTickCount();
+	// calculate the exit point
+
+	ButtonState buttons = getButtonState();
+	while (buttons) {
+		buttons = getButtonState();
+		GUIDelay();
+		if (xTaskGetTickCount() > timeout)
+			return;
+	}
+	while (!buttons) {
+		buttons = getButtonState();
+		GUIDelay();
+		if (xTaskGetTickCount() > timeout)
+			return;
+	}
+}
diff --git a/workspace/TS100/Core/Src/FRToSI2C.cpp b/workspace/TS100/Core/Src/FRToSI2C.cpp
index aabd6906..a562e6fa 100644
--- a/workspace/TS100/Core/Src/FRToSI2C.cpp
+++ b/workspace/TS100/Core/Src/FRToSI2C.cpp
@@ -1,189 +1,142 @@
-/*
- * FRToSI2C.cpp
- *
- *  Created on: 14Apr.,2018
- *      Author: Ralim
- */
-#include "hardware.h"
-#include "FRToSI2C.hpp"
-#define I2CUSESDMA
-I2C_HandleTypeDef* FRToSI2C::i2c;
-SemaphoreHandle_t FRToSI2C::I2CSemaphore;
-StaticSemaphore_t FRToSI2C::xSemaphoreBuffer;
-
-void FRToSI2C::CpltCallback() {
-	i2c->State = HAL_I2C_STATE_READY;  // Force state reset (even if tx error)
-	if (I2CSemaphore) {
-		xSemaphoreGiveFromISR(I2CSemaphore, NULL);
-	}
-}
-
-void FRToSI2C::Mem_Read(uint16_t DevAddress, uint16_t MemAddress,
-		uint16_t MemAddSize, uint8_t* pData, uint16_t Size) {
-
-	if (I2CSemaphore == NULL) {
-		// no RToS, run blocking code
-		HAL_I2C_Mem_Read(i2c, DevAddress, MemAddress, MemAddSize, pData, Size,
-				5000);
-	} else {
-		// RToS is active, run threading
-		// Get the mutex so we can use the I2C port
-		// Wait up to 1 second for the mutex
-		if (xSemaphoreTake(I2CSemaphore, (TickType_t)50) == pdTRUE) {
-#ifdef I2CUSESDMA
-			if (HAL_I2C_Mem_Read(i2c, DevAddress, MemAddress, MemAddSize, pData,
-					Size, 500) != HAL_OK) {
-
-				I2C1_ClearBusyFlagErratum();
-				xSemaphoreGive(I2CSemaphore);
-			}
-			xSemaphoreGive(I2CSemaphore);
-#else
-
-			HAL_I2C_Mem_Read(i2c, DevAddress, MemAddress, MemAddSize, pData, Size,
-					5000);
-			xSemaphoreGive(I2CSemaphore);
-#endif
-		} else {
-		}
-	}
-
-}
-void FRToSI2C::I2C_RegisterWrite(uint8_t address, uint8_t reg, uint8_t data) {
-	Mem_Write(address, reg, I2C_MEMADD_SIZE_8BIT, &data, 1);
-}
-
-uint8_t FRToSI2C::I2C_RegisterRead(uint8_t add, uint8_t reg) {
-	uint8_t tx_data[1];
-	Mem_Read(add, reg, I2C_MEMADD_SIZE_8BIT, tx_data, 1);
-	return tx_data[0];
-}
-void FRToSI2C::Mem_Write(uint16_t DevAddress, uint16_t MemAddress,
-		uint16_t MemAddSize, uint8_t* pData, uint16_t Size) {
-
-	if (I2CSemaphore == NULL) {
-		// no RToS, run blocking code
-		HAL_I2C_Mem_Write(i2c, DevAddress, MemAddress, MemAddSize, pData, Size,
-				5000);
-	} else {
-		// RToS is active, run threading
-		// Get the mutex so we can use the I2C port
-		// Wait up to 1 second for the mutex
-		if (xSemaphoreTake(I2CSemaphore, (TickType_t)50) == pdTRUE) {
-#ifdef I2CUSESDMA
-			if (HAL_I2C_Mem_Write(i2c, DevAddress, MemAddress, MemAddSize,
-					pData, Size, 500) != HAL_OK) {
-
-				I2C1_ClearBusyFlagErratum();
-				xSemaphoreGive(I2CSemaphore);
-			}
-			xSemaphoreGive(I2CSemaphore);
-#else
-			if (HAL_I2C_Mem_Write(i2c, DevAddress, MemAddress, MemAddSize, pData,
-							Size, 5000) != HAL_OK) {
-			}
-			xSemaphoreGive(I2CSemaphore);
-#endif
-		} else {
-		}
-	}
-
-}
-
-void FRToSI2C::Transmit(uint16_t DevAddress, uint8_t* pData, uint16_t Size) {
-	if (I2CSemaphore == NULL) {
-		// no RToS, run blocking code
-		HAL_I2C_Master_Transmit(i2c, DevAddress, pData, Size, 5000);
-	} else {
-		// RToS is active, run threading
-		// Get the mutex so we can use the I2C port
-		// Wait up to 1 second for the mutex
-		if (xSemaphoreTake(I2CSemaphore, (TickType_t)50) == pdTRUE) {
-#ifdef I2CUSESDMA
-
-			if (HAL_I2C_Master_Transmit_DMA(i2c, DevAddress, pData, Size)
-					!= HAL_OK) {
-
-				I2C1_ClearBusyFlagErratum();
-				xSemaphoreGive(I2CSemaphore);
-
-			}
-#else
-			HAL_I2C_Master_Transmit(i2c, DevAddress, pData, Size, 5000);
-			xSemaphoreGive(I2CSemaphore);
-#endif
-
-		} else {
-		}
-	}
-
-}
-
-void FRToSI2C::I2C1_ClearBusyFlagErratum() {
-	GPIO_InitTypeDef GPIO_InitStruct;
-	int timeout = 100;
-	int timeout_cnt = 0;
-
-	// 1. Clear PE bit.
-	i2c->Instance->CR1 &= ~(0x0001);
-	/**I2C1 GPIO Configuration
-	 PB6     ------> I2C1_SCL
-	 PB7     ------> I2C1_SDA
-	 */
-	//  2. Configure the SCL and SDA I/Os as General Purpose Output Open-Drain, High level (Write 1 to GPIOx_ODR).
-	GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
-	GPIO_InitStruct.Pull = GPIO_PULLUP;
-	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-
-	GPIO_InitStruct.Pin = SCL_Pin;
-	HAL_GPIO_Init(SCL_GPIO_Port, &GPIO_InitStruct);
-	HAL_GPIO_WritePin(SCL_GPIO_Port, SCL_Pin, GPIO_PIN_SET);
-
-	GPIO_InitStruct.Pin = SDA_Pin;
-	HAL_GPIO_Init(SDA_GPIO_Port, &GPIO_InitStruct);
-	HAL_GPIO_WritePin(SDA_GPIO_Port, SDA_Pin, GPIO_PIN_SET);
-
-	while (GPIO_PIN_SET != HAL_GPIO_ReadPin(SDA_GPIO_Port, SDA_Pin)) {
-		//Move clock to release I2C
-		HAL_GPIO_WritePin(SCL_GPIO_Port, SCL_Pin, GPIO_PIN_RESET);
-		asm("nop");
-		asm("nop");
-		asm("nop");
-		asm("nop");
-		HAL_GPIO_WritePin(SCL_GPIO_Port, SCL_Pin, GPIO_PIN_SET);
-
-		timeout_cnt++;
-		if (timeout_cnt > timeout)
-			return;
-	}
-
-	// 12. Configure the SCL and SDA I/Os as Alternate function Open-Drain.
-	GPIO_InitStruct.Mode = GPIO_MODE_AF_OD;
-	GPIO_InitStruct.Pull = GPIO_PULLUP;
-	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
-
-	GPIO_InitStruct.Pin = SCL_Pin;
-	HAL_GPIO_Init(SCL_GPIO_Port, &GPIO_InitStruct);
-
-	GPIO_InitStruct.Pin = SDA_Pin;
-	HAL_GPIO_Init(SDA_GPIO_Port, &GPIO_InitStruct);
-
-	HAL_GPIO_WritePin(SCL_GPIO_Port, SCL_Pin, GPIO_PIN_SET);
-	HAL_GPIO_WritePin(SDA_GPIO_Port, SDA_Pin, GPIO_PIN_SET);
-
-	// 13. Set SWRST bit in I2Cx_CR1 register.
-	i2c->Instance->CR1 |= 0x8000;
-
-	asm("nop");
-
-	// 14. Clear SWRST bit in I2Cx_CR1 register.
-	i2c->Instance->CR1 &= ~0x8000;
-
-	asm("nop");
-
-	// 15. Enable the I2C peripheral by setting the PE bit in I2Cx_CR1 register
-	i2c->Instance->CR1 |= 0x0001;
-
-	// Call initialization function.
-	HAL_I2C_Init(i2c);
-}
+/*
+ * FRToSI2C.cpp
+ *
+ *  Created on: 14Apr.,2018
+ *      Author: Ralim
+ */
+#include "BSP.h"
+#include "FRToSI2C.hpp"
+#define I2CUSESDMA
+I2C_HandleTypeDef *FRToSI2C::i2c;
+SemaphoreHandle_t FRToSI2C::I2CSemaphore;
+StaticSemaphore_t FRToSI2C::xSemaphoreBuffer;
+
+void FRToSI2C::CpltCallback() {
+	i2c->State = HAL_I2C_STATE_READY;  // Force state reset (even if tx error)
+	if (I2CSemaphore) {
+		xSemaphoreGiveFromISR(I2CSemaphore, NULL);
+	}
+}
+
+bool FRToSI2C::Mem_Read(uint16_t DevAddress, uint16_t MemAddress,
+		uint16_t MemAddSize, uint8_t *pData, uint16_t Size) {
+
+	if (I2CSemaphore == NULL) {
+		// no RToS, run blocking code
+		HAL_I2C_Mem_Read(i2c, DevAddress, MemAddress, MemAddSize, pData, Size,
+				5000);
+		return true;
+	} else {
+		// RToS is active, run threading
+		// Get the mutex so we can use the I2C port
+		// Wait up to 1 second for the mutex
+		if (xSemaphoreTake(I2CSemaphore, (TickType_t)50) == pdTRUE) {
+#ifdef I2CUSESDMA
+			if (HAL_I2C_Mem_Read(i2c, DevAddress, MemAddress, MemAddSize, pData,
+					Size, 500) != HAL_OK) {
+
+				I2C1_ClearBusyFlagErratum();
+				xSemaphoreGive(I2CSemaphore);
+				return false;
+			} else {
+				xSemaphoreGive(I2CSemaphore);
+				return true;
+			}
+#else
+
+			if (HAL_I2C_Mem_Read(i2c, DevAddress, MemAddress, MemAddSize, pData, Size,
+					5000)==HAL_OK){
+				xSemaphoreGive(I2CSemaphore);
+				return true;
+			}
+			xSemaphoreGive(I2CSemaphore);
+			return false;
+#endif
+		} else {
+			return false;
+		}
+	}
+
+}
+void FRToSI2C::I2C_RegisterWrite(uint8_t address, uint8_t reg, uint8_t data) {
+	Mem_Write(address, reg, I2C_MEMADD_SIZE_8BIT, &data, 1);
+}
+
+uint8_t FRToSI2C::I2C_RegisterRead(uint8_t add, uint8_t reg) {
+	uint8_t tx_data[1];
+	Mem_Read(add, reg, I2C_MEMADD_SIZE_8BIT, tx_data, 1);
+	return tx_data[0];
+}
+void FRToSI2C::Mem_Write(uint16_t DevAddress, uint16_t MemAddress,
+		uint16_t MemAddSize, uint8_t *pData, uint16_t Size) {
+
+	if (I2CSemaphore == NULL) {
+		// no RToS, run blocking code
+		HAL_I2C_Mem_Write(i2c, DevAddress, MemAddress, MemAddSize, pData, Size,
+				5000);
+	} else {
+		// RToS is active, run threading
+		// Get the mutex so we can use the I2C port
+		// Wait up to 1 second for the mutex
+		if (xSemaphoreTake(I2CSemaphore, (TickType_t)50) == pdTRUE) {
+#ifdef I2CUSESDMA
+			if (HAL_I2C_Mem_Write(i2c, DevAddress, MemAddress, MemAddSize,
+					pData, Size, 500) != HAL_OK) {
+
+				I2C1_ClearBusyFlagErratum();
+				xSemaphoreGive(I2CSemaphore);
+			}
+			xSemaphoreGive(I2CSemaphore);
+#else
+			if (HAL_I2C_Mem_Write(i2c, DevAddress, MemAddress, MemAddSize, pData,
+							Size, 5000) != HAL_OK) {
+			}
+			xSemaphoreGive(I2CSemaphore);
+#endif
+		} else {
+		}
+	}
+
+}
+
+void FRToSI2C::Transmit(uint16_t DevAddress, uint8_t *pData, uint16_t Size) {
+	if (I2CSemaphore == NULL) {
+		// no RToS, run blocking code
+		HAL_I2C_Master_Transmit(i2c, DevAddress, pData, Size, 5000);
+	} else {
+		// RToS is active, run threading
+		// Get the mutex so we can use the I2C port
+		// Wait up to 1 second for the mutex
+		if (xSemaphoreTake(I2CSemaphore, (TickType_t)50) == pdTRUE) {
+#ifdef I2CUSESDMA
+
+			if (HAL_I2C_Master_Transmit_DMA(i2c, DevAddress, pData, Size)
+					!= HAL_OK) {
+
+				I2C1_ClearBusyFlagErratum();
+				xSemaphoreGive(I2CSemaphore);
+
+			}
+#else
+			HAL_I2C_Master_Transmit(i2c, DevAddress, pData, Size, 5000);
+			xSemaphoreGive(I2CSemaphore);
+#endif
+
+		} else {
+		}
+	}
+
+}
+
+bool FRToSI2C::probe(uint16_t DevAddress) {
+	uint8_t buffer[1];
+	if (Mem_Read(DevAddress, 0, I2C_MEMADD_SIZE_8BIT, buffer, 1)) {
+		//ACK'd
+		return true;
+	}
+	return false;
+}
+
+void FRToSI2C::I2C1_ClearBusyFlagErratum() {
+	unstick_I2C();
+}
diff --git a/workspace/TS100/Core/Src/FreeRTOSHooks.c b/workspace/TS100/Core/Src/FreeRTOSHooks.c
new file mode 100644
index 00000000..d5a15dd7
--- /dev/null
+++ b/workspace/TS100/Core/Src/FreeRTOSHooks.c
@@ -0,0 +1,24 @@
+/*
+ * FreeRTOSHooks.c
+ *
+ *  Created on: 29 May 2020
+ *      Author: Ralim
+ */
+
+#include "FreeRTOSHooks.h"
+#include "BSP.h"
+void vApplicationIdleHook(void) {
+	resetWatchdog();
+}
+
+/* USER CODE BEGIN GET_IDLE_TASK_MEMORY */
+static StaticTask_t xIdleTaskTCBBuffer;
+static StackType_t xIdleStack[configMINIMAL_STACK_SIZE];
+
+void vApplicationGetIdleTaskMemory(StaticTask_t **ppxIdleTaskTCBBuffer,
+		StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize) {
+	*ppxIdleTaskTCBBuffer = &xIdleTaskTCBBuffer;
+	*ppxIdleTaskStackBuffer = &xIdleStack[0];
+	*pulIdleTaskStackSize = configMINIMAL_STACK_SIZE;
+	/* place for user code */
+}
diff --git a/workspace/TS100/Core/Src/GUIThread.cpp b/workspace/TS100/Core/Src/GUIThread.cpp
index 28e0f88b..19bf4650 100644
--- a/workspace/TS100/Core/Src/GUIThread.cpp
+++ b/workspace/TS100/Core/Src/GUIThread.cpp
@@ -19,14 +19,13 @@
 #include "TipThermoModel.h"
 #include "unit.h"
 #include "../../configuration.h"
-
+#include "Buttons.hpp"
 extern uint8_t PCBVersion;
 // File local variables
 extern uint32_t currentTempTargetDegC;
 extern uint8_t accelInit;
 extern uint32_t lastMovementTime;
 extern int16_t idealQCVoltage;
-uint32_t lastButtonTime = 0;
 extern osThreadId GUITaskHandle;
 extern osThreadId MOVTaskHandle;
 extern osThreadId PIDTaskHandle;
@@ -87,113 +86,8 @@ void gui_drawTipTemp(bool symbol) {
 		}
 	}
 }
-ButtonState getButtonState() {
-	/*
-	 * Read in the buttons and then determine if a state change needs to occur
-	 */
 
-	/*
-	 * If the previous state was  00 Then we want to latch the new state if
-	 * different & update time
-	 * If the previous state was !00 Then we want to search if we trigger long
-	 * press (buttons still down), or if release we trigger press
-	 * (downtime>filter)
-	 */
-	static uint8_t previousState = 0;
-	static uint32_t previousStateChange = 0;
-	const uint16_t timeout = 40;
-	uint8_t currentState;
-	currentState = (
-			HAL_GPIO_ReadPin(KEY_A_GPIO_Port, KEY_A_Pin) == GPIO_PIN_RESET ?
-					1 : 0) << 0;
-	currentState |= (
-			HAL_GPIO_ReadPin(KEY_B_GPIO_Port, KEY_B_Pin) == GPIO_PIN_RESET ?
-					1 : 0) << 1;
 
-	if (currentState)
-		lastButtonTime = xTaskGetTickCount();
-	if (currentState == previousState) {
-		if (currentState == 0)
-			return BUTTON_NONE;
-		if ((xTaskGetTickCount() - previousStateChange) > timeout) {
-			// User has been holding the button down
-			// We want to send a buttong is held message
-			if (currentState == 0x01)
-				return BUTTON_F_LONG;
-			else if (currentState == 0x02)
-				return BUTTON_B_LONG;
-			else
-				return BUTTON_NONE; // Both being held case, we dont long hold this
-		} else
-			return BUTTON_NONE;
-	} else {
-		// A change in button state has occurred
-		ButtonState retVal = BUTTON_NONE;
-		if (currentState) {
-			// User has pressed a button down (nothing done on down)
-			if (currentState != previousState) {
-				// There has been a change in the button states
-				// If there is a rising edge on one of the buttons from double press we
-				// want to mask that out As users are having issues with not release
-				// both at once
-				if (previousState == 0x03)
-					currentState = 0x03;
-			}
-		} else {
-			// User has released buttons
-			// If they previously had the buttons down we want to check if they were <
-			// long hold and trigger a press
-			if ((xTaskGetTickCount() - previousStateChange) < timeout) {
-				// The user didn't hold the button for long
-				// So we send button press
-
-				if (previousState == 0x01)
-					retVal = BUTTON_F_SHORT;
-				else if (previousState == 0x02)
-					retVal = BUTTON_B_SHORT;
-				else
-					retVal = BUTTON_BOTH;  // Both being held case
-			}
-		}
-		previousState = currentState;
-		previousStateChange = xTaskGetTickCount();
-		return retVal;
-	}
-	return BUTTON_NONE;
-}
-
-void waitForButtonPress() {
-	// we are just lazy and sleep until user confirms button press
-	// This also eats the button press event!
-	ButtonState buttons = getButtonState();
-	while (buttons) {
-		buttons = getButtonState();
-		GUIDelay();
-	}
-	while (!buttons) {
-		buttons = getButtonState();
-		GUIDelay();
-	}
-}
-
-void waitForButtonPressOrTimeout(uint32_t timeout) {
-	timeout += xTaskGetTickCount();
-	// calculate the exit point
-
-	ButtonState buttons = getButtonState();
-	while (buttons) {
-		buttons = getButtonState();
-		GUIDelay();
-		if (xTaskGetTickCount() > timeout)
-			return;
-	}
-	while (!buttons) {
-		buttons = getButtonState();
-		GUIDelay();
-		if (xTaskGetTickCount() > timeout)
-			return;
-	}
-}
 #ifdef MODEL_TS100
 // returns true if undervoltage has occured
 static bool checkVoltageForExit() {
diff --git a/workspace/TS100/Core/Src/LIS2DH12.cpp b/workspace/TS100/Core/Src/LIS2DH12.cpp
index f4fcbbfe..3557f0d9 100644
--- a/workspace/TS100/Core/Src/LIS2DH12.cpp
+++ b/workspace/TS100/Core/Src/LIS2DH12.cpp
@@ -15,17 +15,16 @@ typedef struct {
 	const uint8_t value;
 } LIS_REG;
 
-static const LIS_REG i2c_registers[] =
-		{ { LIS_CTRL_REG1, 0x17 }, // 25Hz
-				{ LIS_CTRL_REG2, 0b00001000 }, // Highpass filter off
-				{ LIS_CTRL_REG3, 0b01100000 }, // Setup interrupt pins
-				{ LIS_CTRL_REG4, 0b00001000 }, // Block update mode off, HR on
-				{ LIS_CTRL_REG5, 0b00000010 }, { LIS_CTRL_REG6, 0b01100010 },
-				//Basically setup the unit to run, and enable 4D orientation detection
-				{ LIS_INT2_CFG, 0b01111110 }, //setup for movement detection
-				{ LIS_INT2_THS, 0x28 }, { LIS_INT2_DURATION, 64 }, {
-						LIS_INT1_CFG, 0b01111110 }, { LIS_INT1_THS, 0x28 }, {
-						LIS_INT1_DURATION, 64 } };
+static const LIS_REG i2c_registers[] = { { LIS_CTRL_REG1, 0x17 }, // 25Hz
+		{ LIS_CTRL_REG2, 0b00001000 }, // Highpass filter off
+		{ LIS_CTRL_REG3, 0b01100000 }, // Setup interrupt pins
+		{ LIS_CTRL_REG4, 0b00001000 }, // Block update mode off, HR on
+		{ LIS_CTRL_REG5, 0b00000010 }, { LIS_CTRL_REG6, 0b01100010 },
+		//Basically setup the unit to run, and enable 4D orientation detection
+		{ LIS_INT2_CFG, 0b01111110 }, //setup for movement detection
+		{ LIS_INT2_THS, 0x28 }, { LIS_INT2_DURATION, 64 }, {
+		LIS_INT1_CFG, 0b01111110 }, { LIS_INT1_THS, 0x28 }, {
+		LIS_INT1_DURATION, 64 } };
 
 void LIS2DH12::initalize() {
 	for (size_t index = 0;
@@ -49,12 +48,5 @@ void LIS2DH12::getAxisReadings(int16_t &x, int16_t &y, int16_t &z) {
 }
 
 bool LIS2DH12::detect() {
-	uint8_t buffer[1];
-	if (HAL_I2C_Mem_Read(&hi2c1, 25 << 1, 0x0F, I2C_MEMADD_SIZE_8BIT, buffer, 1,
-			1000) == HAL_OK) {
-		//ACK'd
-		return true;
-	}
-	//NAK'd
-	return false;
+	return FRToSI2C::probe(LIS2DH_I2C_ADDRESS);
 }
diff --git a/workspace/TS100/Core/Src/MMA8652FC.cpp b/workspace/TS100/Core/Src/MMA8652FC.cpp
index bec2fcef..cfa4775e 100644
--- a/workspace/TS100/Core/Src/MMA8652FC.cpp
+++ b/workspace/TS100/Core/Src/MMA8652FC.cpp
@@ -84,12 +84,5 @@ void MMA8652FC::getAxisReadings(int16_t &x, int16_t &y, int16_t &z) {
 }
 
 bool MMA8652FC::detect() {
-	uint8_t buffer[1];
-	if (HAL_I2C_Mem_Read(&hi2c1, 29 << 1, 0x0F, I2C_MEMADD_SIZE_8BIT, buffer, 1,
-			1000) == HAL_OK) {
-		//The device ACK'd
-		return true;
-	}
-	//NAK'd
-	return false;
+	return FRToSI2C::probe(MMA8652FC_I2C_ADDRESS);
 }
diff --git a/workspace/TS100/Core/Src/OLED.cpp b/workspace/TS100/Core/Src/OLED.cpp
index 3949b17a..93390973 100644
--- a/workspace/TS100/Core/Src/OLED.cpp
+++ b/workspace/TS100/Core/Src/OLED.cpp
@@ -95,10 +95,6 @@ void OLED::initialize() {
 	displayOffset = 0;
 	memcpy(&screenBuffer[0], &REFRESH_COMMANDS[0], sizeof(REFRESH_COMMANDS));
 
-	HAL_Delay(50);
-	HAL_GPIO_WritePin(OLED_RESET_GPIO_Port, OLED_RESET_Pin, GPIO_PIN_SET);
-	HAL_Delay(50);
-
 	// Set the display to be ON once the settings block is sent and send the
 	// initialisation data to the OLED.
 
diff --git a/workspace/TS100/Core/Src/QC3.c b/workspace/TS100/Core/Src/QC3.c
new file mode 100644
index 00000000..6e47b7c1
--- /dev/null
+++ b/workspace/TS100/Core/Src/QC3.c
@@ -0,0 +1,203 @@
+/*
+ * QC3.c
+ *
+ *  Created on: 29 May 2020
+ *      Author: Ralim
+ */
+
+
+//Quick charge 3.0 supporting functions
+
+
+
+#ifdef MODEL_TS80
+void DPlusZero_Six() {
+	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_RESET);		// pull down D+
+}
+void DNegZero_Six() {
+	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_SET);
+	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_RESET);
+}
+void DPlusThree_Three() {
+	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_SET);			// pull up D+
+}
+void DNegThree_Three() {
+	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_SET);
+	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_SET);
+}
+
+void QC_Seek9V() {
+	DNegZero_Six();
+	DPlusThree_Three();
+}
+void QC_Seek12V() {
+	DNegZero_Six();
+	DPlusZero_Six();
+}
+void QC_Seek20V() {
+	DNegThree_Three();
+	DPlusThree_Three();
+}
+void QC_SeekContMode() {
+	DNegThree_Three();
+	DPlusZero_Six();
+}
+void QC_SeekContPlus() {
+	QC_SeekContMode();
+	vTaskDelay(3);
+	QC_Seek20V();
+	vTaskDelay(1);
+	QC_SeekContMode();
+}
+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) {
+	if (QCMode == 5)
+		startQC(divisor);
+	if (QCMode == 0)
+		return;  // NOT connected to a QC Charger
+
+	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, 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) {
+			QC_SeekContNeg();
+			vTaskDelay(3);
+			steps++;
+		}
+		while (steps > 0) {
+			QC_SeekContPlus();
+			vTaskDelay(3);
+			steps--;
+		}
+		vTaskDelay(10);
+	}
+#ifdef ENABLE_QC2
+	// Re-measure
+	/* Disabled due to nothing to test and code space of around 1k*/
+	steps = vStart - getInputVoltageX10(divisor, 1);
+	if (steps < 0)
+		steps = -steps;
+	if (steps > 4) {
+		// No continuous mode, so QC2
+		QCMode = 2;
+		// Goto nearest
+		if (Vx10 > 110) {
+			// request 12V
+			// D- = 0.6V, D+ = 0.6V
+			// Clamp PB3
+			QC_Seek12V();
+
+		} else {
+			// request 9V
+			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, user has probably over-ridden this
+		return;
+	}
+	GPIO_InitTypeDef GPIO_InitStruct;
+	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);
+	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 < 200 && enteredQC == 0; i++) {
+		vTaskDelay(1);	//10mS pause
+		if (i > 130) {
+			if (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_11) == GPIO_PIN_RESET) {
+				enteredQC = 1;
+			}
+			if (i == 140) {
+				//For some marginal QC chargers, we try adding a pulldown
+				GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
+				GPIO_InitStruct.Pull = GPIO_PULLDOWN;
+				GPIO_InitStruct.Pin = GPIO_PIN_11;
+				HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+			}
+		}
+	}
+	GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
+	GPIO_InitStruct.Pull = GPIO_NOPULL;
+	GPIO_InitStruct.Pin = GPIO_PIN_11;
+	HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+	if (enteredQC) {
+		// We have a QC capable charger
+		QC_Seek9V();
+		GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_10;
+		GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
+		HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
+		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
+				return;
+			}
+			vTaskDelay(10);  // 100mS
+		}
+		QCMode = 5;
+		QCTries++;
+		if (QCTries > 10) // 10 goes to get it going
+			QCMode = 0;
+	} else {
+		// no QC
+		QCMode = 0;
+	}
+	if (QCTries > 10)
+		QCMode = 0;
+}
+
+
+#endif
diff --git a/workspace/TS100/Core/Src/TipThermoModel.cpp b/workspace/TS100/Core/Src/TipThermoModel.cpp
index e35b2915..ac1c9389 100644
--- a/workspace/TS100/Core/Src/TipThermoModel.cpp
+++ b/workspace/TS100/Core/Src/TipThermoModel.cpp
@@ -7,7 +7,7 @@
 
 #include "TipThermoModel.h"
 #include "Settings.h"
-#include "hardware.h"
+#include "BSP.h"
 #include "../../configuration.h"
 
 /*
diff --git a/workspace/TS100/Core/Src/gui.cpp b/workspace/TS100/Core/Src/gui.cpp
index ca4e92c1..4de6b484 100644
--- a/workspace/TS100/Core/Src/gui.cpp
+++ b/workspace/TS100/Core/Src/gui.cpp
@@ -13,7 +13,7 @@
 #include "string.h"
 #include "unit.h"
 #include "../../configuration.h"
-
+#include "Buttons.hpp"
 extern uint32_t lastButtonTime;
 void gui_Menu(const menuitem *menu);
 
diff --git a/workspace/TS100/Core/Src/hardware.cpp b/workspace/TS100/Core/Src/hardware.cpp
deleted file mode 100644
index 7deb1380..00000000
--- a/workspace/TS100/Core/Src/hardware.cpp
+++ /dev/null
@@ -1,393 +0,0 @@
-/*
- * hardware.c
- *
- *  Created on: 2Sep.,2017
- *      Author: Ben V. Brown
- */
-
-// These are all the functions for interacting with the hardware
-#include "hardware.h"
-#include "history.hpp"
-volatile uint16_t PWMSafetyTimer = 0;
-
-uint16_t getHandleTemperature() {
-	// We return the current handle temperature in X10 C
-	// TMP36 in handle, 0.5V offset and then 10mV per deg C (0.75V @ 25C for
-	// example) STM32 = 4096 count @ 3.3V input -> But We oversample by 32/(2^2) =
-	// 8 times oversampling Therefore 32768 is the 3.3V input, so 0.1007080078125
-	// mV per count So we need to subtract an offset of 0.5V to center on 0C
-	// (4964.8 counts)
-	//
-	int32_t result = getADC(0);
-	result -= 4965;  // remove 0.5V offset
-	// 10mV per C
-	// 99.29 counts per Deg C above 0C
-	result *= 100;
-	result /= 993;
-	return result;
-}
-
-uint16_t getTipInstantTemperature() {
-	uint16_t sum = 0;	// 12 bit readings * 8 -> 15 bits
-	uint16_t readings[8];
-	//Looking to reject the highest outlier readings.
-	//As on some hardware these samples can run into the op-amp recovery time
-	//Once this time is up the signal stabilises quickly, so no need to reject minimums
-	readings[0] = hadc1.Instance->JDR1;
-	readings[1] = hadc1.Instance->JDR2;
-	readings[2] = hadc1.Instance->JDR3;
-	readings[3] = hadc1.Instance->JDR4;
-	readings[4] = hadc2.Instance->JDR1;
-	readings[5] = hadc2.Instance->JDR2;
-	readings[6] = hadc2.Instance->JDR3;
-	readings[7] = hadc2.Instance->JDR4;
-
-	for (int i = 0; i < 8; i++) {
-		sum += readings[i];
-	}
-	return sum;  // 8x over sample
-}
-
-//2 second filter (ADC is PID_TIM_HZ Hz)
-history<uint16_t, PID_TIM_HZ> rawTempFilter = { { 0 }, 0, 0 };
-
-uint16_t getTipRawTemp(uint8_t refresh) {
-	if (refresh) {
-		uint16_t lastSample = getTipInstantTemperature();
-		rawTempFilter.update(lastSample);
-		return lastSample;
-	} else {
-		return rawTempFilter.average();
-	}
-}
-
-uint16_t getInputVoltageX10(uint16_t divisor, uint8_t sample) {
-	// ADC maximum is 32767 == 3.3V at input == 28.05V at VIN
-	// 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;
-	if (preFillneeded) {
-		for (uint8_t i = 0; i < BATTFILTERDEPTH; i++)
-			samples[i] = getADC(1);
-		preFillneeded--;
-	}
-	if (sample) {
-		samples[index] = getADC(1);
-		index = (index + 1) % BATTFILTERDEPTH;
-	}
-	uint32_t sum = 0;
-
-	for (uint8_t i = 0; i < BATTFILTERDEPTH; i++)
-		sum += samples[i];
-
-	sum /= BATTFILTERDEPTH;
-	return sum * 4 / divisor;
-}
-#ifdef MODEL_TS80
-void DPlusZero_Six() {
-	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_RESET);		// pull down D+
-}
-void DNegZero_Six() {
-	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_SET);
-	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_RESET);
-}
-void DPlusThree_Three() {
-	HAL_GPIO_WritePin(GPIOB, GPIO_PIN_3, GPIO_PIN_SET);			// pull up D+
-}
-void DNegThree_Three() {
-	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_10, GPIO_PIN_SET);
-	HAL_GPIO_WritePin(GPIOA, GPIO_PIN_8, GPIO_PIN_SET);
-}
-
-void QC_Seek9V() {
-	DNegZero_Six();
-	DPlusThree_Three();
-}
-void QC_Seek12V() {
-	DNegZero_Six();
-	DPlusZero_Six();
-}
-void QC_Seek20V() {
-	DNegThree_Three();
-	DPlusThree_Three();
-}
-void QC_SeekContMode() {
-	DNegThree_Three();
-	DPlusZero_Six();
-}
-void QC_SeekContPlus() {
-	QC_SeekContMode();
-	vTaskDelay(3);
-	QC_Seek20V();
-	vTaskDelay(1);
-	QC_SeekContMode();
-}
-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) {
-	if (QCMode == 5)
-		startQC(divisor);
-	if (QCMode == 0)
-		return;  // NOT connected to a QC Charger
-
-	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, 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) {
-			QC_SeekContNeg();
-			vTaskDelay(3);
-			steps++;
-		}
-		while (steps > 0) {
-			QC_SeekContPlus();
-			vTaskDelay(3);
-			steps--;
-		}
-		vTaskDelay(10);
-	}
-#ifdef ENABLE_QC2
-	// Re-measure
-	/* Disabled due to nothing to test and code space of around 1k*/
-	steps = vStart - getInputVoltageX10(divisor, 1);
-	if (steps < 0)
-		steps = -steps;
-	if (steps > 4) {
-		// No continuous mode, so QC2
-		QCMode = 2;
-		// Goto nearest
-		if (Vx10 > 110) {
-			// request 12V
-			// D- = 0.6V, D+ = 0.6V
-			// Clamp PB3
-			QC_Seek12V();
-
-		} else {
-			// request 9V
-			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, user has probably over-ridden this
-		return;
-	}
-	GPIO_InitTypeDef GPIO_InitStruct;
-	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);
-	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 < 200 && enteredQC == 0; i++) {
-		vTaskDelay(1);	//10mS pause
-		if (i > 130) {
-			if (HAL_GPIO_ReadPin(GPIOA, GPIO_PIN_11) == GPIO_PIN_RESET) {
-				enteredQC = 1;
-			}
-			if (i == 140) {
-				//For some marginal QC chargers, we try adding a pulldown
-				GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
-				GPIO_InitStruct.Pull = GPIO_PULLDOWN;
-				GPIO_InitStruct.Pin = GPIO_PIN_11;
-				HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
-			}
-		}
-	}
-	GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
-	GPIO_InitStruct.Pull = GPIO_NOPULL;
-	GPIO_InitStruct.Pin = GPIO_PIN_11;
-	HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
-	if (enteredQC) {
-		// We have a QC capable charger
-		QC_Seek9V();
-		GPIO_InitStruct.Pin = GPIO_PIN_8 | GPIO_PIN_10;
-		GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
-		HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
-		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
-				return;
-			}
-			vTaskDelay(10);  // 100mS
-		}
-		QCMode = 5;
-		QCTries++;
-		if (QCTries > 10) // 10 goes to get it going
-			QCMode = 0;
-	} else {
-		// no QC
-		QCMode = 0;
-	}
-	if (QCTries > 10)
-		QCMode = 0;
-}
-
-static unsigned int sqrt32(unsigned long n) {
-	unsigned int c = 0x8000;
-	unsigned int g = 0x8000;
-
-	for (;;) {
-		if (g * g > n)
-			g ^= c;
-		c >>= 1;
-		if (c == 0)
-			return g;
-		g |= c;
-	}
-}
-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 = 4500;
-// Check no tip
-	if (milliOhms > 10000)
-		return -1;
-//Because of tolerance, if a user has asked for the higher power mode, then just goto 12V and call it a day
-	if (useHP)
-		return 120;
-//
-// V = sqrt(18W*R)
-// Convert this to sqrt(18W)*sqrt(milli ohms)*sqrt(1/1000)
-
-	uint32_t Vx = sqrt32(milliOhms);
-	if (useHP)
-		Vx *= 1549;	//sqrt(24)*sqrt(1/1000)*10000
-	else
-		Vx *= 1342;	// sqrt(18) * sqrt(1/1000)*10000
-
-// Round to nearest 200mV,
-// So divide by 100 to start, to get in Vxx
-	Vx /= 100;
-	if (Vx % 10 >= 5)
-		Vx += 10;
-	Vx /= 10;
-// Round to nearest increment of 2
-	if (Vx % 2 == 1)
-		Vx++;
-//Because of how bad the tolerance is on detecting the tip resistance is
-//Its more functional to bin this
-	if (Vx < 90)
-		Vx = 90;
-	else if (Vx >= 105)
-		Vx = 120;
-	return Vx;
-}
-
-#endif
-volatile uint8_t pendingPWM = 0;
-
-void setTipPWM(uint8_t pulse) {
-	PWMSafetyTimer = 10; // This is decremented in the handler for PWM so that the tip pwm is
-						 // disabled if the PID task is not scheduled often enough.
-
-	pendingPWM = pulse;
-}
-
-// These are called by the HAL after the corresponding events from the system
-// timers.
-
-void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim) {
-// Period has elapsed
-	if (htim->Instance == TIM2) {
-		// we want to turn on the output again
-		PWMSafetyTimer--;
-		// We decrement this safety value so that lockups in the
-		// scheduler will not cause the PWM to become locked in an
-		// active driving state.
-		// While we could assume this could never happen, its a small price for
-		// increased safety
-		htim2.Instance->CCR4 = pendingPWM;
-		if (htim2.Instance->CCR4 && PWMSafetyTimer) {
-			HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
-		} else {
-			HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_1);
-		}
-	} else if (htim->Instance == TIM1) {
-		// STM uses this for internal functions as a counter for timeouts
-		HAL_IncTick();
-	}
-}
-
-void HAL_TIM_PWM_PulseFinishedCallback(TIM_HandleTypeDef *htim) {
-// This was a when the PWM for the output has timed out
-	if (htim->Channel == HAL_TIM_ACTIVE_CHANNEL_4) {
-		HAL_TIM_PWM_Stop(&htim3, TIM_CHANNEL_1);
-	}
-}
-
-void vApplicationIdleHook(void) {
-	HAL_IWDG_Refresh(&hiwdg);
-}
-
-/* USER CODE BEGIN GET_IDLE_TASK_MEMORY */
-static StaticTask_t xIdleTaskTCBBuffer;
-static StackType_t xIdleStack[configMINIMAL_STACK_SIZE];
-
-void vApplicationGetIdleTaskMemory(StaticTask_t **ppxIdleTaskTCBBuffer,
-		StackType_t **ppxIdleTaskStackBuffer, uint32_t *pulIdleTaskStackSize) {
-	*ppxIdleTaskTCBBuffer = &xIdleTaskTCBBuffer;
-	*ppxIdleTaskStackBuffer = &xIdleStack[0];
-	*pulIdleTaskStackSize = configMINIMAL_STACK_SIZE;
-	/* place for user code */
-}
-/* USER CODE END GET_IDLE_TASK_MEMORY */
diff --git a/workspace/TS100/Core/Src/power.cpp b/workspace/TS100/Core/Src/power.cpp
index 99887079..87507d15 100644
--- a/workspace/TS100/Core/Src/power.cpp
+++ b/workspace/TS100/Core/Src/power.cpp
@@ -7,7 +7,7 @@
 
 #include <power.hpp>
 #include <Settings.h>
-#include <hardware.h>
+#include <BSP.h>
 
 const uint16_t powerPWM = 255;
 const uint16_t totalPWM = 255 + 17; //htim2.Init.Period, the full PWM cycle