./workspace/TS100 -> ./source/

source/Core/Drivers/BMA223.cpp

@@ -0,0 +1,64 @@
+/*
+ * BMA223.cpp
+ *
+ *  Created on: 18 Sep. 2020
+ *      Author: Ralim
+ */
+
+#include <BMA223.hpp>
+#include <array>
+
+bool BMA223::detect() {
+	if (FRToSI2C::probe(BMA223_ADDRESS)) {
+		//Read chip id to ensure its not an address collision
+		uint8_t id = 0;
+		if (FRToSI2C::Mem_Read(BMA223_ADDRESS, BMA223_BGW_CHIPID, &id, 1)) {
+			return id == 0b11111000;
+		}
+	}
+
+	return false;
+}
+
+static const FRToSI2C::I2C_REG i2c_registers[] = { //
+		//
+				{ BMA223_PMU_RANGE, 0b00000011, 0 },    //2G range
+				{ BMA223_PMU_BW, 0b00001101, 0 },    //250Hz filter
+				{ BMA223_PMU_LPW, 0b00000000, 0 },    //Full power
+				{ BMA223_ACCD_HBW, 0b00000000, 0 },    //filtered data out
+				{ BMA223_INT_OUT_CTRL, 0b00001010, 0 },    //interrupt active low and OD to get it hi-z
+				{ BMA223_INT_RST_LATCH, 0b10000000, 0 },    //interrupt active low and OD to get it hi-z
+				{ BMA223_INT_EN_0, 0b01000000, 0 },    //Enable orientation
+				{ BMA223_INT_A, 0b00100111, 0 },    //Setup orientation detection
+
+		//
+		};
+bool BMA223::initalize() {
+	//Setup acceleration readings
+	//2G range
+	//bandwidth = 250Hz
+	//High pass filter on (Slow compensation)
+	//Turn off IRQ output pins
+	//Orientation recognition in symmetrical mode
+	// Hysteresis is set to ~ 16 counts
+	//Theta blocking is set to 0b10
+
+	return FRToSI2C::writeRegistersBulk(BMA223_ADDRESS, i2c_registers, sizeof(i2c_registers) / sizeof(i2c_registers[0]));
+
+}
+
+void BMA223::getAxisReadings(int16_t &x, int16_t &y, int16_t &z) {
+	//The BMA is odd in that its output data width is only 8 bits
+	//And yet there are MSB and LSB registers _sigh_.
+	uint8_t sensorData[6] = { 0, 0, 0, 0, 0, 0 };
+
+	if (FRToSI2C::Mem_Read(BMA223_ADDRESS, BMA223_ACCD_X_LSB, sensorData, 6) == false) {
+		x = y = z = 0;
+		return;
+	}
+	//Shift 6 to make its range ~= the other accelerometers
+	x = sensorData[1] << 6;
+	y = sensorData[3] << 6;
+	z = sensorData[5] << 6;
+
+}

source/Core/Drivers/BMA223.hpp

@@ -0,0 +1,39 @@
+/*
+ * BMA223.hpp
+ *
+ *  Created on: 18 Sep. 2020
+ *      Author: Ralim
+ */
+
+#ifndef CORE_DRIVERS_BMA223_HPP_
+#define CORE_DRIVERS_BMA223_HPP_
+#include "I2C_Wrapper.hpp"
+#include "BSP.h"
+#include "BMA223_defines.h"
+
+class BMA223 {
+public:
+	static bool detect();
+	static bool initalize();
+	//1 = rh, 2,=lh, 8=flat
+	static Orientation getOrientation() {
+		uint8_t val = FRToSI2C::I2C_RegisterRead(BMA223_ADDRESS,
+		BMA223_INT_STATUS_3);
+		val >>= 4; //we dont need high values
+		val &= 0b11;
+		if(val &0b10){
+			return ORIENTATION_FLAT;
+		}else{
+			return static_cast<Orientation>(!val);
+		}
+		//0 = rhs
+		//1 =lhs
+		//2 & 3 == ignore
+
+	}
+	static void getAxisReadings(int16_t& x, int16_t& y, int16_t& z);
+
+private:
+};
+
+#endif /* CORE_DRIVERS_BMA223_HPP_ */

source/Core/Drivers/BMA223_defines.h

@@ -0,0 +1,68 @@
+/*
+ * BMA223_defines.h
+ *
+ *  Created on: 18 Sep. 2020
+ *      Author: Ralim
+ */
+
+#ifndef CORE_DRIVERS_BMA223_DEFINES_H_
+#define CORE_DRIVERS_BMA223_DEFINES_H_
+
+#define BMA223_ADDRESS 0x18<<1
+#define BMA223_BGW_CHIPID 0x00
+#define BMA223_ACCD_X_LSB 0x02
+#define BMA223_ACCD_X_MSB 0x03
+#define BMA223_ACCD_Y_LSB 0x04
+#define BMA223_ACCD_Y_MSB 0x05
+#define BMA223_ACCD_Z_LSB 0x06
+#define BMA223_ACCD_Z_MSB 0x07
+#define BMA223_ACCD_TEMP  0x08
+#define BMA223_INT_STATUS_0 0x09
+#define BMA223_INT_STATUS_1 0x0A
+#define BMA223_INT_STATUS_2 0x0B
+#define BMA223_INT_STATUS_3 0x0C
+#define BMA223_FIFO_STATUS 0x0E
+#define BMA223_PMU_RANGE 0x0F
+#define BMA223_PMU_BW 0x10
+#define BMA223_PMU_LPW 0x11
+#define BMA223_PMU_LOW_POWER 0x012
+#define BMA223_ACCD_HBW 0x13
+#define BMA223_BGW_SOFTRESET 0x14
+#define BMA223_INT_EN_0 0x16
+#define BMA223_INT_EN_1 0x17
+#define BMA223_INT_EN_2 0x18
+#define BMA223_INT_MAP_0 0x19
+#define BMA223_INT_MAP_1 0x1A
+#define BMA223_INT_MAP_2 0x1B
+#define BMA223_INT_SRC 0x1E
+#define BMA223_INT_OUT_CTRL 0x20
+#define BMA223_INT_RST_LATCH 0x21
+#define BMA223_INT_0 0x22
+#define BMA223_INT_1 0x23
+#define BMA223_INT_2 0x24
+#define BMA223_INT_3 0x25
+#define BMA223_INT_4 0x26
+#define BMA223_INT_5 0x27
+#define BMA223_INT_6 0x28
+#define BMA223_INT_7 0x29
+#define BMA223_INT_8 0x2A
+#define BMA223_INT_9 0x2B
+#define BMA223_INT_A 0x2C
+#define BMA223_INT_B 0x2D
+#define BMA223_INT_C 0x2E
+#define BMA223_INT_D 0x2F
+#define BMA223_FIFO_CONFIG_0 0x30
+#define BMA223_PMU_SELF_TEST 0x32
+#define BMA223_TRIM_NVM_CTRL 0x33
+#define BMA223_BGW_SPI3_WDT 0x34
+#define BMA223_OFC_CTRL 0x36
+#define BMA223_OFC_SETTING 0x37
+#define BMA223_OFC_OFFSET_X 0x38
+#define BMA223_OFC_OFFSET_Y 0x39
+#define BMA223_OFC_OFFSET_Z 0x3A
+#define BMA223_TRIM_GP0 0x3B
+#define BMA223_TRIM_GP1 0x3C
+#define BMA223_FIFO_CONFIG_1 0x3E
+#define BMA223_FIFO_DATA 0x3F
+
+#endif /* CORE_DRIVERS_BMA223_DEFINES_H_ */

source/Core/Drivers/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 = 400;
+	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_BOTH_LONG; // Both being held case
+		} 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;
+	}
+}

source/Core/Drivers/Buttons.hpp

@@ -0,0 +1,36 @@
+/*
+ * 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*/
+	BUTTON_BOTH_LONG = 32, /* User is holding 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_ */

source/Core/Drivers/FUSB302/fusb302b.h

@@ -0,0 +1,305 @@
+/*
+ * PD Buddy Firmware Library - USB Power Delivery for everyone
+ * Copyright 2017-2018 Clayton G. Hobbs
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef PDB_FUSB302B_H
+#define PDB_FUSB302B_H
+
+#include <stdint.h>
+
+#include "pd.h"
+#include <pdb_msg.h>
+
+/* I2C addresses of the FUSB302B chips */
+#define FUSB302B_ADDR (0x22<<1)
+#define FUSB302B01_ADDR (0x23<<1)
+#define FUSB302B10_ADDR (0x24<<1)
+#define FUSB302B11_ADDR (0x25<<1)
+
+/* Device ID register */
+#define FUSB_DEVICE_ID 0x01
+#define FUSB_DEVICE_ID_VERSION_ID_SHIFT 4
+#define FUSB_DEVICE_ID_VERSION_ID (0xF << FUSB_DEVICE_ID_VERSION_ID_SHIFT)
+#define FUSB_DEVICE_ID_PRODUCT_ID_SHIFT 2
+#define FUSB_DEVICE_ID_PRODUCT_ID (0x3 << FUSB_DEVICE_ID_PRODUCT_ID_SHIFT)
+#define FUSB_DEVICE_ID_REVISION_ID_SHIFT 0
+#define FUSB_DEVICE_ID_REVISION_ID (0x3 << FUSB_DEVICE_ID_REVISION_ID_SHIFT)
+
+/* Switches0 register */
+#define FUSB_SWITCHES0 0x02
+#define FUSB_SWITCHES0_PU_EN2 (1 << 7)
+#define FUSB_SWITCHES0_PU_EN1 (1 << 6)
+#define FUSB_SWITCHES0_VCONN_CC2 (1 << 5)
+#define FUSB_SWITCHES0_VCONN_CC1 (1 << 4)
+#define FUSB_SWITCHES0_MEAS_CC2 (1 << 3)
+#define FUSB_SWITCHES0_MEAS_CC1 (1 << 2)
+#define FUSB_SWITCHES0_PDWN_2 (1 << 1)
+#define FUSB_SWITCHES0_PDWN_1 1
+
+/* Switches1 register */
+#define FUSB_SWITCHES1 0x03
+#define FUSB_SWITCHES1_POWERROLE (1 << 7)
+#define FUSB_SWITCHES1_SPECREV_SHIFT 5
+#define FUSB_SWITCHES1_SPECREV (0x3 << FUSB_SWITCHES1_SPECREV_SHIFT)
+#define FUSB_SWITCHES1_DATAROLE (1 << 4)
+#define FUSB_SWITCHES1_AUTO_CRC (1 << 2)
+#define FUSB_SWITCHES1_TXCC2 (1 << 1)
+#define FUSB_SWITCHES1_TXCC1 1
+
+/* Measure register */
+#define FUSB_MEASURE 0x04
+#define FUSB_MEASURE_MEAS_VBUS (1 << 6)
+#define FUSB_MEASURE_MDAC_SHIFT 0
+#define FUSB_MEASURE_MDAC (0x3F << FUSB_MEASURE_MDAC_SHIFT)
+
+/* Slice register */
+#define FUSB_SLICE 0x05
+#define FUSB_SLICE_SDAC_HYS_SHIFT 6
+#define FUSB_SLICE_SDAC_HYS (0x3 << FUSB_SLICE_SDAC_HYS_SHIFT)
+#define FUSB_SLICE_SDAC_SHIFT 0
+#define FUSB_SLICE_SDAC (0x3F << FUSB_SLICE_SDAC_SHIFT)
+
+/* Control0 register */
+#define FUSB_CONTROL0 0x06
+#define FUSB_CONTROL0_TX_FLUSH (1 << 6)
+#define FUSB_CONTROL0_INT_MASK (1 << 5)
+#define FUSB_CONTROL0_HOST_CUR_SHIFT 2
+#define FUSB_CONTROL0_HOST_CUR (0x3 << FUSB_CONTROL0_HOST_CUR_SHIFT)
+#define FUSB_CONTROL0_AUTO_PRE (1 << 1)
+#define FUSB_CONTROL0_TX_START 1
+
+/* Control1 register */
+#define FUSB_CONTROL1 0x07
+#define FUSB_CONTROL1_ENSOP2DB (1 << 6)
+#define FUSB_CONTROL1_ENSOP1DB (1 << 5)
+#define FUSB_CONTROL1_BIST_MODE2 (1 << 4)
+#define FUSB_CONTROL1_RX_FLUSH (1 << 2)
+#define FUSB_CONTROL1_ENSOP2 (1 << 1)
+#define FUSB_CONTROL1_ENSOP1 1
+
+/* Control2 register */
+#define FUSB_CONTROL2 0x08
+#define FUSB_CONTROL2_TOG_SAVE_PWR_SHIFT 6
+#define FUSB_CONTROL2_TOG_SAVE_PWR (0x3 << FUSB_CONTROL2_TOG_SAVE_PWR)
+#define FUSB_CONTROL2_TOG_RD_ONLY (1 << 5)
+#define FUSB_CONTROL2_WAKE_EN (1 << 3)
+#define FUSB_CONTROL2_MODE_SHIFT 1
+#define FUSB_CONTROL2_MODE (0x3 << FUSB_CONTROL2_MODE_SHIFT)
+#define FUSB_CONTROL2_TOGGLE 1
+
+/* Control3 register */
+#define FUSB_CONTROL3 0x09
+#define FUSB_CONTROL3_SEND_HARD_RESET (1 << 6)
+#define FUSB_CONTROL3_BIST_TMODE (1 << 5)
+#define FUSB_CONTROL3_AUTO_HARDRESET (1 << 4)
+#define FUSB_CONTROL3_AUTO_SOFTRESET (1 << 3)
+#define FUSB_CONTROL3_N_RETRIES_SHIFT 1
+#define FUSB_CONTROL3_N_RETRIES (0x3 << FUSB_CONTROL3_N_RETRIES_SHIFT)
+#define FUSB_CONTROL3_AUTO_RETRY 1
+
+/* Mask1 register */
+#define FUSB_MASK1 0x0A
+#define FUSB_MASK1_M_VBUSOK (1 << 7)
+#define FUSB_MASK1_M_ACTIVITY (1 << 6)
+#define FUSB_MASK1_M_COMP_CHNG (1 << 5)
+#define FUSB_MASK1_M_CRC_CHK (1 << 4)
+#define FUSB_MASK1_M_ALERT (1 << 3)
+#define FUSB_MASK1_M_WAKE (1 << 2)
+#define FUSB_MASK1_M_COLLISION (1 << 1)
+#define FUSB_MASK1_M_BC_LVL (1 << 0)
+
+/* Power register */
+#define FUSB_POWER 0x0B
+#define FUSB_POWER_PWR3 (1 << 3)
+#define FUSB_POWER_PWR2 (1 << 2)
+#define FUSB_POWER_PWR1 (1 << 1)
+#define FUSB_POWER_PWR0 1
+
+/* Reset register */
+#define FUSB_RESET 0x0C
+#define FUSB_RESET_PD_RESET (1 << 1)
+#define FUSB_RESET_SW_RES 1
+
+/* OCPreg register */
+#define FUSB_OCPREG 0x0D
+#define FUSB_OCPREG_OCP_RANGE (1 << 3)
+#define FUSB_OCPREG_OCP_CUR_SHIFT 0
+#define FUSB_OCPREG_OCP_CUR (0x7 << FUSB_OCPREG_OCP_CUR_SHIFT)
+
+/* Maska register */
+#define FUSB_MASKA 0x0E
+#define FUSB_MASKA_M_OCP_TEMP (1 << 7)
+#define FUSB_MASKA_M_TOGDONE (1 << 6)
+#define FUSB_MASKA_M_SOFTFAIL (1 << 5)
+#define FUSB_MASKA_M_RETRYFAIL (1 << 4)
+#define FUSB_MASKA_M_HARDSENT (1 << 3)
+#define FUSB_MASKA_M_TXSENT (1 << 2)
+#define FUSB_MASKA_M_SOFTRST (1 << 1)
+#define FUSB_MASKA_M_HARDRST 1
+
+/* Maskb register */
+#define FUSB_MASKB 0x0F
+#define FUSB_MASKB_M_GCRCSENT 1
+
+/* Control4 register */
+#define FUSB_CONTROL4 0x10
+#define FUSB_CONTROL4_TOG_EXIT_AUD 1
+
+/* Status0a register */
+#define FUSB_STATUS0A 0x3C
+#define FUSB_STATUS0A_SOFTFAIL (1 << 5)
+#define FUSB_STATUS0A_RETRYFAIL (1 << 4)
+#define FUSB_STATUS0A_POWER3 (1 << 3)
+#define FUSB_STATUS0A_POWER2 (1 << 2)
+#define FUSB_STATUS0A_SOFTRST (1 << 1)
+#define FUSB_STATUS0A_HARDRST 1
+
+/* Status1a register */
+#define FUSB_STATUS1A 0x3D
+#define FUSB_STATUS1A_TOGSS_SHIFT 3
+#define FUSB_STATUS1A_TOGSS (0x7 << FUSB_STATUS1A_TOGSS_SHIFT)
+#define FUSB_STATUS1A_RXSOP2DB (1 << 2)
+#define FUSB_STATUS1A_RXSOP1DB (1 << 1)
+#define FUSB_STATUS1A_RXSOP 1
+
+/* Interrupta register */
+#define FUSB_INTERRUPTA 0x3E
+#define FUSB_INTERRUPTA_I_OCP_TEMP (1 << 7)
+#define FUSB_INTERRUPTA_I_TOGDONE (1 << 6)
+#define FUSB_INTERRUPTA_I_SOFTFAIL (1 << 5)
+#define FUSB_INTERRUPTA_I_RETRYFAIL (1 << 4)
+#define FUSB_INTERRUPTA_I_HARDSENT (1 << 3)
+#define FUSB_INTERRUPTA_I_TXSENT (1 << 2)
+#define FUSB_INTERRUPTA_I_SOFTRST (1 << 1)
+#define FUSB_INTERRUPTA_I_HARDRST 1
+
+/* Interruptb register */
+#define FUSB_INTERRUPTB 0x3F
+#define FUSB_INTERRUPTB_I_GCRCSENT 1
+
+/* Status0 register */
+#define FUSB_STATUS0 0x40
+#define FUSB_STATUS0_VBUSOK (1 << 7)
+#define FUSB_STATUS0_ACTIVITY (1 << 6)
+#define FUSB_STATUS0_COMP (1 << 5)
+#define FUSB_STATUS0_CRC_CHK (1 << 4)
+#define FUSB_STATUS0_ALERT (1 << 3)
+#define FUSB_STATUS0_WAKE (1 << 2)
+#define FUSB_STATUS0_BC_LVL_SHIFT 0
+#define FUSB_STATUS0_BC_LVL (0x3 << FUSB_STATUS0_BC_LVL_SHIFT)
+
+/* Status1 register */
+#define FUSB_STATUS1 0x41
+#define FUSB_STATUS1_RXSOP2 (1 << 7)
+#define FUSB_STATUS1_RXSOP1 (1 << 6)
+#define FUSB_STATUS1_RX_EMPTY (1 << 5)
+#define FUSB_STATUS1_RX_FULL (1 << 4)
+#define FUSB_STATUS1_TX_EMPTY (1 << 3)
+#define FUSB_STATUS1_TX_FULL (1 << 2)
+#define FUSB_STATUS1_OVRTEMP (1 << 1)
+#define FUSB_STATUS1_OCP 1
+
+/* Interrupt register */
+#define FUSB_INTERRUPT 0x42
+#define FUSB_INTERRUPT_I_VBUSOK (1 << 7)
+#define FUSB_INTERRUPT_I_ACTIVITY (1 << 6)
+#define FUSB_INTERRUPT_I_COMP_CHNG (1 << 5)
+#define FUSB_INTERRUPT_I_CRC_CHK (1 << 4)
+#define FUSB_INTERRUPT_I_ALERT (1 << 3)
+#define FUSB_INTERRUPT_I_WAKE (1 << 2)
+#define FUSB_INTERRUPT_I_COLLISION (1 << 1)
+#define FUSB_INTERRUPT_I_BC_LVL 1
+
+/* FIFOs register */
+#define FUSB_FIFOS 0x43
+
+#define FUSB_FIFO_TX_TXON 0xA1
+#define FUSB_FIFO_TX_SOP1 0x12
+#define FUSB_FIFO_TX_SOP2 0x13
+#define FUSB_FIFO_TX_SOP3 0x1B
+#define FUSB_FIFO_TX_RESET1 0x15
+#define FUSB_FIFO_TX_RESET2 0x16
+#define FUSB_FIFO_TX_PACKSYM 0x80
+#define FUSB_FIFO_TX_JAM_CRC 0xFF
+#define FUSB_FIFO_TX_EOP 0x14
+#define FUSB_FIFO_TX_TXOFF 0xFE
+
+#define FUSB_FIFO_RX_TOKEN_BITS 0xE0
+#define FUSB_FIFO_RX_SOP 0xE0
+#define FUSB_FIFO_RX_SOP1 0xC0
+#define FUSB_FIFO_RX_SOP2 0xA0
+#define FUSB_FIFO_RX_SOP1DB 0x80
+#define FUSB_FIFO_RX_SOP2DB 0x60
+
+/*
+ * FUSB status union
+ *
+ * Provides a nicer structure than just an array of uint8_t for working with
+ * the FUSB302B status and interrupt flags.
+ */
+union fusb_status {
+	uint8_t bytes[7];
+	struct {
+		uint8_t status0a;
+		uint8_t status1a;
+		uint8_t interrupta;
+		uint8_t interruptb;
+		uint8_t status0;
+		uint8_t status1;
+		uint8_t interrupt;
+	};
+};
+
+/* FUSB functions */
+
+/*
+ * Send a USB Power Delivery message to the FUSB302B
+ */
+void fusb_send_message(const union pd_msg *msg);
+
+/*
+ * Read a USB Power Delivery message from the FUSB302B
+ */
+uint8_t fusb_read_message(union pd_msg *msg);
+
+/*
+ * Tell the FUSB302B to send a hard reset signal
+ */
+void fusb_send_hardrst();
+
+/*
+ * Read the FUSB302B status and interrupt flags into *status
+ */
+void fusb_get_status(union fusb_status *status);
+
+/*
+ * Read the FUSB302B BC_LVL as an enum fusb_typec_current
+ */
+enum fusb_typec_current fusb_get_typec_current();
+
+/*
+ * Initialization routine for the FUSB302B
+ */
+bool fusb_setup();
+
+/*
+ * Reset the FUSB302B
+ */
+void fusb_reset();
+
+bool fusb_read_id();
+
+#endif /* PDB_FUSB302B_H */

source/Core/Drivers/FUSB302/fusbpd.cpp

@@ -0,0 +1,28 @@
+/*
+ * fusbpd.cpp
+ *
+ *  Created on: 13 Jun 2020
+ *      Author: Ralim
+ */
+#include "Model_Config.h"
+#ifdef POW_PD
+#include <fusbpd.h>
+#include <pd.h>
+#include "BSP.h"
+#include "I2CBB.hpp"
+#include "fusb302b.h"
+#include "policy_engine.h"
+#include "protocol_rx.h"
+#include "protocol_tx.h"
+#include "int_n.h"
+
+void fusb302_start_processing() {
+	/* Initialize the FUSB302B */
+	if (fusb_setup()) {
+		PolicyEngine::init();
+		ProtocolTransmit::init();
+		ProtocolReceive::init();
+		InterruptHandler::init();
+	}
+}
+#endif

source/Core/Drivers/FUSB302/fusbpd.h

@@ -0,0 +1,18 @@
+/*
+ * fusbpd.h
+ *
+ *  Created on: 13 Jun 2020
+ *      Author: Ralim
+ */
+
+#ifndef DRIVERS_FUSB302_FUSBPD_H_
+#define DRIVERS_FUSB302_FUSBPD_H_
+//Wrapper for all of the FUSB302 PD work
+extern struct pdb_config pdb_config_data;
+#include <stdint.h>
+
+//returns 1 if the FUSB302 is on the I2C bus
+uint8_t fusb302_detect();
+
+void fusb302_start_processing();
+#endif /* DRIVERS_FUSB302_FUSBPD_H_ */

source/Core/Drivers/FUSB302/int_n.cpp

@@ -0,0 +1,80 @@
+/*
+ * PD Buddy Firmware Library - USB Power Delivery for everyone
+ * Copyright 2017-2018 Clayton G. Hobbs
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "int_n.h"
+#include "fusbpd.h"
+#include <pd.h>
+#include "fusb302b.h"
+#include "protocol_rx.h"
+#include "protocol_tx.h"
+#include "policy_engine.h"
+#include "protocol_rx.h"
+#include "protocol_tx.h"
+#include "task.h"
+#include "BSP.h"
+
+osThreadId InterruptHandler::TaskHandle = NULL;
+uint32_t InterruptHandler::TaskBuffer[InterruptHandler::TaskStackSize];
+osStaticThreadDef_t InterruptHandler::TaskControlBlock;
+
+void InterruptHandler::init() {
+	osThreadStaticDef(intTask, Thread, PDB_PRIO_PRL_INT_N, 0, TaskStackSize, TaskBuffer, &TaskControlBlock);
+	TaskHandle = osThreadCreate(osThread(intTask), NULL);
+}
+
+void InterruptHandler::Thread(const void *arg) {
+	(void) arg;
+	union fusb_status status;
+	while (true) {
+		/* If the INT_N line is low */
+		if (xTaskNotifyWait(0x00, 0x0F, NULL, PolicyEngine::setupCompleteOrTimedOut() ? 1000 : 10) == pdPASS) {
+			//delay slightly so we catch the crc with better timing
+			osDelay(1);
+		}
+		/* Read the FUSB302B status and interrupt registers */
+		fusb_get_status(&status);
+		/* If the I_TXSENT or I_RETRYFAIL flag is set, tell the Protocol TX
+		 * thread */
+		if (status.interrupta & FUSB_INTERRUPTA_I_TXSENT) {
+			ProtocolTransmit::notify(ProtocolTransmit::Notifications::PDB_EVT_PRLTX_I_TXSENT);
+		}
+		if (status.interrupta & FUSB_INTERRUPTA_I_RETRYFAIL) {
+			ProtocolTransmit::notify(ProtocolTransmit::Notifications::PDB_EVT_PRLTX_I_RETRYFAIL);
+		}
+
+		/* If the I_GCRCSENT flag is set, tell the Protocol RX thread */
+		//This means a message was recieved with a good CRC
+		if (status.interruptb & FUSB_INTERRUPTB_I_GCRCSENT) {
+			ProtocolReceive::notify(PDB_EVT_PRLRX_I_GCRCSENT);
+		}
+
+		/* If the I_OCP_TEMP and OVRTEMP flags are set, tell the Policy
+		 * Engine thread */
+		if ((status.interrupta & FUSB_INTERRUPTA_I_OCP_TEMP) && (status.status1 & FUSB_STATUS1_OVRTEMP)) {
+			PolicyEngine::notify(PDB_EVT_PE_I_OVRTEMP);
+		}
+	}
+}
+void InterruptHandler::irqCallback() {
+	if (xTaskGetSchedulerState() != taskSCHEDULER_NOT_STARTED) {
+		if (TaskHandle != NULL) {
+			BaseType_t taskWoke = pdFALSE;
+			xTaskNotifyFromISR(TaskHandle, 0x01, eNotifyAction::eSetBits, &taskWoke);
+			portYIELD_FROM_ISR(taskWoke);
+		}
+	}
+}

source/Core/Drivers/FUSB302/int_n.h

@@ -0,0 +1,57 @@
+/*
+ * PD Buddy Firmware Library - USB Power Delivery for everyone
+ * Copyright 2017-2018 Clayton G. Hobbs
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef PDB_INT_N_OLD_H
+#define PDB_INT_N_OLD_H
+
+#include <pd.h>
+
+class InterruptHandler {
+public:
+	//Creates the thread to handle the Interrupt pin
+	static void init();
+
+
+	static void irqCallback();
+private:
+	static void Thread(const void *arg);
+	static osThreadId TaskHandle;
+	static const size_t TaskStackSize = 1536 / 3;
+	static uint32_t TaskBuffer[TaskStackSize];
+	static osStaticThreadDef_t TaskControlBlock;
+	/*
+	 * Hard Reset machine states
+	 */
+	enum hardrst_state {
+		PRLHRResetLayer,
+		PRLHRIndicateHardReset,
+		PRLHRRequestHardReset,
+		PRLHRWaitPHY,
+		PRLHRHardResetRequested,
+		PRLHRWaitPE,
+		PRLHRComplete
+	};
+	static enum hardrst_state hardrst_reset_layer();
+	static enum hardrst_state hardrst_indicate_hard_reset();
+	static enum hardrst_state hardrst_request_hard_reset();
+	static enum hardrst_state hardrst_wait_phy();
+	static enum hardrst_state hardrst_hard_reset_requested();
+	static enum hardrst_state hardrst_wait_pe();
+	static enum hardrst_state hardrst_complete();
+};
+
+#endif /* PDB_INT_N_OLD_H */

source/Core/Drivers/FUSB302/pd.h

@@ -0,0 +1,400 @@
+/*
+ * PD Buddy Firmware Library - USB Power Delivery for everyone
+ * Copyright 2017-2018 Clayton G. Hobbs
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef PDB_PD_H
+#define PDB_PD_H
+
+#include <stdint.h>
+#include "FreeRTOS.h"
+#include "pdb_msg.h"
+#include "cmsis_os.h"
+#include "pdb_conf.h"
+/*
+ * Macros for working with USB Power Delivery messages.
+ *
+ * This file is mostly written from the PD Rev. 2.0 spec, but the header is
+ * written from the Rev. 3.0 spec.
+ */
+
+/*
+ * PD Header
+ */
+#define PD_HDR_MSGTYPE_SHIFT 0
+#define PD_HDR_MSGTYPE (0x1F << PD_HDR_MSGTYPE_SHIFT)
+#define PD_HDR_DATAROLE_SHIFT 5
+#define PD_HDR_DATAROLE (0x1 << PD_HDR_DATAROLE_SHIFT)
+#define PD_HDR_SPECREV_SHIFT 6
+#define PD_HDR_SPECREV (0x3 << PD_HDR_SPECREV_SHIFT)
+#define PD_HDR_POWERROLE_SHIFT 8
+#define PD_HDR_POWERROLE (1 << PD_HDR_POWERROLE_SHIFT)
+#define PD_HDR_MESSAGEID_SHIFT 9
+#define PD_HDR_MESSAGEID (0x7 << PD_HDR_MESSAGEID_SHIFT)
+#define PD_HDR_NUMOBJ_SHIFT 12
+#define PD_HDR_NUMOBJ (0x7 << PD_HDR_NUMOBJ_SHIFT)
+#define PD_HDR_EXT (1 << 15)
+
+/* Message types */
+#define PD_MSGTYPE_GET(msg) (((msg)->hdr & PD_HDR_MSGTYPE) >> PD_HDR_MSGTYPE_SHIFT)
+/* Control Message */
+#define PD_MSGTYPE_GOODCRC 0x01
+#define PD_MSGTYPE_GOTOMIN 0x02
+#define PD_MSGTYPE_ACCEPT 0x03
+#define PD_MSGTYPE_REJECT 0x04
+#define PD_MSGTYPE_PING 0x05
+#define PD_MSGTYPE_PS_RDY 0x06
+#define PD_MSGTYPE_GET_SOURCE_CAP 0x07
+#define PD_MSGTYPE_GET_SINK_CAP 0x08
+#define PD_MSGTYPE_DR_SWAP 0x09
+#define PD_MSGTYPE_PR_SWAP 0x0A
+#define PD_MSGTYPE_VCONN_SWAP 0x0B
+#define PD_MSGTYPE_WAIT 0x0C
+#define PD_MSGTYPE_SOFT_RESET 0x0D
+#define PD_MSGTYPE_NOT_SUPPORTED 0x10
+#define PD_MSGTYPE_GET_SOURCE_CAP_EXTENDED 0x11
+#define PD_MSGTYPE_GET_STATUS 0x12
+#define PD_MSGTYPE_FR_SWAP 0x13
+#define PD_MSGTYPE_GET_PPS_STATUS 0x14
+#define PD_MSGTYPE_GET_COUNTRY_CODES 0x15
+/* Data Message */
+#define PD_MSGTYPE_SOURCE_CAPABILITIES 0x01
+#define PD_MSGTYPE_REQUEST 0x02
+#define PD_MSGTYPE_BIST 0x03
+#define PD_MSGTYPE_SINK_CAPABILITIES 0x04
+#define PD_MSGTYPE_BATTERY_STATUS 0x05
+#define PD_MSGTYPE_ALERT 0x06
+#define PD_MSGTYPE_GET_COUNTRY_INFO 0x07
+#define PD_MSGTYPE_VENDOR_DEFINED 0x0F
+/* Extended Message */
+#define PD_MSGTYPE_SOURCE_CAPABILITIES_EXTENDED 0x01
+#define PD_MSGTYPE_STATUS 0x02
+#define PD_MSGTYPE_GET_BATTERY_CAP 0x03
+#define PD_MSGTYPE_GET_BATTERY_STATUS 0x04
+#define PD_MSGTYPE_BATTERY_CAPABILITIES 0x05
+#define PD_MSGTYPE_GET_MANUFACTURER_INFO 0x06
+#define PD_MSGTYPE_MANUFACTURER_INFO 0x07
+#define PD_MSGTYPE_SECURITY_REQUEST 0x08
+#define PD_MSGTYPE_SECURITY_RESPONSE 0x09
+#define PD_MSGTYPE_FIRMWARE_UPDATE_REQUEST 0x0A
+#define PD_MSGTYPE_FIRMWARE_UPDATE_RESPONSE 0x0B
+#define PD_MSGTYPE_PPS_STATUS 0x0C
+#define PD_MSGTYPE_COUNTRY_INFO 0x0D
+#define PD_MSGTYPE_COUNTRY_CODES 0x0E
+
+/* Data roles */
+#define PD_DATAROLE_UFP (0x0 << PD_HDR_DATAROLE_SHIFT)
+#define PD_DATAROLE_DFP (0x1 << PD_HDR_DATAROLE_SHIFT)
+
+/* Specification revisions */
+#define PD_SPECREV_1_0 (0x0 << PD_HDR_SPECREV_SHIFT)
+#define PD_SPECREV_2_0 (0x1 << PD_HDR_SPECREV_SHIFT)
+#define PD_SPECREV_3_0 (0x2 << PD_HDR_SPECREV_SHIFT)
+
+/* Port power roles */
+#define PD_POWERROLE_SINK (0x0 << PD_HDR_POWERROLE_SHIFT)
+#define PD_POWERROLE_SOURCE (0x1 << PD_HDR_POWERROLE_SHIFT)
+
+/* Message ID */
+#define PD_MESSAGEID_GET(msg) (((msg)->hdr & PD_HDR_MESSAGEID) >> PD_HDR_MESSAGEID_SHIFT)
+
+/* Number of data objects */
+#define PD_NUMOBJ(n) (((n) << PD_HDR_NUMOBJ_SHIFT) & PD_HDR_NUMOBJ)
+#define PD_NUMOBJ_GET(msg) (((msg)->hdr & PD_HDR_NUMOBJ) >> PD_HDR_NUMOBJ_SHIFT)
+
+/*
+ * PD Extended Message Header
+ */
+#define PD_EXTHDR_DATA_SIZE_SHIFT 0
+#define PD_EXTHDR_DATA_SIZE (0x1FF << PD_EXTHDR_DATA_SIZE_SHIFT)
+#define PD_EXTHDR_REQUEST_CHUNK_SHIFT 10
+#define PD_EXTHDR_REQUEST_CHUNK (1 << PD_EXTHDR_REQUEST_CHUNK_SHIFT)
+#define PD_EXTHDR_CHUNK_NUMBER_SHIFT 11
+#define PD_EXTHDR_CHUNK_NUMBER (0xF << PD_EXTHDR_CHUNK_NUMBER_SHIFT)
+#define PD_EXTHDR_CHUNKED_SHIFT 15
+#define PD_EXTHDR_CHUNKED (1 << PD_EXTHDR_CHUNKED_SHIFT)
+
+/* Data size */
+#define PD_DATA_SIZE(n) (((n) << PD_EXTHDR_DATA_SIZE_SHIFT) & PD_EXTHDR_DATA_SIZE)
+#define PD_DATA_SIZE_GET(msg) (((msg)->exthdr & PD_EXTHDR_DATA_SIZE) >> PD_EXTHDR_DATA_SIZE_SHIFT)
+
+/* Chunk number */
+#define PD_CHUNK_NUMBER(n) (((n) << PD_EXTHDR_CHUNK_NUMBER_SHIFT) & PD_EXTHDR_CHUNK_NUMBER)
+#define PD_CHUNK_NUMBER_GET(msg) (((msg)->exthdr & PD_EXTHDR_CHUNK_NUMBER) >> PD_EXTHDR_CHUNK_NUMBER_SHIFT)
+
+/*
+ * PD Power Data Object
+ */
+#define PD_PDO_TYPE_SHIFT 30
+#define PD_PDO_TYPE (0x3 << PD_PDO_TYPE_SHIFT)
+
+/* PDO types */
+#define PD_PDO_TYPE_FIXED ((unsigned) (0x0 << PD_PDO_TYPE_SHIFT))
+#define PD_PDO_TYPE_BATTERY ((unsigned) (0x1 << PD_PDO_TYPE_SHIFT))
+#define PD_PDO_TYPE_VARIABLE ((unsigned) (0x2 << PD_PDO_TYPE_SHIFT))
+#define PD_PDO_TYPE_AUGMENTED ((unsigned) (0x3 << PD_PDO_TYPE_SHIFT))
+
+#define PD_APDO_TYPE_SHIFT 28
+#define PD_APDO_TYPE (0x3 << PD_APDO_TYPE_SHIFT)
+
+/* APDO types */
+#define PD_APDO_TYPE_PPS (0x0 << PD_APDO_TYPE_SHIFT)
+
+/* PD Source Fixed PDO */
+#define PD_PDO_SRC_FIXED_DUAL_ROLE_PWR_SHIFT 29
+#define PD_PDO_SRC_FIXED_DUAL_ROLE_PWR (1 << PD_PDO_SRC_FIXED_DUAL_ROLE_PWR_SHIFT)
+#define PD_PDO_SRC_FIXED_USB_SUSPEND_SHIFT 28
+#define PD_PDO_SRC_FIXED_USB_SUSPEND (1 << PD_PDO_SRC_FIXED_USB_SUSPEND_SHIFT)
+#define PD_PDO_SRC_FIXED_UNCONSTRAINED_SHIFT 27
+#define PD_PDO_SRC_FIXED_UNCONSTRAINED (1 << PD_PDO_SRC_FIXED_UNCONSTRAINED_SHIFT)
+#define PD_PDO_SRC_FIXED_USB_COMMS_SHIFT 26
+#define PD_PDO_SRC_FIXED_USB_COMMS (1 << PD_PDO_SRC_FIXED_USB_COMMS_SHIFT)
+#define PD_PDO_SRC_FIXED_DUAL_ROLE_DATA_SHIFT 25
+#define PD_PDO_SRC_FIXED_DUAL_ROLE_DATA (1 << PD_PDO_SRC_FIXED_DUAL_ROLE_DATA_SHIFT)
+#define PD_PDO_SRC_FIXED_UNCHUNKED_EXT_MSG_SHIFT 24
+#define PD_PDO_SRC_FIXED_UNCHUNKED_EXT_MSG (1 << PD_PDO_SRC_FIXED_UNCHUNKED_EXT_MSG_SHIFT)
+#define PD_PDO_SRC_FIXED_PEAK_CURRENT_SHIFT 20
+#define PD_PDO_SRC_FIXED_PEAK_CURRENT (0x3 << PD_PDO_SRC_FIXED_PEAK_CURRENT_SHIFT)
+#define PD_PDO_SRC_FIXED_VOLTAGE_SHIFT 10
+#define PD_PDO_SRC_FIXED_VOLTAGE (0x3FF << PD_PDO_SRC_FIXED_VOLTAGE_SHIFT)
+#define PD_PDO_SRC_FIXED_CURRENT_SHIFT 0
+#define PD_PDO_SRC_FIXED_CURRENT (0x3FF << PD_PDO_SRC_FIXED_CURRENT_SHIFT)
+
+/* PD Source Fixed PDO current */
+#define PD_PDO_SRC_FIXED_CURRENT_GET(pdo) (((pdo) & PD_PDO_SRC_FIXED_CURRENT) >> PD_PDO_SRC_FIXED_CURRENT_SHIFT)
+
+/* PD Source Fixed PDO voltage */
+#define PD_PDO_SRC_FIXED_VOLTAGE_GET(pdo) (((pdo) & PD_PDO_SRC_FIXED_VOLTAGE) >> PD_PDO_SRC_FIXED_VOLTAGE_SHIFT)
+
+/* PD Programmable Power Supply APDO */
+#define PD_APDO_PPS_MAX_VOLTAGE_SHIFT 17
+#define PD_APDO_PPS_MAX_VOLTAGE (0xFF << PD_APDO_PPS_MAX_VOLTAGE_SHIFT)
+#define PD_APDO_PPS_MIN_VOLTAGE_SHIFT 8
+#define PD_APDO_PPS_MIN_VOLTAGE (0xFF << PD_APDO_PPS_MIN_VOLTAGE_SHIFT)
+#define PD_APDO_PPS_CURRENT_SHIFT 0
+#define PD_APDO_PPS_CURRENT (0x7F << PD_APDO_PPS_CURRENT_SHIFT)
+
+/* PD Programmable Power Supply APDO voltages */
+#define PD_APDO_PPS_MAX_VOLTAGE_GET(pdo) (((pdo) & PD_APDO_PPS_MAX_VOLTAGE) >> PD_APDO_PPS_MAX_VOLTAGE_SHIFT)
+#define PD_APDO_PPS_MIN_VOLTAGE_GET(pdo) (((pdo) & PD_APDO_PPS_MIN_VOLTAGE) >> PD_APDO_PPS_MIN_VOLTAGE_SHIFT)
+
+#define PD_APDO_PPS_MAX_VOLTAGE_SET(v) (((v) << PD_APDO_PPS_MAX_VOLTAGE_SHIFT) & PD_APDO_PPS_MAX_VOLTAGE)
+#define PD_APDO_PPS_MIN_VOLTAGE_SET(v) (((v) << PD_APDO_PPS_MIN_VOLTAGE_SHIFT) & PD_APDO_PPS_MIN_VOLTAGE)
+
+/* PD Programmable Power Supply APDO current */
+#define PD_APDO_PPS_CURRENT_GET(pdo) ((uint8_t) (((pdo) & PD_APDO_PPS_CURRENT) >> PD_APDO_PPS_CURRENT_SHIFT))
+
+#define PD_APDO_PPS_CURRENT_SET(i) (((i) << PD_APDO_PPS_CURRENT_SHIFT) & PD_APDO_PPS_CURRENT)
+
+
+/* PD Sink Fixed PDO */
+#define PD_PDO_SNK_FIXED_DUAL_ROLE_PWR_SHIFT 29
+#define PD_PDO_SNK_FIXED_DUAL_ROLE_PWR (1 << PD_PDO_SNK_FIXED_DUAL_ROLE_PWR_SHIFT)
+#define PD_PDO_SNK_FIXED_HIGHER_CAP_SHIFT 28
+#define PD_PDO_SNK_FIXED_HIGHER_CAP (1 << PD_PDO_SNK_FIXED_HIGHER_CAP_SHIFT)
+#define PD_PDO_SNK_FIXED_UNCONSTRAINED_SHIFT 27
+#define PD_PDO_SNK_FIXED_UNCONSTRAINED (1 << PD_PDO_SNK_FIXED_UNCONSTRAINED_SHIFT)
+#define PD_PDO_SNK_FIXED_USB_COMMS_SHIFT 26
+#define PD_PDO_SNK_FIXED_USB_COMMS (1 << PD_PDO_SNK_FIXED_USB_COMMS_SHIFT)
+#define PD_PDO_SNK_FIXED_DUAL_ROLE_DATA_SHIFT 25
+#define PD_PDO_SNK_FIXED_DUAL_ROLE_DATA (1 << PD_PDO_SNK_FIXED_DUAL_ROLE_DATA_SHIFT)
+#define PD_PDO_SNK_FIXED_VOLTAGE_SHIFT 10
+#define PD_PDO_SNK_FIXED_VOLTAGE (0x3FF << PD_PDO_SNK_FIXED_VOLTAGE_SHIFT)
+#define PD_PDO_SNK_FIXED_CURRENT_SHIFT 0
+#define PD_PDO_SNK_FIXED_CURRENT (0x3FF << PD_PDO_SNK_FIXED_CURRENT_SHIFT)
+
+/* PD Sink Fixed PDO current */
+#define PD_PDO_SNK_FIXED_CURRENT_SET(i) (((i) << PD_PDO_SNK_FIXED_CURRENT_SHIFT) & PD_PDO_SNK_FIXED_CURRENT)
+
+/* PD Sink Fixed PDO voltage */
+#define PD_PDO_SNK_FIXED_VOLTAGE_SET(v) (((v) << PD_PDO_SNK_FIXED_VOLTAGE_SHIFT) & PD_PDO_SNK_FIXED_VOLTAGE)
+
+
+/*
+ * PD Request Data Object
+ */
+#define PD_RDO_OBJPOS_SHIFT 28
+#define PD_RDO_OBJPOS (0x7 << PD_RDO_OBJPOS_SHIFT)
+#define PD_RDO_GIVEBACK_SHIFT 27
+#define PD_RDO_GIVEBACK (1 << PD_RDO_GIVEBACK_SHIFT)
+#define PD_RDO_CAP_MISMATCH_SHIFT 26
+#define PD_RDO_CAP_MISMATCH (1 << PD_RDO_CAP_MISMATCH_SHIFT)
+#define PD_RDO_USB_COMMS_SHIFT 25
+#define PD_RDO_USB_COMMS (1 << PD_RDO_USB_COMMS_SHIFT)
+#define PD_RDO_NO_USB_SUSPEND_SHIFT 24
+#define PD_RDO_NO_USB_SUSPEND (1 << PD_RDO_NO_USB_SUSPEND_SHIFT)
+#define PD_RDO_UNCHUNKED_EXT_MSG_SHIFT 23
+#define PD_RDO_UNCHUNKED_EXT_MSG (1 << PD_RDO_UNCHUNKED_EXT_MSG_SHIFT)
+
+#define PD_RDO_OBJPOS_SET(i) (((i) << PD_RDO_OBJPOS_SHIFT) & PD_RDO_OBJPOS)
+#define PD_RDO_OBJPOS_GET(msg) (((msg)->obj[0] & PD_RDO_OBJPOS) >> PD_RDO_OBJPOS_SHIFT)
+
+/* Fixed and Variable RDO, no GiveBack support */
+#define PD_RDO_FV_CURRENT_SHIFT 10
+#define PD_RDO_FV_CURRENT (0x3FF << PD_RDO_FV_CURRENT_SHIFT)
+#define PD_RDO_FV_MAX_CURRENT_SHIFT 0
+#define PD_RDO_FV_MAX_CURRENT (0x3FF << PD_RDO_FV_MAX_CURRENT_SHIFT)
+
+#define PD_RDO_FV_CURRENT_SET(i) (((i) << PD_RDO_FV_CURRENT_SHIFT) & PD_RDO_FV_CURRENT)
+#define PD_RDO_FV_MAX_CURRENT_SET(i) (((i) << PD_RDO_FV_MAX_CURRENT_SHIFT) & PD_RDO_FV_MAX_CURRENT)
+
+/* Fixed and Variable RDO with GiveBack support */
+#define PD_RDO_FV_MIN_CURRENT_SHIFT 0
+#define PD_RDO_FV_MIN_CURRENT (0x3FF << PD_RDO_FV_MIN_CURRENT_SHIFT)
+
+#define PD_RDO_FV_MIN_CURRENT_SET(i) (((i) << PD_RDO_FV_MIN_CURRENT_SHIFT) & PD_RDO_FV_MIN_CURRENT)
+
+/* TODO: Battery RDOs */
+
+/* Programmable RDO */
+#define PD_RDO_PROG_VOLTAGE_SHIFT 9
+#define PD_RDO_PROG_VOLTAGE (0x7FF << PD_RDO_PROG_VOLTAGE_SHIFT)
+#define PD_RDO_PROG_CURRENT_SHIFT 0
+#define PD_RDO_PROG_CURRENT (0x7F << PD_RDO_PROG_CURRENT_SHIFT)
+
+#define PD_RDO_PROG_VOLTAGE_SET(i) (((i) << PD_RDO_PROG_VOLTAGE_SHIFT) & PD_RDO_PROG_VOLTAGE)
+#define PD_RDO_PROG_CURRENT_SET(i) (((i) << PD_RDO_PROG_CURRENT_SHIFT) & PD_RDO_PROG_CURRENT)
+
+/*
+ * Time values
+ *
+ * Where a range is specified, the middle of the range (rounded down to the
+ * nearest millisecond) is used.
+ */
+#define PD_T_CHUNKING_NOT_SUPPORTED (450)
+#define PD_T_HARD_RESET_COMPLETE (1000)
+#define PD_T_PS_TRANSITION (5000)
+#define PD_T_SENDER_RESPONSE (2700)
+#define PD_T_SINK_REQUEST (1000)
+#define PD_T_TYPEC_SINK_WAIT_CAP (1000)
+#define PD_T_PD_DEBOUNCE (2000)
+
+/*
+ * Counter maximums
+ */
+#define PD_N_HARD_RESET_COUNT 2
+
+/*
+ * Value parameters
+ */
+#define PD_MAX_EXT_MSG_LEN 260
+#define PD_MAX_EXT_MSG_CHUNK_LEN 26
+#define PD_MAX_EXT_MSG_LEGACY_LEN 26
+
+/*
+ * Unit conversions
+ *
+ * V: volt
+ * CV: centivolt
+ * MV: millivolt
+ * PRV: Programmable RDO voltage unit (20 mV)
+ * PDV: Power Delivery voltage unit (50 mV)
+ * PAV: PPS APDO voltage unit (100 mV)
+ *
+ * A: ampere
+ * CA: centiampere
+ * MA: milliampere
+ * PDI: Power Delivery current unit (10 mA)
+ * PAI: PPS APDO current unit (50 mA)
+ *
+ * W: watt
+ * CW: centiwatt
+ * MW: milliwatt
+ *
+ * O: ohm
+ * CO: centiohm
+ * MO: milliohm
+ */
+#define PD_MV2PRV(mv) ((mv) / 20)
+#define PD_MV2PDV(mv) ((mv) / 50)
+#define PD_MV2PAV(mv) ((mv) / 100)
+#define PD_PRV2MV(prv) ((prv) * 20)
+#define PD_PDV2MV(pdv) ((pdv) * 50)
+#define PD_PAV2MV(pav) ((pav) * 100)
+
+#define PD_MA2CA(ma) (((ma) + 10 - 1) / 10)
+#define PD_MA2PDI(ma) (((ma) + 10 - 1) / 10)
+#define PD_MA2PAI(ma) (((ma) + 50 - 1) / 50)
+#define PD_CA2PAI(ca) (((ca) + 5 - 1) / 5)
+#define PD_PDI2MA(pdi) ((pdi) * 10)
+#define PD_PAI2MA(pai) ((pai) * 50)
+#define PD_PAI2CA(pai) ((pai) * 5)
+
+#define PD_MW2CW(mw) ((mw) / 10)
+
+#define PD_MO2CO(mo) ((mo) / 10)
+
+/* Get portions of a voltage in more normal units */
+#define PD_MV_V(mv) ((mv) / 1000)
+#define PD_MV_MV(mv) ((mv) % 1000)
+
+#define PD_PDV_V(pdv) ((pdv) / 20)
+#define PD_PDV_CV(pdv) (5 * ((pdv) % 20))
+
+#define PD_PAV_V(pav) ((pav) / 10)
+#define PD_PAV_CV(pav) (10 * ((pav) % 10))
+
+/* Get portions of a PD current in more normal units */
+#define PD_PDI_A(pdi) ((pdi) / 100)
+#define PD_PDI_CA(pdi) ((pdi) % 100)
+
+#define PD_PAI_A(pai) ((pai) / 20)
+#define PD_PAI_CA(pai) (5 * ((pai) % 20))
+
+/* Get portions of a power in more normal units */
+#define PD_CW_W(cw) ((cw) / 100)
+#define PD_CW_CW(cw) ((cw) % 100)
+
+/* Get portions of a resistance in more normal units */
+#define PD_CO_O(co) ((co) / 100)
+#define PD_CO_CO(co) ((co) % 100)
+
+/*
+ * Unit constants
+ */
+#define PD_MV_MIN 0
+#define PD_MV_MAX 21000
+#define PD_PDV_MIN PD_MV2PDV(PD_MV_MIN)
+#define PD_PDV_MAX PD_MV2PDV(PD_MV_MAX)
+
+#define PD_MA_MIN 0
+#define PD_MA_MAX 5000
+#define PD_CA_MIN PD_MA2CA(PD_MA_MIN)
+#define PD_CA_MAX PD_MA2CA(PD_MA_MAX)
+#define PD_PDI_MIN PD_MA2PDI(PD_MA_MIN)
+#define PD_PDI_MAX PD_MA2PDI(PD_MA_MAX)
+
+#define PD_MW_MIN 0
+#define PD_MW_MAX 100000
+
+#define PD_MO_MIN 500
+#define PD_MO_MAX 655350
+
+
+/*
+ * FUSB Type-C Current level enum
+ */
+enum fusb_typec_current {
+    fusb_tcc_none = 0,
+    fusb_tcc_default = 1,
+    fusb_tcc_1_5 = 2,
+    fusb_sink_tx_ng = 2,
+    fusb_tcc_3_0 = 3,
+    fusb_sink_tx_ok = 3
+};
+
+
+
+#endif /* PDB_PD_H */

source/Core/Drivers/FUSB302/pdb_conf.h

@@ -0,0 +1,32 @@
+/*
+ * PD Buddy Firmware Library - USB Power Delivery for everyone
+ * Copyright 2017-2018 Clayton G. Hobbs
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef PDB_CONF_H
+#define PDB_CONF_H
+
+
+/* Number of messages in the message pool */
+#define PDB_MSG_POOL_SIZE 4
+
+#define EVENT_MASK(x) (1<<x)
+#define eventmask_t uint32_t
+/* PD Buddy thread priorities */
+#define PDB_PRIO_PE (osPriorityNormal)
+#define PDB_PRIO_PRL (osPriorityBelowNormal)
+#define PDB_PRIO_PRL_INT_N (osPriorityLow)
+
+#endif /* PDB_CONF_H */

source/Core/Drivers/FUSB302/pdb_msg.h

@@ -0,0 +1,55 @@
+/*
+ * PD Buddy Firmware Library - USB Power Delivery for everyone
+ * Copyright 2017-2018 Clayton G. Hobbs
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef PDB_MSG_H
+#define PDB_MSG_H
+
+#include <stdint.h>
+
+
+
+/*
+ * PD message union
+ *
+ * This can be safely read from or written to in any form without any
+ * transformations because everything in the system is little-endian.
+ *
+ * Two bytes of padding are required at the start to prevent problems due to
+ * alignment.  Specifically, without the padding, &obj[0] != &bytes[2], making
+ * the statement in the previous paragraph invalid.
+ */
+union pd_msg {
+    struct {
+        uint8_t _pad1[2];
+        uint8_t bytes[30];
+    } __attribute__((packed));
+    struct {
+        uint8_t _pad2[2];
+        uint16_t hdr;
+        union {
+            uint32_t obj[7];
+            struct {
+                uint16_t exthdr;
+                uint8_t data[26];
+            };
+        };
+    } __attribute__((packed));
+};
+
+
+
+#endif /* PDB_MSG_H */

source/Core/Drivers/FUSB302/policy_engine.cpp

@@ -0,0 +1,692 @@
+/*
+ * PD Buddy Firmware Library - USB Power Delivery for everyone
+ * Copyright 2017-2018 Clayton G. Hobbs
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "policy_engine.h"
+#include <stdbool.h>
+#include "int_n.h"
+#include <pd.h>
+#include "protocol_tx.h"
+#include "fusb302b.h"
+bool PolicyEngine::pdNegotiationComplete;
+int PolicyEngine::current_voltage_mv;
+int PolicyEngine::_requested_voltage;
+bool PolicyEngine::_unconstrained_power;
+union pd_msg PolicyEngine::currentMessage;
+uint16_t PolicyEngine::hdr_template;
+bool PolicyEngine::_explicit_contract;
+int8_t PolicyEngine::_hard_reset_counter;
+int8_t PolicyEngine::_old_tcc_match;
+uint8_t PolicyEngine::_pps_index;
+uint8_t PolicyEngine::_last_pps;
+osThreadId PolicyEngine::TaskHandle = NULL;
+uint32_t PolicyEngine::TaskBuffer[PolicyEngine::TaskStackSize];
+osStaticThreadDef_t PolicyEngine::TaskControlBlock;
+union pd_msg PolicyEngine::tempMessage;
+union pd_msg PolicyEngine::_last_dpm_request;
+PolicyEngine::policy_engine_state PolicyEngine::state = PESinkStartup;
+StaticQueue_t PolicyEngine::xStaticQueue;
+uint8_t PolicyEngine::ucQueueStorageArea[PDB_MSG_POOL_SIZE
+		* sizeof(union pd_msg)];
+QueueHandle_t PolicyEngine::messagesWaiting = NULL;
+EventGroupHandle_t PolicyEngine::xEventGroupHandle = NULL;
+StaticEventGroup_t PolicyEngine::xCreatedEventGroup;
+void PolicyEngine::init() {
+	messagesWaiting = xQueueCreateStatic(PDB_MSG_POOL_SIZE,
+			sizeof(union pd_msg), ucQueueStorageArea, &xStaticQueue);
+	//Create static thread at PDB_PRIO_PE priority
+	osThreadStaticDef(PolEng, pe_task, PDB_PRIO_PE, 0, TaskStackSize,
+			TaskBuffer, &TaskControlBlock);
+	TaskHandle = osThreadCreate(osThread(PolEng), NULL);
+	xEventGroupHandle = xEventGroupCreateStatic(&xCreatedEventGroup);
+}
+
+void PolicyEngine::notify(uint32_t notification) {
+	if (xEventGroupHandle != NULL) {
+		xEventGroupSetBits(xEventGroupHandle, notification);
+	}
+}
+
+void PolicyEngine::pe_task(const void *arg) {
+	(void) arg;
+//Internal thread loop
+	hdr_template = PD_DATAROLE_UFP | PD_POWERROLE_SINK;
+	/* Initialize the old_tcc_match */
+	_old_tcc_match = -1;
+	/* Initialize the pps_index */
+	_pps_index = 8;
+	/* Initialize the last_pps */
+	_last_pps = 8;
+
+	for (;;) {
+		//Loop based on state
+		switch (state) {
+
+		case PESinkStartup:
+			state = pe_sink_startup();
+			break;
+		case PESinkDiscovery:
+			state = pe_sink_discovery();
+			break;
+		case PESinkWaitCap:
+			state = pe_sink_wait_cap();
+			break;
+		case PESinkEvalCap:
+			state = pe_sink_eval_cap();
+			break;
+		case PESinkSelectCap:
+			state = pe_sink_select_cap();
+			break;
+		case PESinkTransitionSink:
+			state = pe_sink_transition_sink();
+			break;
+		case PESinkReady:
+			state = pe_sink_ready();
+			break;
+		case PESinkGetSourceCap:
+			state = pe_sink_get_source_cap();
+			break;
+		case PESinkGiveSinkCap:
+			state = pe_sink_give_sink_cap();
+			break;
+		case PESinkHardReset:
+			state = pe_sink_hard_reset();
+			break;
+		case PESinkTransitionDefault:
+			state = pe_sink_transition_default();
+			break;
+		case PESinkSoftReset:
+			state = pe_sink_soft_reset();
+			break;
+		case PESinkSendSoftReset:
+			state = pe_sink_send_soft_reset();
+			break;
+		case PESinkSendNotSupported:
+			state = pe_sink_send_not_supported();
+			break;
+		case PESinkChunkReceived:
+			state = pe_sink_chunk_received();
+			break;
+		case PESinkSourceUnresponsive:
+			state = pe_sink_source_unresponsive();
+			break;
+		case PESinkNotSupportedReceived:
+			state = pe_sink_not_supported_received();
+			break;
+		default:
+			state = PESinkStartup;
+			break;
+		}
+	}
+}
+
+PolicyEngine::policy_engine_state PolicyEngine::pe_sink_startup() {
+	/* We don't have an explicit contract currently */
+	_explicit_contract = false;
+
+//If desired could send an alert that PD is starting
+
+	/* No need to reset the protocol layer here.  There are two ways into this
+	 * state: startup and exiting hard reset.  On startup, the protocol layer
+	 * is reset by the startup procedure.  When exiting hard reset, the
+	 * protocol layer is reset by the hard reset state machine.  Since it's
+	 * already done somewhere else, there's no need to do it again here. */
+
+	return PESinkDiscovery;
+}
+
+PolicyEngine::policy_engine_state PolicyEngine::pe_sink_discovery() {
+	/* Wait for VBUS.  Since it's our only power source, we already know that
+	 * we have it, so just move on. */
+
+	return PESinkWaitCap;
+}
+
+PolicyEngine::policy_engine_state PolicyEngine::pe_sink_wait_cap() {
+	/* Fetch a message from the protocol layer */
+	eventmask_t evt = 0;
+	if (readMessage()) {
+		evt = PDB_EVT_PE_MSG_RX_PEND;
+	} else {
+		evt = waitForEvent(
+		PDB_EVT_PE_MSG_RX | PDB_EVT_PE_I_OVRTEMP | PDB_EVT_PE_RESET,
+		//Wait for cap timeout
+				PD_T_TYPEC_SINK_WAIT_CAP);
+	}
+	/* If we timed out waiting for Source_Capabilities, send a hard reset */
+	if (evt == 0) {
+		return PESinkHardReset;
+	}
+	/* If we got reset signaling, transition to default */
+	if (evt & PDB_EVT_PE_RESET) {
+		return PESinkWaitCap;
+	}
+	/* If we're too hot, we shouldn't negotiate power yet */
+	if (evt & PDB_EVT_PE_I_OVRTEMP) {
+		return PESinkWaitCap;
+	}
+
+	/* If we got a message */
+	if (evt & (PDB_EVT_PE_MSG_RX | PDB_EVT_PE_MSG_RX_PEND)) {
+		/* Get the message */
+		while ((evt & PDB_EVT_PE_MSG_RX_PEND) || readMessage() == true) {
+			/* If we got a Source_Capabilities message, read it. */
+			if (PD_MSGTYPE_GET(&tempMessage) == PD_MSGTYPE_SOURCE_CAPABILITIES
+					&& PD_NUMOBJ_GET(&tempMessage) > 0) {
+				/* First, determine what PD revision we're using */
+				if ((hdr_template & PD_HDR_SPECREV) == PD_SPECREV_1_0) {
+					/* If the other end is using at least version 3.0, we'll
+					 * use version 3.0. */
+					if ((tempMessage.hdr & PD_HDR_SPECREV) >= PD_SPECREV_3_0) {
+						hdr_template |= PD_SPECREV_3_0;
+						/* Otherwise, use 2.0.  Don't worry about the 1.0 case
+						 * because we don't have hardware for PD 1.0 signaling. */
+					} else {
+						hdr_template |= PD_SPECREV_2_0;
+					}
+				}
+				return PESinkEvalCap;
+				/* If the message was a Soft_Reset, do the soft reset procedure */
+			}
+			evt = 0;
+		}
+		return PESinkWaitCap; //wait for more messages?
+
+	}
+
+	/* If we failed to get a message, send a hard reset */
+	return PESinkHardReset;
+}
+
+PolicyEngine::policy_engine_state PolicyEngine::pe_sink_eval_cap() {
+	/* If we have a Source_Capabilities message, remember the index of the
+	 * first PPS APDO so we can check if the request is for a PPS APDO in
+	 * PE_SNK_Select_Cap. */
+	/* Start by assuming we won't find a PPS APDO (set the index greater
+	 * than the maximum possible) */
+	_pps_index = 8;
+	/* Search for the first PPS APDO */
+	for (int8_t i = 0; i < PD_NUMOBJ_GET(&tempMessage); i++) {
+		if ((tempMessage.obj[i] & PD_PDO_TYPE) == PD_PDO_TYPE_AUGMENTED
+				&& (tempMessage.obj[i] & PD_APDO_TYPE) == PD_APDO_TYPE_PPS) {
+			_pps_index = i + 1;
+			break;
+		}
+	}
+	/* New capabilities also means we can't be making a request from the
+	 * same PPS APDO */
+	_last_pps = 8;
+
+	/* Ask the DPM what to request */
+	if (pdbs_dpm_evaluate_capability(&tempMessage, &_last_dpm_request)) {
+
+		return PESinkSelectCap;
+	}
+
+	return PESinkWaitCap;
+}
+
+PolicyEngine::policy_engine_state PolicyEngine::pe_sink_select_cap() {
+
+	/* Transmit the request */
+	waitForEvent(0xFFFF, 0); //clear pending
+	ProtocolTransmit::pushMessage(&_last_dpm_request);
+	//Send indication that there is a message pending
+	ProtocolTransmit::notify(
+			ProtocolTransmit::Notifications::PDB_EVT_PRLTX_MSG_TX);
+	eventmask_t evt = waitForEvent(
+	PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR | PDB_EVT_PE_RESET);
+	/* If we got reset signaling, transition to default */
+	if (evt & PDB_EVT_PE_RESET || evt == 0) {
+		return PESinkTransitionDefault;
+	}
+	/* If the message transmission failed, send a hard reset */
+	if ((evt & PDB_EVT_PE_TX_ERR) == PDB_EVT_PE_TX_ERR) {
+		return PESinkHardReset;
+	}
+
+	/* Wait for a response */
+	evt = waitForEvent(PDB_EVT_PE_MSG_RX | PDB_EVT_PE_RESET,
+	PD_T_SENDER_RESPONSE);
+	/* If we got reset signaling, transition to default */
+	if (evt & PDB_EVT_PE_RESET) {
+		return PESinkTransitionDefault;
+	}
+	/* If we didn't get a response before the timeout, send a hard reset */
+	if (evt == 0) {
+		return PESinkHardReset;
+	}
+
+	/* Get the response message */
+	if (messageWaiting()) {
+		readMessage();
+		/* If the source accepted our request, wait for the new power */
+		if (PD_MSGTYPE_GET(&tempMessage) == PD_MSGTYPE_ACCEPT
+				&& PD_NUMOBJ_GET(&tempMessage) == 0) {
+
+			return PESinkTransitionSink;
+			/* If the message was a Soft_Reset, do the soft reset procedure */
+		} else if (PD_MSGTYPE_GET(&tempMessage) == PD_MSGTYPE_SOFT_RESET
+				&& PD_NUMOBJ_GET(&tempMessage) == 0) {
+			return PESinkSoftReset;
+			/* If the message was Wait or Reject */
+		} else if ((PD_MSGTYPE_GET(&tempMessage) == PD_MSGTYPE_REJECT
+				|| PD_MSGTYPE_GET(&tempMessage) == PD_MSGTYPE_WAIT)
+				&& PD_NUMOBJ_GET(&tempMessage) == 0) {
+			/* If we don't have an explicit contract, wait for capabilities */
+			if (!_explicit_contract) {
+				return PESinkWaitCap;
+				/* If we do have an explicit contract, go to the ready state */
+			} else {
+				return PESinkReady;
+			}
+		} else {
+			return PESinkSendSoftReset;
+		}
+	}
+	return PESinkHardReset;
+}
+
+PolicyEngine::policy_engine_state PolicyEngine::pe_sink_transition_sink() {
+	/* Wait for the PS_RDY message */
+	eventmask_t evt = waitForEvent(PDB_EVT_PE_MSG_RX | PDB_EVT_PE_RESET,
+	PD_T_PS_TRANSITION);
+	/* If we got reset signaling, transition to default */
+	if (evt & PDB_EVT_PE_RESET) {
+		return PESinkTransitionDefault;
+	}
+	/* If no message was received, send a hard reset */
+	if (evt == 0) {
+		return PESinkHardReset;
+	}
+
+	/* If we received a message, read it */
+	if (messageWaiting()) {
+		readMessage();
+		/* If we got a PS_RDY, handle it */
+		if (PD_MSGTYPE_GET(&tempMessage) == PD_MSGTYPE_PS_RDY
+				&& PD_NUMOBJ_GET(&tempMessage) == 0) {
+			/* We just finished negotiating an explicit contract */
+			_explicit_contract = true;
+
+			/* Set the output appropriately */
+			pdbs_dpm_transition_requested();
+
+			return PESinkReady;
+			/* If there was a protocol error, send a hard reset */
+		} else {
+			/* Turn off the power output before this hard reset to make sure we
+			 * don't supply an incorrect voltage to the device we're powering.
+			 */
+			pdbs_dpm_transition_default();
+
+			return PESinkHardReset;
+		}
+	}
+
+	return PESinkHardReset;
+}
+
+PolicyEngine::policy_engine_state PolicyEngine::pe_sink_ready() {
+	eventmask_t evt;
+
+	/* Wait for an event */
+	evt = waitForEvent(
+	PDB_EVT_PE_MSG_RX | PDB_EVT_PE_RESET | PDB_EVT_PE_I_OVRTEMP);
+
+	/* If we got reset signaling, transition to default */
+	if (evt & PDB_EVT_PE_RESET) {
+		return PESinkTransitionDefault;
+	}
+
+	/* If we overheated, send a hard reset */
+	if (evt & PDB_EVT_PE_I_OVRTEMP) {
+		return PESinkHardReset;
+	}
+
+	/* If we received a message */
+	if (evt & PDB_EVT_PE_MSG_RX) {
+		if (messageWaiting()) {
+			readMessage();
+			/* Ignore vendor-defined messages */
+			if (PD_MSGTYPE_GET(&tempMessage) == PD_MSGTYPE_VENDOR_DEFINED
+					&& PD_NUMOBJ_GET(&tempMessage) > 0) {
+
+				return PESinkReady;
+				/* Ignore Ping messages */
+			} else if (PD_MSGTYPE_GET(&tempMessage) == PD_MSGTYPE_PING
+					&& PD_NUMOBJ_GET(&tempMessage) == 0) {
+
+				return PESinkReady;
+				/* DR_Swap messages are not supported */
+			} else if (PD_MSGTYPE_GET(&tempMessage) == PD_MSGTYPE_DR_SWAP
+					&& PD_NUMOBJ_GET(&tempMessage) == 0) {
+
+				return PESinkSendNotSupported;
+				/* Get_Source_Cap messages are not supported */
+			} else if (PD_MSGTYPE_GET(&tempMessage) == PD_MSGTYPE_GET_SOURCE_CAP
+					&& PD_NUMOBJ_GET(&tempMessage) == 0) {
+
+				return PESinkSendNotSupported;
+				/* PR_Swap messages are not supported */
+			} else if (PD_MSGTYPE_GET(&tempMessage) == PD_MSGTYPE_PR_SWAP
+					&& PD_NUMOBJ_GET(&tempMessage) == 0) {
+
+				return PESinkSendNotSupported;
+				/* VCONN_Swap messages are not supported */
+			} else if (PD_MSGTYPE_GET(&tempMessage) == PD_MSGTYPE_VCONN_SWAP
+					&& PD_NUMOBJ_GET(&tempMessage) == 0) {
+
+				return PESinkSendNotSupported;
+				/* Request messages are not supported */
+			} else if (PD_MSGTYPE_GET(&tempMessage) == PD_MSGTYPE_REQUEST
+					&& PD_NUMOBJ_GET(&tempMessage) > 0) {
+
+				return PESinkSendNotSupported;
+				/* Sink_Capabilities messages are not supported */
+			} else if (PD_MSGTYPE_GET(&tempMessage)
+					== PD_MSGTYPE_SINK_CAPABILITIES
+					&& PD_NUMOBJ_GET(&tempMessage) > 0) {
+
+				return PESinkSendNotSupported;
+				/* Handle GotoMin messages */
+			} else if (PD_MSGTYPE_GET(&tempMessage) == PD_MSGTYPE_GOTOMIN
+					&& PD_NUMOBJ_GET(&tempMessage) == 0) {
+				/* GiveBack is not supported */
+				return PESinkSendNotSupported;
+
+				/* Evaluate new Source_Capabilities */
+			} else if (PD_MSGTYPE_GET(&tempMessage)
+					== PD_MSGTYPE_SOURCE_CAPABILITIES
+					&& PD_NUMOBJ_GET(&tempMessage) > 0) {
+				/* Don't free the message: we need to keep the
+				 * Source_Capabilities message so we can evaluate it. */
+				return PESinkEvalCap;
+				/* Give sink capabilities when asked */
+			} else if (PD_MSGTYPE_GET(&tempMessage) == PD_MSGTYPE_GET_SINK_CAP
+					&& PD_NUMOBJ_GET(&tempMessage) == 0) {
+
+				return PESinkGiveSinkCap;
+				/* If the message was a Soft_Reset, do the soft reset procedure */
+			} else if (PD_MSGTYPE_GET(&tempMessage) == PD_MSGTYPE_SOFT_RESET
+					&& PD_NUMOBJ_GET(&tempMessage) == 0) {
+
+				return PESinkSoftReset;
+				/* PD 3.0 messges */
+			} else if ((hdr_template & PD_HDR_SPECREV) == PD_SPECREV_3_0) {
+				/* If the message is a multi-chunk extended message, let it
+				 * time out. */
+				if ((tempMessage.hdr & PD_HDR_EXT)
+						&& (PD_DATA_SIZE_GET(&tempMessage)
+								> PD_MAX_EXT_MSG_LEGACY_LEN)) {
+
+					return PESinkChunkReceived;
+					/* Tell the DPM a message we sent got a response of
+					 * Not_Supported. */
+				} else if (PD_MSGTYPE_GET(&tempMessage)
+						== PD_MSGTYPE_NOT_SUPPORTED
+						&& PD_NUMOBJ_GET(&tempMessage) == 0) {
+
+					return PESinkNotSupportedReceived;
+					/* If we got an unknown message, send a soft reset */
+				} else {
+
+					return PESinkSendSoftReset;
+				}
+				/* If we got an unknown message, send a soft reset ??? */
+			} else {
+
+				return PESinkSendSoftReset;
+			}
+		}
+	}
+
+	return PESinkReady;
+}
+
+PolicyEngine::policy_engine_state PolicyEngine::pe_sink_get_source_cap() {
+	/* Get a message object */
+	union pd_msg *get_source_cap = &tempMessage;
+	/* Make a Get_Source_Cap message */
+	get_source_cap->hdr = hdr_template | PD_MSGTYPE_GET_SOURCE_CAP
+			| PD_NUMOBJ(0);
+	/* Transmit the Get_Source_Cap */
+	ProtocolTransmit::pushMessage(get_source_cap);
+	ProtocolTransmit::notify(
+			ProtocolTransmit::Notifications::PDB_EVT_PRLTX_MSG_TX);
+	eventmask_t evt = waitForEvent(
+	PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR | PDB_EVT_PE_RESET);
+	/* Free the sent message */
+	/* If we got reset signaling, transition to default */
+	if (evt & PDB_EVT_PE_RESET) {
+		return PESinkTransitionDefault;
+	}
+	/* If the message transmission failed, send a hard reset */
+	if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
+		return PESinkHardReset;
+	}
+
+	return PESinkReady;
+}
+
+PolicyEngine::policy_engine_state PolicyEngine::pe_sink_give_sink_cap() {
+	/* Get a message object */
+	union pd_msg *snk_cap = &tempMessage;
+	/* Get our capabilities from the DPM */
+	pdbs_dpm_get_sink_capability(snk_cap);
+
+	/* Transmit our capabilities */
+	ProtocolTransmit::pushMessage(snk_cap);
+	ProtocolTransmit::notify(
+			ProtocolTransmit::Notifications::PDB_EVT_PRLTX_MSG_TX);
+	eventmask_t evt = waitForEvent(
+	PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR | PDB_EVT_PE_RESET);
+
+	/* Free the Sink_Capabilities message */
+
+	/* If we got reset signaling, transition to default */
+	if (evt & PDB_EVT_PE_RESET) {
+		return PESinkTransitionDefault;
+	}
+	/* If the message transmission failed, send a hard reset */
+	if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
+		return PESinkHardReset;
+	}
+
+	return PESinkReady;
+}
+
+PolicyEngine::policy_engine_state PolicyEngine::pe_sink_hard_reset() {
+	/* If we've already sent the maximum number of hard resets, assume the
+	 * source is unresponsive. */
+	if (_hard_reset_counter > PD_N_HARD_RESET_COUNT) {
+		return PESinkSourceUnresponsive;
+	}
+	//So, we could send a hardreset here; however that will cause a power cycle on the PSU end.. Which will then reset this MCU
+	//So therefore we went get anywhere :)
+	/* Increment HardResetCounter */
+	_hard_reset_counter++;
+
+	return PESinkTransitionDefault;
+}
+
+PolicyEngine::policy_engine_state PolicyEngine::pe_sink_transition_default() {
+	_explicit_contract = false;
+
+	/* Tell the DPM to transition to default power */
+	pdbs_dpm_transition_default();
+
+	/* There is no local hardware to reset. */
+	/* Since we never change our data role from UFP, there is no reason to set
+	 * it here. */
+
+	return PESinkStartup;
+}
+
+PolicyEngine::policy_engine_state PolicyEngine::pe_sink_soft_reset() {
+	/* No need to explicitly reset the protocol layer here.  It resets itself
+	 * when a Soft_Reset message is received. */
+
+	/* Get a message object */
+	union pd_msg accept;
+	/* Make an Accept message */
+	accept.hdr = hdr_template | PD_MSGTYPE_ACCEPT | PD_NUMOBJ(0);
+	/* Transmit the Accept */
+	ProtocolTransmit::pushMessage(&accept);
+	ProtocolTransmit::notify(
+			ProtocolTransmit::Notifications::PDB_EVT_PRLTX_MSG_TX);
+	eventmask_t evt = waitForEvent(
+	PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR | PDB_EVT_PE_RESET);
+	/* Free the sent message */
+
+	/* If we got reset signaling, transition to default */
+	if (evt & PDB_EVT_PE_RESET) {
+		return PESinkTransitionDefault;
+	}
+	/* If the message transmission failed, send a hard reset */
+	if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
+		return PESinkHardReset;
+	}
+
+	return PESinkWaitCap;
+}
+
+PolicyEngine::policy_engine_state PolicyEngine::pe_sink_send_soft_reset() {
+	/* No need to explicitly reset the protocol layer here.  It resets itself
+	 * just before a Soft_Reset message is transmitted. */
+
+	/* Get a message object */
+	union pd_msg *softrst = &tempMessage;
+	/* Make a Soft_Reset message */
+	softrst->hdr = hdr_template | PD_MSGTYPE_SOFT_RESET | PD_NUMOBJ(0);
+	/* Transmit the soft reset */
+	ProtocolTransmit::pushMessage(softrst);
+	ProtocolTransmit::notify(
+			ProtocolTransmit::Notifications::PDB_EVT_PRLTX_MSG_TX);
+	eventmask_t evt = waitForEvent(
+	PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR | PDB_EVT_PE_RESET);
+	/* If we got reset signaling, transition to default */
+	if (evt & PDB_EVT_PE_RESET) {
+		return PESinkTransitionDefault;
+	}
+	/* If the message transmission failed, send a hard reset */
+	if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
+		return PESinkHardReset;
+	}
+
+	/* Wait for a response */
+	evt = waitForEvent(PDB_EVT_PE_MSG_RX | PDB_EVT_PE_RESET,
+	PD_T_SENDER_RESPONSE);
+	/* If we got reset signaling, transition to default */
+	if (evt & PDB_EVT_PE_RESET) {
+		return PESinkTransitionDefault;
+	}
+	/* If we didn't get a response before the timeout, send a hard reset */
+	if (evt == 0) {
+		return PESinkHardReset;
+	}
+
+	/* Get the response message */
+	if (messageWaiting()) {
+		readMessage();
+		/* If the source accepted our soft reset, wait for capabilities. */
+		if (PD_MSGTYPE_GET(&tempMessage) == PD_MSGTYPE_ACCEPT
+				&& PD_NUMOBJ_GET(&tempMessage) == 0) {
+
+			return PESinkWaitCap;
+			/* If the message was a Soft_Reset, do the soft reset procedure */
+		} else if (PD_MSGTYPE_GET(&tempMessage) == PD_MSGTYPE_SOFT_RESET
+				&& PD_NUMOBJ_GET(&tempMessage) == 0) {
+
+			return PESinkSoftReset;
+			/* Otherwise, send a hard reset */
+		} else {
+
+			return PESinkHardReset;
+		}
+	}
+	return PESinkHardReset;
+}
+
+PolicyEngine::policy_engine_state PolicyEngine::pe_sink_send_not_supported() {
+	/* Get a message object */
+	union pd_msg *not_supported = &tempMessage;
+
+	if ((hdr_template & PD_HDR_SPECREV) == PD_SPECREV_2_0) {
+		/* Make a Reject message */
+		not_supported->hdr = hdr_template | PD_MSGTYPE_REJECT | PD_NUMOBJ(0);
+	} else if ((hdr_template & PD_HDR_SPECREV) == PD_SPECREV_3_0) {
+		/* Make a Not_Supported message */
+		not_supported->hdr = hdr_template | PD_MSGTYPE_NOT_SUPPORTED
+				| PD_NUMOBJ(0);
+	}
+
+	/* Transmit the message */
+	ProtocolTransmit::pushMessage(not_supported);
+	ProtocolTransmit::notify(
+			ProtocolTransmit::Notifications::PDB_EVT_PRLTX_MSG_TX);
+	eventmask_t evt = waitForEvent(
+	PDB_EVT_PE_TX_DONE | PDB_EVT_PE_TX_ERR | PDB_EVT_PE_RESET);
+
+	/* If we got reset signaling, transition to default */
+	if (evt & PDB_EVT_PE_RESET) {
+		return PESinkTransitionDefault;
+	}
+	/* If the message transmission failed, send a soft reset */
+	if ((evt & PDB_EVT_PE_TX_DONE) == 0) {
+		return PESinkSendSoftReset;
+	}
+
+	return PESinkReady;
+}
+
+PolicyEngine::policy_engine_state PolicyEngine::pe_sink_chunk_received() {
+
+	/* Wait for tChunkingNotSupported */
+	eventmask_t evt = waitForEvent(PDB_EVT_PE_RESET,
+	PD_T_CHUNKING_NOT_SUPPORTED);
+	/* If we got reset signaling, transition to default */
+	if (evt & PDB_EVT_PE_RESET) {
+		return PESinkTransitionDefault;
+	}
+
+	return PESinkSendNotSupported;
+}
+
+PolicyEngine::policy_engine_state PolicyEngine::pe_sink_not_supported_received() {
+	/* Inform the Device Policy Manager that we received a Not_Supported
+	 * message. */
+
+	return PESinkReady;
+}
+
+PolicyEngine::policy_engine_state PolicyEngine::pe_sink_source_unresponsive() {
+//Sit and chill, as PD is not working
+	osDelay(PD_T_PD_DEBOUNCE);
+
+	return PESinkSourceUnresponsive;
+}
+
+uint32_t PolicyEngine::waitForEvent(uint32_t mask, TickType_t ticksToWait) {
+	return xEventGroupWaitBits(xEventGroupHandle, mask, mask, pdFALSE,
+			ticksToWait);
+
+}
+
+bool PolicyEngine::isPD3_0() {
+	return (hdr_template & PD_HDR_SPECREV) == PD_SPECREV_3_0;
+}
+

source/Core/Drivers/FUSB302/policy_engine.h

@@ -0,0 +1,198 @@
+/*
+ * PD Buddy Firmware Library - USB Power Delivery for everyone
+ * Copyright 2017-2018 Clayton G. Hobbs
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef PDB_POLICY_ENGINE_H
+#define PDB_POLICY_ENGINE_H
+
+#include <pd.h>
+
+/*
+ * Events for the Policy Engine thread, used internally + sent by user code
+ *
+ */
+
+#define PDB_EVT_PE_RESET EVENT_MASK(0)
+#define PDB_EVT_PE_MSG_RX EVENT_MASK(1)
+#define PDB_EVT_PE_TX_DONE EVENT_MASK(2)
+#define PDB_EVT_PE_TX_ERR EVENT_MASK(3)
+#define PDB_EVT_PE_HARD_SENT EVENT_MASK(4)
+#define PDB_EVT_PE_I_OVRTEMP EVENT_MASK(5)
+#define PDB_EVT_PE_MSG_RX_PEND EVENT_MASK(7) /* Never SEND THIS DIRECTLY*/
+
+class PolicyEngine {
+public:
+	//Sets up internal state and registers the thread
+	static void init();
+	//Push an incoming message to the Policy Engine
+	static void handleMessage(union pd_msg *msg);
+	//Send a notification
+	static void notify(uint32_t notification);
+	//Returns true if headers indicate PD3.0 compliant
+	static bool isPD3_0();
+	static bool setupCompleteOrTimedOut() {
+		if (pdNegotiationComplete)
+			return true;
+		if (state == policy_engine_state::PESinkSourceUnresponsive)
+			return true;
+		if (state == policy_engine_state::PESinkReady)
+			return true;
+		return false;
+	}
+	//Has pd negotiation completed
+	static bool pdHasNegotiated() {
+		return pdNegotiationComplete;
+	}
+private:
+	static bool pdNegotiationComplete;
+	static int current_voltage_mv; //The current voltage PD is expecting
+	static int _requested_voltage; //The voltage the unit wanted to requests
+	static bool _unconstrained_power; // If the source is unconstrained
+	//Current message being handled
+	static union pd_msg currentMessage;
+	/* PD message header template */
+	static uint16_t hdr_template;
+	/* Whether or not we have an explicit contract */
+	static bool _explicit_contract;
+	/* The number of hard resets we've sent */
+	static int8_t _hard_reset_counter;
+	/* The result of the last Type-C Current match comparison */
+	static int8_t _old_tcc_match;
+	/* The index of the first PPS APDO */
+	static uint8_t _pps_index;
+	/* The index of the just-requested PPS APDO */
+	static uint8_t _last_pps;
+	static void pe_task(const void *arg);
+	enum policy_engine_state {
+		PESinkStartup,
+		PESinkDiscovery,
+		PESinkWaitCap,
+		PESinkEvalCap,
+		PESinkSelectCap,
+		PESinkTransitionSink,
+		PESinkReady,
+		PESinkGetSourceCap,
+		PESinkGiveSinkCap,
+		PESinkHardReset,
+		PESinkTransitionDefault,
+		PESinkSoftReset,
+		PESinkSendSoftReset,
+		PESinkSendNotSupported,
+		PESinkChunkReceived,
+		PESinkNotSupportedReceived,
+		PESinkSourceUnresponsive
+	};
+	static enum policy_engine_state pe_sink_startup();
+	static enum policy_engine_state pe_sink_discovery();
+	static enum policy_engine_state pe_sink_wait_cap();
+	static enum policy_engine_state pe_sink_eval_cap();
+	static enum policy_engine_state pe_sink_select_cap();
+	static enum policy_engine_state pe_sink_transition_sink();
+	static enum policy_engine_state pe_sink_ready();
+	static enum policy_engine_state pe_sink_get_source_cap();
+	static enum policy_engine_state pe_sink_give_sink_cap();
+	static enum policy_engine_state pe_sink_hard_reset();
+	static enum policy_engine_state pe_sink_transition_default();
+	static enum policy_engine_state pe_sink_soft_reset();
+	static enum policy_engine_state pe_sink_send_soft_reset();
+	static enum policy_engine_state pe_sink_send_not_supported();
+	static enum policy_engine_state pe_sink_chunk_received();
+	static enum policy_engine_state pe_sink_not_supported_received();
+	static enum policy_engine_state pe_sink_source_unresponsive();
+	static EventGroupHandle_t xEventGroupHandle;
+	static StaticEventGroup_t xCreatedEventGroup;
+	static uint32_t waitForEvent(uint32_t mask, TickType_t ticksToWait =
+	portMAX_DELAY);
+	//Task resources
+	static osThreadId TaskHandle;
+	static const size_t TaskStackSize = 2048 / 4;
+	static uint32_t TaskBuffer[TaskStackSize];
+	static osStaticThreadDef_t TaskControlBlock;
+	static union pd_msg tempMessage;
+	static union pd_msg _last_dpm_request;
+	static policy_engine_state state;
+	//queue of up to PDB_MSG_POOL_SIZE messages to send
+	static StaticQueue_t xStaticQueue;
+	/* The array to use as the queue's storage area.  This must be at least
+	 uxQueueLength * uxItemSize bytes. */
+	static uint8_t ucQueueStorageArea[PDB_MSG_POOL_SIZE * sizeof(union pd_msg)];
+	static QueueHandle_t messagesWaiting;
+	static bool messageWaiting();
+//Read a pending message into the temp message
+	static bool readMessage();
+
+	// These callbacks are called to implement the logic for the iron to select the desired voltage
+
+	/*
+	 * Create a Request message based on the given Source_Capabilities message.  If
+	 * capabilities is NULL, the last non-null Source_Capabilities message passes
+	 * is used.  If none has been provided, the behavior is undefined.
+	 *
+	 * Returns true if sufficient power is available, false otherwise.
+	 */
+	static bool pdbs_dpm_evaluate_capability(const union pd_msg *capabilities,
+			union pd_msg *request);
+
+	/*
+	 * Create a Sink_Capabilities message for our current capabilities.
+	 */
+	static void pdbs_dpm_get_sink_capability(union pd_msg *cap);
+
+	/*
+	 * Return whether or not GiveBack support is enabled.
+	 */
+	static bool pdbs_dpm_giveback_enabled();
+
+	/*
+	 * Evaluate whether or not the currently offered Type-C Current can fulfill our
+	 * power needs.
+	 *
+	 * Returns true if sufficient power is available, false otherwise.
+	 */
+	static bool pdbs_dpm_evaluate_typec_current(enum fusb_typec_current tcc);
+
+	/*
+	 * Indicate that power negotiations are starting.
+	 */
+	static void pdbs_dpm_pd_start();
+
+	/*
+	 * Transition the sink to default power.
+	 */
+	static void pdbs_dpm_transition_default();
+
+	/*
+	 * Transition to the requested minimum current.
+	 */
+	static void pdbs_dpm_transition_min();
+
+	/*
+	 * Transition to Sink Standby if necessary.
+	 */
+	static void pdbs_dpm_transition_standby();
+
+	/*
+	 * Transition to the requested power level
+	 */
+	static void pdbs_dpm_transition_requested();
+
+	/*
+	 * Transition to the Type-C Current power level
+	 */
+	static void pdbs_dpm_transition_typec();
+};
+
+#endif /* PDB_POLICY_ENGINE_H */

source/Core/Drivers/FUSB302/policy_engine_user.cpp

@@ -0,0 +1,227 @@
+/*
+ * policy_engine_user.cpp
+ *
+ *  Created on: 14 Jun 2020
+ *      Author: Ralim
+ */
+#include "pd.h"
+#include "policy_engine.h"
+#include "BSP_PD.h"
+/* The current draw when the output is disabled */
+#define DPM_MIN_CURRENT PD_MA2PDI(50)
+/*
+ * Find the index of the first PDO from capabilities in the voltage range,
+ * using the desired order.
+ *
+ * If there is no such PDO, returns -1 instead.
+ */
+static int8_t dpm_get_range_fixed_pdo_index(const union pd_msg *caps) {
+	/* Get the number of PDOs */
+	uint8_t numobj = PD_NUMOBJ_GET(caps);
+
+	/* Get ready to iterate over the PDOs */
+	int8_t i;
+	int8_t step;
+	i = numobj - 1;
+	step = -1;
+	uint16_t current = 100; // in centiamps
+	uint16_t voltagemin = 8000;
+	uint16_t voltagemax = 10000;
+	/* Look at the PDOs to see if one falls in our voltage range. */
+	while (0 <= i && i < numobj) {
+		/* If we have a fixed PDO, its V is within our range, and its I is at
+		 * least our desired I */
+		uint16_t v = PD_PDO_SRC_FIXED_VOLTAGE_GET(caps->obj[i]);
+		if ((caps->obj[i] & PD_PDO_TYPE) == PD_PDO_TYPE_FIXED) {
+			if ( PD_PDO_SRC_FIXED_CURRENT_GET(caps->obj[i]) >= current) {
+				if (v >= PD_MV2PDV(voltagemin) && v <= PD_MV2PDV(voltagemax)) {
+					return i;
+				}
+			}
+		}
+		i += step;
+	}
+	return -1;
+}
+bool PolicyEngine::pdbs_dpm_evaluate_capability(
+		const union pd_msg *capabilities, union pd_msg *request) {
+
+	/* Get the number of PDOs */
+	uint8_t numobj = PD_NUMOBJ_GET(capabilities);
+
+	/* Get whether or not the power supply is constrained */
+	_unconstrained_power =
+			capabilities->obj[0] & PD_PDO_SRC_FIXED_UNCONSTRAINED;
+
+	/* Make sure we have configuration */
+	/* Look at the PDOs to see if one matches our desires */
+//Look against USB_PD_Desired_Levels to select in order of preference
+	for (uint8_t desiredLevel = 0; desiredLevel < USB_PD_Desired_Levels_Len;
+			desiredLevel++) {
+		for (uint8_t i = 0; i < numobj; i++) {
+			/* If we have a fixed PDO, its V equals our desired V, and its I is
+			 * at least our desired I */
+			if ((capabilities->obj[i] & PD_PDO_TYPE) == PD_PDO_TYPE_FIXED) {
+				//This is a fixed PDO entry
+				int voltage = PD_PDV2MV(
+						PD_PDO_SRC_FIXED_VOLTAGE_GET(capabilities->obj[i]));
+				int current = PD_PDO_SRC_FIXED_CURRENT_GET(
+						capabilities->obj[i]);
+				uint16_t desiredVoltage = USB_PD_Desired_Levels[(desiredLevel
+						* 2) + 0];
+				uint16_t desiredminCurrent = USB_PD_Desired_Levels[(desiredLevel
+						* 2) + 1];
+				//As pd stores current in 10mA increments, divide by 10
+				desiredminCurrent /= 10;
+				if (voltage == desiredVoltage) {
+					if (current >= desiredminCurrent) {
+						/* We got what we wanted, so build a request for that */
+						request->hdr = hdr_template | PD_MSGTYPE_REQUEST
+								| PD_NUMOBJ(1);
+
+						/* GiveBack disabled */
+						request->obj[0] =
+								PD_RDO_FV_MAX_CURRENT_SET(
+										current) | PD_RDO_FV_CURRENT_SET(current)
+										| PD_RDO_NO_USB_SUSPEND | PD_RDO_OBJPOS_SET(i + 1);
+						//We support usb comms (ish)
+						request->obj[0] |= PD_RDO_USB_COMMS;
+
+						/* Update requested voltage */
+						_requested_voltage = voltage;
+
+						return true;
+					}
+				}
+			}
+
+		}
+	}
+
+	/* Nothing matched (or no configuration), so get 5 V at low current */
+	request->hdr = hdr_template | PD_MSGTYPE_REQUEST | PD_NUMOBJ(1);
+	request->obj[0] =
+			PD_RDO_FV_MAX_CURRENT_SET(
+					DPM_MIN_CURRENT) | PD_RDO_FV_CURRENT_SET(DPM_MIN_CURRENT) | PD_RDO_NO_USB_SUSPEND
+					| PD_RDO_OBJPOS_SET(1);
+	/* If the output is enabled and we got here, it must be a capability
+	 * mismatch. */
+	if (pdNegotiationComplete) {
+		request->obj[0] |= PD_RDO_CAP_MISMATCH;
+	}
+	request->obj[0] |= PD_RDO_USB_COMMS;
+
+	/* Update requested voltage */
+	_requested_voltage = 5000;
+
+	return false;
+}
+
+void PolicyEngine::pdbs_dpm_get_sink_capability(union pd_msg *cap) {
+	/* Keep track of how many PDOs we've added */
+	int numobj = 0;
+
+	/* If we have no configuration or want something other than 5 V, add a PDO
+	 * for vSafe5V */
+	/* Minimum current, 5 V, and higher capability. */
+	cap->obj[numobj++] =
+			PD_PDO_TYPE_FIXED
+					| PD_PDO_SNK_FIXED_VOLTAGE_SET(
+							PD_MV2PDV(5000)) | PD_PDO_SNK_FIXED_CURRENT_SET(DPM_MIN_CURRENT);
+
+	/* Get the current we want */
+	uint16_t current = USB_PD_Desired_Levels[1] / 10; // In centi-amps
+	uint16_t voltage = USB_PD_Desired_Levels[0]; // in mv
+	/* Add a PDO for the desired power. */
+	cap->obj[numobj++] = PD_PDO_TYPE_FIXED
+			| PD_PDO_SNK_FIXED_VOLTAGE_SET(
+					PD_MV2PDV(voltage)) | PD_PDO_SNK_FIXED_CURRENT_SET(current);
+
+	/* Get the PDO from the voltage range */
+	int8_t i = dpm_get_range_fixed_pdo_index(cap);
+
+	/* If it's vSafe5V, set our vSafe5V's current to what we want */
+	if (i == 0) {
+		cap->obj[0] &= ~PD_PDO_SNK_FIXED_CURRENT;
+		cap->obj[0] |= PD_PDO_SNK_FIXED_CURRENT_SET(current);
+	} else {
+		/* If we want more than 5 V, set the Higher Capability flag */
+		if (PD_MV2PDV(voltage) != PD_MV2PDV(5000)) {
+			cap->obj[0] |= PD_PDO_SNK_FIXED_HIGHER_CAP;
+		}
+
+		/* If the range PDO is a different voltage than the preferred
+		 * voltage, add it to the array. */
+		if (i
+				> 0&& PD_PDO_SRC_FIXED_VOLTAGE_GET(cap->obj[i]) != PD_MV2PDV(voltage)) {
+			cap->obj[numobj++] =
+					PD_PDO_TYPE_FIXED
+							| PD_PDO_SNK_FIXED_VOLTAGE_SET(
+									PD_PDO_SRC_FIXED_VOLTAGE_GET(cap->obj[i])) | PD_PDO_SNK_FIXED_CURRENT_SET(
+											PD_PDO_SRC_FIXED_CURRENT_GET(cap->obj[i]));
+		}
+
+		/* If we have three PDOs at this point, make sure the last two are
+		 * sorted by voltage. */
+		if (numobj == 3
+				&& (cap->obj[1] & PD_PDO_SNK_FIXED_VOLTAGE)
+						> (cap->obj[2] & PD_PDO_SNK_FIXED_VOLTAGE)) {
+			cap->obj[1] ^= cap->obj[2];
+			cap->obj[2] ^= cap->obj[1];
+			cap->obj[1] ^= cap->obj[2];
+		}
+	}
+
+	/* Set the unconstrained power flag. */
+	if (_unconstrained_power) {
+		cap->obj[0] |= PD_PDO_SNK_FIXED_UNCONSTRAINED;
+	}
+	/* Set the USB communications capable flag. */
+	cap->obj[0] |= PD_PDO_SNK_FIXED_USB_COMMS;
+
+	/* Set the Sink_Capabilities message header */
+	cap->hdr = hdr_template | PD_MSGTYPE_SINK_CAPABILITIES | PD_NUMOBJ(numobj);
+}
+
+bool PolicyEngine::pdbs_dpm_evaluate_typec_current(
+		enum fusb_typec_current tcc) {
+	(void) tcc;
+	//This is for evaluating 5V static current advertised by resistors
+	/* We don't control the voltage anymore; it will always be 5 V. */
+	current_voltage_mv = _requested_voltage = 5000;
+	//For the soldering iron we accept this as a fallback, but it sucks
+	pdNegotiationComplete = false;
+	return true;
+}
+
+void PolicyEngine::pdbs_dpm_transition_default() {
+	/* Cast the dpm_data to the right type */
+
+	/* Pretend we requested 5 V */
+	current_voltage_mv = 5000;
+	/* Turn the output off */
+	pdNegotiationComplete = false;
+}
+
+void PolicyEngine::pdbs_dpm_transition_requested() {
+	/* Cast the dpm_data to the right type */
+	pdNegotiationComplete = true;
+}
+
+void PolicyEngine::handleMessage(union pd_msg *msg) {
+	xQueueSend(messagesWaiting, msg, 100);
+}
+
+bool PolicyEngine::messageWaiting() {
+	return uxQueueMessagesWaiting(messagesWaiting) > 0;
+}
+
+bool PolicyEngine::readMessage() {
+	return xQueueReceive(messagesWaiting, &tempMessage, 0) == pdTRUE;
+}
+
+void PolicyEngine::pdbs_dpm_transition_typec() {
+//This means PD failed, so we either have a dump 5V only type C or a QC charger
+//For now; treat this as failed neg
+	pdNegotiationComplete = false;
+}

source/Core/Drivers/FUSB302/protocol_rx.cpp

@@ -0,0 +1,189 @@
+/*
+ * PD Buddy Firmware Library - USB Power Delivery for everyone
+ * Copyright 2017-2018 Clayton G. Hobbs
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "protocol_rx.h"
+
+#include <stdlib.h>
+#include "string.h"
+#include <pd.h>
+#include "policy_engine.h"
+#include "protocol_tx.h"
+#include "fusb302b.h"
+osThreadId ProtocolReceive::TaskHandle = NULL;
+EventGroupHandle_t ProtocolReceive::xEventGroupHandle = NULL;
+StaticEventGroup_t ProtocolReceive::xCreatedEventGroup;
+uint32_t ProtocolReceive::TaskBuffer[ProtocolReceive::TaskStackSize];
+osStaticThreadDef_t ProtocolReceive::TaskControlBlock;
+union pd_msg ProtocolReceive::tempMessage;
+uint8_t ProtocolReceive::_rx_messageid;
+uint8_t ProtocolReceive::_tx_messageidcounter;
+/*
+ * PRL_Rx_Wait_for_PHY_Message state
+ */
+ProtocolReceive::protocol_rx_state ProtocolReceive::protocol_rx_wait_phy() {
+	/* Wait for an event */
+	_rx_messageid = 0;
+	eventmask_t evt = waitForEvent(
+	PDB_EVT_PRLRX_RESET | PDB_EVT_PRLRX_I_GCRCSENT | PDB_EVT_PRLRX_I_RXPEND);
+
+	/* If we got a reset event, reset */
+	if (evt & PDB_EVT_PRLRX_RESET) {
+		waitForEvent(PDB_EVT_PRLRX_RESET, 0);
+		return PRLRxWaitPHY;
+	}
+	/* If we got an I_GCRCSENT event, read the message and decide what to do */
+	if (evt & PDB_EVT_PRLRX_I_GCRCSENT) {
+		/* Get a buffer to read the message into.  Guaranteed to not fail
+		 * because we have a big enough pool and are careful. */
+		union pd_msg *_rx_message = &tempMessage;
+		memset(&tempMessage, 0, sizeof(tempMessage));
+		/* Read the message */
+		fusb_read_message(_rx_message);
+		/* If it's a Soft_Reset, go to the soft reset state */
+		if (PD_MSGTYPE_GET(_rx_message) == PD_MSGTYPE_SOFT_RESET
+				&& PD_NUMOBJ_GET(_rx_message) == 0) {
+			return PRLRxReset;
+		} else {
+			/* Otherwise, check the message ID */
+			return PRLRxCheckMessageID;
+		}
+	} else if (evt & PDB_EVT_PRLRX_I_RXPEND) {
+		//There is an RX message pending that is not a Good CRC
+		union pd_msg *_rx_message = &tempMessage;
+		/* Read the message */
+		fusb_read_message(_rx_message);
+		return PRLRxWaitPHY;
+	}
+
+	return PRLRxWaitPHY;
+}
+
+/*
+ * PRL_Rx_Layer_Reset_for_Receive state
+ */
+ProtocolReceive::protocol_rx_state ProtocolReceive::protocol_rx_reset() {
+	/* Reset MessageIDCounter */
+	_tx_messageidcounter = 0;
+
+	/* Clear stored MessageID */
+	_rx_messageid = -1;
+
+	/* TX transitions to its reset state */
+	ProtocolTransmit::notify(
+			ProtocolTransmit::Notifications::PDB_EVT_PRLTX_RESET);
+	taskYIELD();
+
+	/* If we got a RESET signal, reset the machine */
+	if (waitForEvent(PDB_EVT_PRLRX_RESET, 0) != 0) {
+		return PRLRxWaitPHY;
+	}
+
+	/* Go to the Check_MessageID state */
+	return PRLRxCheckMessageID;
+}
+volatile uint32_t rxCounter = 0;
+/*
+ * PRL_Rx_Check_MessageID state
+ */
+ProtocolReceive::protocol_rx_state ProtocolReceive::protocol_rx_check_messageid() {
+	/* If we got a RESET signal, reset the machine */
+//	if (waitForEvent(PDB_EVT_PRLRX_RESET, 0) == PDB_EVT_PRLRX_RESET) {
+//		return PRLRxWaitPHY;
+//	}
+	/* If the message has the stored ID, we've seen this message before.  Free
+	 * it and don't pass it to the policy engine. */
+
+	/* Otherwise, there's either no stored ID or this message has an ID we
+	 * haven't just seen.  Transition to the Store_MessageID state. */
+//	if (PD_MESSAGEID_GET(&tempMessage) == _rx_messageid) {
+//		return PRLRxWaitPHY;
+//	} else
+	{
+		rxCounter++;
+		return PRLRxStoreMessageID;
+	}
+}
+
+/*
+ * PRL_Rx_Store_MessageID state
+ */
+ProtocolReceive::protocol_rx_state ProtocolReceive::protocol_rx_store_messageid() {
+	/* Tell ProtocolTX to discard the message being transmitted */
+
+	ProtocolTransmit::notify(
+			ProtocolTransmit::Notifications::PDB_EVT_PRLTX_DISCARD);
+
+	/* Update the stored MessageID */
+	_rx_messageid = PD_MESSAGEID_GET(&tempMessage);
+
+	/* Pass the message to the policy engine. */
+
+	PolicyEngine::handleMessage(&tempMessage);
+	PolicyEngine::notify(PDB_EVT_PE_MSG_RX);
+	taskYIELD();
+	/* Don't check if we got a RESET because we'd do nothing different. */
+
+	return PRLRxWaitPHY;
+}
+
+void ProtocolReceive::init() {
+	osThreadStaticDef(protRX, thread, PDB_PRIO_PRL, 0, TaskStackSize,
+			TaskBuffer, &TaskControlBlock);
+	xEventGroupHandle = xEventGroupCreateStatic(&xCreatedEventGroup);
+	TaskHandle = osThreadCreate(osThread(protRX), NULL);
+}
+
+void ProtocolReceive::thread(const void *args) {
+	(void) args;
+	ProtocolReceive::protocol_rx_state state = PRLRxWaitPHY;
+
+	while (true) {
+		switch (state) {
+		case PRLRxWaitPHY:
+			state = protocol_rx_wait_phy();
+			break;
+		case PRLRxReset:
+			state = protocol_rx_reset();
+			break;
+		case PRLRxCheckMessageID:
+			state = protocol_rx_check_messageid();
+			break;
+		case PRLRxStoreMessageID:
+			state = protocol_rx_store_messageid();
+			break;
+		default:
+			/* This is an error.  It really shouldn't happen.  We might
+			 * want to handle it anyway, though. */
+			state = PRLRxWaitPHY;
+			break;
+		}
+	}
+}
+
+void ProtocolReceive::notify(uint32_t notification) {
+	if (xEventGroupHandle != NULL) {
+		xEventGroupSetBits(xEventGroupHandle, notification);
+	}
+}
+
+uint32_t ProtocolReceive::waitForEvent(uint32_t mask, TickType_t ticksToWait) {
+	if (xEventGroupHandle != NULL) {
+		return xEventGroupWaitBits(xEventGroupHandle, mask, mask,
+		pdFALSE, ticksToWait);
+	}
+	return 0;
+}

source/Core/Drivers/FUSB302/protocol_rx.h

@@ -0,0 +1,64 @@
+/*
+ * PD Buddy Firmware Library - USB Power Delivery for everyone
+ * Copyright 2017-2018 Clayton G. Hobbs
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef PDB_PROTOCOL_RX_H
+#define PDB_PROTOCOL_RX_H
+
+#include <stdint.h>
+
+#include <pd.h>
+
+/* Events for the Protocol RX thread */
+#define PDB_EVT_PRLRX_RESET EVENT_MASK(0)
+#define PDB_EVT_PRLRX_I_GCRCSENT EVENT_MASK(1)
+#define PDB_EVT_PRLRX_I_RXPEND EVENT_MASK(2)
+
+class ProtocolReceive {
+public:
+	static void init();
+	static void notify(uint32_t notification);
+private:
+	static void thread(const void *args);
+
+	static EventGroupHandle_t xEventGroupHandle;
+	static StaticEventGroup_t xCreatedEventGroup;
+	static osThreadId TaskHandle;
+	static const size_t TaskStackSize = 1024 / 4;
+	static uint32_t TaskBuffer[TaskStackSize];
+	static osStaticThreadDef_t TaskControlBlock;
+	/*
+	 * Protocol RX machine states
+	 *
+	 * There is no Send_GoodCRC state because the PHY sends the GoodCRC for us.
+	 * All transitions that would go to that state instead go to Check_MessageID.
+	 */
+	enum protocol_rx_state {
+		PRLRxWaitPHY, PRLRxReset, PRLRxCheckMessageID, PRLRxStoreMessageID
+	};
+	static protocol_rx_state protocol_rx_store_messageid();
+	static protocol_rx_state protocol_rx_check_messageid();
+	static protocol_rx_state protocol_rx_reset();
+	static protocol_rx_state protocol_rx_wait_phy();
+	static union pd_msg tempMessage;
+	static uint8_t _rx_messageid;
+	static uint8_t _tx_messageidcounter;
+	static uint32_t waitForEvent(uint32_t mask, TickType_t ticksToWait =
+	portMAX_DELAY);
+
+};
+
+#endif /* PDB_PROTOCOL_RX_H */

source/Core/Drivers/FUSB302/protocol_tx.cpp

@@ -0,0 +1,298 @@
+/*
+ * PD Buddy Firmware Library - USB Power Delivery for everyone
+ * Copyright 2017-2018 Clayton G. Hobbs
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "protocol_tx.h"
+#include <pd.h>
+#include "policy_engine.h"
+#include "protocol_rx.h"
+#include "fusb302b.h"
+#include "fusbpd.h"
+
+osThreadId ProtocolTransmit::TaskHandle = NULL;
+uint32_t ProtocolTransmit::TaskBuffer[ProtocolTransmit::TaskStackSize];
+osStaticThreadDef_t ProtocolTransmit::TaskControlBlock;
+StaticQueue_t ProtocolTransmit::xStaticQueue;
+bool ProtocolTransmit::messageSending = false;
+uint8_t ProtocolTransmit::ucQueueStorageArea[PDB_MSG_POOL_SIZE
+		* sizeof(union pd_msg)];
+QueueHandle_t ProtocolTransmit::messagesWaiting = NULL;
+uint8_t ProtocolTransmit::_tx_messageidcounter;
+union pd_msg ProtocolTransmit::temp_msg;
+EventGroupHandle_t ProtocolTransmit::xEventGroupHandle = NULL;
+StaticEventGroup_t ProtocolTransmit::xCreatedEventGroup;
+/*
+ * PRL_Tx_PHY_Layer_Reset state
+ */
+ProtocolTransmit::protocol_tx_state ProtocolTransmit::protocol_tx_phy_reset() {
+	/* Reset the PHY */
+	fusb_reset();
+
+	/* If a message was pending when we got here, tell the policy engine that
+	 * we failed to send it */
+	if (messagePending()) {
+		/* Tell the policy engine that we failed */
+		PolicyEngine::notify( PDB_EVT_PE_TX_ERR);
+		/* Finish failing to send the message */
+		while (messagePending()) {
+			getMessage(); //Discard
+		}
+	}
+
+	/* Wait for a message request */
+	return PRLTxWaitMessage;
+}
+
+/*
+ * PRL_Tx_Wait_for_Message_Request state
+ */
+ProtocolTransmit::protocol_tx_state ProtocolTransmit::protocol_tx_wait_message() {
+	/* Wait for an event */
+	ProtocolTransmit::Notifications evt = waitForEvent(
+			(uint32_t) Notifications::PDB_EVT_PRLTX_RESET
+					| (uint32_t) Notifications::PDB_EVT_PRLTX_DISCARD
+					| (uint32_t) Notifications::PDB_EVT_PRLTX_MSG_TX);
+
+	if ((uint32_t) evt & (uint32_t) Notifications::PDB_EVT_PRLTX_RESET) {
+		return PRLTxPHYReset;
+	}
+
+	/* If the policy engine is trying to send a message */
+	if ((uint32_t) evt & (uint32_t) Notifications::PDB_EVT_PRLTX_MSG_TX) {
+		/* Get the message */
+		getMessage();
+
+		/* If it's a Soft_Reset, reset the TX layer first */
+		if (PD_MSGTYPE_GET(&temp_msg) == PD_MSGTYPE_SOFT_RESET
+				&& PD_NUMOBJ_GET(&(temp_msg)) == 0) {
+			return PRLTxReset;
+			/* Otherwise, just send the message */
+		} else {
+			return PRLTxConstructMessage;
+		}
+	}
+
+	/* Silence the compiler warning */
+	return PRLTxWaitMessage;
+}
+
+ProtocolTransmit::protocol_tx_state ProtocolTransmit::protocol_tx_reset() {
+	/* Clear MessageIDCounter */
+	_tx_messageidcounter = 0;
+
+	/* Tell the Protocol RX thread to reset */
+	ProtocolReceive::notify( PDB_EVT_PRLRX_RESET);
+	taskYIELD();
+
+	return PRLTxConstructMessage;
+}
+
+/*
+ * PRL_Tx_Construct_Message state
+ */
+ProtocolTransmit::protocol_tx_state ProtocolTransmit::protocol_tx_construct_message() {
+	/* Set the correct MessageID in the message */
+	temp_msg.hdr &= ~PD_HDR_MESSAGEID;
+	temp_msg.hdr |= (_tx_messageidcounter % 8) << PD_HDR_MESSAGEID_SHIFT;
+
+	/* PD 3.0 collision avoidance */
+//	if (PolicyEngine::isPD3_0()) {
+//		/* If we're starting an AMS, wait for permission to transmit */
+//		evt = waitForEvent((uint32_t) Notifications::PDB_EVT_PRLTX_START_AMS,
+//				0);
+//		if ((uint32_t) evt
+//				& (uint32_t) Notifications::PDB_EVT_PRLTX_START_AMS) {
+//			while (fusb_get_typec_current() != fusb_sink_tx_ok) {
+//				osDelay(1);
+//			}
+//		}
+//	}
+	messageSending = true;
+	/* Send the message to the PHY */
+	fusb_send_message(&temp_msg);
+
+	return PRLTxWaitResponse;
+}
+
+/*
+ * PRL_Tx_Wait_for_PHY_Response state
+ */
+ProtocolTransmit::protocol_tx_state ProtocolTransmit::protocol_tx_wait_response() {
+	/* Wait for an event.  There is no need to run CRCReceiveTimer, since the
+	 * FUSB302B handles that as part of its retry mechanism. */
+	ProtocolTransmit::Notifications evt = waitForEvent(
+			(uint32_t) Notifications::PDB_EVT_PRLTX_RESET
+					| (uint32_t) Notifications::PDB_EVT_PRLTX_DISCARD
+					| (uint32_t) Notifications::PDB_EVT_PRLTX_I_TXSENT
+					| (uint32_t) Notifications::PDB_EVT_PRLTX_I_RETRYFAIL);
+
+	if ((uint32_t) evt & (uint32_t) Notifications::PDB_EVT_PRLTX_RESET) {
+		return PRLTxPHYReset;
+	}
+	if ((uint32_t) evt & (uint32_t) Notifications::PDB_EVT_PRLTX_DISCARD) {
+		return PRLTxDiscardMessage;
+	}
+
+	/* If the message was sent successfully */
+	if ((uint32_t) evt & (uint32_t) Notifications::PDB_EVT_PRLTX_I_TXSENT) {
+		return PRLTxMatchMessageID;
+	}
+	/* If the message failed to be sent */
+	if ((uint32_t) evt & (uint32_t) Notifications::PDB_EVT_PRLTX_I_RETRYFAIL) {
+		return PRLTxTransmissionError;
+	}
+
+	/* Silence the compiler warning */
+	return PRLTxDiscardMessage;
+}
+
+/*
+ * PRL_Tx_Match_MessageID state
+ */
+ProtocolTransmit::protocol_tx_state ProtocolTransmit::protocol_tx_match_messageid() {
+	union pd_msg goodcrc;
+
+	/* Read the GoodCRC */
+	fusb_read_message(&goodcrc);
+
+	/* Check that the message is correct */
+	if (PD_MSGTYPE_GET(&goodcrc) == PD_MSGTYPE_GOODCRC
+			&& PD_NUMOBJ_GET(&goodcrc) == 0
+			&& PD_MESSAGEID_GET(&goodcrc) == _tx_messageidcounter) {
+		return PRLTxMessageSent;
+	} else {
+		return PRLTxTransmissionError;
+	}
+}
+
+ProtocolTransmit::protocol_tx_state ProtocolTransmit::protocol_tx_transmission_error() {
+	/* Increment MessageIDCounter */
+	_tx_messageidcounter = (_tx_messageidcounter + 1) % 8;
+
+	/* Tell the policy engine that we failed */
+	PolicyEngine::notify( PDB_EVT_PE_TX_ERR);
+
+	return PRLTxWaitMessage;
+}
+
+ProtocolTransmit::protocol_tx_state ProtocolTransmit::protocol_tx_message_sent() {
+	messageSending = false;
+	/* Increment MessageIDCounter */
+	_tx_messageidcounter = (_tx_messageidcounter + 1) % 8;
+
+	/* Tell the policy engine that we succeeded */
+	PolicyEngine::notify( PDB_EVT_PE_TX_DONE);
+
+	return PRLTxWaitMessage;
+}
+
+ProtocolTransmit::protocol_tx_state ProtocolTransmit::protocol_tx_discard_message() {
+	/* If we were working on sending a message, increment MessageIDCounter */
+	if (messageSending) {
+		_tx_messageidcounter = (_tx_messageidcounter + 1) % 8;
+
+		return PRLTxPHYReset;
+	} else {
+		return PRLTxWaitMessage;
+	}
+}
+void ProtocolTransmit::thread(const void *args) {
+	(void) args;
+	ProtocolTransmit::protocol_tx_state state = PRLTxPHYReset;
+
+	//Init the incoming message queue
+
+	while (true) {
+		switch (state) {
+		case PRLTxPHYReset:
+			state = protocol_tx_phy_reset();
+			break;
+		case PRLTxWaitMessage:
+			state = protocol_tx_wait_message();
+			break;
+		case PRLTxReset:
+			state = protocol_tx_reset();
+			break;
+		case PRLTxConstructMessage:
+			state = protocol_tx_construct_message();
+			break;
+		case PRLTxWaitResponse:
+			state = protocol_tx_wait_response();
+			break;
+		case PRLTxMatchMessageID:
+			state = protocol_tx_match_messageid();
+			break;
+		case PRLTxTransmissionError:
+			state = protocol_tx_transmission_error();
+			break;
+		case PRLTxMessageSent:
+			state = protocol_tx_message_sent();
+			break;
+		case PRLTxDiscardMessage:
+			state = protocol_tx_discard_message();
+			break;
+		default:
+			state = PRLTxPHYReset;
+			break;
+		}
+	}
+}
+
+void ProtocolTransmit::notify(ProtocolTransmit::Notifications notification) {
+	if (xEventGroupHandle != NULL) {
+		xEventGroupSetBits(xEventGroupHandle, (uint32_t) notification);
+	}
+}
+
+void ProtocolTransmit::init() {
+	messagesWaiting = xQueueCreateStatic(PDB_MSG_POOL_SIZE,
+			sizeof(union pd_msg), ucQueueStorageArea, &xStaticQueue);
+
+	osThreadStaticDef(pd_txTask, thread, PDB_PRIO_PRL, 0, TaskStackSize,
+			TaskBuffer, &TaskControlBlock);
+	TaskHandle = osThreadCreate(osThread(pd_txTask), NULL);
+	xEventGroupHandle = xEventGroupCreateStatic(&xCreatedEventGroup);
+}
+
+void ProtocolTransmit::pushMessage(union pd_msg *msg) {
+	if (messagesWaiting) {
+		xQueueSend(messagesWaiting, msg, 100);
+	}
+}
+
+bool ProtocolTransmit::messagePending() {
+	if (messagesWaiting) {
+		return uxQueueMessagesWaiting(messagesWaiting) > 0;
+	}
+	return false;
+}
+
+void ProtocolTransmit::getMessage() {
+	//Loads the pending message into the buffer
+	if (messagesWaiting) {
+		xQueueReceive(messagesWaiting, &temp_msg, 1);
+	}
+}
+
+ProtocolTransmit::Notifications ProtocolTransmit::waitForEvent(uint32_t mask,
+		TickType_t ticksToWait) {
+	if (xEventGroupHandle) {
+		return (Notifications) xEventGroupWaitBits(xEventGroupHandle, mask,
+				mask,
+				pdFALSE, ticksToWait);
+	}
+	return (Notifications)0;
+}

source/Core/Drivers/FUSB302/protocol_tx.h

@@ -0,0 +1,97 @@
+/*
+ * PD Buddy Firmware Library - USB Power Delivery for everyone
+ * Copyright 2017-2018 Clayton G. Hobbs
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef PDB_PROTOCOL_TX_H
+#define PDB_PROTOCOL_TX_H
+
+#include <stdint.h>
+#include "policy_engine.h"
+#include "protocol_rx.h"
+#include <pd.h>
+
+/* Events for the Protocol TX thread */
+
+class ProtocolTransmit {
+public:
+	static void init();
+	//Push a message to the queue to be sent out the pd comms bus
+	static void pushMessage(union pd_msg *msg);
+
+	enum class Notifications {
+
+		PDB_EVT_PRLTX_RESET = EVENT_MASK(0), //
+		PDB_EVT_PRLTX_I_TXSENT = EVENT_MASK(1), //
+		PDB_EVT_PRLTX_I_RETRYFAIL = EVENT_MASK(2), //
+		PDB_EVT_PRLTX_DISCARD = EVENT_MASK(3), //
+		PDB_EVT_PRLTX_MSG_TX = EVENT_MASK(4), //
+		PDB_EVT_PRLTX_START_AMS = EVENT_MASK(5), //
+	};
+	static void notify(Notifications notification);
+private:
+	static void thread(const void *args);
+	static EventGroupHandle_t xEventGroupHandle;
+	static StaticEventGroup_t xCreatedEventGroup;
+	static osThreadId TaskHandle;
+	static const size_t TaskStackSize = 1024 / 4;
+	static uint32_t TaskBuffer[TaskStackSize];
+	static osStaticThreadDef_t TaskControlBlock;
+	static bool messageSending;
+	/*
+	 * Protocol TX machine states
+	 *
+	 * Because the PHY can automatically send retries, the Check_RetryCounter state
+	 * has been removed, transitions relating to it are modified appropriately, and
+	 * we don't even keep a RetryCounter.
+	 */
+	enum protocol_tx_state {
+		PRLTxPHYReset,
+		PRLTxWaitMessage,
+		PRLTxReset,
+		PRLTxConstructMessage,
+		PRLTxWaitResponse,
+		PRLTxMatchMessageID,
+		PRLTxTransmissionError,
+		PRLTxMessageSent,
+		PRLTxDiscardMessage
+	};
+	//Internal states
+	static protocol_tx_state protocol_tx_discard_message();
+	static protocol_tx_state protocol_tx_message_sent();
+	static protocol_tx_state protocol_tx_transmission_error();
+	static protocol_tx_state protocol_tx_match_messageid();
+	static protocol_tx_state protocol_tx_wait_response();
+	static protocol_tx_state protocol_tx_construct_message();
+	static protocol_tx_state protocol_tx_reset();
+	static protocol_tx_state protocol_tx_wait_message();
+	static protocol_tx_state protocol_tx_phy_reset();
+	//queue of up to PDB_MSG_POOL_SIZE messages to send
+	static StaticQueue_t xStaticQueue;
+	/* The array to use as the queue's storage area.  This must be at least
+	 uxQueueLength * uxItemSize bytes. */
+	static uint8_t ucQueueStorageArea[PDB_MSG_POOL_SIZE * sizeof(union pd_msg)];
+	static QueueHandle_t messagesWaiting;
+	static uint8_t _tx_messageidcounter;
+	static bool messagePending();
+	//Reads a message off the queue into the temp message
+	static void getMessage();
+	static union pd_msg temp_msg;
+	static Notifications waitForEvent(uint32_t mask, TickType_t ticksToWait =
+	portMAX_DELAY);
+
+};
+
+#endif /* PDB_PROTOCOL_TX_H */

source/Core/Drivers/Font.h

@@ -0,0 +1,181 @@
+/*
+ * Font.h
+ *
+ *  Created on: 17 Sep 2016
+ *      Author: Ralim
+ *
+ *      ... This file contains the font...
+ */
+
+#ifndef FONT_H_
+#define FONT_H_
+#include "Translation.h"
+
+#define FONT_12_WIDTH   12
+// THE MAIN FONTS ARE NO LONGER HERE, MOVED TO PYTHON AUTO GEN
+// THESE ARE ONLY THE SYMBOL FONTS
+
+const uint8_t ExtraFontChars[] = {
+//width = 12
+//height = 16
+		0x00, 0x18, 0x24, 0x24, 0x18, 0xC0, 0x40, 0x40, 0x40, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0x02, 0x02, 0x02, 0x00, 0x00, 0x00, // Degrees F
+		0x00, 0x18, 0x24, 0x24, 0x18, 0x80, 0x40, 0x20, 0x20, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x08, 0x10, 0x10, 0x10, 0x00, 0x00, // Degrees C
+		0x00, 0x00, 0x20, 0x30, 0x38, 0xFC, 0xFE, 0xFC, 0x38, 0x30, 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x7F, 0x7F, 0x7F, 0x00, 0x00, 0x00, 0x00, // UP arrow
+
+		0x00, 0xF0, 0x08, 0x0E, 0x02, 0x02, 0x02, 0x02, 0x0E, 0x08, 0xF0, 0x00, 0x00, 0x3F, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x3F, 0x00, // Battery Empty
+		0x00, 0xF0, 0x08, 0x0E, 0x02, 0x02, 0x02, 0x02, 0x0E, 0x08, 0xF0, 0x00, 0x00, 0x3F, 0x40, 0x50, 0x50, 0x50, 0x50, 0x50, 0x50, 0x40, 0x3F, 0x00, // Battery 1*/
+		0x00, 0xF0, 0x08, 0x0E, 0x02, 0x02, 0x02, 0x02, 0x0E, 0x08, 0xF0, 0x00, 0x00, 0x3F, 0x40, 0x58, 0x58, 0x58, 0x58, 0x58, 0x58, 0x40, 0x3F, 0x00, // Battery 2*/
+		0x00, 0xF0, 0x08, 0x0E, 0x02, 0x02, 0x02, 0x02, 0x0E, 0x08, 0xF0, 0x00, 0x00, 0x3F, 0x40, 0x5C, 0x5C, 0x5C, 0x5C, 0x5C, 0x5C, 0x40, 0x3F, 0x00, // Battery 3*/
+		0x00, 0xF0, 0x08, 0x0E, 0x02, 0x02, 0x02, 0x02, 0x0E, 0x08, 0xF0, 0x00, 0x00, 0x3F, 0x40, 0x5E, 0x5E, 0x5E, 0x5E, 0x5E, 0x5E, 0x40, 0x3F, 0x00, // Battery 4*/
+		0x00, 0xF0, 0x08, 0x0E, 0x02, 0x02, 0x02, 0x02, 0x0E, 0x08, 0xF0, 0x00, 0x00, 0x3F, 0x40, 0x5F, 0x5F, 0x5F, 0x5F, 0x5F, 0x5F, 0x40, 0x3F, 0x00, // Battery 5*/
+		0x00, 0xF0, 0x08, 0x8E, 0x82, 0x82, 0x82, 0x82, 0x8E, 0x08, 0xF0, 0x00, 0x00, 0x3F, 0x40, 0x5F, 0x5F, 0x5F, 0x5F, 0x5F, 0x5F, 0x40, 0x3F, 0x00, // Battery 6*/
+		0x00, 0xF0, 0x08, 0xCE, 0xC2, 0xC2, 0xC2, 0xC2, 0xCE, 0x08, 0xF0, 0x00, 0x00, 0x3F, 0x40, 0x5F, 0x5F, 0x5F, 0x5F, 0x5F, 0x5F, 0x40, 0x3F, 0x00, // Battery 7*/
+		0x00, 0xF0, 0x08, 0xEE, 0xE2, 0xE2, 0xE2, 0xE2, 0xEE, 0x08, 0xF0, 0x00, 0x00, 0x3F, 0x40, 0x5F, 0x5F, 0x5F, 0x5F, 0x5F, 0x5F, 0x40, 0x3F, 0x00, // Battery 8*/
+		0x00, 0xF0, 0x08, 0xEE, 0xE2, 0xF2, 0xF2, 0xE2, 0xEE, 0x08, 0xF0, 0x00, 0x00, 0x3F, 0x40, 0x5F, 0x5F, 0x5F, 0x5F, 0x5F, 0x5F, 0x40, 0x3F, 0x00, // Battery 9*/
+		0x00, 0xF0, 0x08, 0xEE, 0xE2, 0xFA, 0xFA, 0xE2, 0xEE, 0x08, 0xF0, 0x00, 0x00, 0x3F, 0x40, 0x5F, 0x5F, 0x5F, 0x5F, 0x5F, 0x5F, 0x40, 0x3F, 0x00, // Battery 10*/
+
+		0x00, 0x00, 0x38, 0xC4, 0x00, 0x38, 0xC4, 0x00, 0x38, 0xC4, 0x00, 0x00, 0x00, 0x38, 0x3A, 0x39, 0x38, 0x3A, 0x39, 0x38, 0x3A, 0x39, 0x10, 0x10, // heating
+		0x00, 0x60, 0xE0, 0xFE, 0xE0, 0xE0, 0xE0, 0xE0, 0xFE, 0xE0, 0x60, 0x00, 0x00, 0x00, 0x00, 0x01, 0x03, 0xFF, 0xFF, 0x03, 0x01, 0x00, 0x00, 0x00, // AC
+
+		0xFC, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x82, 0x62, 0x1A, 0x02, 0xFC, 0x3F, 0x40, 0x42, 0x46, 0x4C, 0x58, 0x46, 0x41, 0x40, 0x40, 0x40, 0x3F, // ☑ (check box on, menu true)
+		0xFC, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0x02, 0xFC, 0x3F, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x3F, // ☐ (check box off, menu false)
+
+		/*
+		 0x00,0x00,0x00,0x80,0x80,0xFE,0xFF,0x83,0x87,0x06,0x00,0x00,0x00,0x00,0x30,0x70,0x60,0x7F,0x3F,0x00,0x00,0x00,0x00,0x00, // Function?
+		 0x00,0x70,0xFA,0xDB,0xDB,0xDB,0xDB,0xDB,0xDB,0xFF,0xFE,0x00,0x00,0x00,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x00,0x00, // a_
+		 0x00,0x3C,0x7E,0xE7,0xC3,0xC3,0xC3,0xC3,0xE7,0x7E,0x3C,0x00,0x00,0x00,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x00,0x00, // 0_
+		 0x55,0x00,0xAA,0x00,0x55,0x00,0xAA,0x00,0x55,0x00,0xAA,0x00,0x55,0x00,0xAA,0x00,0x55,0x00,0xAA,0x00,0x55,0x00,0xAA,0x00, // 25%  block
+		 0xAA,0x55,0xAA,0x55,0xAA,0x55,0xAA,0x55,0xAA,0x55,0xAA,0x55,0xAA,0x55,0xAA,0x55,0xAA,0x55,0xAA,0x55,0xAA,0x55,0xAA,0x55, // 50% pipe
+		 0xAA,0xFF,0x55,0xFF,0xAA,0xFF,0x55,0xFF,0xAA,0xFF,0x55,0xFF,0xAA,0xFF,0x55,0xFF,0xAA,0xFF,0x55,0xFF,0xAA,0xFF,0x55,0xFF, // 75% block
+		 0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00, // | pipe
+		 0x80,0x80,0x80,0x80,0x80,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x01,0x01,0x01,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00, // T pipe ,|
+		 0xC0,0xC0,0xFF,0xFF,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x06,0x06,0xFE,0xFE,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00, // ,| double pipe
+		 0x00,0x00,0xFF,0xFF,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00, //  || double pipe
+		 0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0x00,0x00,0x00,0x00,0x00,0x06,0x06,0xFE,0xFE,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00, // #NAME?//#NAME?
+		 0xC0,0xC0,0xFF,0xFF,0x00,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x06,0x06,0x06,0x06,0x06,0x07,0x07,0x00,0x00,0x00,0x00,0x00, // ,^ double pupe
+		 0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x01,0x01,0x01,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00, // #NAME?//#NAME?
+		 0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x80,0x80,0x80,0x80,0x80,0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x01,0x01,0x01,0x01,0x01, // ,> pipe
+		 0x80,0x80,0x80,0x80,0x80,0xFF,0xFF,0x80,0x80,0x80,0x80,0x80,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01, // _|_ pipe
+		 0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x01,0x01,0x01,0x01,0x01,0xFF,0xFF,0x01,0x01,0x01,0x01,0x01, // ,|, pipe
+		 0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x80,0x80,0x80,0x80,0x80,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x01,0x01,0x01,0x01,0x01, // |, pipe
+		 0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x01, // #NAME?//#NAME?
+		 0x80,0x80,0x80,0x80,0x80,0xFF,0xFF,0x80,0x80,0x80,0x80,0x80,0x01,0x01,0x01,0x01,0x01,0xFF,0xFF,0x01,0x01,0x01,0x01,0x01, // #NAME?//#NAME?
+		 0x00,0x00,0xFF,0xFF,0x00,0xFF,0xFF,0xC0,0xC0,0xC0,0xC0,0xC0,0x00,0x00,0x07,0x07,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06, // ,> double pipe
+		 0x00,0x00,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0x00,0x00,0xFF,0xFF,0x00,0xFE,0xFE,0x06,0x06,0x06,0x06,0x06, // ^, double pipe
+		 0xC0,0xC0,0xFF,0xFF,0x00,0xFF,0xFF,0xC0,0xC0,0xC0,0xC0,0xC0,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06, // _|_ double pipe
+		 0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0x06,0x06,0xFE,0xFE,0x00,0xFE,0xFE,0x06,0x06,0x06,0x06,0x06, // ,|, double pipe
+		 0x00,0x00,0xFF,0xFF,0x00,0xFF,0xFF,0xC0,0xC0,0xC0,0xC0,0xC0,0x00,0x00,0xFF,0xFF,0x00,0xFE,0xFE,0x06,0x06,0x06,0x06,0x06, // |, double pipe
+		 0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06,0x06, // == double pipe
+		 0xC0,0xC0,0xFF,0xFF,0x00,0xFF,0xFF,0xC0,0xC0,0xC0,0xC0,0xC0,0x06,0x06,0xFE,0xFE,0x00,0xFE,0xFE,0x06,0x06,0x06,0x06,0x06, // #NAME?//#NAME?
+		 0x00,0x00,0x00,0x78,0xFC,0xCC,0x8C,0x0C,0x18,0x00,0x00,0x00,0x00,0x00,0x00,0x1C,0x3E,0x33,0x33,0x3F,0x1E,0x00,0x00,0x00, // Delta lowercase
+		 0x00,0x00,0x00,0x00,0x00,0x7E,0x7E,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, // 27 (')
+		 0x80,0x80,0x80,0x80,0x80,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x01,0x01,0x01,0x01,0x01,0x01,0x01,0x00,0x00,0x00,0x00,0x00, // ,^ pipe
+		 0x00,0x00,0x00,0x00,0x00,0x80,0x80,0x80,0x80,0x80,0x80,0x80,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0x01,0x01,0x01,0x01,0x01, // | , pipe
+		 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, // solid block
+		 0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF, // half block bottom
+		 0x00,0x00,0x00,0x00,0x00,0xBF,0xBF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x3F,0x3F,0x00,0x00,0x00,0x00,0x00, // 7C (|)
+		 0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0xFF,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, // top half solid block
+		 0x00,0x00,0x0C,0xFC,0xFC,0x6C,0x60,0x60,0xE0,0xC0,0x00,0x00,0x00,0x00,0x30,0x3F,0x3F,0x36,0x06,0x06,0x07,0x03,0x00,0x00, // DE small
+		 0x00,0x00,0x03,0xFF,0xFF,0x1B,0x18,0x18,0xF8,0xF0,0x00,0x00,0x00,0x00,0x30,0x3F,0x3F,0x36,0x06,0x06,0x07,0x03,0x00,0x00, // DE large
+		 0x00,0x00,0x00,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, // ? (,)
+		 0x00,0x00,0x00,0xC0,0xC0,0xC0,0xC0,0xC0,0xC0,0x00,0x00,0x00,0x00,0x00,0x00,0x06,0x06,0x06,0x06,0x06,0x06,0x00,0x00,0x00, // =
+		 0x00,0x00,0x00,0x40,0x80,0x80,0xC0,0xC0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, // sideways comma
+		 0x00,0x00,0x80,0xC0,0x80,0x00,0x00,0x80,0xC0,0x80,0x00,0x00,0x00,0x00,0x01,0x03,0x01,0x00,0x00,0x01,0x03,0x01,0x00,0x00, // ..
+		 0x00,0x00,0x00,0x00,0x00,0x80,0xC0,0x80,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x01,0x03,0x01,0x00,0x00,0x00,0x00, // .
+		 0x00,0x00,0x02,0x1F,0x1F,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00, // tiny 1
+		 0x00,0x00,0x00,0x00,0xF0,0xF0,0xF0,0xF0,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x03,0x03,0x03,0x03,0x00,0x00,0x00,0x00, // small block
+		 */
+		};
+
+const uint8_t FontSymbols[] = { 0x00, 0x00, 0x00, 0xFC, 0xF8, 0xF0, 0xE0, 0xC0, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x1F, 0x0F, 0x07, 0x03, 0x01, 0x00, 0x00, 0x00,
+		0x00, // Right block
+		0x00, 0x00, 0x00, 0x80, 0xC0, 0xE0, 0xF0, 0xF8, 0xFC, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x03, 0x07, 0x0F, 0x1F, 0x00, 0x00,
+		0x00, // left block
+		0x00, 0x00, 0x00, 0x10, 0x18, 0x1C, 0xFE, 0x1C, 0x18, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x0C, 0x1C, 0x3F, 0x1C, 0x0C, 0x04, 0x00,
+		0x00, // UD arrow
+		0x00, 0x00, 0x00, 0xFE, 0xFE, 0x00, 0x00, 0xFE, 0xFE, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x37, 0x37, 0x00, 0x00, 0x37, 0x37, 0x00, 0x00,
+		0x00, // !!
+		0x00, 0x38, 0x7C, 0xC6, 0x82, 0xFE, 0xFE, 0x02, 0xFE, 0xFE, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0x3F, 0x00, 0x3F, 0x3F, 0x00,
+		0x00, // paragraph
+		0x00, 0x00, 0xDC, 0xFE, 0x22, 0x22, 0x22, 0x22, 0xE6, 0xC4, 0x00, 0x00, 0x00, 0x00, 0x08, 0x19, 0x11, 0x11, 0x11, 0x11, 0x1F, 0x0E, 0x00,
+		0x00, // section
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38, 0x38,
+		0x00, // cursor
+		0x00, 0x00, 0x00, 0x08, 0x0C, 0x0E, 0xFF, 0x0E, 0x0C, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x4C, 0x5C, 0x7F, 0x5C, 0x4C, 0x44, 0x00,
+		0x00, // UD arrow
+		0x00, 0x00, 0x00, 0x10, 0x18, 0x1C, 0xFE, 0x1C, 0x18, 0x10, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0x00, 0x00, 0x00, 0x00,
+		0x00, // UP arrow
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFE, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x0C, 0x1C, 0x3F, 0x1C, 0x0C, 0x04, 0x00,
+		0x00, // Down arrow
+		0x00, 0x00, 0x80, 0x80, 0x80, 0x80, 0x80, 0xF0, 0xE0, 0xC0, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x07, 0x03, 0x01, 0x00,
+		0x00, // right arrow
+		0x00, 0x00, 0x80, 0xC0, 0xE0, 0xF0, 0x80, 0x80, 0x80, 0x80, 0x80, 0x00, 0x00, 0x00, 0x00, 0x01, 0x03, 0x07, 0x00, 0x00, 0x00, 0x00, 0x00,
+		0x00, // left arrow
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x00, 0x00, 0x80, 0xC0, 0xE0, 0xF0, 0x80, 0x80, 0x80,
+		0xF0, 0xE0, 0xC0, 0x80, 0x00, 0x00, 0x01, 0x03, 0x07, 0x00, 0x00, 0x00, 0x07, 0x03, 0x01, 0x00, // LR arrow
+		0x00, 0x00, 0x00, 0x00, 0x80, 0xC0, 0xE0, 0xC0, 0x80, 0x00, 0x00, 0x00, 0x00, 0x04, 0x06, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x06, 0x04, // UP block
+		0x00, 0x20, 0x60, 0xE0, 0xE0, 0xE0, 0xE0, 0xE0, 0xE0, 0xE0, 0x60, 0x20, 0x00, 0x00, 0x00, 0x00, 0x01, 0x03, 0x07, 0x03, 0x01, 0x00, 0x00, 0x00  // Down block
+		};
+
+const uint8_t WarningBlock24[] = {
+		//width = 24
+		//height = 16
+		0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xC0, 0x30, 0x0C, 0x02, 0xF1, 0xF1, 0xF1, 0x02, 0x0C, 0x30, 0xC0, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xC0, 0xB0, 0x8C, 0x83, 0x80,
+		0x80, 0x80, 0x80, 0xB3, 0xB3, 0xB3, 0x80, 0x80, 0x80, 0x80, 0x83, 0x8C, 0xB0, 0xC0, 0x00, 0x00 };
+
+const uint8_t idleScreenBG[] = {
+		//width = 84
+		//height = 16
+		0x00, 0xE0, 0x18, 0x04, 0x02, 0x02, 0x01, 0x41, 0x61, 0x61, 0x61, 0xE1, 0xC1, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0xC1, 0xE1, 0x61, 0x61,
+		0x61, 0x41, 0x01, 0x01, 0x02, 0x02, 0x04, 0x18, 0xE0, 0x00, 0x00, 0xE0, 0x18, 0x04, 0x02, 0x02, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01,
+		0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x99, 0x65, 0x01, 0x01, 0x81, 0x41, 0x01, 0x02, 0x02, 0x04, 0x18, 0xE0, 0x00, 0x07, 0x18, 0x20, 0x40, 0x40, 0x80, 0x82, 0x86, 0x86, 0x86, 0x87,
+		0x83, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x81, 0x83, 0x87, 0x86, 0x86, 0x86, 0x82, 0x80, 0x80, 0x40, 0x40, 0x20, 0x18, 0x07, 0x00, 0x00, 0x07,
+		0x18, 0x20, 0x40, 0x40, 0x80, 0x82, 0x87, 0x85, 0x85, 0x85, 0x85, 0x87, 0x87, 0x85, 0x87, 0x85, 0x87, 0x87, 0x82, 0x82, 0x82, 0x80, 0x82, 0x80, 0x82, 0x82, 0x82, 0x92, 0x8A, 0x84, 0x82, 0x81,
+		0x80, 0x80, 0x80, 0x40, 0x40, 0x20, 0x18, 0x07 };
+
+const uint8_t disconnectedTipIcon[] = {		//
+				//41 x 16
+				0xE0, 0x18, 0x04, 0x02, 0x02, 0x01, 0x09, 0x11, 0x21, 0x41, 0x81, 0x81, 0x41, 0x21, 0x11, 0x09, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0xF9, 0x09, 0xF9, 0x01, 0xF9, 0x09, 0xF9,
+				0x01, 0x01, 0xF9, 0x09, 0xF9, 0x01, 0x02, 0x02, 0x04, 0x18, 0xE0,
+				//
+				0x07, 0x18, 0x20, 0x40, 0x40, 0x80, 0x90, 0x88, 0x84, 0x82, 0x81, 0x81, 0x82, 0x84, 0x88, 0x90, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0xBB, 0xAA, 0xBB, 0x80, 0xBB, 0xAA, 0xBB,
+				0x80, 0x80, 0xBB, 0xAA, 0xBB, 0x80, 0x40, 0x40, 0x20, 0x18, 0x07
+		//
+		};
+
+/*
+ * 16x16 icons
+ * */
+const uint8_t SettingsMenuIcons[] = {
+
+// Soldering
+//width = 16
+//height = 16
+		0x00, 0x02, 0x04, 0x08, 0x12, 0x24, 0xC4, 0x42, 0x82, 0x04, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x02, 0x07, 0x0A, 0x14, 0x28, 0x50, 0x60, 0x00,
+
+		//Sleep
+		//width = 16
+		//height = 16
+		0x00, 0xC6, 0xE6, 0xF6, 0xBE, 0x9E, 0x8E, 0x86, 0x00, 0x00, 0x40, 0x40, 0xC0, 0xC0, 0xC0, 0x00, 0x00, 0x01, 0x01, 0x01, 0x45, 0x65, 0x75, 0x5D, 0x4C, 0x00, 0x06, 0x07, 0x07, 0x05, 0x04, 0x00,
+
+		//Menu
+		//width = 16
+		//height = 16
+		0x00, 0x80, 0x06, 0x86, 0x46, 0x06, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x86, 0x00, 0x00, 0x00, 0x61, 0x60, 0x00, 0x00, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x61, 0x00,
+
+		//Wrench
+		///width = 16
+		//height = 16
+		0x00, 0x18, 0x30, 0x32, 0x7E, 0x7C, 0xF0, 0xC0, 0x80, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x03, 0x0F, 0x3E, 0x7E, 0x4C, 0x0C, 0x18, 0x00,
+#ifdef NOTUSED
+	//Calibration (Not used, kept for future menu layouts)
+		//width = 16
+		//height = 16
+	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x80, 0xC0, 0xE8, 0x70,
+	0x7A, 0x5E, 0x8E, 0x1C, 0x30, 0x00, 0x00, 0x10, 0x38, 0x1C,
+	0x0E, 0x07, 0x03, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
+	0x00, 0x00,
+	#endif
+		};
+
+#endif /* FONT_H_ */

source/Core/Drivers/I2CBB.cpp

@@ -0,0 +1,313 @@
+/*
+ * I2CBB.cpp
+ *
+ *  Created on: 12 Jun 2020
+ *      Author: Ralim
+ */
+#include "Model_Config.h"
+#ifdef I2C_SOFT
+#include <I2CBB.hpp>
+#include "FreeRTOS.h"
+SemaphoreHandle_t I2CBB::I2CSemaphore = NULL;
+StaticSemaphore_t I2CBB::xSemaphoreBuffer;
+SemaphoreHandle_t I2CBB::I2CSemaphore2 = NULL;
+StaticSemaphore_t I2CBB::xSemaphoreBuffer2;
+void I2CBB::init() {
+	//Set GPIO's to output open drain
+	GPIO_InitTypeDef GPIO_InitStruct;
+	__HAL_RCC_GPIOA_CLK_ENABLE();
+	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM;
+	GPIO_InitStruct.Pin = SDA2_Pin;
+	GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
+	GPIO_InitStruct.Pull = GPIO_PULLUP;
+	HAL_GPIO_Init(SDA2_GPIO_Port, &GPIO_InitStruct);
+	GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_MEDIUM;
+	GPIO_InitStruct.Pin = SCL2_Pin;
+	GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_OD;
+	GPIO_InitStruct.Pull = GPIO_PULLUP;
+	HAL_GPIO_Init(SCL2_GPIO_Port, &GPIO_InitStruct);
+	SOFT_SDA_HIGH();
+	SOFT_SCL_HIGH();
+	I2CSemaphore = xSemaphoreCreateMutexStatic(&xSemaphoreBuffer);
+	I2CSemaphore2 = xSemaphoreCreateMutexStatic(&xSemaphoreBuffer2);
+	unlock();
+	unlock2();
+
+}
+
+bool I2CBB::probe(uint8_t address) {
+	if (!lock())
+		return false;
+	start();
+	bool ack = send(address);
+	stop();
+	unlock();
+	return ack;
+}
+
+bool I2CBB::Mem_Read(uint16_t DevAddress, uint16_t MemAddress, uint8_t *pData,
+		uint16_t Size) {
+	if (!lock())
+		return false;
+	start();
+	bool ack = send(DevAddress);
+	if (!ack) {
+		stop();
+		unlock();
+		return false;
+	}
+	ack = send(MemAddress);
+	if (!ack) {
+		stop();
+		unlock();
+		return false;
+	}
+	SOFT_SCL_LOW();
+	SOFT_I2C_DELAY();
+//	stop();
+	start();
+	ack = send(DevAddress | 1);
+	if (!ack) {
+		stop();
+		unlock();
+		return false;
+	}
+	while (Size) {
+		pData[0] = read(Size > 1);
+		pData++;
+		Size--;
+	}
+	stop();
+	unlock();
+	return true;
+}
+
+bool I2CBB::Mem_Write(uint16_t DevAddress, uint16_t MemAddress,
+		const uint8_t *pData, uint16_t Size) {
+	if (!lock())
+		return false;
+	start();
+	bool ack = send(DevAddress);
+	if (!ack) {
+		stop();
+		asm("bkpt");
+		unlock();
+		return false;
+	}
+	ack = send(MemAddress);
+	if (!ack) {
+		stop();
+		asm("bkpt");
+		unlock();
+		return false;
+	}
+	while (Size) {
+		resetWatchdog();
+		ack = send(pData[0]);
+		if (!ack) {
+			stop();
+			asm("bkpt");
+			unlock();
+			return false;
+		}
+		pData++;
+		Size--;
+	}
+	stop();
+	unlock();
+	return true;
+}
+
+void I2CBB::Transmit(uint16_t DevAddress, uint8_t *pData, uint16_t Size) {
+	if (!lock())
+		return;
+	start();
+	bool ack = send(DevAddress);
+	if (!ack) {
+		stop();
+		unlock();
+		return;
+	}
+	while (Size) {
+		ack = send(pData[0]);
+		if (!ack) {
+			stop();
+			unlock();
+			return;
+		}
+		pData++;
+		Size--;
+	}
+	stop();
+	unlock();
+
+}
+
+void I2CBB::Receive(uint16_t DevAddress, uint8_t *pData, uint16_t Size) {
+	if (!lock())
+		return;
+	start();
+	bool ack = send(DevAddress | 1);
+	if (!ack) {
+		stop();
+		unlock();
+		return;
+	}
+	while (Size) {
+		pData[0] = read(Size > 1);
+		pData++;
+		Size--;
+	}
+	stop();
+	unlock();
+}
+
+void I2CBB::TransmitReceive(uint16_t DevAddress, uint8_t *pData_tx,
+		uint16_t Size_tx, uint8_t *pData_rx, uint16_t Size_rx) {
+	if (Size_tx == 0 && Size_rx == 0)
+		return;
+	if (lock() == false)
+		return;
+	if (Size_tx) {
+		start();
+		bool ack = send(DevAddress);
+		if (!ack) {
+			stop();
+			unlock();
+			return;
+		}
+		while (Size_tx) {
+			ack = send(pData_tx[0]);
+			if (!ack) {
+				stop();
+				unlock();
+				return;
+			}
+			pData_tx++;
+			Size_tx--;
+		}
+	}
+	if (Size_rx) {
+		start();
+		bool ack = send(DevAddress | 1);
+		if (!ack) {
+			stop();
+			unlock();
+			return;
+		}
+		while (Size_rx) {
+			pData_rx[0] = read(Size_rx > 1);
+			pData_rx++;
+			Size_rx--;
+		}
+	}
+	stop();
+	unlock();
+}
+
+void I2CBB::start() {
+	/* I2C Start condition, data line goes low when clock is high */
+	SOFT_SCL_HIGH();
+	SOFT_SDA_HIGH();
+	SOFT_I2C_DELAY();
+	SOFT_SDA_LOW();
+	SOFT_I2C_DELAY();
+	SOFT_SCL_LOW();
+	SOFT_I2C_DELAY();
+	SOFT_SDA_HIGH();
+}
+
+void I2CBB::stop() {
+	/* I2C Stop condition, clock goes high when data is low */
+	SOFT_SDA_LOW();
+	SOFT_I2C_DELAY();
+	SOFT_SCL_HIGH();
+	SOFT_I2C_DELAY();
+	SOFT_SDA_HIGH();
+	SOFT_I2C_DELAY();
+}
+
+bool I2CBB::send(uint8_t value) {
+
+	for (uint8_t i = 0; i < 8; i++) {
+		write_bit(value & 0x80);   // write the most-significant bit
+		value <<= 1;
+	}
+
+	SOFT_SDA_HIGH();
+	bool ack = (read_bit() == 0);
+	return ack;
+}
+
+uint8_t I2CBB::read(bool ack) {
+	uint8_t B = 0;
+
+	uint8_t i;
+	for (i = 0; i < 8; i++) {
+		B <<= 1;
+		B |= read_bit();
+	}
+
+	SOFT_SDA_HIGH();
+	if (ack)
+		write_bit(0);
+	else
+		write_bit(1);
+	return B;
+}
+
+uint8_t I2CBB::read_bit() {
+	uint8_t b;
+
+	SOFT_SDA_HIGH();
+	SOFT_I2C_DELAY();
+	SOFT_SCL_HIGH();
+	SOFT_I2C_DELAY();
+
+	if (SOFT_SDA_READ())
+		b = 1;
+	else
+		b = 0;
+
+	SOFT_SCL_LOW();
+	return b;
+}
+
+void I2CBB::unlock() {
+	xSemaphoreGive(I2CSemaphore);
+}
+
+bool I2CBB::lock() {
+	if (I2CSemaphore == NULL) {
+		asm("bkpt");
+	}
+	bool a = xSemaphoreTake(I2CSemaphore, (TickType_t) 100) == pdTRUE;
+	return a;
+}
+
+void I2CBB::write_bit(uint8_t val) {
+	if (val) {
+		SOFT_SDA_HIGH();
+	} else {
+		SOFT_SDA_LOW();
+	}
+
+	SOFT_I2C_DELAY();
+	SOFT_SCL_HIGH();
+	SOFT_I2C_DELAY();
+	SOFT_SCL_LOW();
+}
+
+void I2CBB::unlock2() {
+	xSemaphoreGive(I2CSemaphore2);
+}
+
+bool I2CBB::lock2() {
+	if (I2CSemaphore2 == NULL) {
+		asm("bkpt");
+	}
+	bool a = xSemaphoreTake(I2CSemaphore2, (TickType_t) 500) == pdTRUE;
+
+	return a;
+}
+#endif

source/Core/Drivers/I2CBB.hpp

@@ -0,0 +1,51 @@
+/*
+ * I2CBB.hpp
+ *
+ *  Created on: 12 Jun 2020
+ *      Author: Ralim
+ */
+
+#ifndef BSP_MINIWARE_I2CBB_HPP_
+#define BSP_MINIWARE_I2CBB_HPP_
+#include "Model_Config.h"
+#ifdef I2C_SOFT
+#include "BSP.h"
+#include "Setup.h"
+#include "Pins.h"
+#include "FreeRTOS.h"
+#include "semphr.h"
+#include "Software_I2C.h"
+
+class I2CBB {
+public:
+	static void init();
+	//Probe if device ACK's address or not
+	static bool probe(uint8_t address);
+	//Issues a complete 8bit register read
+	static bool Mem_Read(uint16_t DevAddress, uint16_t MemAddress,
+			uint8_t *pData, uint16_t Size);
+	//Implements a register write
+	static bool Mem_Write(uint16_t DevAddress, uint16_t MemAddress,
+			const uint8_t *pData, uint16_t Size);
+	static void Transmit(uint16_t DevAddress, uint8_t *pData, uint16_t Size);
+	static void Receive(uint16_t DevAddress, uint8_t *pData, uint16_t Size);
+	static void TransmitReceive(uint16_t DevAddress, uint8_t *pData_tx,
+			uint16_t Size_tx, uint8_t *pData_rx, uint16_t Size_rx);
+	static void unlock2();
+	static bool lock2();
+private:
+	static SemaphoreHandle_t I2CSemaphore;
+	static StaticSemaphore_t xSemaphoreBuffer;
+	static SemaphoreHandle_t I2CSemaphore2;
+	static StaticSemaphore_t xSemaphoreBuffer2;
+	static void unlock();
+	static bool lock();
+	static void start();
+	static void stop();
+	static bool send(uint8_t value);
+	static uint8_t read(bool ack);
+	static uint8_t read_bit();
+	static void write_bit(uint8_t val);
+};
+#endif
+#endif /* BSP_MINIWARE_I2CBB_HPP_ */

source/Core/Drivers/I2C_Wrapper.hpp

@@ -0,0 +1,58 @@
+/*
+ * FRToSI2C.hpp
+ *
+ *  Created on: 14Apr.,2018
+ *      Author: Ralim
+ */
+
+#ifndef FRTOSI2C_HPP_
+#define FRTOSI2C_HPP_
+
+#include "cmsis_os.h"
+
+/*
+ * Wrapper class to work with the device I2C bus
+ *
+ * This provides mutex protection of the peripheral
+ * Also allows hardware to use DMA should it want to
+ *
+ *
+ */
+class FRToSI2C {
+public:
+
+	static void FRToSInit() {
+		if (I2CSemaphore == nullptr) {
+			I2CSemaphore = xSemaphoreCreateBinaryStatic(&xSemaphoreBuffer);
+			xSemaphoreGive(I2CSemaphore);
+		}
+	}
+
+	static void CpltCallback(); //Normal Tx Callback
+
+	static bool Mem_Read(uint16_t DevAddress, uint16_t MemAddress, uint8_t *pData, uint16_t Size);
+	static bool Mem_Write(uint16_t DevAddress, uint16_t MemAddress, uint8_t *pData, uint16_t Size);
+	//Returns true if device ACK's being addressed
+	static bool probe(uint16_t DevAddress);
+	static bool wakePart(uint16_t DevAddress);
+	static bool Transmit(uint16_t DevAddress, uint8_t *pData, uint16_t Size);
+	static void Receive(uint16_t DevAddress, uint8_t *pData, uint16_t Size);
+	static void TransmitReceive(uint16_t DevAddress, uint8_t *pData_tx, uint16_t Size_tx, uint8_t *pData_rx, uint16_t Size_rx);
+	static bool I2C_RegisterWrite(uint8_t address, uint8_t reg, uint8_t data);
+	static uint8_t I2C_RegisterRead(uint8_t address, uint8_t reg);
+
+	typedef struct {
+		const uint8_t reg; // The register to write to
+		uint8_t val; // The value to write to this register
+		const uint8_t pause_ms; //How many ms to pause _after_ writing this reg
+	} I2C_REG;
+	static bool writeRegistersBulk(const uint8_t address, const I2C_REG* registers, const uint8_t registersLength);
+private:
+	static void unlock();
+	static bool lock();
+	static void I2C_Unstick();
+	static SemaphoreHandle_t I2CSemaphore;
+	static StaticSemaphore_t xSemaphoreBuffer;
+};
+
+#endif /* FRTOSI2C_HPP_ */

source/Core/Drivers/LIS2DH12.cpp

@@ -0,0 +1,43 @@
+/*
+ * LIS2DH12.cpp
+ *
+ *  Created on: 27Feb.,2018
+ *      Author: Ralim
+ */
+
+#include <array>
+
+#include "LIS2DH12.hpp"
+#include "cmsis_os.h"
+
+static const FRToSI2C::I2C_REG i2c_registers[] = { { LIS_CTRL_REG1, 0x17, 0 }, // 25Hz
+		{ LIS_CTRL_REG2, 0b00001000, 0 }, // Highpass filter off
+		{ LIS_CTRL_REG3, 0b01100000, 0 }, // Setup interrupt pins
+		{ LIS_CTRL_REG4, 0b00001000, 0 }, // Block update mode off, HR on
+		{ LIS_CTRL_REG5, 0b00000010, 0 }, //
+		{ LIS_CTRL_REG6, 0b01100010, 0 },
+		//Basically setup the unit to run, and enable 4D orientation detection
+		{ LIS_INT2_CFG, 0b01111110, 0 }, //setup for movement detection
+		{ LIS_INT2_THS, 0x28, 0 }, //
+		{ LIS_INT2_DURATION, 64, 0 }, //
+		{ LIS_INT1_CFG, 0b01111110, 0 }, //
+		{ LIS_INT1_THS, 0x28, 0 }, //
+		{ LIS_INT1_DURATION, 64, 0 } };
+
+bool LIS2DH12::initalize() {
+	return FRToSI2C::writeRegistersBulk(LIS2DH_I2C_ADDRESS, i2c_registers, sizeof(i2c_registers) / sizeof(i2c_registers[0]));
+}
+
+void LIS2DH12::getAxisReadings(int16_t &x, int16_t &y, int16_t &z) {
+	std::array<int16_t, 3> sensorData;
+
+	FRToSI2C::Mem_Read(LIS2DH_I2C_ADDRESS, 0xA8, reinterpret_cast<uint8_t*>(sensorData.begin()), sensorData.size() * sizeof(int16_t));
+
+	x = sensorData[0];
+	y = sensorData[1];
+	z = sensorData[2];
+}
+
+bool LIS2DH12::detect() {
+	return FRToSI2C::probe(LIS2DH_I2C_ADDRESS);
+}

source/Core/Drivers/LIS2DH12.hpp

@@ -0,0 +1,41 @@
+/*
+ * LIS2DH12.hpp
+ *
+ *  Created on: 27Feb.,2018
+ *      Author: Ralim
+ */
+
+#ifndef LIS2DH12_HPP_
+#define LIS2DH12_HPP_
+#include "I2C_Wrapper.hpp"
+#include "LIS2DH12_defines.hpp"
+#include "BSP.h"
+
+class LIS2DH12 {
+public:
+	static bool detect();
+	static bool initalize();
+	//1 = rh, 2,=lh, 8=flat
+	static Orientation getOrientation() {
+#ifdef LIS_ORI_FLIP
+		uint8_t val = (FRToSI2C::I2C_RegisterRead(LIS2DH_I2C_ADDRESS,
+		LIS_INT2_SRC) >> 2);
+		if (val == 8)
+			val = 3;
+		else if (val == 1)
+			val = 1;
+		else if (val == 2)
+			val = 0;
+		else
+			val = 3;
+		return static_cast<Orientation>(val);
+#else
+		return static_cast<Orientation>((FRToSI2C::I2C_RegisterRead(LIS2DH_I2C_ADDRESS,LIS_INT2_SRC) >> 2) - 1);
+#endif
+	}
+	static void getAxisReadings(int16_t& x, int16_t& y, int16_t& z);
+
+private:
+};
+
+#endif /* LIS2DH12_HPP_ */

source/Core/Drivers/LIS2DH12_defines.hpp

@@ -0,0 +1,28 @@
+/*
+ * LIS2DH12_defines.hpp
+ *
+ *  Created on: 27Feb.,2018
+ *      Author: Ralim
+ */
+
+#ifndef LIS2DH12_DEFINES_HPP_
+#define LIS2DH12_DEFINES_HPP_
+
+
+#define LIS2DH_I2C_ADDRESS (25<<1)
+
+#define LIS_CTRL_REG1 0x20|0x80
+#define LIS_CTRL_REG2 0x21|0x80
+#define LIS_CTRL_REG3 0x22|0x80
+#define LIS_CTRL_REG4 0x23|0x80
+#define LIS_CTRL_REG5 0x24|0x80
+#define LIS_CTRL_REG6 0x25|0x80
+#define LIS_INT1_CFG  0xB0|0x80
+#define LIS_INT2_CFG  0xB4|0x80
+#define LIS_INT1_DURATION 0x33|0x80
+#define LIS_INT1_THS 0x32|0x80
+#define LIS_INT1_SRC 0x31|0x80
+#define LIS_INT2_DURATION 0x37|0x80
+#define LIS_INT2_THS 0x36|0x80
+#define LIS_INT2_SRC 0x35|0x80
+#endif /* LIS2DH12_DEFINES_HPP_ */

source/Core/Drivers/MMA8652FC.cpp

@@ -0,0 +1,63 @@
+/*
+ * MMA8652FC.cpp
+ *
+ *  Created on: 31Aug.,2017
+ *      Author: Ben V. Brown
+ */
+
+#include <array>
+
+#include "MMA8652FC.hpp"
+#include "cmsis_os.h"
+
+
+static const FRToSI2C::I2C_REG i2c_registers[] = { { CTRL_REG2, 0, 0 },    //Normal mode
+		{ CTRL_REG2, 0x40, 2 },      // Reset all registers to POR values
+		{ FF_MT_CFG_REG, 0x78, 0 }, // Enable motion detection for X, Y, Z axis, latch disabled
+		{ PL_CFG_REG, 0x40, 0 },     //Enable the orientation detection
+		{ PL_COUNT_REG, 200, 0 },    //200 count debounce
+		{ PL_BF_ZCOMP_REG, 0b01000111, 0 }, //Set the threshold to 42 degrees
+		{ P_L_THS_REG, 0b10011100, 0 },    //Up the trip angles
+		{ CTRL_REG4, 0x01 | (1 << 4), 0 }, // Enable dataready interrupt & orientation interrupt
+		{ CTRL_REG5, 0x01, 0 }, // Route data ready interrupts to INT1 ->PB5 ->EXTI5, leaving orientation routed to INT2
+		{ CTRL_REG2, 0x12, 0 },   //Set maximum resolution oversampling
+		{ XYZ_DATA_CFG_REG, (1 << 4), 0 }, //select high pass filtered data
+		{ HP_FILTER_CUTOFF_REG, 0x03, 0 }, //select high pass filtered data
+		{ CTRL_REG1, 0x19, 0 } // ODR=12 Hz, Active mode
+};
+
+bool MMA8652FC::initalize() {
+	return FRToSI2C::writeRegistersBulk(MMA8652FC_I2C_ADDRESS, i2c_registers, sizeof(i2c_registers) / sizeof(i2c_registers[0]));
+
+}
+
+Orientation MMA8652FC::getOrientation() {
+	//First read the PL_STATUS register
+	uint8_t plStatus = FRToSI2C::I2C_RegisterRead(MMA8652FC_I2C_ADDRESS,
+	PL_STATUS_REG);
+	if ((plStatus & 0b10000000) == 0b10000000) {
+		plStatus >>= 1;    //We don't need the up/down bit
+		plStatus &= 0x03;    //mask to the two lower bits
+
+		//0 == left handed
+		//1 == right handed
+
+		return static_cast<Orientation>(plStatus);
+	}
+
+	return ORIENTATION_FLAT;
+}
+
+void MMA8652FC::getAxisReadings(int16_t &x, int16_t &y, int16_t &z) {
+	std::array<int16_t, 3> sensorData;
+
+	FRToSI2C::Mem_Read(MMA8652FC_I2C_ADDRESS, OUT_X_MSB_REG, reinterpret_cast<uint8_t*>(sensorData.begin()), sensorData.size() * sizeof(int16_t));
+
+	x = static_cast<int16_t>(__builtin_bswap16(*reinterpret_cast<uint16_t*>(&sensorData[0])));
+	y = static_cast<int16_t>(__builtin_bswap16(*reinterpret_cast<uint16_t*>(&sensorData[1])));
+	z = static_cast<int16_t>(__builtin_bswap16(*reinterpret_cast<uint16_t*>(&sensorData[2])));
+}
+
+bool MMA8652FC::detect() {
+	return FRToSI2C::probe(MMA8652FC_I2C_ADDRESS);
+}

source/Core/Drivers/MMA8652FC.hpp

@@ -0,0 +1,27 @@
+/*
+ * MMA8652FC.h
+ *
+ *  Created on: 31Aug.,2017
+ *      Author: Ben V. Brown
+ */
+
+#ifndef MMA8652FC_HPP_
+#define MMA8652FC_HPP_
+#include "MMA8652FC_defines.h"
+#include "I2C_Wrapper.hpp"
+#include "BSP.h"
+
+class MMA8652FC {
+
+public:
+	//Returns true if this accelerometer is detected
+	static bool detect();
+	//Init any internal state
+	static bool initalize();
+	static Orientation getOrientation(); // Reads the I2C register and returns the orientation (true == left)
+	static void getAxisReadings(int16_t &x, int16_t &y, int16_t &z);
+
+private:
+};
+
+#endif /* MMA8652FC_HPP_ */

source/Core/Drivers/MMA8652FC_defines.h

@@ -0,0 +1,124 @@
+/*
+ * MMA8652FC_defines.h
+ *
+ *  Created on: 31Aug.,2017
+ *      Author: Ben V. Brown
+ */
+
+#ifndef MMA8652FC_DEFINES_H_
+#define MMA8652FC_DEFINES_H_
+
+//--------------MMA8652 Registers-------------------------------------------//
+
+#define STATUS_REG            0x00		// STATUS Register
+
+#define OUT_X_MSB_REG         0x01		// [7:0] are 8 MSBs of the 14-bit X-axis sample
+#define OUT_X_LSB_REG         0x02		// [7:2] are the 6 LSB of 14-bit X-axis sample
+#define OUT_Y_MSB_REG         0x03		// [7:0] are 8 MSBs of the 14-bit Y-axis sample
+#define OUT_Y_LSB_REG         0x04		// [7:2] are the 6 LSB of 14-bit Y-axis sample
+#define OUT_Z_MSB_REG         0x05		// [7:0] are 8 MSBs of the 14-bit Z-axis sample
+#define OUT_Z_LSB_REG         0x06		// [7:2] are the 6 LSB of 14-bit Z-axis sample
+
+#define F_SETUP_REG           0x09    	// F_SETUP FIFO Setup Register
+#define TRIG_CFG_REG          0x0A    	// TRIG_CFG Map of FIFO data capture events
+#define SYSMOD_REG            0x0B    	// SYSMOD System Mode Register
+#define INT_SOURCE_REG        0x0C    	// INT_SOURCE System Interrupt Status Register
+#define WHO_AM_I_REG          0x0D    	// WHO_AM_I Device ID Register
+#define XYZ_DATA_CFG_REG      0x0E    	// XYZ_DATA_CFG Sensor Data Configuration Register
+#define HP_FILTER_CUTOFF_REG  0x0F    	// HP_FILTER_CUTOFF High Pass Filter Register
+
+#define PL_STATUS_REG         0x10    	// PL_STATUS Portrait/Landscape Status Register
+#define PL_CFG_REG            0x11    	// PL_CFG Portrait/Landscape Configuration Register
+#define PL_COUNT_REG          0x12    	// PL_COUNT Portrait/Landscape Debounce Register
+#define PL_BF_ZCOMP_REG       0x13    	// PL_BF_ZCOMP Back/Front and Z Compensation Register
+#define P_L_THS_REG           0x14    	// P_L_THS Portrait to Landscape Threshold Register
+
+#define FF_MT_CFG_REG         0x15    	// FF_MT_CFG Freefall and Motion Configuration Register
+#define FF_MT_SRC_REG         0x16    	// FF_MT_SRC Freefall and Motion Source Register
+#define FF_MT_THS_REG         0x17    	// FF_MT_THS Freefall and Motion Threshold Register
+#define FF_MT_COUNT_REG       0x18    	// FF_MT_COUNT Freefall Motion Count Register
+
+#define TRANSIENT_CFG_REG     0x1D    	// TRANSIENT_CFG Transient Configuration Register
+#define TRANSIENT_SRC_REG     0x1E    	// TRANSIENT_SRC Transient Source Register
+#define TRANSIENT_THS_REG     0x1F    	// TRANSIENT_THS Transient Threshold Register
+#define TRANSIENT_COUNT_REG   0x20    	// TRANSIENT_COUNT Transient Debounce Counter Register
+
+#define PULSE_CFG_REG         0x21    	// PULSE_CFG Pulse Configuration Register
+#define PULSE_SRC_REG         0x22    	// PULSE_SRC Pulse Source Register
+#define PULSE_THSX_REG        0x23    	// PULSE_THS XYZ Pulse Threshold Registers
+#define PULSE_THSY_REG        0x24
+#define PULSE_THSZ_REG        0x25
+#define PULSE_TMLT_REG        0x26    	// PULSE_TMLT Pulse Time Window Register
+#define PULSE_LTCY_REG        0x27    	// PULSE_LTCY Pulse Latency Timer Register
+#define PULSE_WIND_REG        0x28    	// PULSE_WIND Second Pulse Time Window Register
+
+#define ASLP_COUNT_REG        0x29    	// ASLP_COUNT Auto Sleep Inactivity Timer Register
+
+#define CTRL_REG1             0x2A    	// CTRL_REG1 System Control 1 Register
+#define CTRL_REG2             0x2B    	// CTRL_REG2 System Control 2 Register
+#define CTRL_REG3             0x2C    	// CTRL_REG3 Interrupt Control Register
+#define CTRL_REG4             0x2D    	// CTRL_REG4 Interrupt Enable Register
+#define CTRL_REG5             0x2E    	// CTRL_REG5 Interrupt Configuration Register
+
+#define OFF_X_REG             0x2F    	// XYZ Offset Correction Registers
+#define OFF_Y_REG             0x30
+#define OFF_Z_REG             0x31
+
+//MMA8652FC 7-bit I2C address
+
+#define MMA8652FC_I2C_ADDRESS (0x1D<<1)
+
+//MMA8652FC Sensitivity
+
+#define SENSITIVITY_2G		  1024
+#define SENSITIVITY_4G		  512
+#define SENSITIVITY_8G		  256
+
+#define STATUS_REG    0x00
+#define X_MSB_REG     0X01
+#define X_LSB_REG     0X02
+#define Y_MSB_REG     0X03
+#define Y_LSB_REG     0X04
+#define Z_MSB_REG     0X05
+#define Z_LSB_REG     0X06
+
+#define TRIG_CFG      0X0A
+#define SYSMOD        0X0B
+#define INT_SOURCE    0X0C
+#define DEVICE_ID     0X0D
+
+//-----STATUS_REG(0X00)-----Bit Define----------------------------------------//
+#define ZYXDR_BIT     0X08
+//----XYZ_DATA_CFG_REG(0xE)-Bit Define----------------------------------------//
+#define FS_MASK       0x03
+#define FULL_SCALE_2G 0x00      //2g=0x0,4g=0x1,8g=0x2
+#define FULL_SCALE_4G 0x01
+#define FULL_SCALE_8G 0x02
+//---------CTRL_REG1(0X2A)Bit Define------------------------------------------//
+#define ACTIVE_MASK   1<<0       //bit0
+#define DR_MASK       0x38      //bit D5,D4,D3
+#define FHZ800        0x0       //800hz
+#define FHZ400        0x1       //400hz
+#define FHZ200        0x2       //200hz
+#define FHZ100        0x3       //100hz
+#define FHZ50         0x4       //50hz
+#define FHZ2          0x5       //12.5hz
+#define FHZ1          0x6       //6.25hz
+#define FHZ0          0x7       //1.563hz
+
+//---------CTRL_REG2(0X2B)Bit Define------------------------------------------//
+#define MODS_MASK     0x03     //Oversampling Mode 4
+#define Normal_Mode   0x0      //Normal=0,Low Noise Low Power MODS=1,
+//HI RESOLUTION=2,LOW POWER MODS = 11
+//----CTRL_REG4---Interrupt Enable BIT ---------------------------------------//
+//0 interrupt is disabled (default)
+//1 interrupt is enabled
+#define INT_EN_ASLP     1<<7    //Auto-SLEEP/WAKE Interrupt Enable
+#define INT_EN_FIFO     1<<6    //FIFO Interrupt Enable
+#define INT_EN_TRANS    1<<5    //Transient Interrupt Enable
+#define INT_EN_LNDPRT   1<<4    //Orientation(Landscape/Portrait)Interrupt Enable
+#define INT_EN_PULSE    1<<3    //Pulse Detection Interrupt Enable
+#define INT_EN_FF_MT    1<<2    //Freefall/Motion Interrupt Enable
+#define INT_EN_DRDY     1<<0    //Data Ready Interrupt Enable
+
+#endif /* MMA8652FC_DEFINES_H_ */

source/Core/Drivers/MSA301.cpp

@@ -0,0 +1,50 @@
+/*
+ * MSA301.cpp
+ *
+ *  Created on: 3 Jan 2021
+ *      Author: Ralim
+ */
+
+#include <MSA301.h>
+#include "MSA301_defines.h"
+#define MSA301_I2C_ADDRESS 0x4C
+bool MSA301::detect() {
+	return FRToSI2C::probe(MSA301_I2C_ADDRESS);
+}
+
+static const FRToSI2C::I2C_REG i2c_registers[] = { //
+		//
+				{ MSA301_REG_ODR, 0b00001000, 1 },    //X/Y/Z enabled @ 250Hz
+				{ MSA301_REG_POWERMODE, 0b0001001, 1 },    // Normal mode
+				{ MSA301_REG_RESRANGE, 0b00000001, 0 },    // 14bit resolution @ 4G range
+				{ MSA301_REG_ORIENT_HY, 0b01000000, 0 },    // 4*62.5mg hyst, no blocking, symmetrical
+				{ MSA301_REG_INTSET0, 1 << 6, 0 },    // Turn on orientation detection (by enabling its interrupt)
+
+		};
+
+bool MSA301::initalize() {
+	return FRToSI2C::writeRegistersBulk(MSA301_I2C_ADDRESS, i2c_registers, sizeof(i2c_registers) / sizeof(i2c_registers[0]));
+}
+
+Orientation MSA301::getOrientation() {
+	uint8_t temp = 0;
+	FRToSI2C::Mem_Read(MSA301_I2C_ADDRESS, MSA301_REG_ORIENT_STATUS, &temp, 1);
+	switch (temp) {
+	case 112:
+		return Orientation::ORIENTATION_LEFT_HAND;
+	case 96:
+		return Orientation::ORIENTATION_RIGHT_HAND;
+	default:
+		return Orientation::ORIENTATION_FLAT;
+	}
+}
+
+void MSA301::getAxisReadings(int16_t &x, int16_t &y, int16_t &z) {
+	uint8_t temp[6];
+	//Bulk read all 6 regs
+	FRToSI2C::Mem_Read(MSA301_I2C_ADDRESS, MSA301_REG_OUT_X_L, temp, 6);
+	x = int16_t(((int16_t) temp[1]) << 8 | temp[0]) >> 2;
+	y = int16_t(((int16_t) temp[3]) << 8 | temp[2]) >> 2;
+	z = int16_t(((int16_t) temp[5]) << 8 | temp[4]) >> 2;
+
+}

source/Core/Drivers/MSA301.h

@@ -0,0 +1,27 @@
+/*
+ * MSA301.h
+ *
+ *  Created on: 3 Jan 2021
+ *      Author: Ralim
+ */
+
+#ifndef DRIVERS_MSA301_H_
+#define DRIVERS_MSA301_H_
+#include "I2C_Wrapper.hpp"
+#include "BSP.h"
+
+class MSA301 {
+public:
+	//Returns true if this accelerometer is detected
+	static bool detect();
+	//Init any internal state
+	static bool initalize();
+	// Reads the I2C register and returns the orientation
+	static Orientation getOrientation();
+	//Return the x/y/z axis readings as signed int16's
+	static void getAxisReadings(int16_t &x, int16_t &y, int16_t &z);
+
+private:
+};
+
+#endif /* DRIVERS_MSA301_H_ */

source/Core/Drivers/MSA301_defines.h

@@ -0,0 +1,34 @@
+/*
+ * MSA301_defines.h
+ *
+ *  Created on: 3 Jan 2021
+ *      Author: Ralim
+ */
+
+#ifndef DRIVERS_MSA301_DEFINES_H_
+#define DRIVERS_MSA301_DEFINES_H_
+//Definitions from Adafruit <3
+
+#define MSA301_REG_PARTID 0x01    ///< Register that contains the part ID
+#define MSA301_REG_OUT_X_L 0x02   ///< Register address for X axis lower byte
+#define MSA301_REG_OUT_X_H 0x03   ///< Register address for X axis higher byte
+#define MSA301_REG_OUT_Y_L 0x04   ///< Register address for Y axis lower byte
+#define MSA301_REG_OUT_Y_H 0x05   ///< Register address for Y axis higher byte
+#define MSA301_REG_OUT_Z_L 0x06   ///< Register address for Z axis lower byte
+#define MSA301_REG_OUT_Z_H 0x07   ///< Register address for Z axis higher byte
+#define MSA301_REG_MOTIONINT 0x09 ///< Register address for motion interrupt
+#define MSA301_REG_DATAINT 0x0A   ///< Register address for data interrupt
+#define MSA301_REG_CLICKSTATUS 0x0B ///< Register address for click/doubleclick status
+#define MSA301_REG_RESRANGE 0x0F  ///< Register address for resolution range
+#define MSA301_REG_ODR 0x10       ///< Register address for data rate setting
+#define MSA301_REG_POWERMODE 0x11 ///< Register address for power mode setting
+#define MSA301_REG_INTSET0 0x16   ///< Register address for interrupt setting #0
+#define MSA301_REG_INTSET1 0x17   ///< Register address for interrupt setting #1
+#define MSA301_REG_INTMAP0 0x19   ///< Register address for interrupt map #0
+#define MSA301_REG_INTMAP1 0x1A   ///< Register address for interrupt map #1
+#define MSA301_REG_TAPDUR 0x2A    ///< Register address for tap duration
+#define MSA301_REG_TAPTH 0x2B     ///< Register address for tap threshold
+#define MSA301_REG_ORIENT_HY 0x2C ///< Register address for orientation Hysteresis
+#define MSA301_REG_ORIENT_STATUS 0x0C ///< Register address for orientation hysteresis
+
+#endif /* DRIVERS_MSA301_DEFINES_H_ */

source/Core/Drivers/OLED.cpp

@@ -0,0 +1,489 @@
+/*
+ * OLED.cpp
+ *
+ *  Created on: 29Aug.,2017
+ *      Author: Ben V. Brown
+ */
+
+#include <string.h>
+#include <OLED.hpp>
+#include <stdlib.h>
+#include "Translation.h"
+#include "cmsis_os.h"
+#include "../../configuration.h"
+
+const uint8_t *OLED::currentFont;  // Pointer to the current font used for
+// rendering to the buffer
+uint8_t *OLED::firstStripPtr;      // Pointers to the strips to allow for buffer
+// having extra content
+uint8_t *OLED::secondStripPtr;     // Pointers to the strips
+bool OLED::inLeftHandedMode;   // Whether the screen is in left or not (used for
+// offsets in GRAM)
+OLED::DisplayState OLED::displayState;
+uint8_t OLED::fontWidth, OLED::fontHeight;
+int16_t OLED::cursor_x, OLED::cursor_y;
+bool OLED::initDone = false;
+uint8_t OLED::displayOffset;
+uint8_t OLED::screenBuffer[16 + (OLED_WIDTH * 2) + 10];  // The data buffer
+uint8_t OLED::secondFrameBuffer[OLED_WIDTH * 2];
+
+/*Setup params for the OLED screen*/
+/*http://www.displayfuture.com/Display/datasheet/controller/SSD1307.pdf*/
+/*All commands are prefixed with 0x80*/
+/*Data packets are prefixed with 0x40*/
+FRToSI2C::I2C_REG OLED_Setup_Array[] = {
+/**/
+{ 0x80, 0xAE, 0 }, /*Display off*/
+{ 0x80, 0xD5, 0 }, /*Set display clock divide ratio / osc freq*/
+{ 0x80, 0x52, 0 }, /*Divide ratios*/
+{ 0x80, 0xA8, 0 }, /*Set Multiplex Ratio*/
+{ 0x80, 0x0F, 0 }, /*16 == max brightness,39==dimmest*/
+{ 0x80, 0xC0, 0 }, /*Set COM Scan direction*/
+{ 0x80, 0xD3, 0 }, /*Set vertical Display offset*/
+{ 0x80, 0x00, 0 }, /*0 Offset*/
+{ 0x80, 0x40, 0 }, /*Set Display start line to 0*/
+{ 0x80, 0xA0, 0 }, /*Set Segment remap to normal*/
+{ 0x80, 0x8D, 0 }, /*Charge Pump*/
+{ 0x80, 0x14, 0 }, /*Charge Pump settings*/
+{ 0x80, 0xDA, 0 }, /*Set VCOM Pins hardware config*/
+{ 0x80, 0x02, 0 }, /*Combination 2*/
+{ 0x80, 0x81, 0 }, /*Contrast*/
+{ 0x80, 0x33, 0 }, /*^51*/
+{ 0x80, 0xD9, 0 }, /*Set pre-charge period*/
+{ 0x80, 0xF1, 0 }, /*Pre charge period*/
+{ 0x80, 0xDB, 0 }, /*Adjust VCOMH regulator ouput*/
+{ 0x80, 0x30, 0 }, /*VCOM level*/
+{ 0x80, 0xA4, 0 }, /*Enable the display GDDR*/
+{ 0x80, 0XA6, 0 }, /*Normal display*/
+{ 0x80, 0x20, 0 }, /*Memory Mode*/
+{ 0x80, 0x00, 0 }, /*Wrap memory*/
+{ 0x80, 0xAF, 0 }, /*Display on*/
+};
+// Setup based on the SSD1307 and modified for the SSD1306
+
+const uint8_t REFRESH_COMMANDS[17] = { 0x80, 0xAF, 0x80, 0x21, 0x80, 0x20, 0x80, 0x7F, 0x80, 0xC0, 0x80, 0x22, 0x80, 0x00, 0x80, 0x01, 0x40 };
+
+/*
+ * Animation timing function that follows a bezier curve.
+ * @param t A given percentage value [0..<100]
+ * Returns a new percentage value with ease in and ease out.
+ * Original floating point formula: t * t * (3.0f - 2.0f * t);
+ */
+static uint8_t easeInOutTiming(uint8_t t) {
+	return t * t * (300 - 2 * t) / 10000;
+}
+
+/*
+ * Returns the value between a and b, using a percentage value t.
+ * @param a The value associated with 0%
+ * @param b The value associated with 100%
+ * @param t The percentage [0..<100]
+ */
+static uint8_t lerp(uint8_t a, uint8_t b, uint8_t t) {
+	return a + t * (b - a) / 100;
+}
+
+void OLED::initialize() {
+	cursor_x = cursor_y = 0;
+	currentFont = USER_FONT_12;
+	fontWidth = 12;
+	inLeftHandedMode = false;
+	firstStripPtr = &screenBuffer[FRAMEBUFFER_START];
+	secondStripPtr = &screenBuffer[FRAMEBUFFER_START + OLED_WIDTH];
+	fontHeight = 16;
+	displayOffset = 0;
+	memcpy(&screenBuffer[0], &REFRESH_COMMANDS[0], sizeof(REFRESH_COMMANDS));
+
+	// Set the display to be ON once the settings block is sent and send the
+	// initialisation data to the OLED.
+
+	for (int tries = 0; tries < 10; tries++) {
+		if (FRToSI2C::writeRegistersBulk(DEVICEADDR_OLED, OLED_Setup_Array, sizeof(OLED_Setup_Array) / sizeof(OLED_Setup_Array[0]))) {
+			return;
+		}
+	}
+	setDisplayState(DisplayState::ON);
+	initDone = true;
+}
+void OLED::setFramebuffer(uint8_t *buffer) {
+	if (buffer == NULL) {
+		firstStripPtr = &screenBuffer[FRAMEBUFFER_START];
+		secondStripPtr = &screenBuffer[FRAMEBUFFER_START + OLED_WIDTH];
+		return;
+	}
+
+	firstStripPtr = &buffer[0];
+	secondStripPtr = &buffer[OLED_WIDTH];
+}
+
+/*
+ * Prints a char to the screen.
+ * UTF font handling is done using the two input chars.
+ * Precursor is the command char that is used to select the table.
+ */
+void OLED::drawChar(char c) {
+	if (c == '\x01' && cursor_y == 0) { // 0x01 is used as new line char
+		cursor_x = 0;
+		cursor_y = 8;
+		return;
+	} else if (c == 0) {
+		return;
+	}
+	uint16_t index = c - 2; //First index is \x02
+	uint8_t *charPointer;
+	charPointer = ((uint8_t*) currentFont) + ((fontWidth * (fontHeight / 8)) * index);
+	drawArea(cursor_x, cursor_y, fontWidth, fontHeight, charPointer);
+	cursor_x += fontWidth;
+}
+
+/*
+ * Draws a one pixel wide scrolling indicator. y is the upper vertical position
+ * of the indicator in pixels (0..<16).
+ */
+void OLED::drawScrollIndicator(uint8_t y, uint8_t height) {
+	union u_type {
+		uint16_t whole;
+		uint8_t strips[2];
+	} column;
+
+	column.whole = (1 << height) - 1;
+	column.whole <<= y;
+
+	// Draw a one pixel wide bar to the left with a single pixel as
+	// the scroll indicator.
+	fillArea(OLED_WIDTH - 1, 0, 1, 8, column.strips[0]);
+	fillArea(OLED_WIDTH - 1, 8, 1, 8, column.strips[1]);
+}
+
+/**
+ * Plays a transition animation between two framebuffers.
+ * @param forwardNavigation Direction of the navigation animation.
+ *
+ * If forward is true, this displays a forward navigation to the second framebuffer contents.
+ * Otherwise a rewinding navigation animation is shown to the second framebuffer contents.
+ */
+void OLED::transitionSecondaryFramebuffer(bool forwardNavigation) {
+	uint8_t *firstBackStripPtr = &secondFrameBuffer[0];
+	uint8_t *secondBackStripPtr = &secondFrameBuffer[OLED_WIDTH];
+
+	uint32_t totalDuration = 50; // 500ms
+	uint32_t duration = 0;
+	uint32_t start = xTaskGetTickCount();
+	uint8_t offset = 0;
+
+	while (duration <= totalDuration) {
+		duration = xTaskGetTickCount() - start;
+		uint8_t progress = duration * TICKS_SECOND / totalDuration;
+		progress = easeInOutTiming(progress);
+		progress = lerp(0, OLED_WIDTH, progress);
+		if (progress > OLED_WIDTH) {
+			progress = OLED_WIDTH;
+		}
+
+// When forward, current contents move to the left out.
+// Otherwise the contents move to the right out.
+		uint8_t oldStart = forwardNavigation ? 0 : progress;
+		uint8_t oldPrevious = forwardNavigation ? progress - offset : offset;
+
+// Content from the second framebuffer moves in from the right (forward)
+// or from the left (not forward).
+		uint8_t newStart = forwardNavigation ? OLED_WIDTH - progress : 0;
+		uint8_t newEnd = forwardNavigation ? 0 : OLED_WIDTH - progress;
+
+		offset = progress;
+
+		memmove(&firstStripPtr[oldStart], &firstStripPtr[oldPrevious],
+		OLED_WIDTH - progress);
+		memmove(&secondStripPtr[oldStart], &secondStripPtr[oldPrevious],
+		OLED_WIDTH - progress);
+
+		memmove(&firstStripPtr[newStart], &firstBackStripPtr[newEnd], progress);
+		memmove(&secondStripPtr[newStart], &secondBackStripPtr[newEnd], progress);
+
+		refresh();
+		osDelay(40);
+	}
+}
+
+void OLED::useSecondaryFramebuffer(bool useSecondary) {
+	if (useSecondary) {
+		setFramebuffer(secondFrameBuffer);
+	} else {
+		setFramebuffer(NULL);
+	}
+}
+
+void OLED::setRotation(bool leftHanded) {
+#ifdef OLED_FLIP
+	leftHanded = !leftHanded;
+#endif
+	if (inLeftHandedMode == leftHanded) {
+		return;
+	}
+
+	// send command struct again with changes
+	if (leftHanded) {
+		OLED_Setup_Array[5].val = 0xC8;  // c1?
+		OLED_Setup_Array[9].val = 0xA1;
+	} else {
+		OLED_Setup_Array[5].val = 0xC0;
+		OLED_Setup_Array[9].val = 0xA0;
+	}
+	FRToSI2C::writeRegistersBulk(DEVICEADDR_OLED, OLED_Setup_Array, sizeof(OLED_Setup_Array) / sizeof(OLED_Setup_Array[0]));
+
+	inLeftHandedMode = leftHanded;
+
+	screenBuffer[5] = inLeftHandedMode ? 0 : 32; // display is shifted by 32 in left handed
+												 // mode as driver ram is 128 wide
+	screenBuffer[7] = inLeftHandedMode ? 95 : 0x7F;													 // End address of the ram segment we are writing to (96 wide)
+	screenBuffer[9] = inLeftHandedMode ? 0xC8 : 0xC0;
+}
+
+// print a string to the current cursor location
+void OLED::print(const char *str) {
+	while (str[0]) {
+		drawChar(str[0]);
+		str++;
+	}
+}
+
+void OLED::setFont(uint8_t fontNumber) {
+	if (fontNumber == 1) {
+// small font
+		currentFont = USER_FONT_6x8;
+		fontHeight = 8;
+		fontWidth = 6;
+	} else if (fontNumber == 2) {
+		currentFont = ExtraFontChars;
+		fontHeight = 16;
+		fontWidth = 12;
+	} else {
+		currentFont = USER_FONT_12;
+		fontHeight = 16;
+		fontWidth = 12;
+	}
+}
+uint8_t OLED::getFont() {
+	if (currentFont == USER_FONT_6x8)
+		return 1;
+	else if (currentFont == ExtraFontChars)
+		return 2;
+	else
+		return 0;
+}
+inline void stripLeaderZeros(char *buffer, uint8_t places) {
+	//Removing the leading zero's by swapping them to SymbolSpace
+	// Stop 1 short so that we dont blank entire number if its zero
+	for (int i = 0; i < (places - 1); i++) {
+		if (buffer[i] == 2) {
+			buffer[i] = SymbolSpace[0];
+		} else {
+			return;
+		}
+	}
+}
+// maximum places is 5
+void OLED::printNumber(uint16_t number, uint8_t places, bool noLeaderZeros) {
+	char buffer[7] = { 0 };
+
+	if (places >= 5) {
+		buffer[5] = 2 + number % 10;
+		number /= 10;
+	}
+	if (places > 4) {
+		buffer[4] = 2 + number % 10;
+		number /= 10;
+	}
+
+	if (places > 3) {
+		buffer[3] = 2 + number % 10;
+		number /= 10;
+	}
+
+	if (places > 2) {
+		buffer[2] = 2 + number % 10;
+		number /= 10;
+	}
+
+	if (places > 1) {
+		buffer[1] = 2 + number % 10;
+		number /= 10;
+	}
+
+	buffer[0] = 2 + number % 10;
+	if (noLeaderZeros)
+		stripLeaderZeros(buffer, places);
+	print(buffer);
+}
+
+void OLED::debugNumber(int32_t val) {
+	if (abs(val) > 99999) {
+		OLED::print(SymbolSpace); // out of bounds
+		return;
+	}
+	if (val >= 0) {
+		OLED::print(SymbolSpace);
+		OLED::printNumber(val, 5);
+	} else {
+		OLED::print(SymbolMinus);
+		OLED::printNumber(-val, 5);
+	}
+}
+
+void OLED::drawSymbol(uint8_t symbolID) {
+	// draw a symbol to the current cursor location
+	setFont(2);
+	drawChar(symbolID + 2);
+	setFont(0);
+}
+
+// Draw an area, but y must be aligned on 0/8 offset
+void OLED::drawArea(int16_t x, int8_t y, uint8_t wide, uint8_t height, const uint8_t *ptr) {
+	// Splat this from x->x+wide in two strides
+	if (x <= -wide)
+		return; // cutoffleft
+	if (x > 96)
+		return; // cutoff right
+
+	uint8_t visibleStart = 0;
+	uint8_t visibleEnd = wide;
+
+	// trimming to draw partials
+	if (x < 0) {
+		visibleStart -= x;  // subtract negative value == add absolute value
+	}
+	if (x + wide > 96) {
+		visibleEnd = 96 - x;
+	}
+
+	if (y == 0) {
+// Splat first line of data
+		for (uint8_t xx = visibleStart; xx < visibleEnd; xx++) {
+			firstStripPtr[xx + x] = ptr[xx];
+		}
+	}
+	if (y == 8 || height == 16) {
+// Splat the second line
+		for (uint8_t xx = visibleStart; xx < visibleEnd; xx++) {
+			secondStripPtr[x + xx] = ptr[xx + (height == 16 ? wide : 0)];
+		}
+	}
+}
+
+// Draw an area, but y must be aligned on 0/8 offset
+// For data which has octets swapped in a 16-bit word.
+void OLED::drawAreaSwapped(int16_t x, int8_t y, uint8_t wide, uint8_t height, const uint8_t *ptr) {
+	// Splat this from x->x+wide in two strides
+	if (x <= -wide)
+		return;  // cutoffleft
+	if (x > 96)
+		return;  // cutoff right
+
+	uint8_t visibleStart = 0;
+	uint8_t visibleEnd = wide;
+
+	// trimming to draw partials
+	if (x < 0) {
+		visibleStart -= x;  // subtract negative value == add absolute value
+	}
+	if (x + wide > 96) {
+		visibleEnd = 96 - x;
+	}
+
+	if (y == 0) {
+		// Splat first line of data
+		for (uint8_t xx = visibleStart; xx < visibleEnd; xx += 2) {
+			firstStripPtr[xx + x] = ptr[xx + 1];
+			firstStripPtr[xx + x + 1] = ptr[xx];
+		}
+	}
+	if (y == 8 || height == 16) {
+		// Splat the second line
+		for (uint8_t xx = visibleStart; xx < visibleEnd; xx += 2) {
+			secondStripPtr[x + xx] = ptr[xx + 1 + (height == 16 ? wide : 0)];
+			secondStripPtr[x + xx + 1] = ptr[xx + (height == 16 ? wide : 0)];
+		}
+	}
+}
+
+void OLED::fillArea(int16_t x, int8_t y, uint8_t wide, uint8_t height, const uint8_t value) {
+	// Splat this from x->x+wide in two strides
+	if (x <= -wide)
+		return;  // cutoffleft
+	if (x > 96)
+		return;  // cutoff right
+
+	uint8_t visibleStart = 0;
+	uint8_t visibleEnd = wide;
+
+	// trimming to draw partials
+	if (x < 0) {
+		visibleStart -= x;  // subtract negative value == add absolute value
+	}
+	if (x + wide > 96) {
+		visibleEnd = 96 - x;
+	}
+
+	if (y == 0) {
+// Splat first line of data
+		for (uint8_t xx = visibleStart; xx < visibleEnd; xx++) {
+			firstStripPtr[xx + x] = value;
+		}
+	}
+	if (y == 8 || height == 16) {
+// Splat the second line
+		for (uint8_t xx = visibleStart; xx < visibleEnd; xx++) {
+			secondStripPtr[x + xx] = value;
+		}
+	}
+}
+
+void OLED::drawFilledRect(uint8_t x0, uint8_t y0, uint8_t x1, uint8_t y1, bool clear) {
+	// Draw this in 3 sections
+	// This is basically a N wide version of vertical line
+
+	// Step 1 : Draw in the top few pixels that are not /8 aligned
+	// LSB is at the top of the screen
+	uint8_t mask = 0xFF;
+	if (y0) {
+		mask = mask << (y0 % 8);
+		for (uint8_t col = x0; col < x1; col++)
+			if (clear)
+				firstStripPtr[(y0 / 8) * 96 + col] &= ~mask;
+			else
+				firstStripPtr[(y0 / 8) * 96 + col] |= mask;
+	}
+	// Next loop down the line the total number of solids
+	if (y0 / 8 != y1 / 8)
+		for (uint8_t col = x0; col < x1; col++)
+			for (uint8_t r = (y0 / 8); r < (y1 / 8); r++) {
+				// This gives us the row index r
+				if (clear)
+					firstStripPtr[(r * 96) + col] = 0;
+				else
+					firstStripPtr[(r * 96) + col] = 0xFF;
+			}
+
+	// Finally draw the tail
+	mask = ~(mask << (y1 % 8));
+	for (uint8_t col = x0; col < x1; col++)
+		if (clear)
+			firstStripPtr[(y1 / 8) * 96 + col] &= ~mask;
+		else
+			firstStripPtr[(y1 / 8) * 96 + col] |= mask;
+}
+
+void OLED::drawHeatSymbol(uint8_t state) {
+	// Draw symbol 14
+	// Then draw over it, the bottom 5 pixels always stay. 8 pixels above that are
+	// the levels masks the symbol nicely
+	state /= 31;	// 0-> 8 range
+	// Then we want to draw down (16-(5+state)
+	uint8_t cursor_x_temp = cursor_x;
+	drawSymbol(14);
+	drawFilledRect(cursor_x_temp, 0, cursor_x_temp + 12, 2 + (8 - state), true);
+}
+
+bool OLED::isInitDone() {
+	return initDone;
+}

source/Core/Drivers/OLED.hpp

@@ -0,0 +1,116 @@
+/*
+ * OLED.hpp
+ *
+ *  Created on: 20Jan.,2017
+ *      Author: Ben V. Brown <Ralim>
+ *      Designed for the SSD1307
+ *      Cleared for release for TS100 2017/08/20
+ */
+
+#ifndef OLED_HPP_
+#define OLED_HPP_
+#include <BSP.h>
+#include <stdbool.h>
+#include <string.h>
+#include "I2C_Wrapper.hpp"
+#include "Font.h"
+#ifdef __cplusplus
+extern "C" {
+#endif
+#include "FreeRTOS.h"
+#ifdef __cplusplus
+}
+#endif
+#define DEVICEADDR_OLED   (0x3c<<1)
+#define OLED_WIDTH        96
+#define OLED_HEIGHT       16
+#define FRAMEBUFFER_START 17
+
+class OLED {
+public:
+
+	enum DisplayState : bool {
+		OFF = false, ON = true
+	};
+
+	static void initialize(); // Startup the I2C coms (brings screen out of reset etc)
+	static bool isInitDone();
+	// Draw the buffer out to the LCD using the DMA Channel
+	static void refresh() {
+		FRToSI2C::Transmit( DEVICEADDR_OLED, screenBuffer,
+		FRAMEBUFFER_START + (OLED_WIDTH * 2));
+		//DMA tx time is ~ 20mS Ensure after calling this you delay for at least 25ms
+		//or we need to goto double buffering
+	}
+
+	static void setDisplayState(DisplayState state) {
+		displayState = state;
+		screenBuffer[1] = (state == ON) ? 0xAF : 0xAE;
+	}
+
+	static void setRotation(bool leftHanded); // Set the rotation for the screen
+	// Get the current rotation of the LCD
+	static bool getRotation() {
+		return inLeftHandedMode;
+	}
+	static int16_t getCursorX() {
+		return cursor_x;
+	}
+	static void print(const char *string);	// Draw a string to the current location, with current font
+	// Set the cursor location by pixels
+	static void setCursor(int16_t x, int16_t y) {
+		cursor_x = x;
+		cursor_y = y;
+	}
+	//Set cursor location by chars in current font
+	static void setCharCursor(int16_t x, int16_t y) {
+		cursor_x = x * fontWidth;
+		cursor_y = y * fontHeight;
+	}
+	static void setFont(uint8_t fontNumber); // (Future) Set the font that is being used
+	static uint8_t getFont();
+	static void drawImage(const uint8_t *buffer, uint8_t x, uint8_t width) {
+		drawArea(x, 0, width, 16, buffer);
+	}
+	// Draws an image to the buffer, at x offset from top to bottom (fixed height renders)
+	static void printNumber(uint16_t number, uint8_t places, bool noLeaderZeros = true);
+	// Draws a number at the current cursor location
+	// Clears the buffer
+	static void clearScreen() {
+		memset(firstStripPtr, 0, OLED_WIDTH * 2);
+	}
+	// Draws the battery level symbol
+	static void drawBattery(uint8_t state) {
+		drawSymbol(3 + (state > 10 ? 10 : state));
+	}
+	// Draws a checkbox
+	static void drawCheckbox(bool state) {
+		drawSymbol((state) ? 16 : 17);
+	}
+	static void debugNumber(int32_t val);
+	static void drawSymbol(uint8_t symbolID);	//Used for drawing symbols of a predictable width
+	static void drawArea(int16_t x, int8_t y, uint8_t wide, uint8_t height, const uint8_t *ptr); //Draw an area, but y must be aligned on 0/8 offset
+	static void drawAreaSwapped(int16_t x, int8_t y, uint8_t wide, uint8_t height, const uint8_t *ptr); //Draw an area, but y must be aligned on 0/8 offset
+	static void fillArea(int16_t x, int8_t y, uint8_t wide, uint8_t height, const uint8_t value); //Fill an area, but y must be aligned on 0/8 offset
+	static void drawFilledRect(uint8_t x0, uint8_t y0, uint8_t x1, uint8_t y1, bool clear);
+	static void drawHeatSymbol(uint8_t state);
+	static void drawScrollIndicator(uint8_t p, uint8_t h); // Draws a scrolling position indicator
+	static void transitionSecondaryFramebuffer(bool forwardNavigation);
+	static void useSecondaryFramebuffer(bool useSecondary);
+private:
+	static void drawChar(char c); // Draw a character to a specific location
+	static void setFramebuffer(uint8_t *buffer);
+	static const uint8_t *currentFont; // Pointer to the current font used for rendering to the buffer
+	static uint8_t *firstStripPtr; // Pointers to the strips to allow for buffer having extra content
+	static uint8_t *secondStripPtr;	//Pointers to the strips
+	static bool inLeftHandedMode; // Whether the screen is in left or not (used for offsets in GRAM)
+	static bool initDone;
+	static DisplayState displayState;
+	static uint8_t fontWidth, fontHeight;
+	static int16_t cursor_x, cursor_y;
+	static uint8_t displayOffset;
+	static uint8_t screenBuffer[16 + (OLED_WIDTH * 2) + 10];  // The data buffer
+	static uint8_t secondFrameBuffer[OLED_WIDTH * 2];
+};
+
+#endif /* OLED_HPP_ */

source/Core/Drivers/README.md

@@ -0,0 +1,10 @@
+# Drivers
+
+Drivers are the classes used to represent physical hardware on the board in a more abstract way, that are more complex than just an IO
+
+* OLED Display
+* Accelerometers
+* Button handling logic
+* Tip thermo response modelling
+
+All drivers should be written with minimal hardware assumptions, and defer hardware related logic to the BSP folder where possible

source/Core/Drivers/SC7A20.cpp

@@ -0,0 +1,69 @@
+/*
+ * SC7A20.cpp
+ *
+ *  Created on: 18 Sep. 2020
+ *      Author: Ralim
+ */
+
+#include <SC7A20.hpp>
+#include <SC7A20_defines.h>
+#include <array>
+
+bool SC7A20::detect() {
+	if (FRToSI2C::probe(SC7A20_ADDRESS)) {
+		//Read chip id to ensure its not an address collision
+		uint8_t id = 0;
+		if (FRToSI2C::Mem_Read(SC7A20_ADDRESS, SC7A20_WHO_AMI_I, &id, 1)) {
+			return id == 0b00010001;
+		}
+	}
+
+	return false;
+}
+
+static const FRToSI2C::I2C_REG i2c_registers[] = { //
+		//
+				{ SC7A20_CTRL_REG1, 0b01100111, 0 },    //200Hz, XYZ enabled
+				{ SC7A20_CTRL_REG2, 0b00000000, 0 },    //Setup filter to 0x00 ??
+				{ SC7A20_CTRL_REG3, 0b00000000, 0 },    //int1 off
+				{ SC7A20_CTRL_REG4, 0b01001000, 0 },    //Block mode off,little-endian,2G,High-pres,self test off
+				{ SC7A20_CTRL_REG5, 0b00000100, 0 },    //fifo off, D4D on int1
+				{ SC7A20_CTRL_REG6, 0x00, 0 },    //INT2 off
+				//Basically setup the unit to run, and enable 4D orientation detection
+				{ SC7A20_INT2_CFG, 0b01111110, 0 }, //setup for movement detection
+				{ SC7A20_INT2_THS, 0x28, 0 }, //
+				{ SC7A20_INT2_DURATION, 64, 0 }, //
+				{ SC7A20_INT1_CFG, 0b01111110, 0 }, //
+				{ SC7A20_INT1_THS, 0x28, 0 }, //
+				{ SC7A20_INT1_DURATION, 64, 0 }
+
+		//
+		};
+bool SC7A20::initalize() {
+	//Setup acceleration readings
+	//2G range
+	//bandwidth = 250Hz
+	//High pass filter on (Slow compensation)
+	//Turn off IRQ output pins
+	//Orientation recognition in symmetrical mode
+	// Hysteresis is set to ~ 16 counts
+	//Theta blocking is set to 0b10
+
+	return FRToSI2C::writeRegistersBulk(SC7A20_ADDRESS, i2c_registers, sizeof(i2c_registers) / sizeof(i2c_registers[0]));
+
+}
+
+void SC7A20::getAxisReadings(int16_t &x, int16_t &y, int16_t &z) {
+	//We can tell the accelerometer to output in LE mode which makes this simple
+	uint16_t sensorData[3] = { 0, 0, 0 };
+
+	if (FRToSI2C::Mem_Read(SC7A20_ADDRESS, SC7A20_OUT_X_L, (uint8_t*) sensorData, 6) == false) {
+		x = y = z = 0;
+		return;
+	}
+	//Shift 6 to make its range ~= the other accelerometers
+	x = sensorData[0];
+	y = sensorData[1];
+	z = sensorData[2];
+
+}

source/Core/Drivers/SC7A20.hpp

@@ -0,0 +1,33 @@
+/*
+ * BMA223.hpp
+ *
+ *  Created on: 18 Sep. 2020
+ *      Author: Ralim
+ */
+
+#ifndef CORE_DRIVERS_SC7A20_HPP_
+#define CORE_DRIVERS_SC7A20_HPP_
+#include "I2C_Wrapper.hpp"
+#include "BSP.h"
+#include "SC7A20_defines.h"
+
+class SC7A20 {
+public:
+	static bool detect();
+	static bool initalize();
+	//1 = rh, 2,=lh, 8=flat
+	static Orientation getOrientation() {
+		uint8_t val = ((FRToSI2C::I2C_RegisterRead(SC7A20_ADDRESS, SC7A20_INT2_SOURCE) >> 2) - 1);
+		if (val == 1)
+			return Orientation::ORIENTATION_LEFT_HAND;
+		else if (val == 4 || val == 0)
+			return Orientation::ORIENTATION_RIGHT_HAND;
+		else
+			return Orientation::ORIENTATION_FLAT;
+	}
+	static void getAxisReadings(int16_t &x, int16_t &y, int16_t &z);
+
+private:
+};
+
+#endif /* CORE_DRIVERS_BMA223_HPP_ */

source/Core/Drivers/SC7A20_defines.h

@@ -0,0 +1,46 @@
+/*
+ * BMA223_defines.h
+ *
+ *  Created on: 18 Sep. 2020
+ *      Author: Ralim
+ */
+
+#ifndef CORE_DRIVERS_SC7A20_DEFINES_H_
+#define CORE_DRIVERS_SC7A20_DEFINES_H_
+
+#define SC7A20_ADDRESS 0x18<<1
+#define SC7A20_WHO_AMI_I 0x0F
+#define SC7A20_CTRL_REG1 0x20
+#define SC7A20_CTRL_REG2 0x21
+#define SC7A20_CTRL_REG3 0x22
+#define SC7A20_CTRL_REG4 0x23
+#define SC7A20_CTRL_REG5 0x24
+#define SC7A20_CTRL_REG6 0x25
+#define SC7A20_REFERENCE 0x26
+#define SC7A20_STATUS_REG 0x27
+#define SC7A20_OUT_X_L 0x28
+#define SC7A20_OUT_X_H 0x29
+#define SC7A20_OUT_Y_L 0x2A
+#define SC7A20_OUT_Y_H 0x2B
+#define SC7A20_OUT_Z_L 0x2C
+#define SC7A20_OUT_Z_H 0x2D
+#define SC7A20_FIFO_CTRL 0x2E
+#define SC7A20_FIFO_SRC 0x2F
+#define SC7A20_INT1_CFG 0x30
+#define SC7A20_INT1_SOURCE 0x31
+#define SC7A20_INT1_THS 0x32
+#define SC7A20_INT1_DURATION 0x33
+#define SC7A20_INT2_CFG 0x34
+#define SC7A20_INT2_SOURCE 0x35
+#define SC7A20_INT2_THS 0x36
+#define SC7A20_INT2_DURATION 0x37
+#define SC7A20_CLICK_CFG 0x38
+#define SC7A20_CLICK_SRC 0x39
+#define SC7A20_CLICK_THS 0x3A
+#define SC7A20_TIME_LIMIT 0x3B
+#define SC7A20_TIME_LATENCY 0x3C
+#define SC7A20_TIME_WINDOW 0x3D
+#define SC7A20_ACT_THS 0x3E
+#define SC7A20_ACT_DURATION 0x3F
+
+#endif /* CORE_DRIVERS_BMA223_DEFINES_H_ */

source/Core/Drivers/Si7210.cpp

@@ -0,0 +1,184 @@
+/*
+ * Si7210.cpp
+ *
+ *  Created on: 5 Oct. 2020
+ *      Author: Ralim
+ *
+ *      This is based on the very nice sample code by Sean Farrelly (@FARLY7)
+ *      Over here : https://github.com/FARLY7/si7210-driver
+ *
+ *      This class is licensed as MIT to match this code base
+ */
+
+#include <Si7210.h>
+#include "Si7210_defines.h"
+#include "I2C_Wrapper.hpp"
+bool Si7210::detect() {
+	return FRToSI2C::wakePart(SI7210_ADDRESS);
+
+}
+
+bool Si7210::init() {
+	//Turn on auto increment and sanity check ID
+	//Load OTP cal
+
+	uint8_t temp;
+	if (FRToSI2C::Mem_Read(SI7210_ADDRESS, SI7210_REG_ID, &temp, 1)) {
+		// We don't really care what model it is etc, just probing to check its probably this iC
+		if (temp != 0x00 && temp != 0xFF) {
+			temp = 0x00;
+
+			/* Set device and internal driver settings */
+			if (!write_reg( SI7210_CTRL1, (uint8_t) ~SW_LOW4FIELD_MASK, 0)) {
+				return false;
+			}
+
+			/* Disable periodic auto-wakeup by device, and tamper detect. */
+			if ((!write_reg(SI7210_CTRL3, (uint8_t) ~SL_TIMEENA_MASK, 0)))
+				return false;
+
+			/* Disable tamper detection by setting sw_tamper to 63 */
+			if (!write_reg(SI7210_CTRL3, SL_FAST_MASK | SL_TIMEENA_MASK, 63 << 2))
+				return false;
+
+			if (!set_high_range())
+				return false;
+
+			/* Stop the control loop by setting stop bit */
+			if (!write_reg( SI7210_POWER_CTRL, MEAS_MASK | USESTORE_MASK, STOP_MASK)) /* WARNING: Removed USE_STORE MASK */
+				return false;
+
+			/* Use a burst size of 128/4096 samples in FIR and IIR modes */
+			if (!write_reg(SI7210_CTRL4, 0, DF_BURSTSIZE_128 | DF_BW_4096))
+				return false;
+
+			/* Select field strength measurement */
+			if (!write_reg( SI7210_DSPSIGSEL, 0, DSP_SIGSEL_FIELD_MASK))
+				return false;
+
+			return true;	//start_periodic_measurement();
+
+		}
+	}
+	return false;
+}
+
+int16_t Si7210::read() {
+	//Read the two regs
+	int16_t temp = 0;
+	if (!get_field_strength(&temp)) {
+		temp = 0;
+	}
+	return temp;
+}
+
+bool Si7210::write_reg(const uint8_t reg, const uint8_t mask, const uint8_t val) {
+	uint8_t temp = 0;
+	if (mask) {
+		if (!read_reg(reg, &temp)) {
+			return false;
+		}
+		temp &= mask;
+	}
+	temp |= val;
+	return FRToSI2C::Mem_Write(SI7210_ADDRESS, reg, &temp, 1);
+}
+
+bool Si7210::read_reg(const uint8_t reg, uint8_t* val) {
+	return FRToSI2C::Mem_Read(SI7210_ADDRESS, reg, val, 1);
+}
+
+bool Si7210::start_periodic_measurement() {
+	/* Enable periodic wakeup */
+	if (!write_reg(SI7210_CTRL3, (uint8_t) ~SL_TIMEENA_MASK, SL_TIMEENA_MASK))
+		return false;
+
+	/* Start measurement */
+	/* Change to ~STOP_MASK with STOP_MASK */
+	return write_reg( SI7210_POWER_CTRL, MEAS_MASK | USESTORE_MASK, 0);
+
+}
+
+bool Si7210::get_field_strength(int16_t* field) {
+	*field = 0;
+	uint8_t val = 0;
+	FRToSI2C::wakePart(SI7210_ADDRESS);
+
+	if (!write_reg( SI7210_POWER_CTRL, MEAS_MASK | USESTORE_MASK, STOP_MASK))
+		return false;
+
+	/* Read most-significant byte */
+	if (!read_reg( SI7210_DSPSIGM, &val))
+		return false;
+	*field = (val & DSP_SIGM_DATA_MASK) << 8;
+
+	/* Read least-significant byte of data */
+	if (!read_reg( SI7210_DSPSIGL, &val))
+		return false;
+
+	*field += val;
+	*field -= 16384U;
+	//field is now a +- measurement
+	//In units of 0.0125 mT
+	// Aka 12.5uT
+	//Clear flags
+	read_reg( SI7210_CTRL1, &val);
+	read_reg( SI7210_CTRL2, &val);
+//Start next one
+
+	/* Use a burst size of 128/4096 samples in FIR and IIR modes */
+	write_reg( SI7210_CTRL4, 0, DF_BURSTSIZE_128 | DF_BW_4096);
+
+	/* Selet field strength measurement */
+	write_reg( SI7210_DSPSIGSEL, 0, DSP_SIGSEL_FIELD_MASK);
+
+	/* Start measurement */
+	write_reg( SI7210_POWER_CTRL, MEAS_MASK | USESTORE_MASK, ONEBURST_MASK);
+
+	return true;
+}
+
+bool Si7210::set_high_range() {
+	//To set the unit into 200mT range, no magnet temperature calibration
+	// We want to copy OTP 0x27->0x2C into a0->a5
+	uint8_t base_addr = 0x27; // You can change this to pick the temp calibration
+	bool worked = true;
+	uint8_t val = 0;
+
+	/* Load A0 register */
+	worked &= write_reg( SI7210_OTP_ADDR, 0, base_addr);
+	worked &= write_reg( SI7210_OTP_CTRL, 0, OTP_READ_EN_MASK);
+	worked &= read_reg( SI7210_OTP_DATA, &val);
+	worked &= write_reg( SI7210_A0, 0, val);
+
+	/* Load A1 register */
+	worked &= write_reg( SI7210_OTP_ADDR, 0, base_addr + 1);
+	worked &= write_reg( SI7210_OTP_CTRL, 0, OTP_READ_EN_MASK);
+	worked &= read_reg( SI7210_OTP_DATA, &val);
+	worked &= write_reg( SI7210_A1, 0, val);
+
+	/* Load A2 register */
+	worked &= write_reg( SI7210_OTP_ADDR, 0, base_addr + 2);
+	worked &= write_reg( SI7210_OTP_CTRL, 0, OTP_READ_EN_MASK);
+	worked &= read_reg( SI7210_OTP_DATA, &val);
+	worked &= write_reg( SI7210_A2, 0, val);
+
+	/* Load A3 register */
+	worked &= write_reg( SI7210_OTP_ADDR, 0, base_addr + 3);
+	worked &= write_reg( SI7210_OTP_CTRL, 0, OTP_READ_EN_MASK);
+	worked &= read_reg( SI7210_OTP_DATA, &val);
+	worked &= write_reg( SI7210_A3, 0, val);
+
+	/* Load A4 register */
+	worked &= write_reg( SI7210_OTP_ADDR, 0, base_addr + 4);
+	worked &= write_reg( SI7210_OTP_CTRL, 0, OTP_READ_EN_MASK);
+	worked &= read_reg( SI7210_OTP_DATA, &val);
+	worked &= write_reg( SI7210_A4, 0, val);
+
+	/* Load A5 register */
+	worked &= write_reg( SI7210_OTP_ADDR, 0, base_addr + 5);
+	worked &= write_reg( SI7210_OTP_CTRL, 0, OTP_READ_EN_MASK);
+	worked &= read_reg( SI7210_OTP_DATA, &val);
+	worked &= write_reg( SI7210_A5, 0, val);
+	return worked;
+}

source/Core/Drivers/Si7210.h

@@ -0,0 +1,27 @@
+/*
+ * Si7210.h
+ *
+ *  Created on: 5 Oct. 2020
+ *      Author: Ralim
+ */
+
+#ifndef CORE_DRIVERS_SI7210_H_
+#define CORE_DRIVERS_SI7210_H_
+#include <stdint.h>
+class Si7210 {
+public:
+	//Return true if present
+	static bool detect();
+
+	static bool init();
+	static int16_t read();
+private:
+	static bool write_reg(const uint8_t reg,const  uint8_t mask,const  uint8_t val);
+	static bool read_reg(const uint8_t reg, uint8_t *val);
+	static bool start_periodic_measurement();
+	static bool get_field_strength(int16_t *field);
+	static bool set_high_range();
+
+};
+
+#endif /* CORE_DRIVERS_SI7210_H_ */

source/Core/Drivers/Si7210_defines.h

@@ -0,0 +1,91 @@
+/*
+ * Si7210_defines.h
+ *
+ *  Created on: 5 Oct. 2020
+ *      Author: Ralim
+ */
+
+#ifndef CORE_DRIVERS_SI7210_DEFINES_H_
+#define CORE_DRIVERS_SI7210_DEFINES_H_
+
+#define SI7210_ADDRESS (0x30<<1)
+#define SI7210_REG_ID 0xC0
+
+/* Si7210 Register addresses */
+#define SI7210_HREVID       0xC0U
+#define SI7210_DSPSIGM      0xC1U
+#define SI7210_DSPSIGL      0xC2U
+#define SI7210_DSPSIGSEL    0xC3U
+#define SI7210_POWER_CTRL   0xC4U
+#define SI7210_ARAUTOINC    0xC5U
+#define SI7210_CTRL1        0xC6U
+#define SI7210_CTRL2        0xC7U
+#define SI7210_SLTIME       0xC8U
+#define SI7210_CTRL3        0xC9U
+#define SI7210_A0           0xCAU
+#define SI7210_A1           0xCBU
+#define SI7210_A2           0xCCU
+#define SI7210_CTRL4        0xCDU
+#define SI7210_A3           0xCEU
+#define SI7210_A4           0xCFU
+#define SI7210_A5           0xD0U
+#define SI7210_OTP_ADDR     0xE1U
+#define SI7210_OTP_DATA     0xE2U
+#define SI7210_OTP_CTRL     0xE3U
+#define SI7210_TM_FG        0xE4U
+
+/* Si7210 Register bit masks */
+#define CHIP_ID_MASK        0xF0U
+#define REV_ID_MASK         0x0FU
+#define DSP_SIGSEL_MASK     0x07U
+#define MEAS_MASK           0x80U
+#define USESTORE_MASK       0x08U
+#define ONEBURST_MASK       0x04U
+#define STOP_MASK           0x02U
+#define SLEEP_MASK          0x01U
+#define ARAUTOINC_MASK      0x01U
+#define SW_LOW4FIELD_MASK   0x80U
+#define SW_OP_MASK          0x7FU
+#define SW_FIELDPOLSEL_MASK 0xC0U
+#define SW_HYST_MASK        0x3FU
+#define SW_TAMPER_MASK      0xFCU
+#define SL_FAST_MASK        0x02U
+#define SL_TIMEENA_MASK     0x01U
+#define DF_BURSTSIZE_MASK   0xE0U
+#define DF_BW_MASK          0x1EU
+#define DF_IIR_MASK         0x01U
+#define OTP_READ_EN_MASK    0x02U
+#define OTP_BUSY_MASK       0x01U
+#define TM_FG_MASK          0x03U
+
+#define DSP_SIGM_DATA_FLAG      0x80U
+#define DSP_SIGM_DATA_MASK      0x7FU
+#define DSP_SIGSEL_TEMP_MASK    0x01U
+#define DSP_SIGSEL_FIELD_MASK   0x04U
+
+/* Burst sizes */
+#define DF_BW_1             0x0U << 1
+#define DF_BW_2             0x1U << 1
+#define DF_BW_4             0x2U << 1
+#define DF_BW_8             0x3U << 1
+#define DF_BW_16            0x4U << 1
+#define DF_BW_32            0x5U << 1
+#define DF_BW_64            0x6U << 1
+#define DF_BW_128           0x7U << 1
+#define DF_BW_256           0x8U << 1
+#define DF_BW_512           0x9U << 1
+#define DF_BW_1024          0xAU << 1
+#define DF_BW_2048          0xBU << 1
+#define DF_BW_4096          0xCU << 1
+#define DF_BURSTSIZE_1      0x0U << 5
+#define DF_BURSTSIZE_2      0x1U << 5
+#define DF_BURSTSIZE_4      0x2U << 5
+#define DF_BURSTSIZE_8      0x3U << 5
+#define DF_BURSTSIZE_16     0x4U << 5
+#define DF_BURSTSIZE_32     0x5U << 5
+#define DF_BURSTSIZE_64     0x6U << 5
+#define DF_BURSTSIZE_128    0x7U << 5
+
+
+
+#endif /* CORE_DRIVERS_SI7210_DEFINES_H_ */

source/Core/Drivers/TipThermoModel.cpp

@@ -0,0 +1,245 @@
+/*
+ * TipThermoModel.cpp
+ *
+ *  Created on: 7 Oct 2019
+ *      Author: ralim
+ */
+
+#include "TipThermoModel.h"
+#include "Settings.h"
+#include "BSP.h"
+#include "power.hpp"
+#include "../../configuration.h"
+#include "main.hpp"
+/*
+ * The hardware is laid out  as a non-inverting op-amp
+ * There is a pullup of 39k(TS100) from the +ve input to 3.9V (1M pulup on TS100)
+ *
+ * The simplest case to model this, is to ignore the pullup resistors influence, and assume that its influence is mostly constant
+ * -> Tip resistance *does* change with temp, but this should be much less than the rest of the system.
+ *
+ * When a thermocouple is equal temperature at both sides (hot and cold junction), then the output should be 0uV
+ * Therefore, by measuring the uV when both are equal, the measured reading is the offset value.
+ * This is a mix of the pull-up resistor, combined with tip manufacturing differences.
+ *
+ * All of the thermocouple readings are based on this expired patent
+ * - > https://patents.google.com/patent/US6087631A/en
+ *
+ * This was bought to my attention by <Kuba Sztandera>
+ */
+
+uint32_t TipThermoModel::convertTipRawADCTouV(uint16_t rawADC) {
+	// This takes the raw ADC samples, converts these to uV
+	// Then divides this down by the gain to convert to the uV on the input to the op-amp (A+B terminals)
+	// Then remove the calibration value that is stored as a tip offset
+	uint32_t vddRailmVX10 = 33000;	//The vreg is +-2%, but we have no higher accuracy available
+	// 4096 * 8 readings for full scale
+	// Convert the input ADC reading back into mV times 10 format.
+	uint32_t rawInputmVX10 = (rawADC * vddRailmVX10) / (4096 * 8);
+
+	uint32_t valueuV = rawInputmVX10 * 100;	// shift into uV
+	//Now to divide this down by the gain
+	valueuV /= OP_AMP_GAIN_STAGE;
+
+	if (systemSettings.CalibrationOffset) {
+		//Remove uV tipOffset
+		if (valueuV >= systemSettings.CalibrationOffset)
+			valueuV -= systemSettings.CalibrationOffset;
+		else
+			valueuV = 0;
+	}
+
+	return valueuV;
+}
+
+uint32_t TipThermoModel::convertTipRawADCToDegC(uint16_t rawADC) {
+	return convertuVToDegC(convertTipRawADCTouV(rawADC));
+}
+#ifdef ENABLED_FAHRENHEIT_SUPPORT
+uint32_t TipThermoModel::convertTipRawADCToDegF(uint16_t rawADC) {
+	return convertuVToDegF(convertTipRawADCTouV(rawADC));
+}
+#endif
+
+//Table that is designed to be walked to find the best sample for the lookup
+
+//Extrapolate between two points
+// [x1, y1] = point 1
+// [x2, y2] = point 2
+//  x = input value
+// output is x's interpolated y value
+int32_t LinearInterpolate(int32_t x1, int32_t y1, int32_t x2, int32_t y2, int32_t x) {
+	return y1 + (((((x - x1) * 1000) / (x2 - x1)) * (y2 - y1))) / 1000;
+}
+#ifdef TEMP_uV_LOOKUP_HAKKO
+const uint16_t uVtoDegC[] = { //
+		//
+				0, 0,	//
+				266, 10,	//
+				522, 20,	//
+				770, 30,	//
+				1010, 40,	//
+				1244, 50,	//
+				1473, 60,	//
+				1697, 70,	//
+				1917, 80,	//
+				2135, 90,	//
+				2351, 100,	//
+				2566, 110,	//
+				2780, 120,	//
+				2994, 130,	//
+				3209, 140,	//
+				3426, 150,	//
+				3644, 160,	//
+				3865, 170,	//
+				4088, 180,	//
+				4314, 190,	//
+				4544, 200,	//
+				4777, 210,	//
+				5014, 220,	//
+				5255, 230,	//
+				5500, 240,	//
+				5750, 250,	//
+				6003, 260,	//
+				6261, 270,	//
+				6523, 280,	//
+				6789, 290,	//
+				7059, 300,	//
+				7332, 310,	//
+				7609, 320,	//
+				7889, 330,	//
+				8171, 340,	//
+				8456, 350,	//
+				8742, 360,	//
+				9030, 370,	//
+				9319, 380,	//
+				9607, 390,	//
+				9896, 400,	//
+				10183, 410,	//
+				10468, 420,	//
+				10750, 430,	//
+				11029, 440,	//
+				11304, 450,	//
+				11573, 460,	//
+				11835, 470,	//
+				12091, 480,	//
+				12337, 490,	//
+				12575, 500,	//
+
+		};
+#endif
+
+#ifdef TEMP_uV_LOOKUP_TS80
+
+const uint16_t uVtoDegC[] = {	//
+		//
+        530 , 0,	//
+		1282 , 10,	//
+		2034 , 20,	//
+		2786 , 30,	//
+		3538 , 40,	//
+		4290 , 50,	//
+		5043 , 60,	//
+		5795 , 70,	//
+		6547 , 80,	//
+		7299 , 90,	//
+		8051 , 100,	//
+		8803 , 110,	//
+		9555 , 120,	//
+		10308 , 130,	//
+		11060 , 140,	//
+		11812 , 150,	//
+		12564 , 160,	//
+		13316 , 170,	//
+		14068 , 180,	//
+		14820 , 190,	//
+		15573 , 200,	//
+		16325 , 210,	//
+		17077 , 220,	//
+		17829 , 230,	//
+		18581 , 240,	//
+		19333 , 250,	//
+		20085 , 260,	//
+		20838 , 270,	//
+		21590 , 280,	//
+		22342 , 290,	//
+		23094 , 300,	//
+		23846 , 310,	//
+		24598 , 320,	//
+		25350 , 330,	//
+		26103 , 340,	//
+		26855 , 350,	//
+		27607 , 360,	//
+		28359 , 370,	//
+		29111 , 380,	//
+		29863 , 390,	//
+		30615 , 400,	//
+		31368 , 410,	//
+		32120 , 420,	//
+		32872 , 430,	//
+		33624 , 440,	//
+		34376 , 450,	//
+		35128 , 460,	//
+		35880 , 470,	//
+		36632 , 480,	//
+		37385 , 490,	//
+		38137 , 500,	//
+		};
+#endif
+uint32_t TipThermoModel::convertuVToDegC(uint32_t tipuVDelta) {
+	if (tipuVDelta) {
+		int noItems = sizeof(uVtoDegC) / (2 * sizeof(uint16_t));
+		for (int i = 1; i < (noItems - 1); i++) {
+			//If current tip temp is less than current lookup, then this current lookup is the higher point to interpolate
+			if (tipuVDelta < uVtoDegC[i * 2]) {
+				return LinearInterpolate(uVtoDegC[(i - 1) * 2], uVtoDegC[((i - 1) * 2) + 1], uVtoDegC[i * 2], uVtoDegC[(i * 2) + 1], tipuVDelta);
+			}
+		}
+		return LinearInterpolate(uVtoDegC[(noItems - 2) * 2], uVtoDegC[((noItems - 2) * 2) + 1], uVtoDegC[(noItems - 1) * 2], uVtoDegC[((noItems - 1) * 2) + 1], tipuVDelta);
+	}
+	return 0;
+}
+
+#ifdef ENABLED_FAHRENHEIT_SUPPORT
+uint32_t TipThermoModel::convertuVToDegF(uint32_t tipuVDelta) {
+	return convertCtoF(convertuVToDegC(tipuVDelta));
+}
+
+uint32_t TipThermoModel::convertCtoF(uint32_t degC) {
+	//(Y °C × 9/5) + 32 =Y°F
+	return (32 + ((degC * 9) / 5));
+}
+
+uint32_t TipThermoModel::convertFtoC(uint32_t degF) {
+	//(Y°F − 32) × 5/9 = Y°C
+	if (degF < 32) {
+		return 0;
+	}
+	return ((degF - 32) * 5) / 9;
+}
+#endif
+
+uint32_t TipThermoModel::getTipInC(bool sampleNow) {
+	int32_t currentTipTempInC = TipThermoModel::convertTipRawADCToDegC(getTipRawTemp(sampleNow));
+	currentTipTempInC += getHandleTemperature() / 10; //Add handle offset
+	// Power usage indicates that our tip temp is lower than our thermocouple temp.
+	// I found a number that doesn't unbalance the existing PID, causing overshoot.
+	// This could be tuned in concert with PID parameters...
+	currentTipTempInC -= x10WattHistory.average() / 25;
+	if (currentTipTempInC < 0)
+		return 0;
+	return currentTipTempInC;
+}
+#ifdef ENABLED_FAHRENHEIT_SUPPORT
+uint32_t TipThermoModel::getTipInF(bool sampleNow) {
+	uint32_t currentTipTempInF = getTipInC(sampleNow);
+	currentTipTempInF = convertCtoF(currentTipTempInF);
+	return currentTipTempInF;
+}
+#endif
+
+uint32_t TipThermoModel::getTipMaxInC() {
+	uint32_t maximumTipTemp = TipThermoModel::convertTipRawADCToDegC(0x7FFF - (21 * 5)); //back off approx 5 deg c from ADC max
+	maximumTipTemp += getHandleTemperature() / 10; //Add handle offset
+	return maximumTipTemp - 1;
+}

source/Core/Drivers/TipThermoModel.h

@@ -0,0 +1,42 @@
+/*
+ * TipThermoModel.h
+ *
+ *  Created on: 7 Oct 2019
+ *      Author: ralim
+ */
+
+#ifndef SRC_TIPTHERMOMODEL_H_
+#define SRC_TIPTHERMOMODEL_H_
+#include "stdint.h"
+#include "BSP.h"
+#include "unit.h"
+class TipThermoModel {
+public:
+	//These are the main two functions
+	static uint32_t getTipInC(bool sampleNow = false);
+#ifdef ENABLED_FAHRENHEIT_SUPPORT
+	static uint32_t getTipInF(bool sampleNow = false);
+#endif
+
+	//Calculates the maximum temperature can can be read by the ADC range
+	static uint32_t getTipMaxInC();
+
+	static uint32_t convertTipRawADCToDegC(uint16_t rawADC);
+#ifdef ENABLED_FAHRENHEIT_SUPPORT
+	static uint32_t convertTipRawADCToDegF(uint16_t rawADC);
+#endif
+	//Returns the uV of the tip reading before the op-amp compensating for pullups
+	static uint32_t convertTipRawADCTouV(uint16_t rawADC);
+#ifdef ENABLED_FAHRENHEIT_SUPPORT
+	static uint32_t convertCtoF(uint32_t degC);
+	static uint32_t convertFtoC(uint32_t degF);
+#endif
+
+private:
+	static uint32_t convertuVToDegC(uint32_t tipuVDelta);
+#ifdef ENABLED_FAHRENHEIT_SUPPORT
+	static uint32_t convertuVToDegF(uint32_t tipuVDelta);
+#endif
+};
+
+#endif /* SRC_TIPTHERMOMODEL_H_ */