diff --git a/workspace/TS100/Core/BSP/BSP.h b/workspace/TS100/Core/BSP/BSP.h
index c4fdb144..f1765c0f 100644
--- a/workspace/TS100/Core/BSP/BSP.h
+++ b/workspace/TS100/Core/BSP/BSP.h
@@ -51,8 +51,7 @@ uint8_t showBootLogoIfavailable();
 void delay_ms(uint16_t count) ;
 //Used to allow knowledge of if usb_pd is being used
 uint8_t usb_pd_detect();
-//Returns 0 when the irq line is pulled down
-uint8_t pd_irq_read();
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/workspace/TS100/Core/BSP/BSP_Power.h b/workspace/TS100/Core/BSP/BSP_Power.h
index 1ce7f2bf..5b277a21 100644
--- a/workspace/TS100/Core/BSP/BSP_Power.h
+++ b/workspace/TS100/Core/BSP/BSP_Power.h
@@ -12,9 +12,6 @@
 extern "C" {
 #endif
 
-// Called once at startup, after RToS
-// This can handle negotiations for QC/PD etc
-void power_probe();
 
 // Called periodically in the movement handling thread
 // Can be used to check any details for the power system
diff --git a/workspace/TS100/Core/Src/QC3.c b/workspace/TS100/Core/Src/QC3.c
index 826891c8..0c880a5e 100644
--- a/workspace/TS100/Core/Src/QC3.c
+++ b/workspace/TS100/Core/Src/QC3.c
@@ -13,68 +13,72 @@
 #include "stdint.h"
 
 void QC_Seek9V() {
-    QC_DNegZero_Six();
-    QC_DPlusThree_Three();
+	QC_DNegZero_Six();
+	QC_DPlusThree_Three();
 }
 void QC_Seek12V() {
-    QC_DNegZero_Six();
-    QC_DPlusZero_Six();
+	QC_DNegZero_Six();
+	QC_DPlusZero_Six();
 }
 void QC_Seek20V() {
-    QC_DNegThree_Three();
-    QC_DPlusThree_Three();
+	QC_DNegThree_Three();
+	QC_DPlusThree_Three();
 }
 void QC_SeekContMode() {
-    QC_DNegThree_Three();
-    QC_DPlusZero_Six();
+	QC_DNegThree_Three();
+	QC_DPlusZero_Six();
 }
 void QC_SeekContPlus() {
-    QC_SeekContMode();
-    osDelay(30);
-    QC_Seek20V();
-    osDelay(10);
-    QC_SeekContMode();
+	QC_SeekContMode();
+	osDelay(30);
+	QC_Seek20V();
+	osDelay(10);
+	QC_SeekContMode();
 }
 void QC_SeekContNeg() {
-    QC_SeekContMode();
-    osDelay(30);
-    QC_Seek12V();
-    osDelay(10);
-    QC_SeekContMode();
+	QC_SeekContMode();
+	osDelay(30);
+	QC_Seek12V();
+	osDelay(10);
+	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 (QCMode == 0)
+		startQC(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
+	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;
+	// 1. Measure current voltage
+	int16_t vStart = getInputVoltageX10(divisor, 1);
+	int difference = Vx10 - vStart;
 
-    // 2. calculate ideal steps (0.2V changes)
+	// 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();
-            osDelay(30);
-            steps++;
-        }
-        while (steps > 0) {
-            QC_SeekContPlus();
-            osDelay(30);
-            steps--;
-        }
-        osDelay(100);
-    }
+	int steps = difference / 2;
+	if (QCMode == 3) {
+		if (steps > -2 && steps < 2)
+			return;  // dont bother with small steps
+		while (steps < 0) {
+			QC_SeekContNeg();
+			osDelay(30);
+			steps++;
+		}
+		while (steps > 0) {
+			QC_SeekContPlus();
+			osDelay(30);
+			steps--;
+		}
+		osDelay(100);
+	}
 #ifdef ENABLE_QC2
     // Re-measure
     /* Disabled due to nothing to test and code space of around 1k*/
@@ -99,59 +103,61 @@ void seekQC(int16_t Vx10, uint16_t divisor) {
 }
 // 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;
-    }
-    QC_Init_GPIO();
+	// 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;
+	}
+	if (QCTries > 10) {
+		return;
+	}
+	QC_Init_GPIO();
 
-    // 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-
-    QC_DPlusZero_Six();
+	// 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-
+	QC_DPlusZero_Six();
+
+	// Delay 1.25 seconds
+	uint8_t enteredQC = 0;
+	for (uint16_t i = 0; i < 200 && enteredQC == 0; i++) {
+		osDelay(10);  // 10mS pause
+		if (i > 130) {
+			if (QC_DM_PulledDown()) {
+				enteredQC = 1;
+			}
+			if (i == 140) {
+				// For some marginal QC chargers, we try adding a pulldown
+				QC_DM_PullDown();
+			}
+		}
+	}
+	QC_DM_No_PullDown();
+	if (enteredQC) {
+		// We have a QC capable charger
+		QC_Seek9V();
+		QC_Post_Probe_En();
+		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;
+			}
+			osDelay(100);  // 100mS
+		}
+		QCMode = 0;
+		QCTries++;
+
+	} else {
+		// no QC
+		QCMode = 0;
+	}
 
-    // Delay 1.25 seconds
-    uint8_t enteredQC = 0;
-    for (uint16_t i = 0; i < 200 && enteredQC == 0; i++) {
-    	osDelay(10);  // 10mS pause
-        if (i > 130) {
-            if (QC_DM_PulledDown()) {
-                enteredQC = 1;
-            }
-            if (i == 140) {
-                // For some marginal QC chargers, we try adding a pulldown
-                QC_DM_PullDown();
-            }
-        }
-    }
-    QC_DM_No_PullDown();
-    if (enteredQC) {
-        // We have a QC capable charger
-        QC_Seek9V();
-        QC_Post_Probe_En();
-        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;
-            }
-            osDelay(100);  // 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;
 }
diff --git a/workspace/TS100/Core/Src/main.cpp b/workspace/TS100/Core/Src/main.cpp
index ec190be6..89d2bc45 100644
--- a/workspace/TS100/Core/Src/main.cpp
+++ b/workspace/TS100/Core/Src/main.cpp
@@ -43,17 +43,23 @@ int main(void) {
 	OLED::setFont(0);    // default to bigger font
 	// Testing for which accelerometer is mounted
 	resetWatchdog();
-	usb_pd_available = true;//usb_pd_detect();
+	usb_pd_available = usb_pd_detect();
 	resetWatchdog();
 	settingsWereReset = restoreSettings();  // load the settings from flash
-	/*if (MMA8652FC::detect()) {
-	 PCBVersion = 1;
-	 MMA8652FC::initalize();  // this sets up the I2C registers
-	 } else if (LIS2DH12::detect()) {
-	 PCBVersion = 2;
-	 // Setup the ST Accelerometer
-	 LIS2DH12::initalize();  // startup the accelerometer
-	 } else*/{
+#ifdef ACCEL_MMA
+	if (MMA8652FC::detect()) {
+		PCBVersion = 1;
+		MMA8652FC::initalize();  // this sets up the I2C registers
+	} else
+#endif
+#ifdef ACCEL_LIS
+	if (LIS2DH12::detect()) {
+		PCBVersion = 2;
+		// Setup the ST Accelerometer
+		LIS2DH12::initalize();  // startup the accelerometer
+	} else
+#endif
+	{
 		PCBVersion = 3;
 		systemSettings.SleepTime = 0;
 		systemSettings.ShutdownTime = 0;  // No accel -> disable sleep
diff --git a/workspace/TS100/Core/Threads/MOVThread.cpp b/workspace/TS100/Core/Threads/MOVThread.cpp
index 5d236ba1..4ca5e54b 100644
--- a/workspace/TS100/Core/Threads/MOVThread.cpp
+++ b/workspace/TS100/Core/Threads/MOVThread.cpp
@@ -25,7 +25,6 @@ uint32_t lastMovementTime = 0;
 void startMOVTask(void const *argument __unused) {
 	OLED::setRotation(systemSettings.OrientationMode & 1);
 	postRToSInit();
-	power_probe();
 	lastMovementTime = 0;
 	int16_t datax[MOVFilter] = { 0 };
 	int16_t datay[MOVFilter] = { 0 };
@@ -39,13 +38,20 @@ void startMOVTask(void const *argument __unused) {
 	for (;;) {
 		int32_t threshold = 1500 + (9 * 200);
 		threshold -= systemSettings.sensitivity * 200;  // 200 is the step size
-
+#ifdef ACCEL_LIS
 		if (PCBVersion == 2) {
 			LIS2DH12::getAxisReadings(tx, ty, tz);
 			rotation = LIS2DH12::getOrientation();
-		} else if (PCBVersion == 1) {
+		} else
+#endif
+#ifdef ACCEL_MMA
+			if (PCBVersion == 1) {
 			MMA8652FC::getAxisReadings(tx, ty, tz);
 			rotation = MMA8652FC::getOrientation();
+		}else
+#endif
+		{
+			//do nothing :(
 		}
 		if (systemSettings.OrientationMode == 2) {
 			if (rotation != ORIENTATION_FLAT) {