Check in mods - auto-reset and testing tuning for 17mm motor

firmware/.vscode/extensions.json

@@ -1,7 +1,10 @@
-{
-    // See http://go.microsoft.com/fwlink/?LinkId=827846
-    // for the documentation about the extensions.json format
-    "recommendations": [
-        "platformio.platformio-ide"
-    ]
-}
+{
+    // See http://go.microsoft.com/fwlink/?LinkId=827846
+    // for the documentation about the extensions.json format
+    "recommendations": [
+        "platformio.platformio-ide"
+    ],
+    "unwantedRecommendations": [
+        "ms-vscode.cpptools-extension-pack"
+    ]
+}

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

@@ -18,9 +18,10 @@ monitor_flags =
 	--filter=esp32_exception_decoder
 lib_deps =
     askuric/Simple FOC @ 2.2.0
-    infineon/TLV493D-Magnetic-Sensor @ 1.0.3
+    ; infineon/TLV493D-Magnetic-Sensor @ 1.0.3
     bxparks/AceButton @ 1.9.1
 
+
 build_flags =
   -DCORE_DEBUG_LEVEL=ARDUHAL_LOG_LEVEL_DEBUG
 
@@ -99,7 +100,7 @@ build_flags =
   ${base_config.build_flags}
   -DSK_DISPLAY=1
 
-  -DPOLE_PAIRS=7
+  -DPOLE_PAIRS=6
   -DPIN_UH=17
   -DPIN_UL=2
   -DPIN_VH=13

firmware/src/motor_task.cpp

@@ -49,7 +49,8 @@ void MotorTask::run() {
     // 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 = -0.8; // handheld 2
+    float zero_electric_offset = 0; // 17mm test
     bool tlv_invert = true;
     Direction foc_direction = Direction::CCW;
 
@@ -58,8 +59,8 @@ void MotorTask::run() {
     driver.init();
 
     Wire.begin(PIN_SDA, PIN_SCL);
-    Wire.setClock(400000);
-    tlv.init(Wire, tlv_invert);
+    Wire.setClock(1000000);
+    tlv.init(&Wire, tlv_invert);
 
     motor.linkDriver(&driver);
 
@@ -83,7 +84,33 @@ void MotorTask::run() {
     tlv.update();
     delay(10);
 
+    //Open loop motor test code:
+    motor.controller = MotionControlType::angle_openloop;
+    while (1) {
+        motor.voltage_limit = 5;
+        for (float a = 0; a < _2PI; a += 0.001) {
+            motor.move(a);
+            delay(1);
+            // tlv.update();
+            Serial.printf("%d, %d\n", (int)(motor.electricalAngle() * 1000), (int)(tlv.getMechanicalAngle()*1000));
+        }
+        delay(1000);
+        for (float a = _2PI; a > 0; a -= 0.001) {
+            motor.move(a);
+            delay(1);
+            tlv.update();
+        }
+        motor.voltage_limit = 1;
+        motor.move(0);
+        delay(8000);
+    }
+/*
+1.7571
+4.4333
 
+5.5365
+3.3717
+*/
     motor.initFOC(zero_electric_offset, foc_direction);
     Serial.println(motor.zero_electric_angle);
 

firmware/src/tlv_sensor.cpp

@@ -4,9 +4,10 @@ static const float ALPHA = 0.04;
 
 TlvSensor::TlvSensor() {}
 
-void TlvSensor::init(TwoWire& wire, bool invert) {
+void TlvSensor::init(TwoWire* wire, bool invert) {
+  wire_ = wire;
   invert_ = invert;
-  tlv_.begin(wire);
+  tlv_.begin(*wire);
   tlv_.setAccessMode(Tlv493d::AccessMode_e::MASTERCONTROLLEDMODE);
   tlv_.disableInterrupt();
   tlv_.disableTemp();
@@ -14,11 +15,33 @@ void TlvSensor::init(TwoWire& wire, bool invert) {
 
 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 = (invert_ ? -1 : 1) * atan2f(y_, x_);
     if (rad < 0) {

firmware/src/tlv_sensor.h

@@ -8,7 +8,7 @@ class TlvSensor : public Sensor {
         TlvSensor();
 
         // initialize the sensor hardware
-        void init(TwoWire& wire, bool invert);
+        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,5 +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;
 };