Firmware updates - sensors, calibration, View support, etc (#9)

 - Modify TLV493d library to expose frame counter in order to check for lockup, and implement auto-reset in tlv_sensor in case of lockup
 - Implement MT6701 SimpleFOC sensor
 - Make display optional
 - Add optional LED, strain, ALS support
 - Connect ALS to LED and display brightness
 - Hardcoded strain gauge thresholds and haptic feedback

firmware/lib/tlv/.piopm

@@ -0,0 +1 @@
+{"type": "library", "name": "TLV493D-Magnetic-Sensor", "version": "1.0.3", "spec": {"owner": "infineon", "id": 957, "name": "TLV493D-Magnetic-Sensor", "requirements": null, "url": null}}

firmware/lib/tlv/LICENSE.md

@@ -0,0 +1,35 @@
+# License Summary for Repository
+```
+Important Notice:
+Changes, suggestions and commits in this repository may only be done putting them 
+under the same license of the respective file.
+All rights of the respective copyright holders shall be reserved.
+Brands and product names are trademarks of their respective owners.
+Referred and linked files/pages are out-of-scope of this repository and underly
+their respective licenses.
+```
+## License
+
+Copyright (c) 2018 Infineon Technologies AG
+
+Redistribution and use in source and binary forms, with or without modification, are permitted provided that the 
+following conditions are met:   
+                                                                            
+Redistributions of source code must retain the above copyright notice, this list of conditions and the following 
+disclaimer.                        
+
+Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following 
+disclaimer in the documentation and/or other materials provided with the distribution.                       
+
+Neither the name of the copyright holders nor the names of its contributors may be used to endorse or promote 
+products derived from this software without specific prior written permission.                                           
+                                                                             
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 
+INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE  
+DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE  FOR ANY DIRECT, INDIRECT, INCIDENTAL, 
+SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR  
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+WHETHER IN CONTRACT, STRICT LIABILITY,OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+
+ 

firmware/lib/tlv/README.md

@@ -0,0 +1,72 @@
+# TLV493D-A1B6-3DMagnetic-Sensor
+
+[![Build Status](https://travis-ci.org/Infineon/TLV493D-A1B6-3DMagnetic-Sensor.svg?branch=master)](https://travis-ci.org/Infineon/TLV493D-A1B6-3DMagnetic-Sensor)
+
+<img src="https://github.com/Infineon/Assets/blob/master/Pictures/3D%20Magnetic%20Sensor%202Go.jpg" width=250> <img src="https://github.com/Infineon/Assets/blob/master/Pictures/TLV493D-Sense-Shield2Go_Top_plain.jpg_2045671804.jpg?raw=true" width=300>
+
+Library of Infineon's [TLV493D-A1B6 3D magnetic sensor](https://www.infineon.com/cms/en/product/sensor/magnetic-sensors/magnetic-position-sensors/3d-magnetics/tlv493d-a1b6/) for Arduino. 
+
+## Summary
+The 3D magnetic sensor TLV493D-A1B6 offers accurate three-dimensional sensing with extremely low power consumption in a small 6-pin package. With its magnetic field detection in x, y, and z-direction the sensor reliably measures three-dimensional, linear and rotation movements. Applications include joysticks, control elements (white goods, multifunction knops), or electric meters (anti tampering) and any other application that requires accurate angular measurements or low power consumptions.
+The integrated temperature sensor can furthermore be used for plausibility checks.
+
+Key features are 3D magnetic sensing with a very low power consumption during operations. The sensor has a digital output via 2-wire based standard I2C interface up to 1 MBit/sec and 12 bit data resolution for each measurement direction (Bx, By and Bz linear field measurement up to +-130mT).
+
+## Key Features and Benefits
+* Integrated temperature sensing
+* Low current consumption of 0.007 µA in power down mode and 10 µA in ultra low power mode
+* 2.7 to 3.5 V operating supply voltage
+* Digital output via 2-wire standard I2C interface
+* Bx, By and Bz linear field measurement up to ±130 mT
+* 12-bit data resolution for each measurement direction
+* Resolution 98 µT/LSB
+* Operating temperature range from -40 °C to 125 °C
+* [GUI](https://www.infineon.com/dgdl/Infineon-Software-for-3D-Magnetic-Sensor-2Go%20incl.%20out-of-shaft_05_06-SW-v05_06-EN.zip?fileId=5546d4626102d35a01614626f9644e4e) for free download as well as integration in Arduino IDE with this repository
+
+## Installation
+
+### Integration of Library
+Please download this repository from GitHub by clicking on the following field in the [releases](https://github.com/Infineon/TLV493D-A1B6-3DMagnetic-Sensor/releases) of this repository.
+
+![Download Library](https://raw.githubusercontent.com/infineon/assets/master/Pictures/Releases_Generic.jpg)
+
+To install the 3D magnetic sensor 2GO library in the Arduino IDE, please go now to **Sketch** > **Include Library** > **Add .ZIP Library...** in the Arduino IDE and navigate to the downloaded .ZIP file of this repository. The library will be installed in your Arduino sketch folder in libraries and you can select as well as include this one to your project under **Sketch** > **Include Library** > **TLV493D-A1B6**. 
+
+![Install Library](https://raw.githubusercontent.com/infineon/assets/master/Pictures/Library_Install_ZIP.png)
+
+## Usage
+Please see the example sketches in the `/examples` directory in this library to learn more about the usage of the library.
+
+Currently, there exist two separate evaluation boards:
+
+* [TLV493D-A1B6 MS2GO](https://www.infineon.com/cms/de/product/evaluation-boards/tlv493d-a1b6-ms2go/)
+* [TLV493D-A1B6 3DMagnetic Shield2Go](https://www.infineon.com/cms/en/product/evaluation-boards/s2go_3d-sense_tlv493d/)
+
+### Usage with TLV493D-A1B6 MS2GO
+The 3D Magnetic Sensor 2GO is an evaluation board equipped with the magnetic sensor [TLV493D-A1B6](https://www.infineon.com/cms/en/product/evaluation-boards/tlv493d-a1b6-ms2go/) for three dimensional measurement combined with an ARM® Cortex™-M0 CPU. The 3D Magnetic Sensor 2GO has a complete set of on-board devices, including an on-board debugger. A PDF summarizing the features and layout of the 3D magnetic sensor 2GO board is stored on the Infineon homepage [here](https://www.infineon.com/dgdl/Infineon-3D-Magnetic-Sensor_EvalKit_UM-UM-v01_01-EN.pdf?fileId=5546d462525dbac40152ac4ca1d318c2).
+
+Please note that base of the Sensors 2GO is the XMC 2Go from Infineon. Therefore, please install (if not already done) also the [XMC-for-Arduino](https://github.com/Infineon/XMC-for-Arduino) implementation and choose afterwards **XMC1100 XMC2Go** from the **Tools**>**Board** menu in the Arduino IDE if working with this evaluation board.
+
+### TLV493D-A1B6 3DSense Shield2Go
+The TLV493D-A1B6 3DMagnetic Shield2Go is a standalone break out board with Infineon's Shield2Go formfactor and pin out. You can connect it easily to any microcontroller of your choice which is Arduino compatible and has 3.3V logic level (please note that the Arduino UNO has 5V logic level and cannot be used without level shifting).
+
+* [Link](https://github.com/Infineon/TLV493D-A1B6-3DMagnetic-Sensor/wiki) to the wiki with more information
+
+However, every Shield2Go is directly compatible with Infineon's XMC2Go and the recommended quick start is to use an XMC2Go for evaluation. Therefore, please install (if not already done) also the [XMC-for-Arduino](https://github.com/Infineon/XMC-for-Arduino) implementation and choose afterwards **XMC1100 XMC2Go** from the **Tools**>**Board** menu in the Arduino IDE if working with this evaluation board. To use it, please plug the TLV493D-A1B6 3DMagnetic Shield2Go onto the XMC2Go as shown below.
+
+<img src="https://github.com/Infineon/Assets/blob/master/Pictures/TLV493D-A1B6_S2Go_w_XMC2Go.png" width=250>
+
+## Processing
+This library supports the open-source software [Processing](https://processing.org/) for creating GUIs. It allows you to connect your evaluation board to a PC over serial communication and visualisation of the embedded system. Find out more on the Arduino homepage [here](http://playground.arduino.cc/Interfacing/Processing). The respective files are stored in the /processing folder of this repository. 
+
+## Printables
+The TLx493D 3D magnetic sensor family has additional tools which can be directly mounted on top of the evaluation boards. The 3D print data of the [joystick](https://www.infineon.com/cms/en/product/promopages/sensors-2go/#Add-ons-3D-Magnetic-2GO), the [rotate knob](https://www.infineon.com/cms/en/product/promopages/sensors-2go/#Add-ons-3D-Magnetic-2GO) and the [linear slider](https://www.infineon.com/cms/en/product/promopages/sensors-2go/#Add-ons-3D-Magnetic-2GO) can be found in the folder `printables`.
+
+<img src="https://www.infineon.com/export/sites/default/media/products/Sensors/joystick.jpg_708092179.jpg" width=250>
+
+## Board Information, Datasheet and Additional Information
+
+The datasheet for the TLV493D-A1B6 can be found here [TLV493D-A1B6 Datasheet](https://www.infineon.com/dgdl/Infineon-TLV493D-A1B6-DS-v01_00-EN.pdf?fileId=5546d462525dbac40152a6b85c760e80) while respective application notes are located here [Application Notes](https://www.infineon.com/dgdl/Infineon-TLV493D-A1B6_3DMagnetic-UM-v01_03-EN.pdf?fileId=5546d46261d5e6820161e75721903ddd).
+
+Please check the [wiki](https://github.com/Infineon/TLV493D-A1B6-3DMagnetic-Sensor/wiki) with more information for the TLV493D-A1B6 3DSense Shield2Go as well.
+

firmware/lib/tlv/library.json

@@ -0,0 +1,17 @@
+{
+    "name": "TLV493D-Magnetic-Sensor",
+    "keywords": "3D, magnetic, sensor2go",
+    "description": "This library provides an Interface for Infineon's TLV493D Magnetic Sensor",
+    "repository":
+    {
+      "type": "git",
+      "url": "https://github.com/Infineon/TLV493D-A1B6-3DMagnetic-Sensor.git",
+      "branch": "master"
+    },
+    "version":"1.0.3",
+    "frameworks": "arduino",
+    "platforms": [
+        "infineonxmc",
+        "atmelavr"
+    ]
+  }

firmware/lib/tlv/library.properties

@@ -0,0 +1,10 @@
+name=TLV493D-A1B6
+version=1.0.3
+author=Infineon Technologies
+maintainer=Infineon Technologies <www.infineon.com>
+sentence=This library provides an Interface for Infineon's TLV493D-A1B6 3D Magnetic Sensor.
+paragraph=With this Sensor, you can detect the position of a Joystick etc. without any additional mechanical components. You just have to provide a magnet that is fixed to a movable component. 
+category=Sensors
+url=http://www.infineon.com/cms/de/product/sensor/magnetic-position-sensor/3d-magnetic-sensor/TLV493D-A1B6/productType.html?productType=5546d462525dbac401529cebc74f07b7
+architectures=*
+

firmware/lib/tlv/src/Tlv493d.cpp

@@ -0,0 +1,369 @@
+/**
+ * Tlv493d.cpp - Library for Arduino to control the TLV493D-A1B6 3D magnetic sensor.
+ *
+ * The 3D magnetic sensor TLV493D-A1B6 offers accurate three dimensional sensing with extremely low power consumption 
+ * in a small 6-pin package. With an opportunity to detect the magnetic field in x, y, and z-direction the sensor is 
+ * ideally suited for the measurement of 3D movements, linear movements and rotation movements.
+ * 
+ * Have a look at the application note/reference manual for more information.
+ * 
+ * Copyright (c) 2018 Infineon Technologies AG
+ * 
+ * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the 
+ * following conditions are met:   
+ *                                                                              
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the following 
+ * disclaimer.                        
+ * 
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following 
+ * disclaimer in the documentation and/or other materials provided with the distribution.                       
+ * 
+ * Neither the name of the copyright holders nor the names of its contributors may be used to endorse or promote 
+ * products derived from this software without specific prior written permission.                                           
+ *                                                                              
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 
+ * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE  
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE  FOR ANY DIRECT, INDIRECT, INCIDENTAL, 
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR  
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY,OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.   
+ */
+
+
+#include "Tlv493d.h"
+#include "./util/RegMask.h"
+#include "./util/BusInterface2.h"
+#include <math.h>
+
+
+Tlv493d::Tlv493d(void)
+{
+	mXdata = 0;
+	mYdata = 0;
+	mZdata = 0;
+	mTempdata = 0;
+	mExpectedFrameCount = 0x00;
+}
+
+
+Tlv493d::~Tlv493d(void)
+{
+	end();
+}
+
+void Tlv493d::begin(void)
+{
+	begin(Wire, TLV493D_ADDRESS1, true);
+}
+
+void Tlv493d::begin(TwoWire &bus)
+{
+	begin(bus, TLV493D_ADDRESS1, true);
+}
+
+void Tlv493d::begin(TwoWire &bus, Tlv493d_Address_t slaveAddress, bool reset)
+{
+	/**
+	 * Workaround for kit2go vdd enable
+	 */
+	#ifdef TLV493D_A1B6_KIT2GO
+	pinMode(LED2, OUTPUT);
+	digitalWrite(LED2, HIGH);
+	delay(50);
+	#endif
+	
+	initInterface(&mInterface, &bus, slaveAddress);
+	delay(TLV493D_STARTUPDELAY);
+
+	mInterface.bus->begin();
+
+	if(reset == true)
+	{
+		resetSensor(mInterface.adress);
+	}
+
+	// get all register data from sensor
+	tlv493d::readOut(&mInterface);
+	// copy factory settings to write registers
+	setRegBits(tlv493d::W_RES1, getRegBits(tlv493d::R_RES1));
+	setRegBits(tlv493d::W_RES1, getRegBits(tlv493d::R_RES1));
+	setRegBits(tlv493d::W_RES1, getRegBits(tlv493d::R_RES1));
+	// enable parity detection
+	setRegBits(tlv493d::W_PARITY_EN, 1);
+	// config sensor to lowpower mode
+	// also contains parity calculation and writeout to sensor
+	setAccessMode(TLV493D_DEFAULTMODE);
+}
+
+
+void Tlv493d::end(void)
+{
+	disableInterrupt();
+	setAccessMode(POWERDOWNMODE);
+}
+
+
+bool Tlv493d::setAccessMode(AccessMode_e mode)
+{
+	bool ret = BUS_ERROR;
+	const tlv493d::AccessMode_t *modeConfig = &(tlv493d::accModes[mode]);
+	setRegBits(tlv493d::W_FAST, modeConfig->fast);
+	setRegBits(tlv493d::W_LOWPOWER, modeConfig->lp);
+	setRegBits(tlv493d::W_LP_PERIOD, modeConfig->lpPeriod);
+	calcParity();
+	ret = tlv493d::writeOut(&mInterface);
+	if ( ret != BUS_ERROR )
+	{
+		mMode = mode;
+	}
+	return ret;
+}
+
+
+void Tlv493d::enableInterrupt(void)
+{
+	setRegBits(tlv493d::W_INT, 1);
+	calcParity();
+	tlv493d::writeOut(&mInterface);
+}
+
+
+void Tlv493d::disableInterrupt(void)
+{
+	setRegBits(tlv493d::W_INT, 0);
+	calcParity();
+	tlv493d::writeOut(&mInterface);
+}
+
+void Tlv493d::enableTemp(void)
+{
+	setRegBits(tlv493d::W_TEMP_NEN, 0);
+	calcParity();
+	tlv493d::writeOut(&mInterface);
+}
+
+
+void Tlv493d::disableTemp(void)
+{
+	setRegBits(tlv493d::W_TEMP_NEN, 1);
+	calcParity();
+	tlv493d::writeOut(&mInterface);
+}
+
+
+uint16_t Tlv493d::getMeasurementDelay(void)
+{
+	return tlv493d::accModes[mMode].measurementTime;
+}
+
+
+Tlv493d_Error_t Tlv493d::updateData(void)
+{
+	Tlv493d_Error_t ret = TLV493D_NO_ERROR;
+	// in POWERDOWNMODE, sensor has to be switched on for one measurement
+	uint8_t powerdown = 0;
+	if(mMode == POWERDOWNMODE) 
+	{
+		if (setAccessMode(MASTERCONTROLLEDMODE) != BUS_OK)
+		{
+			ret = TLV493D_BUS_ERROR;
+		}
+		delay(getMeasurementDelay());
+		powerdown = 1;
+	}
+	if(ret == TLV493D_NO_ERROR)
+	{
+#ifdef TLV493D_ACCELERATE_READOUT
+		// just read the most important results in FASTMODE, if this behaviour is desired
+		if(mMode == FASTMODE)
+		{
+			if (readOut(&mInterface, TLV493D_FAST_READOUT) != BUS_OK)
+			{
+				ret = TLV493D_BUS_ERROR;
+			}
+		}
+		else
+		{
+			if (readOut(&mInterface, TLV493D_MEASUREMENT_READOUT) != BUS_OK)
+			{
+				ret = TLV493D_BUS_ERROR;
+			}
+		}
+#else
+		if (readOut(&mInterface, TLV493D_MEASUREMENT_READOUT) != BUS_OK)
+		{
+			ret = TLV493D_BUS_ERROR;
+		}
+#endif
+		if (ret == TLV493D_NO_ERROR)
+		{
+			// construct results from registers
+			mXdata = concatResults(getRegBits(tlv493d::R_BX1), getRegBits(tlv493d::R_BX2), true);
+			mYdata = concatResults(getRegBits(tlv493d::R_BY1), getRegBits(tlv493d::R_BY2), true);
+			mZdata = concatResults(getRegBits(tlv493d::R_BZ1), getRegBits(tlv493d::R_BZ2), true);
+			mTempdata = concatResults(getRegBits(tlv493d::R_TEMP1), getRegBits(tlv493d::R_TEMP2), false);
+			// switch sensor back to POWERDOWNMODE, if it was in POWERDOWNMODE before
+			if(powerdown)
+			{
+				if (setAccessMode(POWERDOWNMODE) != BUS_OK)
+				{
+					ret = TLV493D_BUS_ERROR;
+				}
+			}
+			if (ret == TLV493D_NO_ERROR)
+			{
+				// if the return value is 0, all results are from the same frame
+				// otherwise some results may be outdated
+				if(getRegBits(tlv493d::R_CHANNEL) != 0)
+				{
+					ret = TLV493D_FRAME_ERROR;
+				}
+// Todo: removed due to a lot of frame errors
+//				// if received frame count does not match expected one (frame count from 0 to 3)
+//				else if( getRegBits(tlv493d::R_FRAMECOUNTER) != (mExpectedFrameCount % 4) )
+//				{
+//					ret = TLV493D_FRAME_ERROR;
+//				}
+			}
+		}
+	}
+	mExpectedFrameCount = getRegBits(tlv493d::R_FRAMECOUNTER) + 1;
+	return ret;
+}
+
+// SBEZEK
+uint8_t Tlv493d::getExpectedFrameCount(void) {
+	return mExpectedFrameCount;
+}
+
+float Tlv493d::getX(void)
+{
+	return static_cast<float>(mXdata) * TLV493D_B_MULT;
+}
+
+
+float Tlv493d::getY(void)
+{
+	return static_cast<float>(mYdata) * TLV493D_B_MULT;
+}
+
+
+float Tlv493d::getZ(void)
+{
+	return static_cast<float>(mZdata) * TLV493D_B_MULT;
+}
+
+
+float Tlv493d::getTemp(void)
+{
+	return static_cast<float>(mTempdata-TLV493D_TEMP_OFFSET) * TLV493D_TEMP_MULT;
+}
+
+
+float Tlv493d::getAmount(void)
+{
+	// sqrt(x^2 + y^2 + z^2)
+	return TLV493D_B_MULT * sqrt(pow(static_cast<float>(mXdata), 2) + pow(static_cast<float>(mYdata), 2) + pow(static_cast<float>(mZdata), 2));
+}
+
+
+float Tlv493d::getAzimuth(void)
+{
+	// arctan(y/x)
+	return atan2(static_cast<float>(mYdata), static_cast<float>(mXdata));
+}
+
+
+float Tlv493d::getPolar(void)
+{
+	// arctan(z/(sqrt(x^2+y^2)))
+	return atan2(static_cast<float>(mZdata), sqrt(pow(static_cast<float>(mXdata), 2) + pow(static_cast<float>(mYdata), 2)));
+}
+
+
+/* internal function called by begin()
+ * The sensor has a special reset sequence which allows to change its i2c address by setting SDA to high or low during a reset. 
+ * As some i2c peripherals may not cope with this, the simplest way is to use for this very few bytes bitbanging on the SCL/SDA lines.
+ * Furthermore, as the uC may be stopped during a i2c transmission, a special recovery sequence allows to bring the bus back to
+ * an operating state.
+ */
+void Tlv493d::resetSensor(uint8_t adr)     // Recovery & Reset - this can be handled by any uC as it uses bitbanging
+{
+	mInterface.bus->beginTransmission(0x00);
+
+	if (adr == TLV493D_ADDRESS1) {
+		// if the sensor shall be initialized with i2c address 0x1F
+		mInterface.bus->write(0xFF);
+	} else {
+		// if the sensor shall be initialized with address 0x5E
+		mInterface.bus->write((uint8_t)0x00);
+	}
+
+	mInterface.bus->endTransmission(true);
+}
+
+void Tlv493d::setRegBits(uint8_t regMaskIndex, uint8_t data)
+{
+	if(regMaskIndex < TLV493D_NUM_OF_REGMASKS)
+	{
+		tlv493d::setToRegs(&(tlv493d::regMasks[regMaskIndex]), mInterface.regWriteData, data);
+	}
+}
+
+uint8_t Tlv493d::getRegBits(uint8_t regMaskIndex)
+{
+	if(regMaskIndex < TLV493D_NUM_OF_REGMASKS)
+	{
+		const tlv493d::RegMask_t *mask = &(tlv493d::regMasks[regMaskIndex]);
+		if(mask->rw == REGMASK_READ)
+		{
+			return tlv493d::getFromRegs(mask, mInterface.regReadData);
+		}
+		else
+		{
+			return tlv493d::getFromRegs(mask, mInterface.regWriteData);
+		}
+	}
+	return 0;
+}
+
+void Tlv493d::calcParity(void) 
+{
+	uint8_t i;
+	uint8_t y = 0x00;
+	// set parity bit to 1
+	// algorithm will calculate an even parity and replace this bit, 
+	// so parity becomes odd
+	setRegBits(tlv493d::W_PARITY, 1);
+	// combine array to one byte first
+	for(i = 0; i < TLV493D_BUSIF_WRITESIZE; i++)
+	{
+		y ^= mInterface.regWriteData[i];
+	}
+	// combine all bits of this byte
+	y = y ^ (y >> 1);
+	y = y ^ (y >> 2);
+	y = y ^ (y >> 4);
+	// parity is in the LSB of y
+	setRegBits(tlv493d::W_PARITY, y&0x01);
+}
+
+
+int16_t Tlv493d::concatResults(uint8_t upperByte, uint8_t lowerByte, bool upperFull)  
+{
+	int16_t value=0x0000;	//16-bit signed integer for 12-bit values of sensor
+	if(upperFull)               
+	{
+		value=upperByte<<8;
+		value|=(lowerByte&0x0F)<<4;
+	}
+	else
+	{
+		value=(upperByte&0x0F)<<12;
+		value|=lowerByte<<4;
+	}
+	value>>=4;				//shift left so that value is a signed 12 bit integer
+	return value;
+}
+

firmware/lib/tlv/src/Tlv493d.h

@@ -0,0 +1,153 @@
+/**
+ * Tlv493d.h - Library for Arduino to control the TLV493D-A1B6 3D magnetic sensor.
+ *
+ * The 3D magnetic sensor TLV493D-A1B6 offers accurate three dimensional sensing with extremely low power consumption 
+ * in a small 6-pin package. With an opportunity to detect the magnetic field in x, y, and z-direction the sensor is 
+ * ideally suited for the measurement of 3D movements, linear movements and rotation movements.
+ * 
+ * Have a look at the application note/reference manual for more information.
+ * 
+ * Copyright (c) 2018 Infineon Technologies AG
+ * 
+ * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the 
+ * following conditions are met:   
+ *                                                                              
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the following 
+ * disclaimer.                        
+ * 
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following 
+ * disclaimer in the documentation and/or other materials provided with the distribution.                       
+ * 
+ * Neither the name of the copyright holders nor the names of its contributors may be used to endorse or promote 
+ * products derived from this software without specific prior written permission.                                           
+ *                                                                              
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 
+ * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE  
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE  FOR ANY DIRECT, INDIRECT, INCIDENTAL, 
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR  
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY,OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.   
+ */
+
+#ifndef TLV493D_H_INCLUDED
+#define TLV493D_H_INCLUDED
+
+
+#include <Arduino.h>
+#include <Wire.h>
+#include "./util/BusInterface.h"
+#include "./util/Tlv493d_conf.h"
+
+typedef enum Tlv493d_Address
+{
+	TLV493D_ADDRESS1	=	0x5E,
+	TLV493D_ADDRESS2	=	0x1F
+}Tlv493d_Address_t;
+
+
+typedef enum Tlv493d_Error
+{
+	TLV493D_NO_ERROR	=	0,
+	TLV493D_BUS_ERROR	=	1,
+	TLV493D_FRAME_ERROR	=	2
+}Tlv493d_Error_t;
+
+
+/*
+ * TLV493D_ACCELERATE_READOUT lets the controller just read out the first 3 bytes when in fast mode. 
+ * This makes the readout faster (half of usual transfer duration), but there is no way to get 
+ * temperature, current channel or high precision (only 8 instead of 12 bits for x, y, z)
+ * It is necessary for slow I2C busses to read the last result before the new measurement is completed. 
+ * It only takes effect in FASTMODE, not in other modes. 
+ *
+ * Feel free to undefine this and increase your I2C bus speed if you need to. 
+ */
+// SBEZEK
+// #define TLV493D_ACCELERATE_READOUT
+
+
+class Tlv493d
+{
+public: 
+
+	Tlv493d(void);
+	~Tlv493d(void);
+	void begin(void);
+	void begin(TwoWire &bus);
+	void begin(TwoWire &bus, Tlv493d_Address_t slaveAddress, bool reset);
+	void end(void);
+	
+	// sensor configuration
+	/* sets the data access mode for TLE493D
+	 * Tle493d is initially in POWERDOWNMODE
+	 * use POWERDOWNMODE for rare and infrequent measurements 
+	 * 	Tle493d will automatically switch to MASTERCONTROLLEDMODE for one measurement if on a readout
+	 *	measurements are quite slow in this mode. The power consumption is very low between measurements. 
+	 * use MASTERCONTROLLEDMODE for low measurement frequencies where results do not have to be up-to-date
+	 *	In this mode a new measurement starts directly after the last result has been read out. 
+	 * use LOWPOWERMODE and ULTRALOWPOWERMODE for continuous measurements
+	 *	each readout returns the latest measurement results
+	 * use FASTMODE for for continuous measurements on high frequencies
+	 *	measurement time might be higher than the time necessary for I2C-readouts in this mode. 
+	 *	Note: Thus, this mode requires a non-standard 1MHz I2C clock to be used to read the data fast enough.
+	 */
+	enum AccessMode_e
+	{
+		POWERDOWNMODE = 0,
+		FASTMODE,
+		LOWPOWERMODE,
+		ULTRALOWPOWERMODE,
+		MASTERCONTROLLEDMODE,
+	};
+	bool setAccessMode(AccessMode_e mode);
+	// interrupt is disabled by default
+	// it is recommended for FASTMODE, LOWPOWERMODE and ULTRALOWPOWERMODE
+	// the interrupt is indicated with a short(1.5 us) low pulse on SCL
+	// you need to capture and react(read the new results) to it by yourself
+	void enableInterrupt(void);
+	void disableInterrupt(void);
+	// temperature measurement is enabled by default
+	// it can be disabled to reduce power consumption
+	void enableTemp(void);
+	void disableTemp(void);
+	
+	// returns the recommended time between two readouts for the sensor's current configuration
+	uint16_t getMeasurementDelay(void);
+	// read measurement results from sensor
+	Tlv493d_Error_t updateData(void);
+	
+	// fieldvector in Cartesian coordinates
+	float getX(void);
+	float getY(void);
+	float getZ(void);
+	
+	// fieldvector in spherical coordinates
+	float getAmount(void);
+	float getAzimuth(void);
+	float getPolar(void);
+	
+	// temperature
+	float getTemp(void);
+
+	// SBEZEK
+	uint8_t getExpectedFrameCount(void);
+	
+private: 
+	tlv493d::BusInterface_t mInterface;
+	AccessMode_e mMode;
+	int16_t mXdata;
+	int16_t mYdata;
+	int16_t mZdata;
+	int16_t	mTempdata;
+	uint8_t mExpectedFrameCount;
+	
+
+	void resetSensor(uint8_t adr);
+	void setRegBits(uint8_t regMaskIndex, uint8_t data);
+	uint8_t getRegBits(uint8_t regMaskIndex);
+	void calcParity(void);
+	int16_t concatResults(uint8_t upperByte, uint8_t lowerByte, bool upperFull);
+};
+
+#endif		/* TLV493D_H_INCLUDED */

firmware/lib/tlv/src/util/BusInterface.cpp

@@ -0,0 +1,97 @@
+/**
+ * BusInterface.cpp - Part of the library for Arduino to control the TLV493D-A1B6 3D magnetic sensor.
+ *
+ * The 3D magnetic sensor TLV493D-A1B6 offers accurate three dimensional sensing with extremely low power consumption 
+ * in a small 6-pin package. With an opportunity to detect the magnetic field in x, y, and z-direction the sensor is 
+ * ideally suited for the measurement of 3D movements, linear movements and rotation movements.
+ * 
+ * Have a look at the application note/reference manual for more information.
+ * 
+ * Copyright (c) 2018 Infineon Technologies AG
+ * 
+ * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the 
+ * following conditions are met:   
+ *                                                                              
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the following 
+ * disclaimer.                        
+ * 
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following 
+ * disclaimer in the documentation and/or other materials provided with the distribution.                       
+ * 
+ * Neither the name of the copyright holders nor the names of its contributors may be used to endorse or promote 
+ * products derived from this software without specific prior written permission.                                           
+ *                                                                              
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 
+ * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE  
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE  FOR ANY DIRECT, INDIRECT, INCIDENTAL, 
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR  
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY,OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.   
+ */
+
+#include "BusInterface2.h"
+
+void tlv493d::initInterface(BusInterface_t *interface, TwoWire *bus, uint8_t adress)
+{
+	uint8_t i;
+	interface->bus = bus;
+	interface->adress = adress;
+	for(i = 0; i < TLV493D_BUSIF_READSIZE; i++) {
+		interface->regReadData[i] = 0x00;;
+	}
+	for(i = 0; i < TLV493D_BUSIF_WRITESIZE; i++) {
+		interface->regWriteData[i] = 0x00;;
+	}
+}
+
+bool tlv493d::readOut(BusInterface_t *interface)
+{
+	return readOut(interface, TLV493D_BUSIF_READSIZE);
+}
+
+bool tlv493d::readOut(BusInterface_t *interface, uint8_t count)
+{
+	bool ret = BUS_ERROR;
+	int i;
+	if(count > TLV493D_BUSIF_READSIZE)
+	{
+		count = TLV493D_BUSIF_READSIZE;
+	}
+	uint8_t received_bytes = interface->bus->requestFrom(interface->adress,count);
+	if (received_bytes == count)
+	{
+		for(i = 0; i < count; i++)
+		{
+			interface->regReadData[i] = interface->bus->read();
+		}
+		ret = BUS_OK;
+	}
+	return ret;
+}
+
+bool tlv493d::writeOut(BusInterface_t *interface)
+{
+	return writeOut(interface, TLV493D_BUSIF_WRITESIZE);
+}
+
+bool tlv493d::writeOut(BusInterface_t *interface, uint8_t count)
+{
+	bool ret = BUS_ERROR;
+	int i;
+	if(count > TLV493D_BUSIF_WRITESIZE)
+	{
+		count = TLV493D_BUSIF_WRITESIZE;
+	}
+	interface->bus->beginTransmission(interface->adress);
+	for(i = 0; i < count; i++)
+	{
+		interface->bus->write(interface->regWriteData[i]);
+	}
+	if (interface->bus->endTransmission() == 0)
+	{
+		ret = BUS_OK;
+	}
+	return ret;
+}
+

firmware/lib/tlv/src/util/BusInterface.h

@@ -0,0 +1,55 @@
+/**
+ * BusInterface.h - Part of the library for Arduino to control the TLV493D-A1B6 3D magnetic sensor.
+ *
+ * The 3D magnetic sensor TLV493D-A1B6 offers accurate three dimensional sensing with extremely low power consumption 
+ * in a small 6-pin package. With an opportunity to detect the magnetic field in x, y, and z-direction the sensor is 
+ * ideally suited for the measurement of 3D movements, linear movements and rotation movements.
+ * 
+ * Have a look at the application note/reference manual for more information.
+ * 
+ * Copyright (c) 2018 Infineon Technologies AG
+ * 
+ * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the 
+ * following conditions are met:   
+ *                                                                              
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the following 
+ * disclaimer.                        
+ * 
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following 
+ * disclaimer in the documentation and/or other materials provided with the distribution.                       
+ * 
+ * Neither the name of the copyright holders nor the names of its contributors may be used to endorse or promote 
+ * products derived from this software without specific prior written permission.                                           
+ *                                                                              
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 
+ * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE  
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE  FOR ANY DIRECT, INDIRECT, INCIDENTAL, 
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR  
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY,OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.   
+ */
+
+#ifndef TLV493D_BUSIF_H_INCLUDED
+#define TLV493D_BUSIF_H_INCLUDED
+
+#include <Arduino.h>
+#include <Wire.h>
+
+#define TLV493D_BUSIF_READSIZE		10
+#define TLV493D_BUSIF_WRITESIZE		4
+
+namespace tlv493d
+{
+
+typedef struct 
+{
+	TwoWire *bus;
+	uint8_t adress;
+	uint8_t regReadData[TLV493D_BUSIF_READSIZE];
+	uint8_t regWriteData[TLV493D_BUSIF_WRITESIZE];
+} BusInterface_t;
+
+}
+
+#endif

firmware/lib/tlv/src/util/BusInterface2.h

@@ -0,0 +1,52 @@
+/**
+ * BusInterface2.h - Part of the library for Arduino to control the TLV493D-A1B6 3D magnetic sensor.
+ *
+ * The 3D magnetic sensor TLV493D-A1B6 offers accurate three dimensional sensing with extremely low power consumption 
+ * in a small 6-pin package. With an opportunity to detect the magnetic field in x, y, and z-direction the sensor is 
+ * ideally suited for the measurement of 3D movements, linear movements and rotation movements.
+ * 
+ * Have a look at the application note/reference manual for more information.
+ * 
+ * Copyright (c) 2018 Infineon Technologies AG
+ * 
+ * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the 
+ * following conditions are met:   
+ *                                                                              
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the following 
+ * disclaimer.                        
+ * 
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following 
+ * disclaimer in the documentation and/or other materials provided with the distribution.                       
+ * 
+ * Neither the name of the copyright holders nor the names of its contributors may be used to endorse or promote 
+ * products derived from this software without specific prior written permission.                                           
+ *                                                                              
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 
+ * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE  
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE  FOR ANY DIRECT, INDIRECT, INCIDENTAL, 
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR  
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY,OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.   
+ */
+
+#ifndef TLV493D_BUSIF_2_H_INCLUDED
+#define TLV493D_BUSIF_2_H_INCLUDED
+
+#include "BusInterface.h"
+
+#define BUS_ERROR 	1
+#define BUS_OK 		0
+
+namespace tlv493d
+{
+	
+void initInterface(BusInterface_t *interface, TwoWire *bus, uint8_t adress);
+bool readOut(BusInterface_t *interface);
+bool readOut(BusInterface_t *interface, uint8_t count);
+bool writeOut(BusInterface_t *interface);
+bool writeOut(BusInterface_t *interface, uint8_t count);
+
+}
+
+#endif

firmware/lib/tlv/src/util/RegMask.cpp

@@ -0,0 +1,54 @@
+/**
+ * RegMask.cpp - Part of the library for Arduino to control the TLV493D-A1B6 3D magnetic sensor.
+ *
+ * The 3D magnetic sensor TLV493D-A1B6 offers accurate three dimensional sensing with extremely low power consumption 
+ * in a small 6-pin package. With an opportunity to detect the magnetic field in x, y, and z-direction the sensor is 
+ * ideally suited for the measurement of 3D movements, linear movements and rotation movements.
+ * 
+ * Have a look at the application note/reference manual for more information.
+ * 
+ * Copyright (c) 2018 Infineon Technologies AG
+ * 
+ * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the 
+ * following conditions are met:   
+ *                                                                              
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the following 
+ * disclaimer.                        
+ * 
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following 
+ * disclaimer in the documentation and/or other materials provided with the distribution.                       
+ * 
+ * Neither the name of the copyright holders nor the names of its contributors may be used to endorse or promote 
+ * products derived from this software without specific prior written permission.                                           
+ *                                                                              
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 
+ * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE  
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE  FOR ANY DIRECT, INDIRECT, INCIDENTAL, 
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR  
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY,OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.   
+ */
+
+#include "RegMask.h"
+
+uint8_t tlv493d::getFromRegs(const RegMask_t *mask, uint8_t *regData)
+{
+	return (regData[mask->byteAdress] & mask->bitMask) >> mask->shift;
+}
+
+
+uint8_t tlv493d::setToRegs(const RegMask_t *mask, uint8_t *regData, uint8_t toWrite)
+{
+	if(mask->rw == REGMASK_WRITE)
+	{
+		uint8_t regValue = regData[mask->byteAdress];
+		regValue &= ~(mask->bitMask);
+		regValue |= (toWrite << mask->shift) & mask->bitMask;
+		regData[mask->byteAdress] = regValue;
+	}
+	return 0;
+}
+
+
+

firmware/lib/tlv/src/util/RegMask.h

@@ -0,0 +1,57 @@
+/**
+ * RegMask.h - Part of the library for Arduino to control the TLV493D-A1B6 3D magnetic sensor.
+ *
+ * The 3D magnetic sensor TLV493D-A1B6 offers accurate three dimensional sensing with extremely low power consumption 
+ * in a small 6-pin package. With an opportunity to detect the magnetic field in x, y, and z-direction the sensor is 
+ * ideally suited for the measurement of 3D movements, linear movements and rotation movements.
+ * 
+ * Have a look at the application note/reference manual for more information.
+ * 
+ * Copyright (c) 2018 Infineon Technologies AG
+ * 
+ * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the 
+ * following conditions are met:   
+ *                                                                              
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the following 
+ * disclaimer.                        
+ * 
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following 
+ * disclaimer in the documentation and/or other materials provided with the distribution.                       
+ * 
+ * Neither the name of the copyright holders nor the names of its contributors may be used to endorse or promote 
+ * products derived from this software without specific prior written permission.                                           
+ *                                                                              
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 
+ * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE  
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE  FOR ANY DIRECT, INDIRECT, INCIDENTAL, 
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR  
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY,OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.   
+ */
+
+#ifndef TLV493D_REGMASK_H_INCLUDED
+#define TLV493D_REGMASK_H_INCLUDED
+
+#include <Arduino.h>
+
+#define REGMASK_READ	0
+#define REGMASK_WRITE	1
+
+namespace tlv493d
+{
+
+typedef struct
+{
+	uint8_t rw;
+	uint8_t byteAdress;
+	uint8_t bitMask;
+	uint8_t shift;
+} RegMask_t;
+
+uint8_t getFromRegs(const RegMask_t *mask, uint8_t *regData);
+uint8_t setToRegs(const RegMask_t *mask, uint8_t *regData, uint8_t toWrite);
+
+}
+
+#endif

firmware/lib/tlv/src/util/Tlv493d_conf.h

@@ -0,0 +1,134 @@
+/**
+ * Tlv493d_conf.h - Part of the library for Arduino to control the TLV493D-A1B6 3D magnetic sensor.
+ *
+ * The 3D magnetic sensor TLV493D-A1B6 offers accurate three dimensional sensing with extremely low power consumption 
+ * in a small 6-pin package. With an opportunity to detect the magnetic field in x, y, and z-direction the sensor is 
+ * ideally suited for the measurement of 3D movements, linear movements and rotation movements.
+ * 
+ * Have a look at the application note/reference manual for more information.
+ * 
+ * Copyright (c) 2018 Infineon Technologies AG
+ * 
+ * Redistribution and use in source and binary forms, with or without modification, are permitted provided that the 
+ * following conditions are met:   
+ *                                                                              
+ * Redistributions of source code must retain the above copyright notice, this list of conditions and the following 
+ * disclaimer.                        
+ * 
+ * Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following 
+ * disclaimer in the documentation and/or other materials provided with the distribution.                       
+ * 
+ * Neither the name of the copyright holders nor the names of its contributors may be used to endorse or promote 
+ * products derived from this software without specific prior written permission.                                           
+ *                                                                              
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 
+ * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE  
+ * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE  FOR ANY DIRECT, INDIRECT, INCIDENTAL, 
+ * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR  
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 
+ * WHETHER IN CONTRACT, STRICT LIABILITY,OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.   
+ */
+
+#ifndef TLV493D_CONF_H_INCLUDED
+#define TLV493D_CONF_H_INCLUDED
+
+#include "RegMask.h"
+#include "Tlv493d.h"
+
+
+#define TLV493D_DEFAULTMODE			POWERDOWNMODE
+
+#define TLV493D_STARTUPDELAY		40
+#define TLV493D_RESETDELAY			60
+
+#define TLV493D_NUM_OF_REGMASKS		25
+#define TLV493D_NUM_OF_ACCMODES		5
+
+#define TLV493D_MEASUREMENT_READOUT	7
+#define TLV493D_FAST_READOUT		3
+
+#define TLV493D_B_MULT 				0.098
+#define TLV493D_TEMP_MULT 			1.1
+#define TLV493D_TEMP_OFFSET 		315
+
+
+namespace tlv493d
+{
+
+typedef struct
+{
+	uint8_t fast;
+	uint8_t lp;
+	uint8_t lpPeriod;
+	uint16_t measurementTime;
+} AccessMode_t;
+
+enum Registers_e
+{
+	R_BX1 = 0,
+	R_BX2, 
+	R_BY1, 
+	R_BY2, 
+	R_BZ1, 
+	R_BZ2, 
+	R_TEMP1, 
+	R_TEMP2, 
+	R_FRAMECOUNTER, 
+	R_CHANNEL,
+	R_POWERDOWNFLAG, 
+	R_RES1,
+	R_RES2,
+	R_RES3,
+	W_PARITY,
+	W_ADDR,
+	W_INT,
+	W_FAST,
+	W_LOWPOWER,
+	W_TEMP_NEN,
+	W_LP_PERIOD,
+	W_PARITY_EN,
+	W_RES1,
+	W_RES2,
+	W_RES3
+};
+
+const RegMask_t regMasks[] = {
+	{ REGMASK_READ, 0, 0xFF, 0 },		// R_BX1
+	{ REGMASK_READ, 4, 0xF0, 4 },		// R_BX2
+	{ REGMASK_READ, 1, 0xFF, 0 },		// R_BY1
+	{ REGMASK_READ, 4, 0x0F, 0 },		// R_BY2
+	{ REGMASK_READ, 2, 0xFF, 0 },		// R_BZ1
+	{ REGMASK_READ, 5, 0x0F, 0 },		// R_BZ2
+	{ REGMASK_READ, 3, 0xF0, 4 },		// R_TEMP1
+	{ REGMASK_READ, 6, 0xFF, 0 },		// R_TEMP2
+	{ REGMASK_READ, 3, 0x0C, 2 },		// R_FRAMECOUNTER
+	{ REGMASK_READ, 3, 0x03, 0 },		// R_CHANNEL
+	{ REGMASK_READ, 5, 0x10, 4 },		// R_POWERDOWNFLAG
+	{ REGMASK_READ, 7, 0x18, 3 },		// R_RES1
+	{ REGMASK_READ, 8, 0xFF, 0 },		// R_RES2
+	{ REGMASK_READ, 9, 0x1F, 0 },		// R_RES3
+	{ REGMASK_WRITE, 1, 0x80, 7 },		// W_PARITY
+	{ REGMASK_WRITE, 1, 0x60, 5 },		// W_ADDR
+	{ REGMASK_WRITE, 1, 0x04, 2 },		// W_INT
+	{ REGMASK_WRITE, 1, 0x02, 1 },		// W_FAST
+	{ REGMASK_WRITE, 1, 0x01, 0 },		// W_LOWPOWER
+	{ REGMASK_WRITE, 3, 0x80, 7 },		// W_TEMP_EN
+	{ REGMASK_WRITE, 3, 0x40, 6 },		// W_LOWPOWER
+	{ REGMASK_WRITE, 3, 0x20, 5 },		// W_POWERDOWN
+	{ REGMASK_WRITE, 1, 0x18, 3 },		// W_RES1
+	{ REGMASK_WRITE, 2, 0xFF, 0 },		// W_RES2
+	{ REGMASK_WRITE, 3, 0x1F, 0 }		// W_RES3
+};
+
+const AccessMode_t accModes[] = {
+	{ 0, 0, 0, 1000 },		// POWERDOWNMODE
+	{ 1, 0, 0, 0 },			// FASTMODE
+	{ 0, 1, 1, 10 },		// LOWPOWERMODE
+	{ 0, 1, 0, 100 },		// ULTRALOWPOWERMODE
+	{ 1, 1, 1, 10 }			// MASTERCONTROLLEDMODE
+};
+
+}
+
+#endif

firmware/platformio.ini

@@ -8,9 +8,8 @@
 ; Please visit documentation for the other options and examples
 ; https://docs.platformio.org/page/projectconf.html
 
-[env:tdisplay]
-platform = espressif32
-board = esp32doit-devkit-v1
+[base_config]
+platform = espressif32@3.4
 framework = arduino
 monitor_speed = 115200
 monitor_flags = 
@@ -18,14 +17,41 @@ monitor_flags =
 	--echo
 	--filter=esp32_exception_decoder
 lib_deps =
-    bodmer/TFT_eSPI@2.4.25
-    askuric/Simple FOC @ ^2.2
-    infineon/TLV493D-Magnetic-Sensor @ ^1.0.3
-    bxparks/AceButton @ ^1.9.1
+    askuric/Simple FOC @ 2.2.0
+    infineon/TLV493D-Magnetic-Sensor @ 1.0.3
+    bxparks/AceButton @ 1.9.1
 
 build_flags =
   -DCORE_DEBUG_LEVEL=ARDUHAL_LOG_LEVEL_DEBUG
 
+[env:proto]
+extends = base_config
+board = esp32doit-devkit-v1
+lib_deps =
+  ${base_config.lib_deps}
+  bodmer/TFT_eSPI@2.4.25
+
+build_flags =
+  ${base_config.build_flags}
+  -DSK_DISPLAY=1
+
+  -DPIN_UH=27
+  -DPIN_UL=26
+  -DPIN_VH=25
+  -DPIN_VL=33
+  -DPIN_WH=32
+  -DPIN_WL=13
+  -DPIN_BUTTON_NEXT=36
+  -DPIN_BUTTON_PREV=-1
+  -DPIN_SDA=-1
+  -DPIN_SCL=-1
+  -DSENSOR_MT6701=1
+
+  -DDESCRIPTION_FONT=Roboto_Thin_24
+  -DDESCRIPTION_Y_OFFSET=20
+  -DVALUE_OFFSET=30
+  -DDRAW_ARC=0
+
   -DUSER_SETUP_LOADED=1
   -DGC9A01_DRIVER=1
   -DCGRAM_OFFSET=1
@@ -41,3 +67,152 @@ build_flags =
   -DLOAD_GLCD=1
   -DLOAD_GFXFF=1
   -DSPI_FREQUENCY=40000000
+
+[env:view]
+extends = base_config
+; platform = https://github.com/platformio/platform-espressif32.git#feature/arduino-upstream
+; platform_packages =
+;    framework-arduinoespressif32 @ https://github.com/espressif/arduino-esp32#master
+board = esp32doit-devkit-v1
+lib_deps =
+  ; askuric/Simple FOC @ 2.2.1
+  ; bxparks/AceButton @ 1.9.1
+  ${base_config.lib_deps}
+  bodmer/TFT_eSPI@2.4.25
+  fastled/FastLED @ 3.5.0
+  bogde/HX711 @ 0.7.5
+  adafruit/Adafruit VEML7700 Library @ 1.1.1
+
+build_flags =
+  ${base_config.build_flags}
+  -DSK_DISPLAY=1
+  -DSK_LEDS=1
+  -DNUM_LEDS=8
+  -DSENSOR_MT6701=1
+  -DSK_STRAIN=1
+  -DSK_INVERT_ROTATION=1
+  -DSK_ALS=1
+
+  -DPIN_UH=26
+  -DPIN_UL=25
+  -DPIN_VH=27
+  -DPIN_VL=32
+  -DPIN_WH=12
+  -DPIN_WL=33
+  -DPIN_BUTTON_NEXT=-1
+  -DPIN_BUTTON_PREV=-1
+  -DPIN_SDA=15
+  -DPIN_SCL=8
+  -DPIN_MT_DATA=37
+  -DPIN_MT_CLOCK=13
+  -DPIN_MT_CSN=14
+  -DPIN_LED_DATA=7
+  -DPIN_LCD_BACKLIGHT=19
+
+  -DLEDC_CHANNEL_LCD_BACKLIGHT=0
+
+  -DDESCRIPTION_FONT=Roboto_Thin_24
+  -DDESCRIPTION_Y_OFFSET=20
+  -DVALUE_OFFSET=30
+  -DDRAW_ARC=0
+
+  -DUSER_SETUP_LOADED=1
+  -DGC9A01_DRIVER=1
+  -DCGRAM_OFFSET=1
+  -DTFT_WIDTH=240
+  -DTFT_HEIGHT=240
+  -DTFT_MISO=-1
+  -DTFT_MOSI=5
+  -DTFT_SCLK=20
+  -DTFT_CS=21
+  -DTFT_DC=22
+  -DTFT_RST=4
+  -DTFT_BL=-1
+  -DLOAD_GLCD=1
+  -DLOAD_GFXFF=1
+  -DSPI_FREQUENCY=40000000
+
+  ; Reduce loop task stack size (only works on newer IDF Arduino core)
+  ; -DARDUINO_LOOP_STACK_SIZE=2048
+
+  ; Modify the default unusable pin mask to allow GPIO 7 (allowed to use on ESP32-PICO-V3-02)
+  ; Unusable bits: 6, 8, 9, 10, 20
+  ; (0ULL | _FL_BIT(6) | _FL_BIT(8) | _FL_BIT(9) | _FL_BIT(10) | _FL_BIT(20))
+  -DFASTLED_UNUSABLE_PIN_MASK=0x100740LL
+  
+  ; 0~39 except from 24, 28~31 are valid
+  ; (0xFFFFFFFFFFULL & ~(0ULL | _FL_BIT(24) | _FL_BIT(28) | _FL_BIT(29) | _FL_BIT(30) | _FL_BIT(31)))
+  -DSOC_GPIO_VALID_GPIO_MASK=0xFF0EFFFFFF
+  ; GPIO >= 34 are input only
+  ; (SOC_GPIO_VALID_GPIO_MASK & ~(0ULL | _FL_BIT(34) | _FL_BIT(35) | _FL_BIT(36) | _FL_BIT(37) | _FL_BIT(38) | _FL_BIT(39)))
+  -DSOC_GPIO_VALID_OUTPUT_GPIO_MASK=0x30EFFFFFF
+
+
+[env:handheld]
+extends = base_config
+board = tinypico
+
+build_flags =
+  ${base_config.build_flags}
+  -DSK_DISPLAY=0
+
+  -DPIN_UH=25
+  -DPIN_UL=26
+  -DPIN_VH=27
+  -DPIN_VL=15
+  -DPIN_WH=14
+  -DPIN_WL=4
+  -DPIN_BUTTON_NEXT=23
+  -DPIN_BUTTON_PREV=-1
+  -DPIN_SDA=33
+  -DPIN_SCL=32
+  -DSENSOR_TLV=1
+  
+[env:handheld_tdisplay]
+extends = base_config
+board = esp32doit-devkit-v1
+lib_deps =
+  ${base_config.lib_deps}
+  bodmer/TFT_eSPI@2.4.25
+
+build_flags =
+  ${base_config.build_flags}
+  -DSK_DISPLAY=1
+  -DSK_STRAIN=0
+  -DSK_LEDS=0
+
+  -DPIN_UH=17
+  -DPIN_UL=2
+  -DPIN_VH=13
+  -DPIN_VL=32
+  -DPIN_WH=33
+  -DPIN_WL=25
+  -DPIN_BUTTON_NEXT=35
+  -DPIN_BUTTON_PREV=0
+  -DPIN_SDA=-1
+  -DPIN_SCL=-1
+  -DSENSOR_MT6701=1
+  -DPIN_MT_DATA=21
+  -DPIN_MT_CLOCK=22
+  -DPIN_MT_CSN=12
+
+  -DDESCRIPTION_FONT=FreeSans9pt7b
+  -DDESCRIPTION_Y_OFFSET=80
+  -DVALUE_OFFSET=0
+  -DDRAW_ARC=1
+
+  -DUSER_SETUP_LOADED=1
+  -DST7789_DRIVER=1
+  -DCGRAM_OFFSET=1
+  -DTFT_WIDTH=135
+  -DTFT_HEIGHT=240
+  -DTFT_MISO=-1
+  -DTFT_MOSI=19
+  -DTFT_SCLK=18
+  -DTFT_CS=5
+  -DTFT_DC=16
+  -DTFT_RST=23
+  -DTFT_BL=4
+  -DLOAD_GLCD=1
+  -DLOAD_GFXFF=1
+  -DSPI_FREQUENCY=40000000

firmware/src/display_task.cpp

@@ -1,18 +1,20 @@
+#if SK_DISPLAY
 #include "display_task.h"
 #include "semaphore_guard.h"
 
 #include "font/roboto_light_60.h"
 
-DisplayTask::DisplayTask(const uint8_t task_core) : Task{"Display", 2048, 1, task_core} {
-    semaphore_ = xSemaphoreCreateMutex();
-    assert(semaphore_ != NULL);
-    xSemaphoreGive(semaphore_);
+DisplayTask::DisplayTask(const uint8_t task_core) : Task{"Display", 4048, 1, task_core} {
+  knob_state_queue_ = xQueueCreate(1, sizeof(KnobState));
+  assert(knob_state_queue_ != NULL);
+
+  mutex_ = xSemaphoreCreateMutex();
+  assert(mutex_ != NULL);
 }
 
 DisplayTask::~DisplayTask() {
-    if (semaphore_ != NULL) {
-        vSemaphoreDelete(semaphore_);
-    }
+  vQueueDelete(knob_state_queue_);
+  vSemaphoreDelete(mutex_);
 }
 
 static void HSV_to_RGB(float h, float s, float v, uint8_t *r, uint8_t *g, uint8_t *b)
@@ -73,16 +75,23 @@ static void HSV_to_RGB(float h, float s, float v, uint8_t *r, uint8_t *g, uint8_
 }
 
 void DisplayTask::run() {
-    delay(100);
     tft_.begin();
     tft_.invertDisplay(1);
     tft_.setRotation(0);
-    tft_.fillScreen(TFT_PURPLE);
+    tft_.fillScreen(TFT_DARKGREEN);
+
+    ledcSetup(LEDC_CHANNEL_LCD_BACKLIGHT, 5000, 16);
+    ledcAttachPin(PIN_LCD_BACKLIGHT, LEDC_CHANNEL_LCD_BACKLIGHT);
+    ledcWrite(LEDC_CHANNEL_LCD_BACKLIGHT, UINT16_MAX);
 
     spr_.setColorDepth(16);
+
     if (spr_.createSprite(TFT_WIDTH, TFT_HEIGHT) == nullptr) {
       Serial.println("ERROR: sprite allocation failed!");
       tft_.fillScreen(TFT_RED);
+    } else {
+      Serial.println("Sprite created!");
+      tft_.fillScreen(TFT_PURPLE);
     }
     spr_.setTextColor(0xFFFF, TFT_BLACK);
     
@@ -100,9 +109,8 @@ void DisplayTask::run() {
     spr_.setTextDatum(CC_DATUM);
     spr_.setTextColor(TFT_WHITE);
     while(1) {
-        {
-            SemaphoreGuard lock(semaphore_);
-            state = state_;
+        if (xQueueReceive(knob_state_queue_, &state, portMAX_DELAY) == pdFALSE) {
+          continue;
         }
 
         spr_.fillSprite(TFT_BLACK);
@@ -112,9 +120,9 @@ void DisplayTask::run() {
         }
 
         spr_.setFreeFont(&Roboto_Light_60);
-        spr_.drawString(String() + state.current_position, TFT_WIDTH / 2, TFT_HEIGHT / 2 - 30, 1);
-        spr_.setFreeFont(&Roboto_Thin_24);
-        int32_t line_y = TFT_HEIGHT / 2 + 20;
+        spr_.drawString(String() + state.current_position, TFT_WIDTH / 2, TFT_HEIGHT / 2 - VALUE_OFFSET, 1);
+        spr_.setFreeFont(&DESCRIPTION_FONT);
+        int32_t line_y = TFT_HEIGHT / 2 + DESCRIPTION_Y_OFFSET;
         char* start = state.config.descriptor;
         char* end = start + strlen(state.config.descriptor);
         while (start < end) {
@@ -139,6 +147,9 @@ void DisplayTask::run() {
           spr_.drawLine(TFT_WIDTH/2 + RADIUS * cosf(left_bound), TFT_HEIGHT/2 - RADIUS * sinf(left_bound), TFT_WIDTH/2 + (RADIUS - 10) * cosf(left_bound), TFT_HEIGHT/2 - (RADIUS - 10) * sinf(left_bound), TFT_WHITE);
           spr_.drawLine(TFT_WIDTH/2 + RADIUS * cosf(right_bound), TFT_HEIGHT/2 - RADIUS * sinf(right_bound), TFT_WIDTH/2 + (RADIUS - 10) * cosf(right_bound), TFT_HEIGHT/2 - (RADIUS - 10) * sinf(right_bound), TFT_WHITE);
         }
+        if (DRAW_ARC) {
+          spr_.drawCircle(TFT_WIDTH/2, TFT_HEIGHT/2, RADIUS, TFT_DARKGREY);
+        }
 
         float adjusted_sub_position = state.sub_position_unit * state.config.position_width_radians;
         if (state.config.num_positions > 0) {
@@ -171,11 +182,22 @@ void DisplayTask::run() {
         }
 
         spr_.pushSprite(0, 0);
+
+        {
+          SemaphoreGuard lock(mutex_);
+          ledcWrite(LEDC_CHANNEL_LCD_BACKLIGHT, brightness_);
+        }
         delay(2);
     }
 }
 
-void DisplayTask::setData(KnobState state) {
-    SemaphoreGuard lock(semaphore_);
-    state_ = state;
+QueueHandle_t DisplayTask::getKnobStateQueue() {
+  return knob_state_queue_;
 }
+
+void DisplayTask::setBrightness(uint16_t brightness) {
+  SemaphoreGuard lock(mutex_);
+  brightness_ = brightness;
+}
+
+#endif

firmware/src/display_task.h

@@ -1,5 +1,7 @@
 #pragma once
 
+#if SK_DISPLAY
+
 #include <Arduino.h>
 #include <TFT_eSPI.h>
 
@@ -13,7 +15,9 @@ class DisplayTask : public Task<DisplayTask> {
         DisplayTask(const uint8_t task_core);
         ~DisplayTask();
 
-        void setData(KnobState state);
+        QueueHandle_t getKnobStateQueue();
+
+        void setBrightness(uint16_t brightness);
 
     protected:
         void run();
@@ -24,7 +28,17 @@ class DisplayTask : public Task<DisplayTask> {
         /** Full-size sprite used as a framebuffer */
         TFT_eSprite spr_ = TFT_eSprite(&tft_);
 
-        SemaphoreHandle_t semaphore_;
+        QueueHandle_t knob_state_queue_;
 
         KnobState state_;
+
+        SemaphoreHandle_t mutex_;
+
+        uint16_t brightness_;
 };
+
+#else
+
+class DisplayTask {};
+
+#endif

firmware/src/interface_task.cpp

@@ -1,10 +1,36 @@
 #include <AceButton.h>
+
+#if SK_LEDS
+#include <FastLED.h>
+#endif
+
+#if SK_STRAIN
+#include <HX711.h>
+#endif
+
+#if SK_ALS
+#include <Adafruit_VEML7700.h>
+#endif
+
 #include "interface_task.h"
+#include "util.h"
 
 using namespace ace_button;
 
 #define COUNT_OF(A) (sizeof(A) / sizeof(A[0]))
 
+#if SK_LEDS
+CRGB leds[NUM_LEDS];
+#endif
+
+#if SK_STRAIN
+HX711 scale;
+#endif
+
+#if SK_ALS
+Adafruit_VEML7700 veml = Adafruit_VEML7700();
+#endif
+
 static KnobConfig configs[] = {
     // int32_t num_positions;
     // int32_t position;
@@ -81,7 +107,7 @@ static KnobConfig configs[] = {
         32,
         0,
         8.225806452 * PI / 180,
-        1,
+        2,
         1,
         1.1,
         "Coarse values\nStrong detents",
@@ -97,25 +123,146 @@ static KnobConfig configs[] = {
     },
 };
 
-InterfaceTask::InterfaceTask(const uint8_t task_core, MotorTask& motor_task) : Task{"Interface", 2048, 1, task_core}, motor_task_(motor_task) {
+InterfaceTask::InterfaceTask(const uint8_t task_core, MotorTask& motor_task, DisplayTask* display_task) : Task("Interface", 4048, 1, task_core), motor_task_(motor_task), display_task_(display_task) {
+    #if SK_DISPLAY
+        assert(display_task != nullptr);
+    #endif
 }
 
 InterfaceTask::~InterfaceTask() {}
 
 void InterfaceTask::run() {
-    AceButton button(36);
-    pinMode(36, INPUT);
-    button.getButtonConfig()->setIEventHandler(this);
+    #if PIN_BUTTON_NEXT >= 34
+        pinMode(PIN_BUTTON_NEXT, INPUT);
+    #else
+        pinMode(PIN_BUTTON_NEXT, INPUT_PULLUP);
+    #endif
+    AceButton button_next((uint8_t) PIN_BUTTON_NEXT);
+    button_next.getButtonConfig()->setIEventHandler(this);
+
+    #if PIN_BUTTON_PREV > -1
+        #if PIN_BUTTON_PREV >= 34
+            pinMode(PIN_BUTTON_PREV, INPUT);
+        #else
+            pinMode(PIN_BUTTON_PREV, INPUT_PULLUP);
+        #endif
+        AceButton button_prev((uint8_t) PIN_BUTTON_PREV);
+        button_prev.getButtonConfig()->setIEventHandler(this);
+    #endif
+    
+    #if SK_LEDS
+        FastLED.addLeds<SK6812, PIN_LED_DATA, GRB>(leds, NUM_LEDS);
+    #endif
+
+    #if PIN_SDA >= 0 && PIN_SCL >= 0
+        Wire.begin(PIN_SDA, PIN_SCL);
+        Wire.setClock(400000);
+    #endif
+    #if SK_STRAIN
+        scale.begin(38, 2);
+    #endif
+
+    #if SK_ALS
+        if (veml.begin()) {
+            veml.setGain(VEML7700_GAIN_2);
+            veml.setIntegrationTime(VEML7700_IT_400MS);
+        } else {
+            Serial.println("ALS sensor not found!");
+        }
+    #endif
 
     motor_task_.setConfig(configs[0]);
+
+    // How far button is pressed, in range [0, 1]
+    float press_value_unit = 0;
+
+    // Interface loop:
     while (1) {
-        button.check();
+        button_next.check();
+        #if PIN_BUTTON_PREV > -1
+            button_prev.check();
+        #endif
         if (Serial.available()) {
             int v = Serial.read();
             if (v == ' ') {
-                nextConfig();
+                changeConfig(true);
             }
         }
+
+        #if SK_ALS
+            const float LUX_ALPHA = 0.005;
+            static float lux_avg;
+            float lux = veml.readLux();
+            lux_avg = lux * LUX_ALPHA + lux_avg * (1 - LUX_ALPHA);
+            static uint32_t last_als;
+            if (millis() - last_als > 1000) {
+                Serial.print("millilux: "); Serial.println(lux*1000);
+                last_als = millis();
+            }
+        #endif
+
+        #if SK_STRAIN
+            // TODO: calibrate and track (long term moving average) zero point (lower); allow calibration of set point offset
+            const int32_t lower = 950000;
+            const int32_t upper = 1800000;
+            if (scale.wait_ready_timeout(100)) {
+                int32_t reading = scale.read();
+
+                // Ignore readings that are way out of expected bounds
+                if (reading >= lower - (upper - lower) && reading < upper + (upper - lower)*2) {
+                    static uint32_t last_reading_display;
+                    if (millis() - last_reading_display > 1000) {
+                        Serial.print("HX711 reading: ");
+                        Serial.println(reading);
+                        last_reading_display = millis();
+                    }
+                    long value = CLAMP(reading, lower, upper);
+                    press_value_unit = 1. * (value - lower) / (upper - lower);
+
+                    static bool pressed;
+                    if (!pressed && press_value_unit > 0.75) {
+                        motor_task_.playHaptic(true);
+                        pressed = true;
+                        changeConfig(true);
+                    } else if (pressed && press_value_unit < 0.25) {
+                        motor_task_.playHaptic(false);
+                        pressed = false;
+                    }
+                }
+            } else {
+                Serial.println("HX711 not found.");
+
+                #if SK_LEDS
+                    for (uint8_t i = 0; i < NUM_LEDS; i++) {
+                        leds[i] = CRGB::Red;
+                    }
+                    FastLED.show();
+                #endif
+            }
+        #endif
+
+        uint16_t brightness = UINT16_MAX;
+        // TODO: brightness scale factor should be configurable (depends on reflectivity of surface)
+        #if SK_ALS
+            brightness = (uint16_t)CLAMP(lux_avg * 13000, (float)1280, (float)UINT16_MAX);
+        #endif
+
+        #if SK_DISPLAY
+            display_task_->setBrightness(brightness); // TODO: apply gamma correction
+        #endif
+
+        #if SK_LEDS
+            for (uint8_t i = 0; i < NUM_LEDS; i++) {
+                leds[i].setHSV(200 * press_value_unit, 255, brightness >> 8);
+
+                // Gamma adjustment
+                leds[i].r = dim8_video(leds[i].r);
+                leds[i].g = dim8_video(leds[i].g);
+                leds[i].b = dim8_video(leds[i].b);
+            }
+            FastLED.show();
+        #endif
+
         delay(10);
     }
 }
@@ -123,15 +270,34 @@ void InterfaceTask::run() {
 void InterfaceTask::handleEvent(AceButton* button, uint8_t event_type, uint8_t button_state) {
     switch (event_type) {
         case AceButton::kEventPressed:
-            nextConfig();
+            if (button->getPin() == PIN_BUTTON_NEXT) {
+                changeConfig(true);
+            }
+            #if PIN_BUTTON_PREV > -1
+                if (button->getPin() == PIN_BUTTON_PREV) {
+                    changeConfig(false);
+                }
+            #endif
             break;
         case AceButton::kEventReleased:
             break;
     }
 }
 
-void InterfaceTask::nextConfig() {
-    current_config_ = (current_config_ + 1) % COUNT_OF(configs);
-    Serial.printf("Changing config to %d:\n%s\n", current_config_, configs[current_config_].descriptor);
+void InterfaceTask::changeConfig(bool next) {
+    if (next) {
+        current_config_ = (current_config_ + 1) % COUNT_OF(configs);
+    } else {
+        if (current_config_ == 0) {
+            current_config_ = COUNT_OF(configs) - 1;
+        } else {
+            current_config_ --;
+        }
+    }
+    
+    Serial.print("Changing config to ");
+    Serial.print(current_config_);
+    Serial.print(" -- ");
+    Serial.println(configs[current_config_].descriptor);
     motor_task_.setConfig(configs[current_config_]);
 }

firmware/src/interface_task.h

@@ -3,6 +3,7 @@
 #include <AceButton.h>
 #include <Arduino.h>
 
+#include "display_task.h"
 #include "motor_task.h"
 #include "task.h"
 
@@ -10,7 +11,7 @@ class InterfaceTask : public Task<InterfaceTask>, public ace_button::IEventHandl
     friend class Task<InterfaceTask>; // Allow base Task to invoke protected run()
 
     public:
-        InterfaceTask(const uint8_t task_core, MotorTask& motor_task);
+        InterfaceTask(const uint8_t task_core, MotorTask& motor_task, DisplayTask* display_task);
         ~InterfaceTask();
 
         void handleEvent(ace_button::AceButton* button, uint8_t event_type, uint8_t button_state) override;
@@ -20,7 +21,9 @@ class InterfaceTask : public Task<InterfaceTask>, public ace_button::IEventHandl
 
     private:
         MotorTask& motor_task_;
+        DisplayTask* display_task_;
+
         int current_config_ = 0;
 
-        void nextConfig();
+        void changeConfig(bool next);
 };

firmware/src/main.cpp

@@ -1,31 +1,66 @@
 #include <Arduino.h>
-#include <FastLED.h>
 #include <SimpleFOC.h>
-#include <TFT_eSPI.h>
 
 #include "display_task.h"
 #include "interface_task.h"
 #include "motor_task.h"
-#include "tlv_sensor.h"
 
-DisplayTask display_task = DisplayTask(1);
-MotorTask motor_task = MotorTask(0, display_task);
-InterfaceTask interface_task = InterfaceTask(1, motor_task);
+#if SK_DISPLAY
+static DisplayTask display_task = DisplayTask(0);
+static DisplayTask* display_task_p = &display_task;
+#else
+static DisplayTask* display_task_p = nullptr;
+#endif
+static MotorTask motor_task = MotorTask(1);
 
-CRGB leds[1];
 
+InterfaceTask interface_task = InterfaceTask(0, motor_task, display_task_p);
+
+static QueueHandle_t knob_state_debug_queue;
 
 void setup() {
   Serial.begin(115200);
 
   motor_task.begin();
   interface_task.begin();
+
+  #if SK_DISPLAY
   display_task.begin();
 
-  vTaskDelete(nullptr);
+  // Connect display to motor_task's knob state feed
+  motor_task.addListener(display_task.getKnobStateQueue());
+  #endif
+
+  // Create a queue and register it with motor_task to print knob state to serial (see loop() below)
+  knob_state_debug_queue = xQueueCreate(1, sizeof(KnobState));
+  assert(knob_state_debug_queue != NULL);
+
+  motor_task.addListener(knob_state_debug_queue);
+
+  // Free up the loop task
+  vTaskDelete(NULL);
 }
 
 
+static KnobState state = {};
+uint32_t last_debug;
+
 void loop() {
-  assert(false);
+  // Print any new state, at most 5 times per second
+  if (millis() - last_debug > 200 && xQueueReceive(knob_state_debug_queue, &state, portMAX_DELAY) == pdTRUE) {
+    Serial.println(state.current_position);
+    last_debug = millis();
+  }
+
+  static uint32_t last_stack_debug;
+  if (millis() - last_stack_debug > 1000) {
+    Serial.println("Stack high water:");
+    Serial.printf("main: %d\n", uxTaskGetStackHighWaterMark(NULL));
+    #if SK_DISPLAY
+      Serial.printf("display: %d\n", uxTaskGetStackHighWaterMark(display_task.getHandle()));
+    #endif
+    Serial.printf("motor: %d\n", uxTaskGetStackHighWaterMark(motor_task.getHandle()));
+    Serial.printf("interface: %d\n", uxTaskGetStackHighWaterMark(interface_task.getHandle()));
+    last_stack_debug = millis();
+  }
 }

firmware/src/motor_task.cpp

@@ -1,13 +1,10 @@
 #include <SimpleFOC.h>
+#include <sensors/MagneticSensorI2C.h>
 
 #include "motor_task.h"
+#include "mt6701_sensor.h"
 #include "tlv_sensor.h"
-
-
-template <typename T> T CLAMP(const T& value, const T& low, const T& high) 
-{
-  return value < low ? low : (value > high ? high : value); 
-}
+#include "util.h"
 
 static const float DEAD_ZONE_DETENT_PERCENT = 0.2;
 static const float DEAD_ZONE_RAD = 1 * _PI / 180;
@@ -19,8 +16,8 @@ static const float IDLE_CORRECTION_MAX_ANGLE_RAD = 5 * PI / 180;
 static const float IDLE_CORRECTION_RATE_ALPHA = 0.0005;
 
 
-MotorTask::MotorTask(const uint8_t task_core, DisplayTask& display_task) : Task{"Motor", 8192, 1, task_core}, display_task_(display_task) {
-    queue_ = xQueueCreate(1, sizeof(KnobConfig));
+MotorTask::MotorTask(const uint8_t task_core) : Task("Motor", 1200, 1, task_core) {
+    queue_ = xQueueCreate(5, sizeof(Command));
     assert(queue_ != NULL);
 }
 
@@ -28,11 +25,15 @@ MotorTask::~MotorTask() {}
 
 
 // BLDC motor & driver instance
-BLDCMotor motor = BLDCMotor(7);
-BLDCDriver6PWM driver = BLDCDriver6PWM(27, 26, 25, 33, 32, 13);
-
-TlvSensor tlv = TlvSensor();
+BLDCMotor motor = BLDCMotor(1);
+BLDCDriver6PWM driver = BLDCDriver6PWM(PIN_UH, PIN_UL, PIN_VH, PIN_VL, PIN_WH, PIN_WL);
 
+#if SENSOR_TLV
+    TlvSensor encoder = TlvSensor();
+#elif SENSOR_MT6701
+    MT6701Sensor encoder = MT6701Sensor();
+#endif
+// MagneticSensorI2C tlv = MagneticSensorI2C(AS5600_I2C);
 
 Commander command = Commander(Serial);
 
@@ -40,18 +41,41 @@ Commander command = Commander(Serial);
 void doMotor(char* cmd) { command.motor(&motor, cmd); }
 
 void MotorTask::run() {
+    // Hardware-specific configuration:
+    // TODO: make this easier to configure
+    // Tune zero offset to the specific hardware (motor + mounted magnetic sensor).
+    // SimpleFOC is supposed to be able to determine this automatically (if you omit params to initFOC), but
+    // it seems to have a bug (or I've misconfigured it) that gets both the offset and direction very wrong!
+    // So this value is based on experimentation.
+    // TODO: dig into SimpleFOC calibration and find/fix the issue
+    // float zero_electric_offset = -0.6; // original proto
+    //float zero_electric_offset = 0.4; // handheld 1
+    // float zero_electric_offset = -0.8; // handheld 2
+    // float zero_electric_offset = 2.93; //0.15; // 17mm test
+    // float zero_electric_offset = 0.66; // 15mm handheld
+    float zero_electric_offset = 7.34;
+    Direction foc_direction = Direction::CW;
+    motor.pole_pairs = 7;
+
     driver.voltage_power_supply = 5;
     driver.init();
 
-    Wire.begin();
-    Wire.setClock(400000);
-    tlv.init();
+    #if SENSOR_TLV
+    encoder.init(Wire, false);
+    #endif
+
+    #if SENSOR_MT6701
+    encoder.init();
+    // motor.LPF_angle = LowPassFilter(0.05);
+    #endif
+    // motor.LPF_current_q = {0.01};
 
     motor.linkDriver(&driver);
 
     motor.controller = MotionControlType::torque;
     motor.voltage_limit = 5;
-    motor.linkSensor(&tlv);
+    motor.velocity_limit = 10000;
+    motor.linkSensor(&encoder);
 
     // Not actually using the velocity loop; but I'm using those PID variables
     // because SimpleFOC studio supports updating them easily over serial for tuning.
@@ -66,16 +90,191 @@ void MotorTask::run() {
 
     motor.init();
 
-    tlv.update();
+    encoder.update();
     delay(10);
 
-    // Tune zero offset to the specific hardware (motor + mounted magnetic sensor).
-    // SimpleFOC is supposed to be able to determine this automatically (if you omit params to initFOC), but
-    // it seems to have a bug (or I've misconfigured it) that gets both the offset and direction very wrong!
-    // So this value is based on experimentation.
-    // TODO: dig into SimpleFOC calibration and find/fix the issue
-    float zero_electric_offset = -0.6;
-    motor.initFOC(zero_electric_offset, Direction::CCW);
+    motor.initFOC(zero_electric_offset, foc_direction);
+
+    bool calibrate = false;
+
+    Serial.println("Press Y to run calibration");
+    uint32_t t = millis();
+    while (millis() - t < 3000) {
+        if (Serial.read() == 'Y') {
+            calibrate = true;
+            break;
+        }
+        delay(10);
+    }
+    if (calibrate) {
+        motor.controller = MotionControlType::angle_openloop;
+        motor.pole_pairs = 1;
+        motor.initFOC(0, Direction::CW);
+
+
+        float a = 0;
+
+        for (uint8_t i = 0; i < 200; i++) {
+            encoder.update();
+            motor.move(a);
+            delay(1);
+        }
+        float start_sensor = encoder.getAngle();
+
+        for (; a < 3 * _2PI; a += 0.01) {
+            encoder.update();
+            motor.move(a);
+            delay(1);
+        }
+
+        for (uint8_t i = 0; i < 200; i++) {
+            encoder.update();
+            delay(1);
+        }
+        float end_sensor = encoder.getAngle();
+
+
+        motor.voltage_limit = 0;
+        motor.move(a);
+        // Serial.println("Did motor turn counterclockwise? Press Y to continue, otherwise change motor wiring and restart");
+        // while (Serial.read() != 'Y') {
+        //     delay(10);
+        // }
+
+        Serial.println();
+
+        // TODO: check for no motor movement!
+
+        Serial.print("Sensor measures positive for positive motor rotation: ");
+        if (end_sensor > start_sensor) {
+            Serial.println("YES, Direction=CW");
+            motor.initFOC(0, Direction::CW);
+        } else {
+            Serial.println("NO, Direction=CCW");
+            motor.initFOC(0, Direction::CCW);
+        }
+
+        // Rotate many electrical revolutions and measure mechanical angle traveled, to calculate pole-pairs
+        uint8_t electrical_revolutions = 20;
+        Serial.printf("Going to measure %d electrical revolutions...\n", electrical_revolutions);
+        motor.voltage_limit = 5;
+        motor.move(a);
+        Serial.println("Going to electrical zero...");
+        float destination = a + _2PI;
+        for (; a < destination; a += 0.03) {
+            encoder.update();
+            motor.move(a);
+            delay(1);
+        }
+        Serial.println("pause...");
+        for (uint16_t i = 0; i < 1000; i++) {
+            encoder.update();
+            delay(1);
+        }
+        Serial.println("Measuring...");
+
+        start_sensor = motor.sensor_direction * encoder.getAngle();
+        destination = a + electrical_revolutions * _2PI;
+        for (; a < destination; a += 0.03) {
+            encoder.update();
+            motor.move(a);
+            delay(1);
+        }
+        for (uint16_t i = 0; i < 1000; i++) {
+            encoder.update();
+            motor.move(a);
+            delay(1);
+        }
+        end_sensor = motor.sensor_direction * encoder.getAngle();
+        motor.voltage_limit = 0;
+        motor.move(a);
+
+        if (fabsf(motor.shaft_angle - motor.target) > 1 * PI / 180) {
+            Serial.println("ERROR: motor did not reach target!");
+            while(1) {}
+        }
+
+        float electrical_per_mechanical = electrical_revolutions * _2PI / (end_sensor - start_sensor);
+        Serial.print("Electrical angle / mechanical angle (i.e. pole pairs) = ");
+        Serial.println(electrical_per_mechanical);
+
+        int measured_pole_pairs = (int)round(electrical_per_mechanical);
+        Serial.printf("Pole pairs set to %d\n", measured_pole_pairs);
+
+        delay(1000);
+
+
+
+        // Measure mechanical angle at every electrical zero for several revolutions
+        motor.voltage_limit = 5;
+        motor.move(a);
+        float offset_x = 0;
+        float offset_y = 0;
+        float destination1 = (floor(a / _2PI) + measured_pole_pairs / 2.) * _2PI;
+        float destination2 = (floor(a / _2PI)) * _2PI;
+        for (; a < destination1; a += 0.4) {
+            motor.move(a);
+            delay(100);
+            for (uint8_t i = 0; i < 100; i++) {
+                encoder.update();
+                delay(1);
+            }
+            float real_electrical_angle = _normalizeAngle(a);
+            float measured_electrical_angle = _normalizeAngle( (float)(motor.sensor_direction * measured_pole_pairs) * encoder.getMechanicalAngle()  - 0);
+
+            float offset_angle = measured_electrical_angle - real_electrical_angle;
+            offset_x += cosf(offset_angle);
+            offset_y += sinf(offset_angle);
+
+            Serial.print(degrees(real_electrical_angle));
+            Serial.print(", ");
+            Serial.print(degrees(measured_electrical_angle));
+            Serial.print(", ");
+            Serial.println(degrees(_normalizeAngle(offset_angle)));
+        }
+        for (; a > destination2; a -= 0.4) {
+            motor.move(a);
+            delay(100);
+            for (uint8_t i = 0; i < 100; i++) {
+                encoder.update();
+                delay(1);
+            }
+            float real_electrical_angle = _normalizeAngle(a);
+            float measured_electrical_angle = _normalizeAngle( (float)(motor.sensor_direction * measured_pole_pairs) * encoder.getMechanicalAngle()  - 0);
+
+            float offset_angle = measured_electrical_angle - real_electrical_angle;
+            offset_x += cosf(offset_angle);
+            offset_y += sinf(offset_angle);
+
+            Serial.print(degrees(real_electrical_angle));
+            Serial.print(", ");
+            Serial.print(degrees(measured_electrical_angle));
+            Serial.print(", ");
+            Serial.println(degrees(_normalizeAngle(offset_angle)));
+        }
+        motor.voltage_limit = 0;
+        motor.move(a);
+
+        float avg_offset_angle = atan2f(offset_y, offset_x);
+
+        // Apply settings
+        motor.pole_pairs = measured_pole_pairs;
+        motor.zero_electric_angle = avg_offset_angle + _3PI_2;
+        motor.voltage_limit = 5;
+        motor.controller = MotionControlType::torque;
+
+        Serial.print("\n\nRESULTS:\n  zero electric angle: ");
+        Serial.println(motor.zero_electric_angle);
+        Serial.print("  direction: ");
+        if (motor.sensor_direction == Direction::CW) {
+            Serial.println("CW");
+        } else {
+            Serial.println("CCW");
+        }
+        Serial.printf("  pole pairs: %d\n", motor.pole_pairs);
+        delay(2000);
+    }
+
     Serial.println(motor.zero_electric_angle);
 
     command.add('M', &doMotor, "foo");
@@ -96,33 +295,59 @@ void MotorTask::run() {
     uint32_t last_idle_start = 0;
     uint32_t last_debug = 0;
 
-    uint32_t last_display_update = 0;
+    uint32_t last_publish = 0;
 
     while (1) {
         motor.loopFOC();
 
-        if (xQueueReceive(queue_, &config, 0) == pdTRUE) {
-            Serial.println("Got new config");
-            current_detent_center = motor.shaft_angle;
-
-            // Update derivative factor of torque controller based on detent width.
-            // If the D factor is large on coarse detents, the motor ends up making noise because the P&D factors amplify the noise from the sensor.
-            // This is a piecewise linear function so that fine detents (small width) get a higher D factor and coarse detents get a small D factor.
-            // Fine detents need a nonzero D factor to artificially create "clicks" each time a new value is reached (the P factor is small
-            // for fine detents due to the smaller angular errors, and the existing P factor doesn't work well for very small angle changes (easy to
-            // get runaway due to sensor noise & lag)).
-            // TODO: consider eliminating this D factor entirely and just "play" a hardcoded haptic "click" (e.g. a quick burst of torque in each
-            // direction) whenever the position changes when the detent width is too small for the P factor to work well.
-            const float derivative_lower_strength = config.detent_strength_unit * 0.04;
-            const float derivative_upper_strength = config.detent_strength_unit * 0;
-            const float derivative_position_width_lower = 5 * PI / 180;
-            const float derivative_position_width_upper = 10 * PI / 180;
-            const float raw = derivative_lower_strength + (derivative_upper_strength - derivative_lower_strength)/(derivative_position_width_upper - derivative_position_width_lower)*(config.position_width_radians - derivative_position_width_lower);
-            motor.PID_velocity.D = CLAMP(
-                raw,
-                min(derivative_lower_strength, derivative_upper_strength),
-                max(derivative_lower_strength, derivative_upper_strength)
-            );
+        Command command;
+        if (xQueueReceive(queue_, &command, 0) == pdTRUE) {
+            switch (command.command_type) {
+                case CommandType::CONFIG: {
+                    config = command.data.config;
+                    Serial.println("Got new config");
+                    current_detent_center = motor.shaft_angle;
+                    #if SK_INVERT_ROTATION
+                        current_detent_center = -motor.shaft_angle;
+                    #endif
+
+                    // Update derivative factor of torque controller based on detent width.
+                    // If the D factor is large on coarse detents, the motor ends up making noise because the P&D factors amplify the noise from the sensor.
+                    // This is a piecewise linear function so that fine detents (small width) get a higher D factor and coarse detents get a small D factor.
+                    // Fine detents need a nonzero D factor to artificially create "clicks" each time a new value is reached (the P factor is small
+                    // for fine detents due to the smaller angular errors, and the existing P factor doesn't work well for very small angle changes (easy to
+                    // get runaway due to sensor noise & lag)).
+                    // TODO: consider eliminating this D factor entirely and just "play" a hardcoded haptic "click" (e.g. a quick burst of torque in each
+                    // direction) whenever the position changes when the detent width is too small for the P factor to work well.
+                    const float derivative_lower_strength = config.detent_strength_unit * 0.08;
+                    const float derivative_upper_strength = config.detent_strength_unit * 0.02;
+                    const float derivative_position_width_lower = radians(3);
+                    const float derivative_position_width_upper = radians(8);
+                    const float raw = derivative_lower_strength + (derivative_upper_strength - derivative_lower_strength)/(derivative_position_width_upper - derivative_position_width_lower)*(config.position_width_radians - derivative_position_width_lower);
+                    motor.PID_velocity.D = CLAMP(
+                        raw,
+                        min(derivative_lower_strength, derivative_upper_strength),
+                        max(derivative_lower_strength, derivative_upper_strength)
+                    );
+                    break;
+                }
+                case CommandType::HAPTIC: {
+                    float strength = command.data.haptic.press ? 5 : 1.5;
+                    motor.move(strength);
+                    for (uint8_t i = 0; i < 3; i++) {
+                        motor.loopFOC();
+                        delay(1);
+                    }
+                    motor.move(-strength);
+                    for (uint8_t i = 0; i < 3; i++) {
+                        motor.loopFOC();
+                        delay(1);
+                    }
+                    motor.move(0);
+                    motor.loopFOC();
+                    break;
+                }
+            }
         }
 
         idle_check_velocity_ewma = motor.shaft_velocity * IDLE_VELOCITY_EWMA_ALPHA + idle_check_velocity_ewma * (1 - IDLE_VELOCITY_EWMA_ALPHA);
@@ -147,6 +372,9 @@ void MotorTask::run() {
         }
 
         float angle_to_detent_center = motor.shaft_angle - current_detent_center;
+        #if SK_INVERT_ROTATION
+            angle_to_detent_center = -motor.shaft_angle - current_detent_center;
+        #endif
         if (angle_to_detent_center > config.position_width_radians * config.snap_point && (config.num_positions <= 0 || config.position > 0)) {
             current_detent_center += config.position_width_radians;
             angle_to_detent_center -= config.position_width_radians;
@@ -168,27 +396,63 @@ void MotorTask::run() {
 
 
 
-        if (fabsf(motor.shaft_velocity) > 20) {
+        if (fabsf(motor.shaft_velocity) > 60) {
             // Don't apply torque if velocity is too high (helps avoid positive feedback loop/runaway)
             motor.move(0);
         } else {
-            motor.move(motor.PID_velocity(-angle_to_detent_center + dead_zone_adjustment));
+            float torque = motor.PID_velocity(-angle_to_detent_center + dead_zone_adjustment);
+            #if SK_INVERT_ROTATION
+                torque = -torque;
+            #endif
+            motor.move(torque);
         }
 
-        if (millis() - last_display_update > 10) {
-            display_task_.setData({
+        if (millis() - last_publish > 10) {
+            publish({
                 .current_position = config.position,
                 .sub_position_unit = -angle_to_detent_center / config.position_width_radians,
                 .config = config,
             });
-            last_display_update = millis();
+            last_publish = millis();
         }
 
         motor.monitor();
         // command.run();
+
+        delay(1);
     }
 }
 
 void MotorTask::setConfig(const KnobConfig& config) {
-    xQueueOverwrite(queue_, &config);
+    Command command = {
+        .command_type = CommandType::CONFIG,
+        .data = {
+            .config = config,
+        }
+    };
+    xQueueSend(queue_, &command, portMAX_DELAY);
+}
+
+
+void MotorTask::playHaptic(bool press) {
+    Command command = {
+        .command_type = CommandType::HAPTIC,
+        .data = {
+            .haptic = {
+                .press = press,
+            },
+        }
+    };
+    xQueueSend(queue_, &command, portMAX_DELAY);
+}
+
+
+void MotorTask::addListener(QueueHandle_t queue) {
+    listeners_.push_back(queue);
+}
+
+void MotorTask::publish(const KnobState& state) {
+    for (auto listener : listeners_) {
+        xQueueOverwrite(listener, &state);
+    }
 }

firmware/src/motor_task.h

@@ -1,24 +1,49 @@
 #pragma once
 
 #include <Arduino.h>
+#include <vector>
 
 #include "knob_data.h"
 #include "task.h"
-#include "display_task.h"
+
+
+enum class CommandType {
+    CONFIG,
+    HAPTIC,
+};
+
+struct HapticData {
+    bool press;
+};
+
+struct Command {
+    CommandType command_type;
+    union CommandData {
+        KnobConfig config;
+        HapticData haptic;
+    };
+    CommandData data;
+};
 
 class MotorTask : public Task<MotorTask> {
     friend class Task<MotorTask>; // Allow base Task to invoke protected run()
 
     public:
-        MotorTask(const uint8_t task_core, DisplayTask& display_task);
+        MotorTask(const uint8_t task_core);
         ~MotorTask();
 
         void setConfig(const KnobConfig& config);
+        void playHaptic(bool press);
+
+        void addListener(QueueHandle_t queue);
 
     protected:
         void run();
 
     private:
-        DisplayTask& display_task_;
         QueueHandle_t queue_;
+
+        std::vector<QueueHandle_t> listeners_;
+
+        void publish(const KnobState& state);
 };

firmware/src/mt6701_sensor.cpp

@@ -0,0 +1,118 @@
+#include "mt6701_sensor.h"
+#include "driver/spi_master.h"
+
+static const float ALPHA = 0.4;
+
+
+static uint8_t tableCRC6[64] = {
+ 0x00, 0x03, 0x06, 0x05, 0x0C, 0x0F, 0x0A, 0x09,
+ 0x18, 0x1B, 0x1E, 0x1D, 0x14, 0x17, 0x12, 0x11,
+ 0x30, 0x33, 0x36, 0x35, 0x3C, 0x3F, 0x3A, 0x39,
+ 0x28, 0x2B, 0x2E, 0x2D, 0x24, 0x27, 0x22, 0x21,
+ 0x23, 0x20, 0x25, 0x26, 0x2F, 0x2C, 0x29, 0x2A,
+ 0x3B, 0x38, 0x3D, 0x3E, 0x37, 0x34, 0x31, 0x32,
+ 0x13, 0x10, 0x15, 0x16, 0x1F, 0x1C, 0x19, 0x1A,
+ 0x0B, 0x08, 0x0D, 0x0E, 0x07, 0x04, 0x01, 0x02
+};
+
+/*32-bit input data, right alignment, Calculation over 18 bits (mult. of 6) */
+static uint8_t CRC6_43_18bit (uint32_t w_InputData)
+{
+ uint8_t b_Index = 0;
+ uint8_t b_CRC = 0;
+
+ b_Index = (uint8_t )(((uint32_t)w_InputData >> 12u) & 0x0000003Fu);
+
+ b_CRC = (uint8_t )(((uint32_t)w_InputData >> 6u) & 0x0000003Fu);
+ b_Index = b_CRC ^ tableCRC6[b_Index];
+
+ b_CRC = (uint8_t )((uint32_t)w_InputData & 0x0000003Fu);
+ b_Index = b_CRC ^ tableCRC6[b_Index];
+
+ b_CRC = tableCRC6[b_Index];
+
+ return b_CRC;
+} 
+
+
+
+MT6701Sensor::MT6701Sensor() {}
+
+void MT6701Sensor::init() {
+
+    pinMode(PIN_MT_CSN, OUTPUT);
+    digitalWrite(PIN_MT_CSN, HIGH);
+
+  spi_bus_config_t tx_bus_config = {
+      .mosi_io_num = -1,
+      .miso_io_num = PIN_MT_DATA,
+      .sclk_io_num = PIN_MT_CLOCK,
+      .quadwp_io_num = -1,
+      .quadhd_io_num = -1,
+      .max_transfer_sz = 1000,
+  };
+  esp_err_t ret = spi_bus_initialize(HSPI_HOST, &tx_bus_config, 1);
+  ESP_ERROR_CHECK(ret);
+
+  spi_device_interface_config_t tx_device_config = {
+      .command_bits=0,
+      .address_bits=0,
+      .dummy_bits=0,
+      .mode=1,
+      .duty_cycle_pos=0,
+      .cs_ena_pretrans=4,
+      .cs_ena_posttrans=0,
+      .clock_speed_hz=4000000,
+      .input_delay_ns=0,
+      .spics_io_num=PIN_MT_CSN,
+      .flags = 0,
+      .queue_size=1,
+      .pre_cb=NULL,
+      .post_cb=NULL,
+  };
+  ret=spi_bus_add_device(HSPI_HOST, &tx_device_config, &spi_device_);
+  ESP_ERROR_CHECK(ret);
+
+
+  spi_transaction_.flags = SPI_TRANS_USE_RXDATA;
+  spi_transaction_.length = 24;
+  spi_transaction_.rxlength = 24;
+  spi_transaction_.tx_buffer = NULL;
+  spi_transaction_.rx_buffer = NULL;
+}
+
+float MT6701Sensor::getSensorAngle() {
+    uint32_t now = micros();
+    if (now - last_update_ > 100) {
+      
+      esp_err_t ret=spi_device_polling_transmit(spi_device_, &spi_transaction_);
+      assert(ret==ESP_OK);
+
+      uint32_t spi_32 = (spi_transaction_.rx_data[0] << 16) | (spi_transaction_.rx_data[1] << 8) | spi_transaction_.rx_data[2];
+      uint32_t angle_spi = spi_32 >> 10;
+
+      uint8_t field_status = (spi_32 >> 6) & 0x3;
+      uint8_t push_status = (spi_32 >> 8) & 0x1;
+      uint8_t loss_status = (spi_32 >> 9) & 0x1;
+
+      uint8_t received_crc = spi_32 & 0x3F;
+      uint8_t calculated_crc = CRC6_43_18bit(spi_32 >> 6);
+      
+      if (received_crc == calculated_crc) {
+        float new_angle = (float)angle_spi * 2 * PI / 16384;
+        float new_x = cosf(new_angle);
+        float new_y = sinf(new_angle);
+        x_ = new_x * ALPHA + x_ * (1-ALPHA);
+        y_ = new_y * ALPHA + y_ * (1-ALPHA);
+      } else {
+        Serial.printf("Bad CRC. expected %d, actual %d\n", calculated_crc, received_crc);
+      }
+
+      last_update_ = now;
+    }
+    float rad = -atan2f(y_, x_);
+    if (rad < 0) {
+        rad += 2*PI;
+    }
+    return rad;
+}

firmware/src/mt6701_sensor.h

@@ -0,0 +1,27 @@
+#pragma once
+
+#include <SimpleFOC.h>
+#include "driver/spi_master.h"
+
+class MT6701Sensor : public Sensor {
+    public:
+        MT6701Sensor();
+
+        // initialize the sensor hardware
+        void init();
+
+        // Get current shaft angle from the sensor hardware, and 
+        // return it as a float in radians, in the range 0 to 2PI.
+        //  - This method is pure virtual and must be implemented in subclasses.
+        //    Calling this method directly does not update the base-class internal fields.
+        //    Use update() when calling from outside code.
+        float getSensorAngle();
+    private:
+
+        spi_device_handle_t spi_device_;
+        spi_transaction_t spi_transaction_ = {};
+
+        float x_;
+        float y_;
+        uint32_t last_update_;
+};

firmware/src/tlv_sensor.cpp

@@ -1,11 +1,13 @@
 #include "tlv_sensor.h"
 
-static const float ALPHA = 0.04;
+static const float ALPHA = 1;
 
 TlvSensor::TlvSensor() {}
 
-void TlvSensor::init() {
-  tlv_.begin();
+void TlvSensor::init(TwoWire* wire, bool invert) {
+  wire_ = wire;
+  invert_ = invert;
+  tlv_.begin(*wire);
   tlv_.setAccessMode(Tlv493d::AccessMode_e::MASTERCONTROLLEDMODE);
   tlv_.disableInterrupt();
   tlv_.disableTemp();
@@ -13,13 +15,35 @@ void TlvSensor::init() {
 
 float TlvSensor::getSensorAngle() {
     uint32_t now = micros();
-    if (now - last_update_ > 100) {
+    if (now - last_update_ > 50) {
       tlv_.updateData();
+      frame_counts_[cur_frame_count_index_] = tlv_.getExpectedFrameCount();
+      cur_frame_count_index_++;
+      if (cur_frame_count_index_ >= sizeof(frame_counts_)) {
+        cur_frame_count_index_ = 0;
+      }
       x_ = tlv_.getX() * ALPHA + x_ * (1-ALPHA);
       y_ = tlv_.getY() * ALPHA + y_ * (1-ALPHA);
       last_update_ = now;
+
+      bool all_same = true;
+      uint8_t match_frame = frame_counts_[0];
+      for (uint8_t i = 1; i < sizeof(frame_counts_); i++) {
+        if (frame_counts_[i] != match_frame) {
+          all_same = false;
+          break;
+        }
+      }
+      if (all_same) {
+        Serial.println("LOCKED!");
+        init(wire_, invert_);
+        // Force unique frame counts to avoid reset loop
+        for (uint8_t i = 1; i < sizeof(frame_counts_); i++) {
+          frame_counts_[i] = i;
+        }
+      }
     }
-    float rad = atan2f(y_, x_);
+    float rad = (invert_ ? -1 : 1) * atan2f(y_, x_);
     if (rad < 0) {
         rad += 2*PI;
     }

firmware/src/tlv_sensor.h

@@ -8,7 +8,7 @@ class TlvSensor : public Sensor {
         TlvSensor();
 
         // initialize the sensor hardware
-        void init();
+        void init(TwoWire* wire, bool invert);
 
         // Get current shaft angle from the sensor hardware, and 
         // return it as a float in radians, in the range 0 to 2PI.
@@ -21,4 +21,9 @@ class TlvSensor : public Sensor {
         float x_;
         float y_;
         uint32_t last_update_;
+        TwoWire* wire_;
+        bool invert_;
+
+        uint8_t frame_counts_[3] = {};
+        uint8_t cur_frame_count_index_ = 0;
 };

firmware/src/util.h

@@ -0,0 +1,7 @@
+
+#pragma once
+
+template <typename T> T CLAMP(const T& value, const T& low, const T& high) 
+{
+  return value < low ? low : (value > high ? high : value); 
+}