try to fix submodule

firmware/lib/Arduino-FOC-drivers/src/encoders/a1334/A1334.cpp

@@ -0,0 +1,54 @@
+
+#include "./A1334.h"
+
+
+
+A1334::A1334(SPISettings settings, int nCS) : settings(settings), nCS(nCS) {
+	// nix
+};
+
+
+
+A1334::~A1334() {
+};
+
+
+
+
+void A1334::init(SPIClass* _spi) {
+	spi = _spi;
+	if (nCS>=0)
+		pinMode(nCS, OUTPUT);
+	digitalWrite(nCS, HIGH);
+	//SPI has an internal SPI-device counter, it is possible to call "begin()" from different devices
+	spi->begin();
+	readRawAngle(); // read an angle
+};
+
+
+
+
+A1334Angle A1334::readRawAngle() {
+	uint16_t command = A1334_REG_ANG;
+	uint16_t cmdResult = spi_transfer16(command);	// TODO fast mode
+	cmdResult = spi_transfer16(command);	
+	A1334Angle result = { .reg = cmdResult };
+	// TODO check parity
+	// errorflag = result.ef;
+	return result;
+};
+
+
+
+
+uint16_t A1334::spi_transfer16(uint16_t outdata) {
+	if (nCS>=0)
+		digitalWrite(nCS, 0);
+	spi->beginTransaction(settings);
+	uint16_t result = spi->transfer16(outdata);
+	spi->endTransaction();
+	if (nCS>=0)
+		digitalWrite(nCS, 1);
+	return result;
+};
+

firmware/lib/Arduino-FOC-drivers/src/encoders/a1334/A1334.h

@@ -0,0 +1,51 @@
+
+
+#ifndef __A1334_H__
+#define __A1334_H__
+
+#include <Arduino.h>
+#include <SPI.h>
+
+
+#define A1334_CPR 4096
+#define A1334_BITORDER MSBFIRST
+
+static SPISettings A1334SPISettings(8000000, A1334_BITORDER, SPI_MODE3); // @suppress("Invalid arguments")
+
+
+typedef union {
+	struct {
+		uint16_t angle:12;
+		uint16_t p:1;
+		uint16_t nf:1;
+		uint16_t ef:1;
+		uint16_t ridc:1;
+	};
+	uint16_t reg;
+} A1334Angle;
+
+
+#define A1334_REG_ANG 0x2000
+
+
+
+
+class A1334 {
+public:
+	A1334(SPISettings settings = A1334SPISettings, int nCS = -1);
+	virtual ~A1334();
+
+	virtual void init(SPIClass* _spi = &SPI);
+	A1334Angle readRawAngle(); // 10 or 12 bit angle value    
+protected:
+	uint16_t spi_transfer16(uint16_t outdata);
+	SPIClass* spi;
+	SPISettings settings;
+	int nCS = -1;
+};
+
+
+
+
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/a1334/MagneticSensorA1334.cpp

@@ -0,0 +1,27 @@
+
+#include "./MagneticSensorA1334.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+
+MagneticSensorA1334::MagneticSensorA1334(int nCS, SPISettings settings) : A1334(settings, nCS) {
+
+}
+
+
+MagneticSensorA1334::~MagneticSensorA1334(){ 
+
+}
+
+
+void MagneticSensorA1334::init(SPIClass* _spi) {
+    this->A1334::init(_spi);
+    this->Sensor::init();
+}
+
+
+float MagneticSensorA1334::getSensorAngle() {
+    A1334Angle angle_data = readRawAngle();
+    float result = ( angle_data.angle / (float)A1334_CPR) * _2PI;
+    // return the shaft angle
+    return result;
+}

firmware/lib/Arduino-FOC-drivers/src/encoders/a1334/MagneticSensorA1334.h

@@ -0,0 +1,24 @@
+
+#ifndef __MAGNETIC_SENSOR_A1334_H__
+#define __MAGNETIC_SENSOR_A1334_H__
+
+#include "common/base_classes/Sensor.h"
+#include "./A1334.h"
+
+
+
+class MagneticSensorA1334 : public Sensor, public A1334 {
+public:
+	MagneticSensorA1334(int nCS = -1, SPISettings settings = A1334SPISettings);
+	virtual ~MagneticSensorA1334();
+
+    virtual float getSensorAngle() override;
+
+	virtual void init(SPIClass* _spi = &SPI);
+private:
+};
+
+
+
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/aeat8800q24/AEAT8800Q24.cpp

@@ -0,0 +1,102 @@
+
+#include "AEAT8800Q24.h"
+
+
+AEAT8800Q24::AEAT8800Q24(int nCS, int pinNSL, SPISettings spiSettings, SPISettings ssiSettings) : nCS(nCS), pinNSL(pinNSL), spiSettings(spiSettings), ssiSettings(ssiSettings) {
+};
+AEAT8800Q24::~AEAT8800Q24(){
+};
+
+void AEAT8800Q24::init(SPIClass* _spi){
+    spi = _spi;
+    spi->beginTransaction(ssiSettings);
+};
+
+
+float AEAT8800Q24::getCurrentAngle() {
+    return ((float)readRawAngle())/(float)AEAT8800Q24_CPR * 2.0f * (float)PI;
+}; // angle in radians, return current value
+
+
+// reads angle via ssi
+uint16_t AEAT8800Q24::readRawAngle() {
+    // digitalWrite(pinNSL, LOW); //TODO maybe we don't need it...
+    // // 300ns delay
+    // delayMicroseconds(1);
+    uint16_t value = spi->transfer16(0x0000);
+    uint8_t status = spi->transfer(0x00);
+    //digitalWrite(pinNSL, HIGH);
+    // // 200ns delay before next read...
+    // delayMicroseconds(1);
+    lastStatus.reg = status;
+    return value;
+};
+
+AEAT8800Q24_Status_t AEAT8800Q24::getLastStatus() {
+    return lastStatus;
+}
+
+uint16_t AEAT8800Q24::getZero(){
+    uint8_t value = readRegister(AEAT8800Q24_REG_ZERO_MSB);
+    return (value<<8)|readRegister(AEAT8800Q24_REG_ZERO_LSB);
+};
+void AEAT8800Q24::setZero(uint16_t value){
+    writeRegister(AEAT8800Q24_REG_ZERO_MSB, (value>>8)&0xFF);
+    writeRegister(AEAT8800Q24_REG_ZERO_LSB, value&0xFF);
+};
+
+AEAT8800Q24_CONF0_t AEAT8800Q24::getConf0(){
+    AEAT8800Q24_CONF0_t result;
+    result.reg = readRegister(AEAT8800Q24_REG_CONF0);
+    return result;
+};
+void AEAT8800Q24::setConf0(AEAT8800Q24_CONF0_t value){
+    writeRegister(AEAT8800Q24_REG_CONF0, value.reg);
+};
+
+AEAT8800Q24_CONF1_t AEAT8800Q24::getConf1(){
+    AEAT8800Q24_CONF1_t result;
+    result.reg = readRegister(AEAT8800Q24_REG_CONF1);
+    return result;
+};
+void AEAT8800Q24::setConf1(AEAT8800Q24_CONF1_t value){
+    writeRegister(AEAT8800Q24_REG_CONF1, value.reg);
+};
+
+AEAT8800Q24_CONF2_t AEAT8800Q24::getConf2(){
+    AEAT8800Q24_CONF2_t result;
+    result.reg = readRegister(AEAT8800Q24_REG_CONF2);
+    return result;
+};
+void AEAT8800Q24::setConf2(AEAT8800Q24_CONF2_t value){
+    writeRegister(AEAT8800Q24_REG_CONF2, value.reg);
+};
+
+
+
+
+uint16_t AEAT8800Q24::transfer16SPI(uint16_t outValue) {
+    // delay 1us between switching the CS line to SPI
+    delayMicroseconds(1);
+    if (nCS >= 0)
+        digitalWrite(nCS, LOW);    
+    spi->endTransaction();
+    spi->beginTransaction(spiSettings);
+    uint16_t value = spi->transfer16(outValue);
+    spi->endTransaction();
+    if (nCS >= 0)
+        digitalWrite(nCS, HIGH);
+    // delay 1us between switching the CS line to SSI
+    delayMicroseconds(1);
+    spi->beginTransaction(ssiSettings);
+    return value;
+};
+uint8_t AEAT8800Q24::readRegister(uint8_t reg) {
+    uint16_t cmd = 0x8000 | ((reg&0x001F)<<8);
+    uint16_t value = transfer16SPI(cmd);
+    return value&0x00FF;
+};
+void AEAT8800Q24::writeRegister(uint8_t reg, uint8_t value) {
+    uint16_t cmd = 0x4000 | ((reg&0x1F)<<8) | value;
+    /*uint16_t result =*/ transfer16SPI(cmd);
+};

firmware/lib/Arduino-FOC-drivers/src/encoders/aeat8800q24/AEAT8800Q24.h

@@ -0,0 +1,119 @@
+#ifndef __AEAT8800Q24_H__
+#define __AEAT8800Q24_H__
+
+
+#include "Arduino.h"
+#include "SPI.h"
+
+#define AEAT8800Q24_CPR 65536
+
+#define AEAT8800Q24_REG_CUST0 0x00
+#define AEAT8800Q24_REG_CUST1 0x01
+#define AEAT8800Q24_REG_ZERO_LSB 0x02
+#define AEAT8800Q24_REG_ZERO_MSB 0x03
+#define AEAT8800Q24_REG_CONF0 0x04
+#define AEAT8800Q24_REG_CONF1 0x05
+#define AEAT8800Q24_REG_CONF2 0x06
+#define AEAT8800Q24_REG_CONF_OTP 0x13
+#define AEAT8800Q24_REG_CAL_OTP 0x1B
+#define AEAT8800Q24_REG_CUST_OTP 0x11
+#define AEAT8800Q24_REG_CAL 0x17
+
+#define AEAT8800Q24_CONF_OTP_ON 0xA3
+#define AEAT8800Q24_CAL_OTP_ON 0xA5
+#define AEAT8800Q24_CUST_OTP_ON 0xA1
+#define AEAT8800Q24_CAL_ON 0x02
+#define AEAT8800Q24_CAL_OFF 0x00
+
+
+typedef union {
+	struct {
+		uint8_t uvw_pwm_mode:1;
+		uint8_t pwm:2;
+		uint8_t iwidth:2;
+		uint8_t uvw:3;
+	};
+	uint8_t reg;
+} AEAT8800Q24_CONF0_t;
+
+
+typedef union {
+	struct {
+		uint8_t cpr1:4;
+		uint8_t hys:4;
+	};
+	uint8_t reg;
+} AEAT8800Q24_CONF1_t;
+
+
+typedef union {
+	struct {
+		uint8_t dir:1;
+		uint8_t zero_latency:1;
+		uint8_t abs_res:2;
+		uint8_t cpr2:4;
+	};
+	uint8_t reg;
+} AEAT8800Q24_CONF2_t;
+
+
+
+typedef union {
+	struct {
+		uint8_t mhi:1;
+		uint8_t mlo:1;
+		uint8_t ready:1;
+		uint8_t parity:1;
+		uint8_t :4;
+	};
+	uint8_t reg;
+} AEAT8800Q24_Status_t;
+
+
+#ifndef MSBFIRST
+#define MSBFIRST BitOrder::MSBFIRST
+#endif
+
+static SPISettings AEAT8800Q24SPISettings(1000000, MSBFIRST, SPI_MODE3); // @suppress("Invalid arguments")
+static SPISettings AEAT8800Q24SSISettings(4000000, MSBFIRST, SPI_MODE2); // @suppress("Invalid arguments")
+
+
+class AEAT8800Q24 {
+public:
+	AEAT8800Q24(int nCS, int pinNSL = MOSI, SPISettings spiSettings = AEAT8800Q24SPISettings, SPISettings ssiSettings = AEAT8800Q24SSISettings);
+	virtual ~AEAT8800Q24();
+
+	virtual void init(SPIClass* _spi = &SPI);
+
+	float getCurrentAngle(); // angle in radians, return current value
+
+	uint16_t readRawAngle(); // 14bit angle value
+	AEAT8800Q24_Status_t getLastStatus(); // get status associated with last angle read
+
+    uint16_t getZero();
+    void setZero(uint16_t);
+
+    AEAT8800Q24_CONF0_t getConf0();
+    void setConf0(AEAT8800Q24_CONF0_t);
+
+    AEAT8800Q24_CONF1_t getConf1();
+    void setConf1(AEAT8800Q24_CONF1_t);
+
+    AEAT8800Q24_CONF2_t getConf2();
+    void setConf2(AEAT8800Q24_CONF2_t);
+
+private:
+	int nCS = -1;
+    int pinNSL = -1;
+	SPISettings spiSettings;
+	SPISettings ssiSettings;
+	SPIClass* spi;
+	AEAT8800Q24_Status_t lastStatus;
+
+    uint16_t transfer16SPI(uint16_t outValue);
+    uint8_t readRegister(uint8_t reg);
+    void writeRegister(uint8_t reg, uint8_t value);
+};
+
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/aeat8800q24/MagneticSensorAEAT8800Q24.cpp

@@ -0,0 +1,24 @@
+#include "./MagneticSensorAEAT8800Q24.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+
+MagneticSensorAEAT8800Q24::MagneticSensorAEAT8800Q24(int nCS, int pinNSL, SPISettings spiSettings, SPISettings ssiSettings) : AEAT8800Q24(nCS, pinNSL, spiSettings, ssiSettings) {
+};
+
+
+MagneticSensorAEAT8800Q24::~MagneticSensorAEAT8800Q24(){ 
+};
+
+
+void MagneticSensorAEAT8800Q24::init(SPIClass* _spi) {
+    this->AEAT8800Q24::init(_spi);
+    this->Sensor::init();
+};
+
+
+float MagneticSensorAEAT8800Q24::getSensorAngle() {
+    float angle_data = readRawAngle();
+    angle_data = ( angle_data / (float)AEAT8800Q24_CPR) * _2PI;
+    // return the shaft angle
+    return angle_data;
+};

firmware/lib/Arduino-FOC-drivers/src/encoders/aeat8800q24/MagneticSensorAEAT8800Q24.h

@@ -0,0 +1,18 @@
+#ifndef __MAGNETICSENSORAEAT8800Q24_H__
+#define __MAGNETICSENSORAEAT8800Q24_H__
+
+
+#include "common/base_classes/Sensor.h"
+#include "./AEAT8800Q24.h"
+
+class MagneticSensorAEAT8800Q24 : public Sensor, public AEAT8800Q24 {
+public:
+	MagneticSensorAEAT8800Q24(int nCS, int pinNSL = MOSI, SPISettings spiSettings = AEAT8800Q24SPISettings, SPISettings ssiSettings = AEAT8800Q24SSISettings);
+	virtual ~MagneticSensorAEAT8800Q24();
+
+    virtual float getSensorAngle() override;
+
+	virtual void init(SPIClass* _spi = &SPI);
+};
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/aeat8800q24/README.md

@@ -0,0 +1,75 @@
+# AEAT-8800-Q24 SimpleFOC driver
+
+SPI/SSI driver for the absolute position magnetic rotary encoder. This encoder is not supported by the
+normal SimpleFOC drivers due to its mixed SPI/SSI mode.
+
+- access to the configuration registers of the AEAT-8800-Q24, enabling you to set parameters
+- angles are read via SSI, with 16 bit (!) precision
+- currently only 16 bit resolution is supported, don't lower the resolution
+
+
+## Hardware setup
+
+Connect as per normal for your SPI bus. No special hardware setup is needed to use this driver. nCS pin is
+required.
+
+Note that due to the way SSI and SPI share pins, you can normally only run one of these sensors per SPI bus.
+
+## Software setup
+
+Its actually easier to use than the standard SPI sensor class, because it is less generic:
+
+```c++
+#include "Arduino.h"
+#include "Wire.h"
+#include "SPI.h"
+#include "SimpleFOC.h"
+#include "SimpleFOCDrivers.h"
+#include "encoders/aeat8800q24/MagneticSensorAEAT8800Q24.h"
+
+#define SENSOR1_CS 5 // some digital pin that you're using as the nCS pin
+MagneticSensorAEAT8800Q24 sensor1(SENSOR1_CS);
+
+
+void setup() {
+    sensor1.init();
+}
+```
+
+Set some options:
+
+```c++
+MagneticSensorAEAT8800Q24 sensor1(SENSOR1_CS, MOSI_pin, mySPISettings, mySSISettings);
+```
+
+Use another SPI bus:
+
+```c++
+void setup() {
+    sensor1.init(SPI2);
+}
+```
+
+Here's how you can use it:
+
+```c++
+    // get the angle, in radians, including full rotations
+    float a1 = sensor1.getAngle();
+
+    // get the velocity, in rad/s - note: you have to call getAngle() on a regular basis for it to work
+    float v1 = sensor1.getVelocity();
+
+    // get the angle, in radians, no full rotations
+    float a2 = sensor1.getCurrentAngle();
+
+    // get the raw 16 bit value
+    uint16_t raw = sensor1.readRawAngle();
+
+    // check status
+    float angle = sensor1.getSensorAngle();
+    AEAT8800Q24_Status_t status = sensor1.getLastStatus();
+    if (status.mlo)
+        Serial.println("Sensor magnet low error");
+    if (status.mhi)
+        Serial.println("Sensor magnet high error");
+```

firmware/lib/Arduino-FOC-drivers/src/encoders/as5047/AS5047.cpp

@@ -0,0 +1,245 @@
+/*
+ * AS5047.cpp
+ *
+ *  Created on: 2 May 2021
+ *      Author: runger
+ */
+
+#include "./AS5047.h"
+
+AS5047::AS5047(SPISettings settings, int nCS) : settings(settings), nCS(nCS) {
+	// nix
+}
+
+AS5047::~AS5047() {
+}
+
+
+void AS5047::init(SPIClass* _spi) {
+	spi = _spi;
+	if (nCS>=0)
+		pinMode(nCS, OUTPUT);
+	digitalWrite(nCS, HIGH);
+	//SPI has an internal SPI-device counter, it is possible to call "begin()" from different devices
+	spi->begin();
+	readRawAngle(); // read an angle
+}
+
+float AS5047::getCurrentAngle(){
+	readCorrectedAngle();
+	return ((float)readCorrectedAngle())/(float)AS5047_CPR * 2.0f * (float)PI;
+}
+
+float AS5047::getFastAngle(){
+	return ((float)readCorrectedAngle())/(float)AS5047_CPR * 2.0f * (float)PI;
+}
+
+
+uint16_t AS5047::readRawAngle(){
+	uint16_t command = AS5047_ANGLEUNC_REG | AS5047_RW; // set r=1 and parity=0, result is 0x7FFE
+	uint16_t lastresult = spi_transfer16(command)&AS5047_RESULT_MASK;
+	return lastresult;
+}
+uint16_t AS5047::readCorrectedAngle(){
+	uint16_t command = AS5047_ANGLECOM_REG | AS5047_PARITY | AS5047_RW; // set r=1 and parity=1, result is 0xFFFF
+	uint16_t lastresult = spi_transfer16(command)&AS5047_RESULT_MASK;
+	return lastresult;
+}
+
+
+uint16_t AS5047::readMagnitude(){
+	uint16_t command = AS5047_MAGNITUDE_REG | AS5047_RW; // set r=1, result is 0x7FFD
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	uint16_t result = nop();
+	return result;
+}
+
+
+bool AS5047::isErrorFlag(){
+	return errorflag;
+}
+
+
+AS5047Error AS5047::clearErrorFlag(){
+	uint16_t command = AS5047_ERROR_REG | AS5047_RW; // set r=1, result is 0x4001
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	uint16_t result = nop();
+	AS5047Error err = {
+			.framingError = ((result&0x0001)!=0x0000),
+			.commandInvalid = ((result&0x0002)!=0x0000),
+			.parityError = ((result&0x0004)!=0x0000)
+	};
+	return err;
+}
+
+
+AS5047Settings1 AS5047::readSettings1(){
+	uint16_t command = AS5047_SETTINGS1_REG | AS5047_PARITY | AS5047_RW; // set r=1, result is 0xC018
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	AS5047Settings1 result = {
+			.reg = nop()
+	};
+	return result;
+}
+
+
+void AS5047::writeSettings1(AS5047Settings1 settings){
+	uint16_t command = AS5047_SETTINGS1_REG;  // set r=0, result is 0x0018
+	spi_transfer16(command);
+	spi_transfer16(calcParity(settings.reg));
+}
+
+
+AS5047Settings2 AS5047::readSettings2(){
+	uint16_t command = AS5047_SETTINGS2_REG | AS5047_RW; // set r=1, result is 0x4019
+	spi_transfer16(command);
+	AS5047Settings2 result = {
+			.reg = nop()
+	};
+	return result;
+}
+
+
+void AS5047::writeSettings2(AS5047Settings2 settings){
+	uint16_t command = AS5047_SETTINGS2_REG | AS5047_PARITY; // set r=0, result is 0x8019
+	spi_transfer16(command);
+	spi_transfer16(calcParity(settings.reg));
+}
+
+
+
+AS5047Diagnostics AS5047::readDiagnostics(){
+	uint16_t command = AS5047_DIAGNOSTICS_REG | AS5047_PARITY | AS5047_RW; // set r=1, result is 0xFFFC
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	AS5047Diagnostics result = {
+			.reg = nop()
+	};
+	return result;
+}
+
+
+void AS5047::enablePWM(bool enable){
+	AS5047Settings1 settings = readSettings1();
+	if (settings.pwmon == enable) return; // no change
+	settings.pwmon = enable;
+	writeSettings1(settings);
+}
+
+void AS5047::enableABI(bool enable){
+	AS5047Settings1 settings = readSettings1();
+	uint8_t val = enable?0:1;
+	if (settings.uvw_abi == val) return; // no change
+	settings.uvw_abi = val;
+	writeSettings1(settings);
+}
+
+
+void AS5047::enableDEAC(bool enable){
+	AS5047Settings1 settings = readSettings1();
+	uint8_t val = enable?0:1;
+	if (settings.daecdis == val) return; // no change
+	settings.daecdis = enable?0:1;
+	writeSettings1(settings);
+}
+
+
+void AS5047::useCorrectedAngle(bool useCorrected){
+	AS5047Settings1 settings = readSettings1();
+	uint8_t val = useCorrected?0:1;
+	if (settings.dataselect == val) return; // no change
+	settings.dataselect = val;
+	writeSettings1(settings);
+}
+
+
+
+void AS5047::setHysteresis(uint8_t hyst){
+	if (hyst>3) hyst = 3;
+	uint8_t val = 3-hyst;
+	AS5047Settings2 settings = readSettings2();
+	if (settings.hys == val) return; // no change
+	settings.hys = val;
+	writeSettings2(settings);
+}
+
+
+
+
+void AS5047::setABIResolution(AS5047ABIRes res){
+	AS5047Settings1 settings = readSettings1();
+	uint8_t val = (res>>3)&0x01;
+	if (settings.abibin!=val || settings.uvw_abi!=0) {
+		settings.abibin = val;
+		settings.uvw_abi = 0;
+		writeSettings1(settings);
+	}
+	AS5047Settings2 settings2 = readSettings2();
+	val = (res&0x07);
+	if (settings2.abires!=val) {
+		settings2.abires = val;
+		writeSettings2(settings2);
+	}
+} 
+
+
+
+uint16_t AS5047::setZero(uint16_t value){
+	uint16_t command = AS5047_ZPOSL_REG | AS5047_PARITY | AS5047_RW;
+	spi_transfer16(command);
+	AS5047ZPosL posL = {
+			.reg = nop()
+	};
+	command = AS5047_ZPOSM_REG | AS5047_PARITY;
+	spi_transfer16(command);
+	spi_transfer16(calcParity((value>>6)&0x00FF));
+	command = AS5047_ZPOSL_REG;
+	posL.zposl = value&0x003F;
+	spi_transfer16(command);
+	spi_transfer16(calcParity(posL.reg));
+
+	return getZero();
+}
+
+
+uint16_t AS5047::getZero() {
+	uint16_t command = AS5047_ZPOSM_REG | AS5047_RW;
+	spi_transfer16(command);
+	command = AS5047_ZPOSL_REG | AS5047_PARITY | AS5047_RW;
+	uint16_t result = spi_transfer16(command);
+	AS5047ZPosL posL = {
+			.reg = nop()
+	};
+	return ((result&0x00FF)<<6) | posL.zposl;
+}
+
+
+uint16_t AS5047::nop(){
+	uint16_t result = spi_transfer16(0xFFFF); // using 0xFFFF as nop instead of 0x0000, then next call to fastAngle will return an angle
+	return result&AS5047_RESULT_MASK;
+}
+
+
+uint16_t AS5047::calcParity(uint16_t data){
+	data = data & 0x7FFF;
+	int sum = 0;
+	for (int i=0;i<15;i++)
+		if ((data>>i)&0x0001)
+			sum++;
+	return (sum&0x01)==0x01?(data|0x8000):data;
+}
+
+
+uint16_t AS5047::spi_transfer16(uint16_t outdata) {
+	if (nCS>=0)
+		digitalWrite(nCS, 0);
+	spi->beginTransaction(settings);
+	uint16_t result = spi->transfer16(outdata);
+	spi->endTransaction();
+	if (nCS>=0)
+		digitalWrite(nCS, 1);
+	// TODO check parity
+	errorflag = ((result&AS5047_ERRFLG)>0);
+	return result;
+}
+
+

firmware/lib/Arduino-FOC-drivers/src/encoders/as5047/AS5047.h

@@ -0,0 +1,167 @@
+/*
+ * AS5047.h
+ *
+ *  Created on: 2 May 2021
+ *      Author: runger
+ */
+
+#ifndef AS5047_H_
+#define AS5047_H_
+
+#include "Arduino.h"
+#include "SPI.h"
+
+
+
+
+union AS5047Diagnostics {
+	struct {
+		uint16_t agc:8;
+		uint16_t lf:1;
+		uint16_t cof:1;
+		uint16_t magh:1;
+		uint16_t magl:1;
+		uint16_t unused:4;
+	};
+	uint16_t reg;
+};
+
+
+union AS5047ZPosM {
+	struct {
+		uint16_t zposm:8;
+		uint16_t unused:8;
+	};
+	uint16_t reg;
+};
+
+
+union AS5047ZPosL {
+	struct {
+		uint16_t zposl:6;
+		uint16_t comp_l_error_en:1;
+		uint16_t comp_h_error_en:1;
+		uint16_t unused:8;
+	};
+	uint16_t reg;
+};
+
+
+union AS5047Settings1 {
+	struct {
+		uint16_t reserved:1;
+		uint16_t noiseset:1;
+		uint16_t dir:1;
+		uint16_t uvw_abi:1;
+		uint16_t daecdis:1;
+		uint16_t abibin:1;
+		uint16_t dataselect:1;
+		uint16_t pwmon:1;
+		uint16_t unused:8;
+	};
+	uint16_t reg;
+};
+
+
+union AS5047Settings2 {
+	struct {
+		uint16_t uvwpp:3;
+		uint16_t hys:2;
+		uint16_t abires:3;
+		uint16_t unused:8;
+	};
+	uint16_t reg;
+};
+
+
+struct AS5047Error {
+	bool framingError;
+	bool commandInvalid;
+	bool parityError;
+};
+
+
+enum AS5047ABIRes : uint8_t {
+	AS5047_ABI_1024 = 0b1010,
+	AS5047_ABI_2048 = 0b1001,
+	AS5047_ABI_4096 = 0b1000,
+	AS5047_ABI_100 = 0b0111,
+	AS5047_ABI_200 = 0b0110,
+	AS5047_ABI_400 = 0b0101,
+	AS5047_ABI_800 = 0b0100,
+	AS5047_ABI_1200 = 0b0011,
+	AS5047_ABI_1600 = 0b0010,
+	AS5047_ABI_2000 = 0b0001,
+	AS5047_ABI_4000 = 0b0000
+};
+
+
+#define AS5047_CPR 16384
+#define AS5047_ANGLECOM_REG 0x3FFF
+#define AS5047_ANGLEUNC_REG 0x3FFE
+#define AS5047_MAGNITUDE_REG 0x3FFD
+#define AS5047_DIAGNOSTICS_REG 0x3FFC
+#define AS5047_ERROR_REG 0x0001
+#define AS5047_PROG_REG 0x0003
+#define AS5047_ZPOSM_REG 0x0016
+#define AS5047_ZPOSL_REG 0x0017
+#define AS5047_SETTINGS1_REG 0x0018
+#define AS5047_SETTINGS2_REG 0x0019
+
+#define AS5047_PARITY 0x8000
+#define AS5047_RW 0x4000
+#define AS5047_ERRFLG 0x4000
+#define AS5047_RESULT_MASK 0x3FFF
+
+
+#define AS5047_BITORDER MSBFIRST
+
+static SPISettings AS5047SPISettings(8000000, AS5047_BITORDER, SPI_MODE1); // @suppress("Invalid arguments")
+
+
+class AS5047 {
+public:
+	AS5047(SPISettings settings = AS5047SPISettings, int nCS = -1);
+	virtual ~AS5047();
+
+	virtual void init(SPIClass* _spi = &SPI);
+
+	float getCurrentAngle(); // angle in radians, return current value
+	float getFastAngle();	 // angle in radians, return last value and read new
+
+	uint16_t readRawAngle(); // 14bit angle value
+	uint16_t readCorrectedAngle(); // 14bit corrected angle value
+	uint16_t readMagnitude(); // 14bit magnitude value
+
+	bool isErrorFlag();
+	AS5047Error clearErrorFlag();
+
+	AS5047Diagnostics readDiagnostics();
+	AS5047Settings1 readSettings1();
+	void writeSettings1(AS5047Settings1 settings);
+	AS5047Settings2 readSettings2();
+	void writeSettings2(AS5047Settings2 settings);
+	void enablePWM(bool enable);
+	void enableABI(bool enable);
+	void setABIResolution(AS5047ABIRes res);
+	void enableDEAC(bool enable);
+	void useCorrectedAngle(bool useCorrected);
+	void setHysteresis(uint8_t hyst);
+
+	uint16_t setZero(uint16_t);
+	uint16_t getZero();
+
+	uint16_t calcParity(uint16_t data);
+
+private:
+
+	uint16_t nop();
+	uint16_t spi_transfer16(uint16_t outdata);
+	SPIClass* spi;
+	SPISettings settings;
+	bool errorflag = false;
+	int nCS = -1;
+
+};
+
+#endif /* AS5047_H_ */

firmware/lib/Arduino-FOC-drivers/src/encoders/as5047/MagneticSensorAS5047.cpp

@@ -0,0 +1,29 @@
+
+#include "./MagneticSensorAS5047.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+
+MagneticSensorAS5047::MagneticSensorAS5047(int nCS, bool fastMode, SPISettings settings) : AS5047(settings, nCS), fastMode(fastMode) {
+
+}
+
+
+MagneticSensorAS5047::~MagneticSensorAS5047(){ 
+
+}
+
+
+void MagneticSensorAS5047::init(SPIClass* _spi) {
+    this->AS5047::init(_spi);
+    this->Sensor::init();
+}
+
+
+float MagneticSensorAS5047::getSensorAngle() {
+    float angle_data = readRawAngle();
+    if (!fastMode) // read again to ensure current value
+        angle_data = readRawAngle();
+    angle_data = ( angle_data / (float)AS5047_CPR) * _2PI;
+    // return the shaft angle
+    return angle_data;
+}

firmware/lib/Arduino-FOC-drivers/src/encoders/as5047/MagneticSensorAS5047.h

@@ -0,0 +1,20 @@
+
+#ifndef __MAGNETICSENSORAS5047PD_H__
+#define __MAGNETICSENSORAS5047PD_H__
+
+#include "common/base_classes/Sensor.h"
+#include "./AS5047.h"
+
+class MagneticSensorAS5047 : public Sensor, public AS5047 {
+public:
+	MagneticSensorAS5047(int nCS = -1, bool fastMode = false, SPISettings settings = AS5047SPISettings);
+	virtual ~MagneticSensorAS5047();
+
+    virtual float getSensorAngle() override;
+
+	virtual void init(SPIClass* _spi = &SPI);
+private:
+    bool fastMode = false;
+};
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/as5047/README.md

@@ -0,0 +1,82 @@
+# AS5047 SimpleFOC driver
+
+While the AS5047 absolute position magnetic rotary encoder is supported by the standard MagneticSensorSPI driver included in the base distribution, this AS5047-specific driver includes some optimisations:
+
+- access to the other registers of the AS5047, including the magnitude value which can be used to check the magnet strength, and the diagnostics register
+- access to the error state of the sensor, and ability to clear errors
+- it has a fastMode setting, in which the sensor is sent only 1 command per getAngle() call - the value returned will be from previous getAngle() invocation
+
+This driver should work with AS5047P and AS5047D models. The AS5047U has it's own driver [here](../as5047u/).
+
+## Hardware setup
+
+Connect as per normal for your SPI bus. No special hardware setup is needed to use this driver.
+
+## Software setup
+
+Its actually easier to use than the standard SPI sensor class, because it is less generic:
+
+```c++
+#include "Arduino.h"
+#include "Wire.h"
+#include "SPI.h"
+#include "SimpleFOC.h"
+#include "SimpleFOCDrivers.h"
+#include "encoders/as5047/MagneticSensorAS5047.h"
+
+#define SENSOR1_CS 5 // some digital pin that you're using as the nCS pin
+MagneticSensorAS5047 sensor1(SENSOR1_CS);
+
+
+void setup() {
+    sensor1.init();
+}
+```
+
+Set some options:
+
+```c++
+MagneticSensorAS5047 sensor1(SENSOR1_CS, true, mySPISettings);
+```
+
+Use another SPI bus:
+
+```c++
+void setup() {
+    sensor1.init(SPI2);
+}
+```
+
+Here's how you can use it:
+
+```c++
+    // update the sensor (only needed if using the sensor without a motor)
+    sensor1.update();
+
+    // get the angle, in radians, including full rotations
+    float a1 = sensor1.getAngle();
+
+    // get the velocity, in rad/s - note: you have to call getAngle() on a regular basis for it to work
+    float v1 = sensor1.getVelocity();
+
+    // get the angle, in radians, no full rotations
+    float a2 = sensor1.getCurrentAngle();
+
+    // get the raw 14 bit value
+    uint16_t raw = sensor1.readRawAngle();
+
+    // read the CORDIC magnitude value, a measure of the magnet field strength
+    float m1 = sensor1.readMagnitude();
+
+    // check for errors
+    if (sensor1.isErrorFlag()) {
+        AS5047Error error = sensor1.clearErrorFlag();
+        if (error.parityError) { // also error.framingError, error.commandInvalid
+            // etc...            
+        }
+    }
+
+    // get diagnostics
+    AS5047Diagnostics diagnostics = sensor1.readDiagnostics();
+```
+

firmware/lib/Arduino-FOC-drivers/src/encoders/as5047u/AS5047U.cpp

@@ -0,0 +1,350 @@
+/*
+ * AS5047U.cpp
+ *
+ *  Created on: 24 Feb 2022
+ *      Author: runger
+ */
+
+#include "./AS5047U.h"
+
+AS5047U::AS5047U(SPISettings settings, int nCS) : settings(settings), nCS(nCS) {
+	// nix
+}
+
+AS5047U::~AS5047U() {
+}
+
+
+void AS5047U::init(SPIClass* _spi) {
+	spi = _spi;
+	if (nCS>=0)
+		pinMode(nCS, OUTPUT);
+	digitalWrite(nCS, HIGH);
+	//SPI has an internal SPI-device counter, it is possible to call "begin()" from different devices
+	spi->begin();
+	readRawAngle(); // read an angle
+}
+
+float AS5047U::getCurrentAngle(){
+	readCorrectedAngle();
+	return ((float)readCorrectedAngle())/(float)AS5047U_CPR * 2.0f * (float)PI;
+}
+
+float AS5047U::getFastAngle(){
+	return ((float)readCorrectedAngle())/(float)AS5047U_CPR * 2.0f * (float)PI;
+}
+
+
+uint16_t AS5047U::readRawAngle(){
+	uint16_t command = AS5047U_ANGLEUNC_REG | AS5047U_RW;
+	uint16_t lastresult = spi_transfer16(command)&AS5047U_RESULT_MASK;
+	return lastresult;
+}
+
+
+uint16_t AS5047U::readCorrectedAngle(){
+	uint16_t command = AS5047U_ANGLECOM_REG | AS5047U_RW;
+	uint16_t lastresult = spi_transfer16(command)&AS5047U_RESULT_MASK;
+	return lastresult;
+}
+
+
+uint16_t AS5047U::readMagnitude(){
+	uint16_t command = AS5047U_MAGNITUDE_REG | AS5047U_RW;
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	uint16_t result = nop16();
+	return result;
+}
+
+
+uint16_t AS5047U::readVelocity(){
+	uint16_t command = AS5047U_VELOCITY_REG | AS5047U_RW;
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	uint16_t result = nop16();
+	return result;
+}
+
+
+bool AS5047U::isErrorFlag(){
+	return errorflag;
+}
+
+bool AS5047U::isWarningFlag(){
+	return warningflag;
+}
+
+
+AS5047UError AS5047U::clearErrorFlag(){
+	uint16_t command = AS5047U_ERROR_REG | AS5047U_RW; // set r=1, result is 0x4001
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	AS5047UError result;
+	result.reg = nop16();
+	return result;
+}
+
+
+AS5047USettings1 AS5047U::readSettings1(){
+	uint16_t command = AS5047U_SETTINGS1_REG | AS5047U_RW; // set r=1, result is 0xC018
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	AS5047USettings1 result = {
+			.reg = nop16()
+	};
+	return result;
+}
+
+
+
+
+void AS5047U::writeSettings1(AS5047USettings1 settings){
+	writeRegister24(AS5047U_SETTINGS1_REG, settings.reg);
+}
+
+
+
+
+AS5047USettings2 AS5047U::readSettings2(){
+	uint16_t command = AS5047U_SETTINGS2_REG | AS5047U_RW; // set r=1, result is 0x4019
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	AS5047USettings2 result = {
+			.reg = nop16()
+	};
+	return result;
+}
+
+
+
+
+
+void AS5047U::writeSettings2(AS5047USettings2 settings){
+	writeRegister24(AS5047U_SETTINGS2_REG, settings.reg);
+}
+
+
+
+
+
+AS5047USettings3 AS5047U::readSettings3(){
+	uint16_t command = AS5047U_SETTINGS3_REG | AS5047U_RW;
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	AS5047USettings3 result = {
+			.reg = nop16()
+	};
+	return result;
+}
+
+
+
+
+
+void AS5047U::writeSettings3(AS5047USettings3 settings){
+	writeRegister24(AS5047U_SETTINGS3_REG, settings.reg);
+}
+
+
+
+
+AS5047UDiagnostics AS5047U::readDiagnostics(){
+	uint16_t command = AS5047U_DIAGNOSTICS_REG | AS5047U_RW;
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	AS5047UDiagnostics result = {
+			.reg = nop16()
+	};
+	return result;
+}
+
+
+
+
+uint8_t AS5047U::readAGC(){
+	uint16_t command = AS5047U_AGC_REG | AS5047U_RW;
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	uint16_t result = nop16();
+	return result & 0x00FF;
+};
+
+
+
+uint8_t AS5047U::readECCCHK(){
+	uint16_t command = AS5047U_ECCCHK_REG | AS5047U_RW;
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	uint16_t result = nop16();
+	return result & 0x007F;
+};
+
+
+
+
+AS5047UDisableSettings AS5047U::readDisableSettings(){
+	uint16_t command = AS5047U_DISABLE_REG | AS5047U_RW;
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	AS5047UDisableSettings result = {
+			.reg = nop16()
+	};
+	return result;
+};
+
+
+
+void AS5047U::writeDisableSettings(AS5047UDisableSettings settings){
+	writeRegister24(AS5047U_DISABLE_REG, settings.reg);
+};
+
+
+
+AS5047UECCSettings AS5047U::readECCSettings(){
+	uint16_t command = AS5047U_ECC_REG | AS5047U_RW;
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	AS5047UECCSettings result = {
+			.reg = nop16()
+	};
+	return result;
+};
+
+
+
+void AS5047U::writeECCSettings(AS5047UECCSettings settings){
+	writeRegister24(AS5047U_ECC_REG, settings.reg);
+};
+
+
+
+
+
+
+
+void AS5047U::enablePWM(bool enable, bool pwmOnWPin){
+	// AS5047UDisableSettings settings = readDisableSettings();
+	// if (settings.uvw_off==1) {
+	// 	settings.uvw_off = 0;
+	// 	writeDiableSettings(settings);
+	// }
+	AS5047USettings2 settings2 = readSettings2();
+	settings2.uvw_abi = pwmOnWPin?0:1;
+	settings2.pwm_on = enable;
+	writeSettings2(settings2);
+}
+
+
+
+
+void AS5047U::enableABI(bool enable){
+	AS5047UDisableSettings settings = readDisableSettings();
+	settings.abi_off = enable?0:1;
+	writeDisableSettings(settings);
+	delayMicroseconds(50);
+	if (enable) {
+		AS5047USettings2 settings2 = readSettings2();
+		settings2.uvw_abi = 0;
+		writeSettings2(settings2);
+	}
+}
+
+
+
+void AS5047U::enableUVW(bool enable){
+	AS5047UDisableSettings settings = readDisableSettings();
+	settings.uvw_off = enable?0:1;
+	writeDisableSettings(settings);
+	if (enable) {
+		AS5047USettings2 settings2 = readSettings2();
+		settings2.uvw_abi = 1;
+		writeSettings2(settings2);
+	}
+}
+
+
+
+
+uint16_t AS5047U::getZero(){
+	uint16_t command = AS5047U_ZPOSM_REG | AS5047U_RW;
+	spi_transfer16(command);
+	command = AS5047U_ZPOSL_REG | AS5047U_RW;
+	uint16_t result = spi_transfer16(command);
+	AS5047UZPosL posL = {
+			.reg = nop16()
+	};
+	return ((result&0x00FF)<<6) | posL.zposl;
+}
+
+
+
+uint16_t AS5047U::setZero(uint16_t value){
+	uint16_t command = AS5047U_ZPOSL_REG | AS5047U_RW;
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	AS5047UZPosL posL = {
+			.reg = nop16()
+	};
+	posL.zposl = value&0x003F;
+	writeRegister24(AS5047U_ZPOSL_REG, posL.reg);
+	writeRegister24(AS5047U_ZPOSM_REG, (value>>6)&0x00FF);
+	return getZero();
+}
+
+
+
+
+uint16_t AS5047U::nop16(){
+	uint16_t result = spi_transfer16(0xFFFF); // using 0xFFFF as nop instead of 0x0000, then next call to fastAngle will return an angle
+	return result&AS5047U_RESULT_MASK;
+}
+
+
+
+uint16_t AS5047U::spi_transfer16(uint16_t outdata) {
+	if (nCS>=0)
+		digitalWrite(nCS, 0);
+	spi->beginTransaction(settings);
+	uint16_t result = spi->transfer16(outdata);
+	spi->endTransaction();
+	if (nCS>=0)
+		digitalWrite(nCS, 1);
+	errorflag = ((result&AS5047U_ERROR)>0);
+	warningflag = ((result&AS5047U_WARNING)>0);
+	return result;
+}
+
+uint8_t AS5047U::calcCRC(uint16_t data){
+	uint8_t crc = 0xC4;  // Initial value
+    for (int i = 0; i < 16; i++) {
+        if ((crc ^ data) & 0x8000) {
+            crc = (crc << 1) ^ 0x1D;
+        } else {
+            crc <<= 1;
+        }
+        data <<= 1;
+    }
+    return crc ^ 0xFF;
+}
+
+uint16_t AS5047U::writeRegister24(uint16_t reg, uint16_t data) {
+	uint8_t buff[3];
+	buff[0] = (reg>>8)&0x3F;
+	buff[1] = reg&0xFF;
+	buff[2] = calcCRC(reg);
+	if (nCS>=0)
+		digitalWrite(nCS, 0);
+	spi->beginTransaction(settings);
+	spi->transfer(buff, 3);
+	spi->endTransaction();
+	if (nCS>=0)
+		digitalWrite(nCS, 1);
+	errorflag = ((buff[0]&0x40)>0);
+	warningflag = ((buff[0]&0x80)>0);
+
+	buff[0] = (data>>8)&0x3F;
+	buff[1] = data&0xFF;
+	buff[2] = calcCRC(data);
+	if (nCS>=0)
+		digitalWrite(nCS, 0);
+	spi->beginTransaction(settings);
+	spi->transfer(buff, 3);
+	spi->endTransaction();
+	if (nCS>=0)
+		digitalWrite(nCS, 1);
+	errorflag = ((buff[0]&0x40)>0);
+	warningflag = ((buff[0]&0x80)>0);
+
+	delayMicroseconds(50);
+
+	return buff[0]<<8 | buff[1];
+}

firmware/lib/Arduino-FOC-drivers/src/encoders/as5047u/AS5047U.h

@@ -0,0 +1,225 @@
+/*
+ * AS5047.h
+ *
+ *  Created on: 24 Feb 2022
+ *      Author: runger
+ */
+
+#ifndef AS5047U_H_
+#define AS5047U_H_
+
+#include "Arduino.h"
+#include "SPI.h"
+
+
+union AS5047UDisableSettings {
+	struct {
+		uint16_t :9;
+		uint16_t filter_disable:1;
+		uint16_t :4;
+		uint16_t abi_off:1;
+		uint16_t uvw_off:1;
+	};
+	uint16_t reg;
+};
+
+
+
+union AS5047UDiagnostics {
+	struct {
+		uint16_t spi_cnt:2;
+		uint16_t :1;
+		uint16_t agc_finished:1;
+		uint16_t off_finished:1;
+		uint16_t sin_finished:1;
+		uint16_t cos_finished:1;
+		uint16_t maghalf_flag:1;
+		uint16_t comp_h:1;
+		uint16_t comp_l:1;
+		uint16_t cordic_ovf:1;
+		uint16_t loops_finished:1;
+		uint16_t vdd_mode:1;
+	};
+	uint16_t reg;
+};
+
+
+union AS5047UZPosM {
+	struct {
+		uint16_t zposm:8;
+	};
+	uint16_t reg;
+};
+
+
+union AS5047UZPosL {
+	struct {
+		uint16_t zposl:6;
+		uint16_t dia1_en:1; // automotive version only
+		uint16_t dia2_en:1; // automotive version only
+	};
+	uint16_t reg;
+};
+
+
+union AS5047USettings1 {
+	struct {
+		uint16_t k_max:3;
+		uint16_t k_min:3;
+		uint16_t dia3_en:1; // not applicable
+		uint16_t dia4_en:1; // not applicable
+	};
+	uint16_t reg;
+};
+
+
+union AS5047USettings2 {
+	struct {
+		uint16_t iwidth:1;
+		uint16_t noiseset:1;
+		uint16_t dir:1;
+		uint16_t uvw_abi:1;
+		uint16_t daecdis:1;
+		uint16_t abi_dec:1;
+		uint16_t data_select:1;
+		uint16_t pwm_on:1;		
+	};
+	uint16_t reg;
+};
+
+
+
+
+union AS5047USettings3 {
+	struct {
+		uint16_t uvwpp:3;
+		uint16_t hys:2;
+		uint16_t abires:3;
+	};
+	uint16_t reg;
+};
+
+
+
+
+union AS5047UECCSettings {
+	struct {
+		uint16_t ecc_chsum:7;
+		uint16_t ecc_en:1;
+	};
+	uint16_t reg;
+};
+
+
+
+
+struct AS5047UError {
+	struct {
+		uint16_t cordic_ovf:1;
+		uint16_t off_notfinished:1;
+		uint16_t :1;
+		uint16_t wdtst:1;
+		uint16_t crc_error:1;
+		uint16_t cmd_error:1;
+		uint16_t frame_error:1;
+		uint16_t p2ram_error:1;
+		uint16_t p2ram_warning:1;
+		uint16_t maghalf:1;
+		uint16_t agc_warning:1;		
+	};
+	uint16_t reg;
+};
+
+
+
+
+
+#define AS5047U_CPR 16384
+#define AS5047U_ANGLECOM_REG 0x3FFF
+#define AS5047U_ANGLEUNC_REG 0x3FFE
+#define AS5047U_MAGNITUDE_REG 0x3FFD
+#define AS5047U_VELOCITY_REG 0x3FFC
+#define AS5047U_SIN_REG 0x3FFA
+#define AS5047U_COS_REG 0x3FFB
+#define AS5047U_AGC_REG 0x3FF9
+#define AS5047U_DIAGNOSTICS_REG 0x3FF5
+#define AS5047U_ERROR_REG 0x0001
+#define AS5047U_PROG_REG 0x0003
+#define AS5047U_ECCCHK_REG 0x00D1
+
+#define AS5047U_DISABLE_REG 0x0015
+#define AS5047U_ZPOSM_REG 0x0016
+#define AS5047U_ZPOSL_REG 0x0017
+#define AS5047U_SETTINGS1_REG 0x0018
+#define AS5047U_SETTINGS2_REG 0x0019
+#define AS5047U_SETTINGS3_REG 0x001A
+#define AS5047U_ECC_REG 0x001B
+
+#define AS5047U_WARNING 0x8000
+#define AS5047U_ERROR 0x4000
+#define AS5047U_RW 0x4000
+#define AS5047U_ERRFLG 0xC000
+#define AS5047U_RESULT_MASK 0x3FFF
+
+
+#define AS5047U_BITORDER MSBFIRST
+
+static SPISettings AS5047USPISettings(8000000, AS5047U_BITORDER, SPI_MODE1); // @suppress("Invalid arguments")
+
+
+class AS5047U {
+public:
+	AS5047U(SPISettings settings = AS5047USPISettings, int nCS = -1);
+	virtual ~AS5047U();
+
+	virtual void init(SPIClass* _spi = &SPI);
+
+	float getCurrentAngle(); // angle in radians, return current value
+	float getFastAngle();	 // angle in radians, return last value and read new
+
+	uint16_t readRawAngle(); // 14bit angle value
+	uint16_t readCorrectedAngle(); // 14bit corrected angle value
+	uint16_t readMagnitude(); // 14bit magnitude value
+	uint16_t readVelocity(); // 14bit magnitude value
+
+	bool isErrorFlag();
+	bool isWarningFlag();
+	AS5047UError clearErrorFlag();
+
+	AS5047UDiagnostics readDiagnostics();
+	uint8_t readAGC();
+	uint8_t readECCCHK();
+
+	AS5047UDisableSettings readDisableSettings();
+	void writeDisableSettings(AS5047UDisableSettings settings);
+	AS5047USettings1 readSettings1();
+	void writeSettings1(AS5047USettings1 settings);
+	AS5047USettings2 readSettings2();
+	void writeSettings2(AS5047USettings2 settings);
+	AS5047USettings3 readSettings3();
+	void writeSettings3(AS5047USettings3 settings);
+	AS5047UECCSettings readECCSettings();
+	void writeECCSettings(AS5047UECCSettings settings);
+
+	void enablePWM(bool enable, bool pwmOnWPin = true);
+	void enableABI(bool enable);
+	void enableUVW(bool enable);
+
+	uint16_t setZero(uint16_t);
+	uint16_t getZero();
+
+private:
+
+	uint16_t nop16();
+	uint16_t spi_transfer16(uint16_t outdata);
+	uint8_t calcCRC(uint16_t data);
+	uint16_t writeRegister24(uint16_t reg, uint16_t data);
+	SPIClass* spi;
+	SPISettings settings;
+	bool errorflag = false;
+	bool warningflag = false;
+	int nCS = -1;
+
+};
+
+#endif /* AS5047U_H_ */

firmware/lib/Arduino-FOC-drivers/src/encoders/as5047u/MagneticSensorAS5047U.cpp

@@ -0,0 +1,29 @@
+
+#include "./MagneticSensorAS5047U.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+
+MagneticSensorAS5047U::MagneticSensorAS5047U(int nCS, bool fastMode, SPISettings settings) : AS5047U(settings, nCS), fastMode(fastMode) {
+
+}
+
+
+MagneticSensorAS5047U::~MagneticSensorAS5047U(){ 
+
+}
+
+
+void MagneticSensorAS5047U::init(SPIClass* _spi) {
+    this->AS5047U::init(_spi);
+    this->Sensor::init();
+}
+
+
+float MagneticSensorAS5047U::getSensorAngle() {
+    float angle_data = readRawAngle();
+    if (!fastMode) // read again to ensure current value
+        angle_data = readRawAngle();
+    angle_data = ( angle_data / (float)AS5047U_CPR) * _2PI;
+    // return the shaft angle
+    return angle_data;
+}

firmware/lib/Arduino-FOC-drivers/src/encoders/as5047u/MagneticSensorAS5047U.h

@@ -0,0 +1,20 @@
+
+#ifndef __MAGNETICSENSORAS5047U_H__
+#define __MAGNETICSENSORAS5047U_H__
+
+#include "common/base_classes/Sensor.h"
+#include "./AS5047U.h"
+
+class MagneticSensorAS5047U : public Sensor, public AS5047U {
+public:
+	MagneticSensorAS5047U(int nCS = -1, bool fastMode = false, SPISettings settings = AS5047USPISettings);
+	virtual ~MagneticSensorAS5047U();
+
+    virtual float getSensorAngle() override;
+
+	virtual void init(SPIClass* _spi = &SPI);
+private:
+    bool fastMode = false;
+};
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/as5047u/README.md

@@ -0,0 +1,81 @@
+# AS5047U SimpleFOC driver
+
+While AS5047U absolute position magnetic rotary encoder is supported by the standard MagneticSensorSPI driver included in the base distribution, this AS5047U-specific driver includes some optimisations:
+
+- access to the other registers of the AS5047U, including the magnitude value which can be used to check the magnet strength, the velocity register and the diagnostics register
+- access to the error state of the sensor, and ability to clear errors
+- it has a fastMode setting, in which the sensor is sent only 1 command per getAngle() call - the value returned will be from previous getAngle() invocation
+
+
+## Hardware setup
+
+Connect as per normal for your SPI bus. No special hardware setup is needed to use this driver.
+
+## Software setup
+
+Its actually easier to use than the standard SPI sensor class, because it is less generic:
+
+```c++
+#include "Arduino.h"
+#include "Wire.h"
+#include "SPI.h"
+#include "SimpleFOC.h"
+#include "SimpleFOCDrivers.h"
+#include "encoders/as5047u/MagneticSensorAS5047U.h"
+
+#define SENSOR1_CS 5 // some digital pin that you're using as the nCS pin
+MagneticSensorAS5047U sensor1(SENSOR1_CS);
+
+
+void setup() {
+    sensor1.init();
+}
+```
+
+Set some options:
+
+```c++
+MagneticSensorAS5047U sensor1(SENSOR1_CS, true, mySPISettings);
+```
+
+Use another SPI bus:
+
+```c++
+void setup() {
+    sensor1.init(SPI2);
+}
+```
+
+Here's how you can use it:
+
+```c++
+    // update the sensor (only needed if using the sensor without a motor)
+    sensor1.update();
+
+    // get the angle, in radians, including full rotations
+    float a1 = sensor1.getAngle();
+
+    // get the velocity, in rad/s - note: you have to call getAngle() on a regular basis for it to work
+    float v1 = sensor1.getVelocity();
+
+    // get the angle, in radians, no full rotations
+    float a2 = sensor1.getCurrentAngle();
+
+    // get the raw 14 bit value
+    uint16_t raw = sensor1.readRawAngle();
+
+    // read the CORDIC magnitude value, a measure of the magnet field strength
+    float m1 = sensor1.readMagnitude();
+
+    // check for errors
+    if (sensor1.isErrorFlag()) {
+        AS5047UError error = sensor1.clearErrorFlag();
+        if (error.parityError) { // also error.framingError, error.commandInvalid
+            // etc...            
+        }
+    }
+
+    // get diagnostics
+    AS5047Diagnostics diagnostics = sensor1.readDiagnostics();
+```
+

firmware/lib/Arduino-FOC-drivers/src/encoders/as5048a/AS5048A.cpp

@@ -0,0 +1,118 @@
+/*
+ * AS5048A.cpp
+ *
+ *  Created on: 8 Mar 2021
+ *      Author: runger
+ */
+
+#include "AS5048A.h"
+
+AS5048A::AS5048A(SPISettings settings, int nCS) : settings(settings), nCS(nCS) {
+	// nix
+}
+
+AS5048A::~AS5048A() {
+}
+
+
+void AS5048A::init(SPIClass* _spi) {
+	spi = _spi;
+	if (nCS>=0)
+		pinMode(nCS, OUTPUT);
+	digitalWrite(nCS, HIGH);
+	//SPI has an internal SPI-device counter, it is possible to call "begin()" from different devices
+	spi->begin();
+	readRawAngle(); // read an angle
+}
+
+float AS5048A::getCurrentAngle(){
+	readRawAngle();
+	return ((float)readRawAngle())/(float)AS5048A_CPR * 2.0f * (float)PI;
+}
+
+float AS5048A::getFastAngle(){
+	return ((float)readRawAngle())/(float)AS5048A_CPR * 2.0f * (float)PI;
+}
+
+
+uint16_t AS5048A::readRawAngle(){
+	uint16_t command = AS5048A_ANGLE_REG | AS5048A_PARITY | AS5048A_RW; // set r=1 and parity=1, result ix 0xFFFF
+	uint16_t lastresult = spi_transfer16(command)&AS5048A_RESULT_MASK;
+	return lastresult;
+}
+
+
+uint16_t AS5048A::readMagnitude(){
+	uint16_t command = AS5048A_MAGNITUDE_REG | AS5048A_RW; // set r=1, result ix 0x7FFE
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	uint16_t result = nop();
+	return result;
+}
+
+
+bool AS5048A::isErrorFlag(){
+	return errorflag;
+}
+
+
+AS5048Error AS5048A::clearErrorFlag(){
+	uint16_t command = AS5048A_ERROR_REG | AS5048A_RW; // set r=1, result ix 0x4001
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	uint16_t result = nop();
+	AS5048Error err = {
+			.parityError = ((result&0x0004)!=0x0000),
+			.commandInvalid = ((result&0x0002)!=0x0000),
+			.framingError = ((result&0x0001)!=0x0000)
+	};
+	return err;
+}
+
+
+AS5048Diagnostics AS5048A::readDiagnostics(){
+	uint16_t command = AS5048A_DIAGNOSTICS_REG | AS5048A_RW; // set r=1, result ix 0x7FFD
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	AS5048Diagnostics result = {
+			.reg = nop()
+	};
+	return result;
+}
+
+
+uint16_t AS5048A::setZero(uint16_t){
+	// TODO implement me!
+	return 0;
+}
+
+
+uint16_t AS5048A::enableOneTimeProgramming(){
+	// no plans to implement this at the moment. one-time-programming a $10 chip isn't really a "maker" thing to do. The zero
+	// position can be easily retained in software, stored on the MCU, and thereby the sensor can be reused in another project.
+	return 0;
+}
+
+
+uint16_t AS5048A::programZero(){
+	// no plans to implement this at the moment. one-time-programming a $10 chip isn't really a "maker" thing to do. The zero
+	// position can be easily retained in software, stored on the MCU, and thereby the sensor can be reused in another project.
+	return 0;
+}
+
+uint16_t AS5048A::nop(){
+	uint16_t result = spi_transfer16(0xFFFF); // using 0xFFFF as nop instead of 0x0000, then next call to fastAngle will return an angle
+	return result&AS5048A_RESULT_MASK;
+}
+
+uint16_t AS5048A::spi_transfer16(uint16_t outdata) {
+	if (nCS>=0)
+		digitalWrite(nCS, 0);
+	spi->beginTransaction(settings);
+	uint16_t result = spi->transfer16(outdata);
+	spi->endTransaction();
+	if (nCS>=0)
+		digitalWrite(nCS, 1);
+	// TODO check parity
+	errorflag = ((result&AS5048A_ERRFLG)>0);
+	return result;
+}
+
+

firmware/lib/Arduino-FOC-drivers/src/encoders/as5048a/AS5048A.h

@@ -0,0 +1,89 @@
+/*
+ * AS5048A.h
+ *
+ *  Created on: 8 Mar 2021
+ *      Author: runger
+ */
+
+#ifndef AS5048A_H_
+#define AS5048A_H_
+
+#include "Arduino.h"
+#include "SPI.h"
+
+
+
+
+union AS5048Diagnostics {
+	struct {
+		uint16_t agc:8;
+		uint16_t ocf:1;
+		uint16_t cof:1;
+		uint16_t compLow:1;
+		uint16_t compHigh:1;
+	};
+	uint16_t reg;
+};
+
+
+struct AS5048Error {
+	bool parityError;
+	bool commandInvalid;
+	bool framingError;
+};
+
+
+#define AS5048A_CPR 16384
+#define AS5048A_ANGLE_REG 0x3FFF
+#define AS5048A_ERROR_REG 0x0001
+#define AS5048A_PROGCTL_REG 0x0003
+#define AS5048A_OTPHIGH_REG 0x0016
+#define AS5048A_OTPLOW_REG 0x0017
+#define AS5048A_DIAGNOSTICS_REG 0x3FFD
+#define AS5048A_MAGNITUDE_REG 0x3FFE
+#define AS5048A_PARITY 0x8000
+#define AS5048A_RW 0x4000
+#define AS5048A_ERRFLG 0x4000
+#define AS5048A_RESULT_MASK 0x3FFF
+
+
+#define AS5048_BITORDER MSBFIRST
+
+
+static SPISettings AS5048SPISettings(8000000, AS5048_BITORDER, SPI_MODE1); // @suppress("Invalid arguments")
+
+
+class AS5048A {
+public:
+	AS5048A(SPISettings settings = AS5048SPISettings, int nCS = -1);
+	virtual ~AS5048A();
+
+	virtual void init(SPIClass* _spi = &SPI);
+
+	float getCurrentAngle(); // angle in radians, return current value
+	float getFastAngle();	 // angle in radians, return last value and read new
+
+	uint16_t readRawAngle(); // 14bit angle value
+	uint16_t readMagnitude(); // 14bit magnitude value
+
+	bool isErrorFlag();
+	AS5048Error clearErrorFlag();
+
+	AS5048Diagnostics readDiagnostics();
+
+	uint16_t setZero(uint16_t);
+	uint16_t enableOneTimeProgramming();
+	uint16_t programZero();
+
+private:
+
+	uint16_t nop();
+	uint16_t spi_transfer16(uint16_t outdata);
+	SPIClass* spi;
+	SPISettings settings;
+	bool errorflag = false;
+	int nCS = -1;
+
+};
+
+#endif /* AS5048A_H_ */

firmware/lib/Arduino-FOC-drivers/src/encoders/as5048a/MagneticSensorAS5048A.cpp

@@ -0,0 +1,26 @@
+
+#include "./MagneticSensorAS5048A.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+
+MagneticSensorAS5048A::MagneticSensorAS5048A(int nCS, bool fastMode, SPISettings settings) : AS5048A(settings, nCS), fastMode(fastMode) {
+
+}
+
+MagneticSensorAS5048A::~MagneticSensorAS5048A(){ 
+
+}
+void MagneticSensorAS5048A::init(SPIClass* _spi) {
+    this->AS5048A::init(_spi);
+    this->Sensor::init();
+}
+
+float MagneticSensorAS5048A::getSensorAngle() {
+    float angle_data = readRawAngle();
+    if (!fastMode) // read again to ensure current value
+        angle_data = readRawAngle();
+
+    angle_data = ( angle_data / (float)AS5048A_CPR ) * _2PI;
+    // return the shaft angle
+    return angle_data;
+}

firmware/lib/Arduino-FOC-drivers/src/encoders/as5048a/MagneticSensorAS5048A.h

@@ -0,0 +1,20 @@
+
+#ifndef __MAGNETICSENSORAS5048A_H__
+#define __MAGNETICSENSORAS5048A_H__
+
+#include "common/base_classes/Sensor.h"
+#include "./AS5048A.h"
+
+class MagneticSensorAS5048A : public Sensor, public AS5048A {
+public:
+	MagneticSensorAS5048A(int nCS = -1, bool fastMode = false, SPISettings settings = AS5048SPISettings);
+	virtual ~MagneticSensorAS5048A();
+
+    virtual float getSensorAngle() override;
+
+	virtual void init(SPIClass* _spi = &SPI);
+protected:
+    bool fastMode = false;
+};
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/as5048a/PreciseMagneticSensorAS5048A.cpp

@@ -0,0 +1,50 @@
+/*
+ * PreciseMagneticSensorAS5048A.cpp
+ *
+ *  Created on: 1 May 2021
+ *      Author: runger
+ */
+
+#include <encoders/as5048a/PreciseMagneticSensorAS5048A.h>
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+
+PreciseMagneticSensorAS5048A::PreciseMagneticSensorAS5048A(int nCS, bool fastMode, SPISettings settings) : AS5048A(settings, nCS), fastMode(fastMode) { }
+
+PreciseMagneticSensorAS5048A::~PreciseMagneticSensorAS5048A() { }
+
+
+void PreciseMagneticSensorAS5048A::init(SPIClass* _spi) {
+    this->AS5048A::init(_spi);
+	// velocity calculation init
+    current_ts = _micros();
+    /*uint16_t angle_data =*/ readRawAngle();
+    current_angle = PreciseAngle(readRawAngle(), 0);
+    getAngle();
+}
+
+
+
+float PreciseMagneticSensorAS5048A::getSensorAngle() {
+    previous_ts = current_ts;
+    previous_angle = current_angle;
+    uint16_t angle_data = readRawAngle();
+    if (!fastMode) // read again to ensure current value
+    	angle_data = readRawAngle();
+    current_ts = _micros();
+    current_angle.update(angle_data);
+    return current_angle.asFloat();
+}
+
+
+
+/*
+unlike the normal MagneticSensorSPI implementation, this one uses the angle previously read by the last call to getAngle to do its
+calculation, and does not directly poll any data from the sensor.
+This is an optimisation for speed, based on the assumption that loopFOC() (which calls getAngle()) is invoked at least as often as
+move() (which calls getVelocity()). If this is the case, getVelocity() should always have a sufficiently "fresh" value to work with.
+If using this function in a different context, simply call getAngle() first to be sure of a fresh angle value.
+*/
+float PreciseMagneticSensorAS5048A::getVelocity() {
+	return current_angle.velocity(previous_angle, (current_ts-previous_ts));
+}

firmware/lib/Arduino-FOC-drivers/src/encoders/as5048a/PreciseMagneticSensorAS5048A.h

@@ -0,0 +1,33 @@
+/*
+ * PreciseMagneticSensorAS5048A.h
+ *
+ *  Created on: 1 May 2021
+ *      Author: runger
+ */
+
+#ifndef LIBRARIES_ARDUNIO_FOC_DRIVERS_SRC_ENCODERS_AS5048A_PRECISEMAGNETICSENSORAS5048A_H_
+#define LIBRARIES_ARDUNIO_FOC_DRIVERS_SRC_ENCODERS_AS5048A_PRECISEMAGNETICSENSORAS5048A_H_
+
+#include "common/base_classes/Sensor.h"
+#include "./AS5048A.h"
+#include "utilities/PreciseAngle.h"
+
+class PreciseMagneticSensorAS5048A : public Sensor, public AS5048A {
+public:
+	PreciseMagneticSensorAS5048A(int nCS = -1, bool fastMode = false, SPISettings settings = AS5048SPISettings);
+	virtual ~PreciseMagneticSensorAS5048A();
+
+    virtual float getSensorAngle() override;
+    virtual float getVelocity() override;
+
+	virtual void init(SPIClass* _spi = &SPI) override;
+
+protected:
+    bool fastMode = false;
+    PreciseAngle previous_angle = PreciseAngle();
+    PreciseAngle current_angle = PreciseAngle();
+    unsigned long previous_ts = 0;
+    unsigned long current_ts = 1;
+};
+
+#endif /* LIBRARIES_ARDUNIO_FOC_DRIVERS_SRC_ENCODERS_AS5048A_PRECISEMAGNETICSENSORAS5048A_H_ */

firmware/lib/Arduino-FOC-drivers/src/encoders/as5048a/README.md

@@ -0,0 +1,86 @@
+# AS5048A SimpleFOC driver
+
+While AS5048A absolute position magnetic rotary encoder is supported by the standard MagneticSensorSPI driver included in the base distribution, this AS5048A-specific driver includes some optimisations:
+
+- access to the other registers of the AS5048A, including the magnitude value which can be used to check the magnet strength, and the diagnostics register
+- access to the error state of the sensor, and ability to clear errors
+- it has a fastMode setting, in which the sensor is sent only 1 command per getAngle() call - the value returned will be from previous getAngle() invocation
+
+
+## Hardware setup
+
+Connect as per normal for your SPI bus. No special hardware setup is needed to use this driver.
+
+## Software setup
+
+Its actually easier to use than the standard SPI sensor class, because it is less generic:
+
+```c++
+#include "Arduino.h"
+#include "Wire.h"
+#include "SPI.h"
+#include "SimpleFOC.h"
+#include "SimpleFOCDrivers.h"
+#include "encoders/as5048a/MagneticSensorAS5048A.h"
+
+#define SENSOR1_CS 5 // some digital pin that you're using as the nCS pin
+MagneticSensorAS5048A sensor1(SENSOR1_CS);
+
+
+void setup() {
+    sensor1.init();
+}
+```
+
+Set some options:
+
+```c++
+MagneticSensorAS5048A sensor1(SENSOR1_CS, true, mySPISettings);
+```
+
+Use another SPI bus:
+
+```c++
+void setup() {
+    sensor1.init(SPI2);
+}
+```
+
+Here's how you can use it:
+
+```c++
+    // update the sensor (only needed if using the sensor without a motor)
+    sensor1.update();
+
+    // get the angle, in radians, including full rotations
+    float a1 = sensor1.getAngle();
+
+    // get the velocity, in rad/s - note: you have to call getAngle() on a regular basis for it to work
+    float v1 = sensor1.getVelocity();
+
+    // get the angle, in radians, no full rotations
+    float a2 = sensor1.getCurrentAngle();
+
+    // get the raw 14 bit value
+    uint16_t raw = sensor1.readRawAngle();
+
+    // read the CORDIC magnitude value, a measure of the magnet field strength
+    float m1 = sensor1.readMagnitude();
+
+    // check for errors
+    if (sensor1.isErrorFlag()) {
+        AS5048Error error = sensor1.clearErrorFlag();
+        if (error.parityError) { // also error.framingError, error.commandInvalid
+            // etc...            
+        }
+    }
+
+    // get diagnostics
+    AS5048Diagnostics diagnostics = sensor1.readDiagnostics();
+```
+
+
+## PreciseMagneticSensorAS5048A
+
+This is a variant of the sensor that uses [PreciseAngle](../utilities) to represent the angle, allowing a (much) greater range of rotation.
+

firmware/lib/Arduino-FOC-drivers/src/encoders/as5145/MagneticSensorAS5145.cpp

@@ -0,0 +1,34 @@
+#include "./MagneticSensorAS5145.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+
+MagneticSensorAS5145::MagneticSensorAS5145(SPISettings settings) : settings(settings) {
+
+}
+
+
+MagneticSensorAS5145::~MagneticSensorAS5145() {
+
+}
+
+void MagneticSensorAS5145::init(SPIClass* _spi) {
+    this->spi=_spi;
+    this->Sensor::init();
+}
+
+// check 40us delay between each read?
+float MagneticSensorAS5145::getSensorAngle() {
+    float angle_data = readRawAngleSSI();
+    angle_data = ( (float)angle_data / AS5145_CPR ) * _2PI;
+    // return the shaft angle
+    return angle_data;
+}
+
+
+uint16_t MagneticSensorAS5145::readRawAngleSSI() {
+    spi->beginTransaction(settings);
+    uint16_t value = spi->transfer16(0x0000);
+    //uint16_t parity = spi->transfer(0x00);
+    spi->endTransaction();
+    return (value>>3)&0x1FFF; // TODO this isn't what I expected from the datasheet... maybe there's a leading 0 bit?
+}; // 12bit angle value

firmware/lib/Arduino-FOC-drivers/src/encoders/as5145/MagneticSensorAS5145.h

@@ -0,0 +1,39 @@
+
+#ifndef __MAGNETIC_SENSOR_AS5145_H__
+#define __MAGNETIC_SENSOR_AS5145_H__
+
+#include "Arduino.h"
+#include "SPI.h"
+#include "common/base_classes/Sensor.h"
+
+#ifndef MSBFIRST
+#define MSBFIRST BitOrder::MSBFIRST
+#endif
+
+#define AS5145_BITORDER MSBFIRST
+#define AS5145_CPR 4096.0f
+#define _2PI 6.28318530718f
+
+
+static SPISettings AS5145SSISettings(1000000, AS5145_BITORDER, SPI_MODE2); // @suppress("Invalid arguments")
+
+
+class MagneticSensorAS5145 : public Sensor {
+public:
+	MagneticSensorAS5145(SPISettings settings = AS5145SSISettings);
+	virtual ~MagneticSensorAS5145();
+
+    virtual float getSensorAngle() override;
+
+	virtual void init(SPIClass* _spi = &SPI);
+
+    uint16_t readRawAngleSSI();
+	
+private:
+	SPIClass* spi;
+	SPISettings settings;
+};
+
+
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/as5145/README.md

@@ -0,0 +1,50 @@
+# AS5145 SimpleFOC driver
+
+SSI protocol driver for the AMS AS5145 magnetic encoder. Any of the A, B or H variants should work. AS5045 encoders should also be supported.
+
+Only angle reading is supported, might get to the status bits at a later time.
+The SSI protocol is "emulated" using the SPI peripheral.
+
+Tested with AS5145A on STM32G491 so far.
+
+## Hardware setup
+
+Wire the sensor's data (DO) line to the MISO (CIPO) pin, nCS, SCK as normal. Leave the MOSI pin unconnected.
+
+## Software setup
+
+```
+#include <Arduino.h>
+#include <SimpleFOC.h>
+#include <SimpleFOCDrivers.h>
+#include "encoders/as5145/MagneticSensorAS5145.h"
+
+MagneticSensorAS5145 sensor;
+SPIClass spi_ssi(PB15, PB14, PB13, PB12);
+
+long ts;
+
+void setup() {
+  Serial.begin(115200);
+  while (!Serial) ;
+  delay(2000);
+
+  Serial.println("Initializing sensor...");
+
+  spi_ssi.begin();
+  sensor.init(&spi_ssi);
+
+  Serial.println("Sensor initialized.");
+
+  ts = millis();
+}
+
+void loop() {
+  sensor.update();
+  if (millis() - ts > 1000) {
+    Serial.println(sensor.getAngle(), 3);
+    ts = millis();
+  }
+  delay(1);
+}
+```

firmware/lib/Arduino-FOC-drivers/src/encoders/as5600/AS5600.cpp

@@ -0,0 +1,175 @@
+
+
+#include "./AS5600.h"
+
+
+AS5600::AS5600(uint8_t address) : _address(address) {};
+AS5600::~AS5600() {};
+
+void AS5600::init(TwoWire* wire){
+    _wire = wire;
+    _wire->begin();
+    if (!closeTransactions) {
+        setAngleRegister();
+    }
+};
+
+
+void AS5600::setAngleRegister() {
+    _wire->beginTransmission(_address);
+    if (useHysteresis)
+        _wire->write(AS5600_REG_ANGLE);
+    else
+        _wire->write(AS5600_REG_ANGLE_RAW);
+    _wire->endTransmission(false);
+}
+
+
+uint16_t AS5600::angle() {
+    uint16_t result = 0;
+    if (!closeTransactions) {
+        setAngleRegister();
+    }
+    _wire->requestFrom(_address, (uint8_t)2, (uint8_t)closeTransactions);
+    result = _wire->read()<<8;
+    result |= _wire->read();
+    return result;
+};
+
+
+uint16_t AS5600::readRawAngle() {
+    return readRegister(AS5600_REG_ANGLE_RAW, 2);
+};
+
+
+uint16_t AS5600::readAngle() {
+    return readRegister(AS5600_REG_ANGLE, 2);
+};
+
+
+
+uint16_t AS5600::readMagnitude() {
+    return readRegister(AS5600_REG_MAGNITUDE, 2);
+};
+
+
+AS5600Status AS5600::readStatus() {
+    AS5600Status result;
+    result.reg = (uint8_t)readRegister(AS5600_REG_STATUS, 1); // TODO: shift bits around
+    return result;
+};
+
+
+uint8_t AS5600::readAGC() {
+    return (uint8_t)readRegister(AS5600_REG_AGC, 1);
+};
+
+
+
+AS5600Conf AS5600::readConf() {
+    AS5600Conf result;
+    result.reg = readRegister(AS5600_REG_CONF, 2);
+    return result;
+};
+
+
+uint16_t AS5600::readMang() {
+    return readRegister(AS5600_REG_MANG, 2);
+};
+
+
+uint16_t AS5600::readMPos() {
+    return readRegister(AS5600_REG_MPOS, 2);
+};
+
+
+uint16_t AS5600::readZPos() {
+    return readRegister(AS5600_REG_ZPOS, 2);
+};
+
+
+uint8_t AS5600::readZMCO() {
+    return (readRegister(AS5600_REG_ZMCO, 1)&0x03);
+};
+
+uint8_t AS5600::readI2CAddr() {
+    return (readRegister(AS5600_REG_I2CADDR, 1)>>1);
+};
+
+
+// set registers
+void AS5600::setConf(AS5600Conf value) {
+    // TODO: read before write
+    writeRegister(AS5600_REG_CONF, value.reg);
+};
+
+
+void AS5600::setMang(uint16_t value) {
+    // TODO: read before write
+    writeRegister(AS5600_REG_MANG, value);
+};
+
+
+void AS5600::setMPos(uint16_t value) {
+    // TODO: read before write
+    writeRegister(AS5600_REG_MPOS, value);
+};
+
+
+void AS5600::setZPos(uint16_t value) {
+    // TODO: read before write
+    writeRegister(AS5600_REG_ZPOS, value);
+};
+
+void AS5600::setI2CAddr(uint8_t value) {
+    uint8_t val = (uint8_t)readRegister(AS5600_REG_I2CADDR, 1);
+    val = (value<<1) | (val&0x01);
+    writeRegister(AS5600_REG_I2CADDR, val);
+};
+
+void AS5600::setI2CUpdt(uint8_t value) {
+    uint8_t val = (uint8_t)readRegister(AS5600_REG_I2CUPDT, 1);
+    val = (value<<1) | (val&0x01);
+    writeRegister(AS5600_REG_I2CUPDT, val);
+};
+
+
+void AS5600::burnSettings(){
+    writeRegister(AS5600_REG_BURN, 0x40, 1);
+}
+
+
+
+uint16_t AS5600::readRegister(uint8_t reg, uint8_t len){
+    uint16_t result = 0;
+    _wire->beginTransmission(_address);
+    _wire->write(reg);
+    _wire->endTransmission(false);
+    _wire->requestFrom(_address, len, (uint8_t)closeTransactions);
+    if (!closeTransactions) {
+        setAngleRegister();
+    }
+    result = _wire->read();
+    if (len == 2) {
+        result <<= 8;
+        result |= _wire->read();
+    }
+    return result;
+};
+
+
+
+void AS5600::writeRegister(uint8_t reg, uint16_t val, uint8_t len){
+    _wire->beginTransmission(_address);
+    _wire->write(reg);
+    if (len == 2) {
+        _wire->write(val>>8);
+    }
+    _wire->write(val&0xFF);
+    _wire->endTransmission(closeTransactions);
+    if (!closeTransactions) {
+        setAngleRegister();
+    }
+};
+
+

firmware/lib/Arduino-FOC-drivers/src/encoders/as5600/AS5600.h

@@ -0,0 +1,104 @@
+
+#pragma once
+
+
+#include <Arduino.h>
+#include <Wire.h>
+
+
+#define AS5600_REG_ZMCO 0x00
+#define AS5600_REG_ZPOS 0x01
+#define AS5600_REG_MPOS 0x03
+#define AS5600_REG_MANG 0x05
+#define AS5600_REG_CONF 0x07
+
+#define AS5600_REG_I2CADDR 0x20
+#define AS5600_REG_I2CUPDT 0x21
+
+#define AS5600_REG_ANGLE 0x0E
+#define AS5600_REG_ANGLE_RAW 0x0C
+
+#define AS5600_REG_STATUS 0x0B
+#define AS5600_REG_AGC 0x1A
+#define AS5600_REG_MAGNITUDE 0x1B
+
+#define AS5600_REG_BURN 0xFF
+
+#define AS5600_CPR (4096.0f)
+
+
+union AS5600Conf {
+	struct {
+		uint16_t pm:2;
+		uint16_t hyst:2;
+		uint16_t outs:2;
+		uint16_t pwmf:2;
+		uint16_t sf:2;
+		uint16_t fth:3;
+		uint16_t wd:1;
+		uint16_t unused:2;
+	};
+	uint16_t reg;
+};
+
+union AS5600Status {
+	struct {
+		uint8_t unused:3;
+		uint8_t mh:1;
+		uint8_t ml:1;
+		uint8_t md:1;
+		uint8_t unused2:2;
+	};
+	uint8_t reg;
+};
+
+
+
+
+class AS5600 {
+public:
+    AS5600(uint8_t address = 0x36);
+    ~AS5600();
+
+    virtual void init(TwoWire* wire = &Wire);
+
+    // read an angle, either with or without hysteresis, depending on the useHysteresis flag
+    // and using fast mode (not closing transactions) if so configured
+    uint16_t angle();
+
+    // read registers
+    uint16_t readRawAngle();
+    uint16_t readAngle();
+
+    uint16_t readMagnitude();
+    AS5600Status readStatus();
+    uint8_t readAGC();
+
+    AS5600Conf readConf();
+    uint16_t readMang();
+    uint16_t readMPos();
+    uint16_t readZPos();
+    uint8_t readZMCO();
+    uint8_t readI2CAddr();
+
+    // set registers
+    void setConf(AS5600Conf value);
+    void setMang(uint16_t value);
+    void setMPos(uint16_t value);
+    void setZPos(uint16_t value);
+    void setI2CAddr(uint8_t value);
+    void setI2CUpdt(uint8_t value);
+
+    void burnSettings();
+
+    bool closeTransactions = true;
+    bool useHysteresis = true;
+    uint8_t _address;
+protected:
+    TwoWire* _wire;
+
+    void setAngleRegister();
+    uint16_t readRegister(uint8_t reg, uint8_t len);
+    void writeRegister(uint8_t reg, uint16_t val, uint8_t len = 2);
+};
+

firmware/lib/Arduino-FOC-drivers/src/encoders/as5600/MagneticSensorAS5600.cpp

@@ -0,0 +1,18 @@
+
+
+#include "./MagneticSensorAS5600.h"
+#include "common/foc_utils.h"
+
+MagneticSensorAS5600::MagneticSensorAS5600(uint8_t _address) : AS5600(_address) {};
+MagneticSensorAS5600::~MagneticSensorAS5600() {};
+
+void MagneticSensorAS5600::init(TwoWire* wire) {
+  AS5600::init(wire);
+  Sensor::init();
+};
+
+
+float MagneticSensorAS5600::getSensorAngle() {
+    uint16_t raw = readRawAngle();
+    return raw / AS5600_CPR * _2PI;
+};

firmware/lib/Arduino-FOC-drivers/src/encoders/as5600/MagneticSensorAS5600.h

@@ -0,0 +1,22 @@
+
+#pragma once
+
+
+#include "./AS5600.h"
+#include "common/base_classes/Sensor.h"
+
+
+class MagneticSensorAS5600 : public Sensor, public AS5600 {
+
+public:
+    MagneticSensorAS5600(uint8_t _address = 0x36);
+    ~MagneticSensorAS5600();
+
+    virtual void init(TwoWire* wire = &Wire);
+
+    virtual float getSensorAngle() override;    
+
+protected:
+
+};
+

firmware/lib/Arduino-FOC-drivers/src/encoders/as5600/README.md

@@ -0,0 +1,131 @@
+# AS5600 SimpleFOC driver
+
+I2C protocol driver for the AMS AS5600 magnetic encoder (digital potentiometer). Also supports the newer AS5600L variant.
+
+:warning: work in progress
+
+## Hardware setup
+
+Connect the sensor to 3.3V or 5V power as appropriate, and to I2C (SDA and SCL).
+
+Important: please make sure the direction pin (DIR) is either pulled up to VDD, or down to GND. Do not leave the direction pin floating.
+
+## Software setup
+
+The sensor driver is easy to use.
+
+```c++
+#include <Arduino.h>
+#include <SimpleFOC.h>
+#include <SimpleFOCDrivers.h>
+#include "encoders/as5600/MagneticSensorAS5600.h"
+
+MagneticSensorAS5600 sensor;
+
+void setup() {
+    sensor.init();
+}
+
+long ts;
+
+void loop() {
+  sensor.update();
+  if (millis() - ts > 1000) {
+    Serial.println(sensor.getAngle(), 3);
+    ts = millis();
+  }
+  delay(1);
+}
+```
+
+You can use a different I2C bus by passing a pointer to its TwoWire object in the init method:
+
+```c++
+MagneticSensorAS5600 sensor1;
+MagneticSensorAS5600 sensor2;
+TwoWire myI2C;
+
+void setup() {
+
+    ...
+
+    sensor1.init(&myI2C);
+    sensor2.init(&Wire2);
+}
+```
+
+Using the sensor without releasing the bus should give you considerably more speed. Setting closeTransactions to false means the sensor will not release the bus between angle reads, and also will not re-write the register to the device, which boosts angle reading throughput significantly.
+
+```c++
+MagneticSensorAS5600 sensor;
+
+void setup() {
+    sensor.closeTransactions = false;
+    sensor.init();
+}
+```
+
+The sensor's other registers are exposed. Note that using the setter functions to set register values only performs a normal write, not a permanent programing of the register. Permanent programming (BURN function) is not supported by this driver.
+
+```c++
+
+MagneticSensorAS5600 sensor;
+
+void setup() {
+    sensor.closeTransactions = false;
+    sensor.init();
+
+    uint16_t magnitude = sensor.readMagnitude();
+    AS5600Status status = sensor.readStatus();
+    // print the values or something
+}
+
+```
+
+The AS5600L has a default address of 0x40, and you can set the I2C address. To temporarily change the address (resets to default on restart):
+
+```c++
+
+MagneticSensorAS5600 sensor(0x40); // default address of AS5600L is 0x40
+
+void setup() {
+    Serial.begin(115200);
+    delay(2000);
+
+    sensor.closeTransactions = true; // use normal transactons
+    sensor.init();
+    sensor.setI2CAddr(0x41); // set address to 0x41
+    sensor.setI2CUpdt(0x41); // i2c address is now 0x41
+    sensor._address = 0x41;
+    delay(10);
+    // sensor is using new address
+    uint16_t magnitude = sensor.readMagnitude();
+}
+```
+
+
+You can program an I2C address permanently (AS5600L only). The address will remain after reset. This programming operation can only be done once.
+
+```c++
+
+MagneticSensorAS5600 sensor(0x40); // default address of AS5600L is 0x40
+
+void setup() {
+    Serial.begin(115200);
+    delay(2000);
+
+    sensor.closeTransactions = true; // use normal transactons
+    sensor.init();
+
+    uint8_t addr = sensor.readI2CAddr(); // read I2C address, should be 0x40
+    Serial.print("Current Address: ");
+    Serial.println(addr);
+
+    sensor.setI2CAddr(0x41); // set address to 0x41
+    sensor.burnSettings(); // permanently set new address
+    Serial.println("Permanently changed address to 0x41. Restart the system.");
+    while (1);
+}
+
+
+```

firmware/lib/Arduino-FOC-drivers/src/encoders/calibrated/CalibratedSensor.cpp

@@ -0,0 +1,257 @@
+#include "CalibratedSensor.h"
+
+
+// CalibratedSensor()
+// sensor              - instance of original sensor object
+CalibratedSensor::CalibratedSensor(Sensor& wrapped) : _wrapped(wrapped) 
+{
+};
+
+
+CalibratedSensor::~CalibratedSensor()
+{
+	delete calibrationLut;
+};
+
+// call update of calibrated sensor
+void CalibratedSensor::update(){
+    _wrapped.update();
+    this->Sensor::update();
+};
+
+// Retrieve the calibrated sensor angle
+void CalibratedSensor::init() {
+    // assume wrapped sensor has already been initialized
+    this->Sensor::init(); // call superclass init
+}
+
+// Retrieve the calibrated sensor angle
+float CalibratedSensor::getSensorAngle(){
+    // raw encoder position e.g. 0-2PI
+    float rawAngle = _wrapped.getMechanicalAngle();
+
+    // index of the bucket that rawAngle is part of. 
+    // e.g. rawAngle = 0 --> bucketIndex = 0.
+    // e.g. rawAngle = 2PI --> bucketIndex = 128.
+    int bucketIndex = floor(rawAngle/(_2PI/n_lut));
+    float remainder = rawAngle - ((_2PI/n_lut)*bucketIndex);
+
+    // Extract the lower and upper LUT value in counts
+    float y0 = calibrationLut[bucketIndex]; 
+    float y1 = calibrationLut[(bucketIndex+1)%n_lut]; 
+
+    // Linear Interpolation Between LUT values y0 and y1 using the remainder
+    // If remainder = 0, interpolated offset = y0
+    // If remainder = 2PI/n_lut, interpolated offset = y1
+    float interpolatedOffset = (((_2PI/n_lut)-remainder)/(_2PI/n_lut))*y0 + (remainder/(_2PI/n_lut))*y1; 
+
+    // add offset to the raw sensor count. Divide multiply by 2PI/CPR to get radians
+    float calibratedAngle = rawAngle+interpolatedOffset; 
+
+    // return calibrated angle in radians
+    return calibratedAngle;
+}
+
+void CalibratedSensor::calibrate(BLDCMotor& motor){
+    
+    Serial.println("Starting Sensor Calibration.");
+
+    int _NPP = motor.pole_pairs;                                    // number of pole pairs which is user input
+    const int n_ticks = 128*_NPP;                                   // number of positions to be sampled per mechanical rotation.  Multiple of NPP for filtering reasons (see later)
+    const int n2_ticks = 40;                                        // increments between saved samples (for smoothing motion)
+    float deltaElectricalAngle = _2PI*_NPP/(n_ticks*n2_ticks);      // Electrical Angle increments for calibration steps    
+    float* error_f  = new float[n_ticks]();                         // pointer to error array rotating forwards
+  //  float* raw_f = new float[n_ticks]();                            // pointer to raw forward position
+    float* error_b  = new float[n_ticks]();                         // pointer to error array rotating forwards
+  //  float* raw_b = new float[n_ticks]();                            // pointer to raw backword position
+    float* error = new float[n_ticks]();                            // pointer to error array (average of forward & backward)
+    float* error_filt = new float[n_ticks]();                       // pointer to filtered error array (low pass filter)
+    const int window = 128;                                         // window size for moving average filter of raw error
+    motor.zero_electric_angle = 0;                                  // Set position sensor offset
+    
+    // find natural direction (this is copy of the init code)
+    // move one electrical revolution forward
+    for (int i = 0; i <=500; i++ ) {
+      float angle = _3PI_2 + _2PI * i / 500.0f;
+      motor.setPhaseVoltage(voltage_calibration, 0,  angle);
+        _wrapped.update();
+      _delay(2);
+    }
+    // take and angle in the middle
+    _wrapped.update();
+    float mid_angle = _wrapped.getAngle();
+    // move one electrical revolution backwards
+    for (int i = 500; i >=0; i-- ) {
+      float angle = _3PI_2 + _2PI * i / 500.0f ;
+      motor.setPhaseVoltage(voltage_calibration, 0,  angle);
+        _wrapped.update();
+      _delay(2);
+    }
+    _wrapped.update();
+    float end_angle = _wrapped.getAngle();
+    motor.setPhaseVoltage(0, 0, 0);
+    _delay(200);
+    // determine the direction the sensor moved
+    int directionSensor;
+    if (mid_angle < end_angle) {
+      Serial.println("MOT: sensor_direction==CCW");
+      directionSensor = -1;
+      motor.sensor_direction = Direction::CCW;
+
+    } else{
+      Serial.println("MOT: sensor_direction==CW");
+      directionSensor = 1;
+      motor.sensor_direction = Direction::CW;
+
+    }
+
+    //Set voltage angle to zero, wait for rotor position to settle
+    // keep the motor in position while getting the initial positions
+    motor.setPhaseVoltage(voltage_calibration, 0, elec_angle);
+    _delay(1000);
+    _wrapped.update();
+    float theta_init = _wrapped.getAngle();
+    float theta_absolute_init = _wrapped.getMechanicalAngle();
+    
+    /* 
+    Start Calibration
+    Loop over  electrical angles from 0 to NPP*2PI, once forward, once backward
+    store actual position and error as compared to electrical angle
+    */
+
+	/* 
+	forwards rotation
+	*/
+	Serial.println("Rotating forwards");
+	int k = 0;
+	for(int i = 0; i<n_ticks; i++)
+	{                                                 
+		for(int j = 0; j<n2_ticks; j++)
+			{   
+				elec_angle += deltaElectricalAngle;
+				motor.setPhaseVoltage(voltage_calibration, 0, elec_angle);
+			}
+		
+		// delay to settle in position before taking a position sample
+		_delay(20);
+		_wrapped.update();
+		theta_actual = _wrapped.getAngle()-theta_init;
+		if (directionSensor == -1)
+		{
+			theta_actual = -theta_actual;
+			error_f[i] = theta_actual - elec_angle/_NPP;
+
+		}
+		else
+		{
+			error_f[i] = elec_angle/_NPP - theta_actual;
+		}
+		// if overflow happened track it as full rotation
+//		raw_f[i] = theta_actual;
+
+		// storing the normalized angle every time the electrical angle 3PI/2 to calculate average zero electrical angle
+		if(i==(k*128+96))
+			{
+				_delay(50);
+				avg_elec_angle += _normalizeAngle(directionSensor*_wrapped.getMechanicalAngle()*_NPP);
+				k += 1;
+			}
+	}
+	
+	_delay(2000);
+
+	/* 
+	backwards rotation
+	*/
+	Serial.println("Rotating backwards");
+	for(int i = 0; i<n_ticks; i++)
+	{                                                 
+		for(int j = 0; j<n2_ticks; j++)
+			{   
+				elec_angle -= deltaElectricalAngle;
+				motor.setPhaseVoltage(voltage_calibration, 0 ,elec_angle);
+			}
+
+			// delay to settle in position before taking a position sample
+			_delay(20);
+			_wrapped.update();
+			theta_actual = _wrapped.getAngle()-theta_init;
+			if (directionSensor == -1)
+			{
+			theta_actual = -theta_actual;
+			error_b[i] =  theta_actual - elec_angle/_NPP;
+			}
+			else
+			{
+			error_b[i] = elec_angle/_NPP - theta_actual;
+			}
+//			raw_b[i] = theta_actual;
+	}
+
+	// get post calibration mechanical angle.
+	_wrapped.update();
+	theta_absolute_post = _wrapped.getMechanicalAngle();
+
+	// done with the measurement
+	motor.setPhaseVoltage(0, 0, 0);
+
+	// raw offset from initial position in absolute radians between 0-2PI
+	float raw_offset = (theta_absolute_init+theta_absolute_post)/2;                   
+
+	// calculating the average zero electrica angle from the forward calibration.
+	motor.zero_electric_angle  = avg_elec_angle/_NPP;
+	Serial.print( "Average Zero Electrical Angle: ");
+	Serial.println( motor.zero_electric_angle); 
+	
+	// Perform filtering to linearize position sensor eccentricity
+	// FIR n-sample average, where n = number of samples in one electrical cycle
+	// This filter has zero gain at electrical frequency and all integer multiples
+	// So cogging effects should be completely filtered out
+	float mean = 0;
+	for (int i = 0; i<n_ticks; i++){                                            //Average the forward and back directions
+		error[i] = 0.5f*(error_f[i] + error_b[n_ticks-i-1]);
+		}
+	for (int i = 0; i<n_ticks; i++){
+		for(int j = 0; j<window; j++){
+			int ind = -window/2 + j + i;                                    	// Indexes from -window/2 to + window/2
+			if(ind<0){
+				ind += n_ticks;}                                                // Moving average wraps around
+			else if(ind > n_ticks-1) {
+				ind -= n_ticks;}
+			error_filt[i] += error[ind]/(float)window;
+		}
+		mean += error_filt[i]/n_ticks;
+	}
+
+	// calculate offset index
+	int index_offset = floor(raw_offset/(_2PI/n_lut));
+
+	// Build Look Up Table
+	for (int i = 0; i<n_lut; i++){                                          
+		int ind =  index_offset + i*directionSensor;
+		if(ind > (n_lut-1)){ 
+			ind -= n_lut;
+		}
+		if(ind < 0 ){
+			ind += n_lut;
+		}
+		calibrationLut[ind] = (float) (error_filt[i*_NPP] - mean);
+		//Serial.print(ind);
+		//Serial.print('\t');
+		//Serial.println(calibrationLut[ind],5);
+		_delay(1);
+	}
+   
+   // de-allocate memory
+    delete error_filt;
+    delete error;
+  //  delete raw_b;
+    delete error_b;
+  //  delete raw_f;
+    delete error_f;
+
+    Serial.println("Sensor Calibration Done.");
+
+}
+
+

firmware/lib/Arduino-FOC-drivers/src/encoders/calibrated/CalibratedSensor.h

@@ -0,0 +1,61 @@
+#ifndef __CALIBRATEDSENSOR_H__
+#define __CALIBRATEDSENSOR_H__
+
+#include "common/base_classes/Sensor.h"
+#include "BLDCMotor.h"
+#include "common/base_classes/FOCMotor.h"
+
+
+class CalibratedSensor: public Sensor{
+
+public:
+    // constructor of class with pointer to base class sensor and driver
+    CalibratedSensor(Sensor& wrapped);
+    ~CalibratedSensor();
+
+    /*
+    Override the update function
+    */
+    virtual void update() override;
+
+    /**
+    * Calibrate method computes the LUT for the correction
+    */
+    virtual void calibrate(BLDCMotor& motor);
+
+    // voltage to run the calibration: user input
+    float voltage_calibration = 1;                              
+
+protected:
+
+    /**
+    * getSenorAngle() method of CalibratedSensor class.
+    * This should call getAngle() on the wrapped instance, and then apply the correction to
+    * the value returned. 
+    */
+    virtual float getSensorAngle() override;
+    /**
+    * init method of CaibratedSensor - call after calibration
+    */
+    virtual void init() override;
+    /**
+    * delegate instance of Sensor class
+    */
+    Sensor& _wrapped;
+    
+     // lut size, currently constan. Perhaps to be made variable by user?
+    const int  n_lut { 128 } ;
+    // create pointer for lut memory
+    float* calibrationLut = new float[n_lut]();
+
+    // Init inital angles
+    float theta_actual { 0.0 };
+    float elecAngle { 0.0 };
+    float elec_angle { 0.0 };
+    float theta_absolute_post { 0.0 };
+    float theta_absolute_init { 0.0 };
+    float theta_init { 0.0 };
+    float avg_elec_angle { 0.0 };
+};
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/calibrated/README.md

@@ -0,0 +1,91 @@
+# Calibrated Sensor
+
+by [@MarethyuPrefect](https://github.com/MarethyuPrefect)
+
+A SimpleFOC Sensor wrapper implementation which adds sensor eccentricity calibration.
+
+Please also see our [forum thread](https://community.simplefoc.com/t/simplefoc-sensor-eccentricity-calibration/2212) on this topic.
+
+
+When you mount your (magnetic) sensor on your frame or motor, there will always be a slight misalignment between magnet and sensor (measurement system). This misalignment between center of rotation and the center of the sensor is called the eccentricity error.
+
+As a result your measurement system output is non-linear with respect to the rotor of the motor. This will cause an error with respect to the ideal torque you attempt to create with the I_q vector as function of the position. You could interpret this as a disturbance on your control loop which you want to minimize for optimal performance. 
+
+This calibration compensates the sensor reading in a feed forward fashion such that your performance improves.
+
+
+## Hardware setup
+
+Connect your sensor as usual. Make sure the sensor is working 'normally' i.e. without calibration first. Once things are working and tuned without sensor calibration, you can add the CalibratedSensor to see if you get an improvement.
+
+Note that during calibration, the motor is turned through several turns, and should be in an unloaded condition. Please ensure your hardware setup can support the motor rotating through full turns.
+
+
+## Softwate setup
+
+The CalibratedSensor acts as a wrapper to the actual sensor class. When creating the CalibratedSensor object, provide the real
+sensor to the constructor of CalibratedSensor.
+
+First, initialize the real sensor instance as normal. Then, call calibrate() on the CalibratedSensor instance. Then link the 
+CalibratedSensor to the motor and call motor.initFOC().
+
+The motor will then use the calibrated sensor instance.
+
+
+```c++
+// magnetic sensor instance - SPI
+MagneticSensorSPI sensor = MagneticSensorSPI(AS5048_SPI, PB6);
+// BLDC motor & driver instance
+BLDCMotor motor = BLDCMotor(11);
+BLDCDriver3PWM driver = BLDCDriver3PWM(PB4,PC7,PB10,PA9);
+// instantiate the calibrated sensor, providing the real sensor as a constructor argument
+CalibratedSensor sensor_calibrated = CalibratedSensor(sensor);
+
+void setup() {
+  sensor.init();
+  // Link motor to sensor
+  motor.linkSensor(&sensor);
+  // power supply voltage
+  driver.voltage_power_supply = 20;
+  driver.init();
+  motor.linkDriver(&driver);
+  // aligning voltage 
+  motor.voltage_sensor_align = 8;
+  motor.voltage_limit = 20;
+  // set motion control loop to be used
+  motor.controller = MotionControlType::torque;
+
+  // use monitoring with serial 
+  Serial.begin(115200);
+  // comment out if not needed
+  motor.useMonitoring(Serial);
+  motor.monitor_variables =  _MON_VEL; 
+  motor.monitor_downsample = 10; // default 10
+
+  // initialize motor
+  motor.init();
+
+  // set voltage to run calibration
+  sensor_calibrated.voltage_calibration = 6;
+  // Running calibration
+  sensor_calibrated.calibrate(motor); 
+
+  //Serial.println("Calibrating Sensor Done.");
+  // Linking sensor to motor object
+  motor.linkSensor(&sensor_calibrated);
+
+  // calibrated init FOC
+  motor.initFOC();
+}
+```
+
+Please see the more complete [example](https://github.com/simplefoc/Arduino-FOC-drivers/blob/master/examples/encoders/calibrated/sensor_calibration.ino) in our examples directory.
+
+
+## Roadmap
+
+Possible future improvements we've thought about:
+
+- Improve memory usage and performance
+- Make calibration able to be saved/restored
+

firmware/lib/Arduino-FOC-drivers/src/encoders/ma330/MA330.cpp

@@ -0,0 +1,146 @@
+#include "MA330.h"
+
+MA330::MA330(SPISettings settings, int nCS) : settings(settings), nCS(nCS) {
+
+};
+MA330::~MA330() {
+
+};
+
+void MA330::init(SPIClass* _spi) {
+    spi = _spi;
+    if (nCS >= 0) {
+        pinMode(nCS, OUTPUT);
+        digitalWrite(nCS, HIGH);
+    }
+};
+
+float MA330::getCurrentAngle() {
+    return (readRawAngle() * _2PI)/MA330_CPR;
+}; // angle in radians, return current value
+
+uint16_t MA330::readRawAngle() {
+    uint16_t angle = transfer16(0x0000);
+    return angle;
+}; // 9-14bit angle value
+
+uint16_t MA330::getZero() {
+    uint16_t result = readRegister(MA330_REG_ZERO_POSITION_MSB)<<8;
+    result |= readRegister(MA330_REG_ZERO_POSITION_LSB);
+    return result;
+};
+uint8_t MA330::getBiasCurrentTrimming() {
+    return readRegister(MA330_REG_BCT);
+};
+bool MA330::isBiasCurrrentTrimmingX() {
+    return (readRegister(MA330_REG_ET) & 0x01)==0x01;
+};
+bool MA330::isBiasCurrrentTrimmingY() {
+    return (readRegister(MA330_REG_ET) & 0x02)==0x02;
+};
+uint16_t MA330::getPulsesPerTurn() {
+    uint16_t result = readRegister(MA330_REG_ILIP_PPT_LSB)>>6;
+    result |= ((uint16_t)readRegister(MA330_REG_PPT_MSB))<<2;
+    return result+1;
+};
+uint8_t MA330::getIndexLength() {
+    return (readRegister(MA330_REG_ILIP_PPT_LSB)>>2)&0x0F;
+};
+uint8_t MA330::getNumberPolePairs() {
+    return (readRegister(MA330_REG_NPP)>>5)&0x07;;
+};
+uint8_t MA330::getRotationDirection() {
+    return (readRegister(MA330_REG_RD)>>7);
+};
+uint8_t MA330::getFieldStrengthHighThreshold() {
+    return (readRegister(MA330_REG_MGLT_MGHT)>>2)&0x07;
+};
+uint8_t MA330::getFieldStrengthLowThreshold() {
+    return (readRegister(MA330_REG_MGLT_MGHT)>>5)&0x07;
+};
+uint8_t MA330::getFilterWidth() {
+    return readRegister(MA330_REG_FW);
+};
+uint8_t MA330::getHysteresis() {
+    return readRegister(MA330_REG_HYS);
+};
+FieldStrength MA330::getFieldStrength() {
+    return (FieldStrength)(readRegister(MA330_REG_MGH_MGL)>>6);
+};
+
+
+
+void MA330::setZero(uint16_t value) {
+    writeRegister(MA330_REG_ZERO_POSITION_MSB, value>>8);
+    writeRegister(MA330_REG_ZERO_POSITION_LSB, value&0x00FF);
+};
+void MA330::setBiasCurrentTrimming(uint8_t value) {
+    writeRegister(MA330_REG_BCT, value);
+};
+void MA330::setBiasCurrrentTrimmingEnabled(bool Xenabled, bool Yenabled) {
+    uint8_t val = Xenabled ? 0x01 : 0x00;
+    val |= (Yenabled ? 0x02 : 0x00);
+    writeRegister(MA330_REG_ET, val);
+};
+void MA330::setPulsesPerTurn(uint16_t value) {
+    uint16_t pptVal = value - 1;
+    writeRegister(MA330_REG_PPT_MSB, pptVal>>2);
+    uint8_t val = readRegister(MA330_REG_ILIP_PPT_LSB);
+    val &= 0x3F;
+    val |= (pptVal&0x03)<<6;
+    writeRegister(MA330_REG_ILIP_PPT_LSB, val);
+};
+void MA330::setIndexLength(uint8_t value) {
+    uint8_t val = readRegister(MA330_REG_ILIP_PPT_LSB);
+    val &= 0xC0;
+    val |= ((value<<2)&0x3F);
+    writeRegister(MA330_REG_ILIP_PPT_LSB, val);
+};
+void MA330::setNumberPolePairs(uint8_t value) {
+    uint8_t val = readRegister(MA330_REG_NPP);
+    val &= 0x1F;
+    val |= (value<<5);
+    writeRegister(MA330_REG_NPP, val);
+};
+void MA330::setRotationDirection(uint8_t value) {
+    if (value==0)
+        writeRegister(MA330_REG_RD, 0x00);
+    else
+        writeRegister(MA330_REG_RD, 0x80);
+};
+void MA330::setFilterWidth(uint8_t value) {
+    writeRegister(MA330_REG_FW, value);
+};
+void MA330::setHysteresis(uint8_t value) {
+    writeRegister(MA330_REG_HYS, value);
+};
+void MA330::setFieldStrengthThresholds(uint8_t high, uint8_t low) {
+    uint8_t val = (low<<5) | (high<<2);
+    writeRegister(MA330_REG_MGLT_MGHT, val);
+};
+
+
+uint16_t MA330::transfer16(uint16_t outValue) {
+    if (nCS >= 0)
+        digitalWrite(nCS, LOW);
+    spi->beginTransaction(settings);
+    uint16_t value = spi->transfer16(outValue);
+    spi->endTransaction();
+    if (nCS >= 0)
+        digitalWrite(nCS, HIGH);
+    return value;
+};
+uint8_t MA330::readRegister(uint8_t reg) {
+    uint16_t cmd = 0x4000 | ((reg&0x001F)<<8);
+    uint16_t value = transfer16(cmd);
+    delayMicroseconds(1);
+    value = transfer16(0x0000);
+    return value>>8;
+};
+uint8_t MA330::writeRegister(uint8_t reg, uint8_t value) {
+    uint16_t cmd = 0x8000 | ((reg&0x1F)<<8) | value;
+    uint16_t result = transfer16(cmd);
+    delay(20); // 20ms delay required
+    result = transfer16(0x0000);
+    return result>>8;
+};

firmware/lib/Arduino-FOC-drivers/src/encoders/ma330/MA330.h

@@ -0,0 +1,82 @@
+#ifndef __MA330_H__
+#define __MA330_H__
+
+
+#include "Arduino.h"
+#include "SPI.h"
+
+enum FieldStrength : uint8_t {
+    FS_NORMAL = 0x00,
+    FS_LOW = 0x01,
+    FS_HIGH = 0x02,
+    FS_ERR = 0x03 // impossible state
+};
+
+
+#define _2PI 6.28318530718f
+#define MA330_CPR 65536.0f
+
+#define MA330_REG_ZERO_POSITION_LSB 0x00
+#define MA330_REG_ZERO_POSITION_MSB 0x01
+#define MA330_REG_BCT 0x02
+#define MA330_REG_ET 0x03
+#define MA330_REG_ILIP_PPT_LSB 0x04
+#define MA330_REG_PPT_MSB 0x05
+#define MA330_REG_MGLT_MGHT 0x06
+#define MA330_REG_NPP 0x07
+#define MA330_REG_RD 0x09
+#define MA330_REG_FW 0x0E
+#define MA330_REG_HYS 0x10
+#define MA330_REG_MGH_MGL 0x1B
+
+#define MA330_BITORDER MSBFIRST
+
+static SPISettings MA330SPISettings(1000000, MA330_BITORDER, SPI_MODE3); // @suppress("Invalid arguments")
+
+class MA330 {
+public:
+	MA330(SPISettings settings = MA330SPISettings, int nCS = -1);
+	virtual ~MA330();
+
+	virtual void init(SPIClass* _spi = &SPI);
+
+	float getCurrentAngle(); // angle in radians, return current value
+
+	uint16_t readRawAngle(); // 9-14bit angle value
+
+    uint16_t getZero();
+    uint8_t getBiasCurrentTrimming();
+    bool isBiasCurrrentTrimmingX();
+    bool isBiasCurrrentTrimmingY();
+    uint16_t getPulsesPerTurn();
+    uint8_t getIndexLength();
+    uint8_t getNumberPolePairs();
+    uint8_t getRotationDirection();
+    uint8_t getFilterWidth();
+    uint8_t getHysteresis();
+    uint8_t getFieldStrengthHighThreshold();
+    uint8_t getFieldStrengthLowThreshold();
+    FieldStrength getFieldStrength();
+
+    void setZero(uint16_t);
+    void setBiasCurrentTrimming(uint8_t);
+    void setBiasCurrrentTrimmingEnabled(bool Xenabled, bool Yenabled);
+    void setPulsesPerTurn(uint16_t);
+    void setIndexLength(uint8_t);
+    void setNumberPolePairs(uint8_t);
+    void setRotationDirection(uint8_t);
+    void setFilterWidth(uint8_t);
+    void setHysteresis(uint8_t);
+    void setFieldStrengthThresholds(uint8_t high, uint8_t low);
+
+private:
+	SPIClass* spi;
+	SPISettings settings;
+	int nCS = -1;
+
+    uint16_t transfer16(uint16_t outValue);
+    uint8_t readRegister(uint8_t reg);
+    uint8_t writeRegister(uint8_t reg, uint8_t value);
+};
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/ma330/MagneticSensorMA330.cpp

@@ -0,0 +1,26 @@
+#include "./MagneticSensorMA330.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+
+MagneticSensorMA330::MagneticSensorMA330(int nCS, SPISettings settings) : MA330(settings, nCS) {
+
+}
+
+
+MagneticSensorMA330::~MagneticSensorMA330(){ 
+
+}
+
+
+void MagneticSensorMA330::init(SPIClass* _spi) {
+    this->MA330::init(_spi);
+    this->Sensor::init();
+}
+
+
+float MagneticSensorMA330::getSensorAngle() {
+    float angle_data = readRawAngle();
+    angle_data = ( angle_data / (float)MA330_CPR) * _2PI;
+    // return the shaft angle
+    return angle_data;
+}

firmware/lib/Arduino-FOC-drivers/src/encoders/ma330/MagneticSensorMA330.h

@@ -0,0 +1,19 @@
+#ifndef __MAGNETIC_SENSOR_MA330_H__
+#define __MAGNETIC_SENSOR_MA330_H__
+
+
+#include "common/base_classes/Sensor.h"
+#include "./MA330.h"
+
+class MagneticSensorMA330 : public Sensor, public MA330 {
+public:
+	MagneticSensorMA330(int nCS = -1, SPISettings settings = MA330SPISettings);
+	virtual ~MagneticSensorMA330();
+
+    virtual float getSensorAngle() override;
+
+	virtual void init(SPIClass* _spi = &SPI);
+};
+
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/ma330/README.md

@@ -0,0 +1,66 @@
+# MA330 SimpleFOC driver
+
+While MA330 absolute position magnetic rotary encoder is supported by the standard MagneticSensorSPI driver included in the base distribution, this MA330-specific driver includes some optimisations:
+
+- access to the other registers of the MA330
+- this driver directly reads the angle with one call to SPI
+- this will halve the number of 16-bit SPI transfers per simpleFOC loop iteration
+
+
+## Hardware setup
+
+Connect as per normal for your SPI bus. No special hardware setup is needed to use this driver.
+
+## Software setup
+
+Its actually easier to use than the standard SPI sensor class, because it is less generic:
+
+```c++
+#include "Arduino.h"
+#include "Wire.h"
+#include "SPI.h"
+#include "SimpleFOC.h"
+#include "SimpleFOCDrivers.h"
+#include "encoders/MA330/MagneticSensorMA330.h"
+
+#define SENSOR1_CS 5 // some digital pin that you're using as the nCS pin
+MagneticSensorMA330 sensor1(SENSOR1_CS);
+
+
+void setup() {
+    sensor1.init();
+}
+```
+
+Set some options:
+
+```c++
+MagneticSensorMA330 sensor1(SENSOR1_CS, true, mySPISettings);
+```
+
+Use another SPI bus:
+
+```c++
+void setup() {
+    sensor1.init(SPI2);
+}
+```
+
+Here's how you can use it:
+
+```c++
+    // update the sensor (only needed if using the sensor without a motor)
+    sensor1.update();
+
+    // get the angle, in radians, including full rotations
+    float a1 = sensor1.getAngle();
+
+    // get the velocity, in rad/s - note: you have to call getAngle() on a regular basis for it to work
+    float v1 = sensor1.getVelocity();
+
+    // get the angle, in radians, no full rotations
+    float a2 = sensor1.getCurrentAngle();
+
+    // get the raw 14 bit value
+    uint16_t raw = sensor1.readRawAngle();
+```

firmware/lib/Arduino-FOC-drivers/src/encoders/ma730/MA730.cpp

@@ -0,0 +1,125 @@
+#include "MA730.h"
+
+MA730::MA730(SPISettings settings, int nCS) : settings(settings), nCS(nCS) {
+
+};
+MA730::~MA730() {
+
+};
+
+void MA730::init(SPIClass* _spi) {
+    spi = _spi;
+    if (nCS >= 0) {
+        pinMode(nCS, OUTPUT);
+        digitalWrite(nCS, HIGH);
+    }
+};
+
+float MA730::getCurrentAngle() {
+    return (readRawAngle() * _2PI)/MA730_CPR;
+}; // angle in radians, return current value
+
+uint16_t MA730::readRawAngle() {
+    uint16_t angle = transfer16(0x0000);
+    return angle;
+}; // 14bit angle value
+
+uint16_t MA730::getZero() {
+    uint16_t result = readRegister(MA730_REG_ZERO_POSITION_MSB)<<8;
+    result |= readRegister(MA730_REG_ZERO_POSITION_LSB);
+    return result;
+};
+uint8_t MA730::getBiasCurrentTrimming() {
+    return readRegister(MA730_REG_BCT);
+};
+bool MA730::isBiasCurrrentTrimmingX() {
+    return (readRegister(MA730_REG_ET) & 0x01)==0x01;
+};
+bool MA730::isBiasCurrrentTrimmingY() {
+    return (readRegister(MA730_REG_ET) & 0x02)==0x02;
+};
+uint16_t MA730::getPulsesPerTurn() {
+    uint16_t result = readRegister(MA730_REG_ILIP_PPT_LSB)>>6;
+    result |= ((uint16_t)readRegister(MA730_REG_PPT_MSB))<<2;
+    return result+1;
+};
+uint8_t MA730::getIndexLength() {
+    return (readRegister(MA730_REG_ILIP_PPT_LSB)>>2)&0x0F;
+};
+uint8_t MA730::getRotationDirection() {
+    return (readRegister(MA730_REG_RD)>>7);
+};
+uint8_t MA730::getFieldStrengthHighThreshold() {
+    return (readRegister(MA730_REG_MGLT_MGHT)>>2)&0x07;
+};
+uint8_t MA730::getFieldStrengthLowThreshold() {
+    return (readRegister(MA730_REG_MGLT_MGHT)>>5)&0x07;
+};
+FieldStrength MA730::getFieldStrength() {
+    return (FieldStrength)(readRegister(MA730_REG_MGH_MGL)>>6);
+};
+
+
+
+void MA730::setZero(uint16_t value) {
+    writeRegister(MA730_REG_ZERO_POSITION_MSB, value>>8);
+    writeRegister(MA730_REG_ZERO_POSITION_LSB, value&0x00FF);
+};
+void MA730::setBiasCurrentTrimming(uint8_t value) {
+    writeRegister(MA730_REG_BCT, value);
+};
+void MA730::setBiasCurrrentTrimmingEnabled(bool Xenabled, bool Yenabled) {
+    uint8_t val = Xenabled ? 0x01 : 0x00;
+    val |= (Yenabled ? 0x02 : 0x00);
+    writeRegister(MA730_REG_ET, val);
+};
+void MA730::setPulsesPerTurn(uint16_t value) {
+    uint16_t pptVal = value - 1;
+    writeRegister(MA730_REG_PPT_MSB, pptVal>>2);
+    uint8_t val = readRegister(MA730_REG_ILIP_PPT_LSB);
+    val &= 0x3F;
+    val |= (pptVal&0x03)<<6;
+    writeRegister(MA730_REG_ILIP_PPT_LSB, val);
+};
+void MA730::setIndexLength(uint8_t value) {
+    uint8_t val = readRegister(MA730_REG_ILIP_PPT_LSB);
+    val &= 0xC0;
+    val |= ((value<<2)&0x3F);
+    writeRegister(MA730_REG_ILIP_PPT_LSB, val);
+};
+void MA730::setRotationDirection(uint8_t value) {
+    if (value==0)
+        writeRegister(MA730_REG_RD, 0x00);
+    else
+        writeRegister(MA730_REG_RD, 0x80);
+};
+void MA730::setFieldStrengthThresholds(uint8_t high, uint8_t low) {
+    uint8_t val = (low<<5) | (high<<2);
+    writeRegister(MA730_REG_MGLT_MGHT, val);
+};
+
+
+uint16_t MA730::transfer16(uint16_t outValue) {
+    if (nCS >= 0)
+        digitalWrite(nCS, LOW);
+    spi->beginTransaction(settings);
+    uint16_t value = spi->transfer16(outValue);
+    spi->endTransaction();
+    if (nCS >= 0)
+        digitalWrite(nCS, HIGH);
+    return value;
+};
+uint8_t MA730::readRegister(uint8_t reg) {
+    uint16_t cmd = 0x4000 | ((reg&0x001F)<<8);
+    uint16_t value = transfer16(cmd);
+    delayMicroseconds(1);
+    value = transfer16(0x0000);
+    return value>>8;
+};
+uint8_t MA730::writeRegister(uint8_t reg, uint8_t value) {
+    uint16_t cmd = 0x8000 | ((reg&0x1F)<<8) | value;
+    uint16_t result = transfer16(cmd);
+    delay(20); // 20ms delay required
+    result = transfer16(0x0000);
+    return result>>8;
+};

firmware/lib/Arduino-FOC-drivers/src/encoders/ma730/MA730.h

@@ -0,0 +1,76 @@
+#ifndef __MA730_H__
+#define __MA730_H__
+
+
+#include "Arduino.h"
+#include "SPI.h"
+
+enum FieldStrength : uint8_t {
+    FS_NORMAL = 0x00,
+    FS_LOW = 0x01,
+    FS_HIGH = 0x02,
+    FS_ERR = 0x03 // impossible state
+};
+
+
+#define _2PI 6.28318530718f
+#define MA730_CPR 65536.0f
+
+#define MA730_REG_ZERO_POSITION_LSB 0x00
+#define MA730_REG_ZERO_POSITION_MSB 0x01
+#define MA730_REG_BCT 0x02
+#define MA730_REG_ET 0x03
+#define MA730_REG_ILIP_PPT_LSB 0x04
+#define MA730_REG_PPT_MSB 0x05
+#define MA730_REG_MGLT_MGHT 0x06
+#define MA730_REG_RD 0x09
+#define MA730_REG_MGH_MGL 0x1B
+
+#define MA730_BITORDER MSBFIRST
+
+static SPISettings MA730SPISettings(1000000, MA730_BITORDER, SPI_MODE3); // @suppress("Invalid arguments")
+static SPISettings MA730SSISettings(4000000, MA730_BITORDER, SPI_MODE1); // @suppress("Invalid arguments")
+
+
+class MA730 {
+public:
+	MA730(SPISettings settings = MA730SPISettings, int nCS = -1);
+	virtual ~MA730();
+
+	virtual void init(SPIClass* _spi = &SPI);
+
+	float getCurrentAngle(); // angle in radians, return current value
+
+	uint16_t readRawAngle(); // 14bit angle value
+    uint16_t readRawAngleSSI(); // 14bit angle value
+
+    uint16_t getZero();
+    uint8_t getBiasCurrentTrimming();
+    bool isBiasCurrrentTrimmingX();
+    bool isBiasCurrrentTrimmingY();
+    uint16_t getPulsesPerTurn();
+    uint8_t getIndexLength();
+    uint8_t getRotationDirection();
+    uint8_t getFieldStrengthHighThreshold();
+    uint8_t getFieldStrengthLowThreshold();
+    FieldStrength getFieldStrength();
+
+    void setZero(uint16_t);
+    void setBiasCurrentTrimming(uint8_t);
+    void setBiasCurrrentTrimmingEnabled(bool Xenabled, bool Yenabled);
+    void setPulsesPerTurn(uint16_t);
+    void setIndexLength(uint8_t);
+    void setRotationDirection(uint8_t);
+    void setFieldStrengthThresholds(uint8_t high, uint8_t low);
+
+private:
+	SPIClass* spi;
+	SPISettings settings;
+	int nCS = -1;
+
+    uint16_t transfer16(uint16_t outValue);
+    uint8_t readRegister(uint8_t reg);
+    uint8_t writeRegister(uint8_t reg, uint8_t value);
+};
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/ma730/MagneticSensorMA730.cpp

@@ -0,0 +1,26 @@
+#include "./MagneticSensorMA730.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+
+MagneticSensorMA730::MagneticSensorMA730(int nCS, SPISettings settings) : MA730(settings, nCS) {
+
+}
+
+
+MagneticSensorMA730::~MagneticSensorMA730(){ 
+
+}
+
+
+void MagneticSensorMA730::init(SPIClass* _spi) {
+    this->MA730::init(_spi);
+    this->Sensor::init();
+}
+
+
+float MagneticSensorMA730::getSensorAngle() {
+    float angle_data = readRawAngle();
+    angle_data = ( angle_data / (float)MA730_CPR) * _2PI;
+    // return the shaft angle
+    return angle_data;
+}

firmware/lib/Arduino-FOC-drivers/src/encoders/ma730/MagneticSensorMA730.h

@@ -0,0 +1,19 @@
+#ifndef __MAGNETIC_SENSOR_MA730_H__
+#define __MAGNETIC_SENSOR_MA730_H__
+
+
+#include "common/base_classes/Sensor.h"
+#include "./MA730.h"
+
+class MagneticSensorMA730 : public Sensor, public MA730 {
+public:
+	MagneticSensorMA730(int nCS = -1, SPISettings settings = MA730SPISettings);
+	virtual ~MagneticSensorMA730();
+
+    virtual float getSensorAngle() override;
+
+	virtual void init(SPIClass* _spi = &SPI);
+};
+
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/ma730/MagneticSensorMA730SSI.cpp

@@ -0,0 +1,34 @@
+#include "./MagneticSensorMA730SSI.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+
+MagneticSensorMA730SSI::MagneticSensorMA730SSI(SPISettings settings) : settings(settings) {
+
+}
+
+
+MagneticSensorMA730SSI::~MagneticSensorMA730SSI() {
+
+}
+
+void MagneticSensorMA730SSI::init(SPIClass* _spi) {
+    this->spi=_spi;
+    this->Sensor::init();
+}
+
+// check 40us delay between each read?
+float MagneticSensorMA730SSI::getSensorAngle() {
+    float angle_data = readRawAngleSSI();
+    angle_data = ( angle_data / (float)MA730_CPR ) * _2PI;
+    // return the shaft angle
+    return angle_data;
+}
+
+
+uint16_t MagneticSensorMA730SSI::readRawAngleSSI() {
+    spi->beginTransaction(settings);
+    uint16_t value = spi->transfer16(0x0000);
+    //uint16_t parity = spi->transfer(0x00);
+    spi->endTransaction();
+    return (value<<1); //>>1)&0x3FFF;
+}; // 14bit angle value

firmware/lib/Arduino-FOC-drivers/src/encoders/ma730/MagneticSensorMA730SSI.h

@@ -0,0 +1,25 @@
+#ifndef __MAGNETIC_SENSOR_MA730SSI_H__
+#define __MAGNETIC_SENSOR_MA730SSI_H__
+
+
+#include "common/base_classes/Sensor.h"
+#include "./MA730.h"
+
+class MagneticSensorMA730SSI : public Sensor {
+public:
+	MagneticSensorMA730SSI(SPISettings settings = MA730SSISettings);
+	virtual ~MagneticSensorMA730SSI();
+
+    virtual float getSensorAngle() override;
+
+	virtual void init(SPIClass* _spi = &SPI);
+
+    uint16_t readRawAngleSSI();
+	
+private:
+	SPIClass* spi;
+	SPISettings settings;
+};
+
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/ma730/README.md

@@ -0,0 +1,74 @@
+# MA730 SimpleFOC driver
+
+While MA730 absolute position magnetic rotary encoder is supported by the standard MagneticSensorSPI driver included in the base distribution, this MA730-specific driver includes some optimisations:
+
+- access to the other registers of the MA730
+- this driver directly reads the angle with one call to SPI
+- this will halve the number of 16-bit SPI transfers per simpleFOC loop iteration
+
+
+## Hardware setup
+
+Connect as per normal for your SPI bus. No special hardware setup is needed to use this driver.
+
+## Software setup
+
+Its actually easier to use than the standard SPI sensor class, because it is less generic:
+
+```c++
+#include "Arduino.h"
+#include "Wire.h"
+#include "SPI.h"
+#include "SimpleFOC.h"
+#include "SimpleFOCDrivers.h"
+#include "encoders/ma730/MagneticSensorMA730.h"
+
+#define SENSOR1_CS 5 // some digital pin that you're using as the nCS pin
+MagneticSensorMA730 sensor1(SENSOR1_CS);
+
+
+void setup() {
+    sensor1.init();
+}
+```
+
+Set some options:
+
+```c++
+MagneticSensorMA730 sensor1(SENSOR1_CS, mySPISettings);
+```
+
+Use another SPI bus:
+
+```c++
+void setup() {
+    sensor1.init(&SPI2);
+}
+```
+
+Here's how you can use it:
+
+```c++
+    // update the sensor (only needed if using the sensor without a motor)
+    sensor1.update();
+
+    // get the angle, in radians, including full rotations
+    float a1 = sensor1.getAngle();
+
+    // get the velocity, in rad/s - note: you have to call getAngle() on a regular basis for it to work
+    float v1 = sensor1.getVelocity();
+
+    // get the angle, in radians, no full rotations
+    float a2 = sensor1.getCurrentAngle();
+
+    // get the raw 14 bit value
+    uint16_t raw = sensor1.readRawAngle();
+
+    // get the field strength
+    FieldStrength fs = sensor1.getFieldStrength();
+    Serial.print("Field strength: ");
+    Serial.println(fs);
+
+    // set pulses per turn for encoder mode
+    sensor1.setPulsesPerTurn(999); // set to 999 if we want 1000 PPR == 4000 CPR
+```

firmware/lib/Arduino-FOC-drivers/src/encoders/mt6701/MagneticSensorMT6701SSI.cpp

@@ -0,0 +1,42 @@
+#include "./MagneticSensorMT6701SSI.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+
+MagneticSensorMT6701SSI::MagneticSensorMT6701SSI(int nCS, SPISettings settings) : settings(settings), nCS(nCS) {
+
+}
+
+
+MagneticSensorMT6701SSI::~MagneticSensorMT6701SSI() {
+
+}
+
+void MagneticSensorMT6701SSI::init(SPIClass* _spi) {
+    this->spi=_spi;
+    if (nCS >= 0) {
+        pinMode(nCS, OUTPUT);
+        digitalWrite(nCS, HIGH);
+    }
+    this->spi->begin();
+    this->Sensor::init();
+}
+
+// check 40us delay between each read?
+float MagneticSensorMT6701SSI::getSensorAngle() {
+    float angle_data = readRawAngleSSI();
+    angle_data = ( angle_data / (float)MT6701_CPR ) * _2PI;
+    // return the shaft angle
+    return angle_data;
+}
+
+
+uint16_t MagneticSensorMT6701SSI::readRawAngleSSI() {
+    if (nCS >= 0)
+        digitalWrite(nCS, LOW);
+    spi->beginTransaction(settings);
+    uint16_t value = spi->transfer16(0x0000);
+    spi->endTransaction();
+    if (nCS >= 0)
+        digitalWrite(nCS, HIGH);
+    return (value>>MT6701_DATA_POS)&0x3FFF;
+};

firmware/lib/Arduino-FOC-drivers/src/encoders/mt6701/MagneticSensorMT6701SSI.h

@@ -0,0 +1,43 @@
+#ifndef __MAGNETIC_SENSOR_MT6701_SSI_H__
+#define __MAGNETIC_SENSOR_MT6701_SSI_H__
+
+#include "Arduino.h"
+#include "SPI.h"
+#include "common/base_classes/Sensor.h"
+
+
+#define MT6701_CPR 16384.0f
+
+#define MT6701_BITORDER MSBFIRST
+
+#if defined(TARGET_RP2040)||defined(ESP_H)||defined(CORE_TEENSY)
+#define MT6701_DATA_POS 1
+#else
+#define MT6701_DATA_POS 2
+#endif
+
+// Use SPI mode 2, capture on falling edge. First bit is not valid data, so have to read 25 bits to get a full SSI frame.
+// SSI frame is 1 bit ignore, 14 bits angle, 4 bit status and 6 bit CRC.
+static SPISettings MT6701SSISettings(1000000, MT6701_BITORDER, SPI_MODE2); // @suppress("Invalid arguments")
+
+
+
+class MagneticSensorMT6701SSI : public Sensor {
+public:
+    MagneticSensorMT6701SSI(int nCS = -1, SPISettings settings = MT6701SSISettings);
+    virtual ~MagneticSensorMT6701SSI();
+
+    virtual void init(SPIClass* _spi = &SPI);
+
+    float getSensorAngle() override; // angle in radians, return current value
+
+protected:
+    uint16_t readRawAngleSSI();
+
+    SPISettings settings;
+    SPIClass* spi;
+	int nCS = -1;
+};
+
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/mt6701/README.md

@@ -0,0 +1,82 @@
+# MT6701 SimpleFOC driver
+
+:warning: work in progress... SSI driver is working. I2C not yet complete.
+
+Due to the peculiarities of its interfaces, this very versatile sensor is not directly supported by SimpleFOC's SPI or I2C magnetic sensor implementations. This folder contains dedicated SimpleFOC sensor drivers for the MT6701 for I2C and SSI.
+
+Note: the ABZ, UVW and Analog outputs of this sensor are supported by the standard SimpleFOC encoder, hall-sensor or analog sensor classes respectively.
+
+:warning: Note: the I2C output of this sensor is probably too slow for high performance motor control, but could be useful to program the sensor IC, and to read the absolute angle values for intializing ABZ or UVW modes.
+
+## Hardware setup
+
+For I2C, connect the sensor as normal for I2C, using the SDA and SCL lines. 
+
+:warning: Note: to program the sensor via I2C, it has to be operated at 5V.
+
+For SSI, connect the CSN line to the nCS output of your MCU, the CLK line to the SCLK output of the MCU, and the DO line to the CIPO (MISO) input of the MCU.
+
+In my tests, the sensor was not able to work correctly together with other SPI devices on the same bus, but your experience might differ from mine.
+
+## Software setup
+
+### SSI code sample
+
+```c++
+#include "Arduino.h"
+#include "SPI.h"
+#include "SimpleFOC.h"
+#include "SimpleFOCDrivers.h"
+#include "encoders/MT6701/MagneticSensorMT6701SSI.h"
+
+
+#define SENSOR1_CS 5 // some digital pin that you're using as the nCS pin
+MagneticSensorMT6701SSI sensor1(SENSOR1_CS);
+
+
+void setup() {
+    sensor1.init();
+}
+```
+
+To use a custom SPI bus:
+
+```c++
+void setup() {
+    sensor1.init(&SPI2);
+}
+```
+
+
+
+### I2C code sample
+
+I2C usage is quite simple:
+
+```c++
+#include "Arduino.h"
+#include "Wire.h"
+#include "SimpleFOC.h"
+#include "SimpleFOCDrivers.h"
+#include "encoders/MT6701/MagneticSensorMT6701I2C.h"
+
+MagneticSensorMT6701I2C sensor1();
+
+
+void setup() {
+    sensor1.init();
+}
+
+```
+
+If you've programmed a different I2C address or want to use a different I2C bus you can:
+
+```c++
+#define I2C_ADDR 0x70
+MagneticSensorMT6701I2C sensor1(I2C_ADDR);
+
+
+void setup() {
+    sensor1.init(&Wire2);
+}
+```

firmware/lib/Arduino-FOC-drivers/src/encoders/mt6816/MT6816.cpp

@@ -0,0 +1,56 @@
+
+#include "MT6816.h"
+
+MT6816::MT6816(SPISettings settings, int nCS) : settings(settings), nCS(nCS) {
+};
+
+MT6816::~MT6816() {
+};
+
+void MT6816::init(SPIClass* _spi) {
+    spi = _spi;
+    if (nCS >= 0) {
+        pinMode(nCS, OUTPUT);
+        digitalWrite(nCS, HIGH);
+        spi->begin();
+    }
+};
+
+uint16_t MT6816::readRawAngle()  {
+    uint16_t angle_data = 0;
+    angle_data = spi_transfer16(MT6816_READ_REG_03) << 8;
+    angle_data |= spi_transfer16(MT6816_READ_REG_04);
+
+    if ((angle_data & MT6816_NO_MAGNET_WARNING_BIT) == MT6816_NO_MAGNET_WARNING_BIT) {
+        this->no_magnetic_reading = true;
+    } else {
+        this->no_magnetic_reading = false;
+    }
+
+    if (!this->parityCheck(angle_data)) {
+        return 0;
+    }
+
+    return (angle_data >> 2);
+}
+
+bool MT6816::parityCheck(uint16_t data) {
+    data ^= data >> 8;
+    data ^= data >> 4;
+    data ^= data >> 2;
+    data ^= data >> 1;
+
+    return (~data) & 1;
+}
+
+uint16_t MT6816::spi_transfer16(uint16_t outdata) {
+    if (nCS>=0)
+        digitalWrite(nCS, 0);
+    spi->beginTransaction(settings);
+    uint16_t result = spi->transfer16(outdata);
+    spi->endTransaction();
+    if (nCS>=0)
+        digitalWrite(nCS, 1);
+
+    return result;
+}

firmware/lib/Arduino-FOC-drivers/src/encoders/mt6816/MT6816.h

@@ -0,0 +1,40 @@
+
+#ifndef MT6816_H
+#define MT6816_H
+
+#include "Arduino.h"
+#include "SPI.h"
+
+#define _2PI                                6.28318530718f
+#define MT6816_CPR                          16384.0f
+
+#define MT6816_READ_REG_03                  0x8300
+#define MT6816_READ_REG_04                  0x8400
+
+#define MT6816_NO_MAGNET_WARNING_BIT        0x0002
+#define MT6816_BITORDER                     MSBFIRST
+
+
+static SPISettings MT6816SPISettings(1000000, MT6816_BITORDER, SPI_MODE3);
+
+class MT6816 {
+public:
+    MT6816(SPISettings settings = MT6816SPISettings, int nCS = -1);
+    virtual ~MT6816();
+
+    virtual void init(SPIClass* _spi = &SPI);
+    uint16_t readRawAngle();
+    bool isNoMagneticReading() {
+        return no_magnetic_reading;
+    }
+
+private:
+    bool parityCheck(uint16_t data);
+    uint16_t spi_transfer16(uint16_t outdata);
+    SPIClass* spi;
+    SPISettings settings;
+    bool no_magnetic_reading = false;
+    int nCS = -1;
+};
+
+#endif /* MT6816_H */

firmware/lib/Arduino-FOC-drivers/src/encoders/mt6816/MagneticSensorMT6816.cpp

@@ -0,0 +1,26 @@
+
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+#include "MagneticSensorMT6816.h"
+
+
+MagneticSensorMT6816::MagneticSensorMT6816(int nCS, SPISettings settings) : MT6816(settings, nCS) {
+}
+
+MagneticSensorMT6816::~MagneticSensorMT6816(){
+}
+
+void MagneticSensorMT6816::init(SPIClass* _spi) {
+    this->MT6816::init(_spi);
+    this->Sensor::init();
+}
+
+float MagneticSensorMT6816::getSensorAngle() {
+    uint16_t raw_angle_data = readRawAngle();
+
+    if (this->MT6816::isNoMagneticReading()) {
+        return 0;
+    }
+
+    return static_cast<float>(raw_angle_data) / MT6816_CPR * _2PI;
+}

firmware/lib/Arduino-FOC-drivers/src/encoders/mt6816/MagneticSensorMT6816.h

@@ -0,0 +1,17 @@
+
+#ifndef MAGNETICSENSOR_MT6816_H
+#define MAGNETICSENSOR_MT6816_H
+
+#include "common/base_classes/Sensor.h"
+#include "MT6816.h"
+
+class MagneticSensorMT6816 : public Sensor, public MT6816 {
+public:
+    MagneticSensorMT6816(int nCS = -1, SPISettings settings = MT6816SPISettings);
+    virtual ~MagneticSensorMT6816();
+
+    virtual float getSensorAngle() override;
+    virtual void init(SPIClass* _spi = &SPI);
+};
+
+#endif /* MAGNETICSENSOR_MT6816_H */

firmware/lib/Arduino-FOC-drivers/src/encoders/mt6835/MT6835.cpp

@@ -0,0 +1,291 @@
+
+#include "./MT6835.h"
+#include "common/foc_utils.h"
+
+
+MT6835::MT6835(SPISettings settings, int nCS) : settings(settings), nCS(nCS) {
+    // nix
+};
+
+MT6835::~MT6835() {
+    // nix
+};
+
+
+
+void MT6835::init(SPIClass* _spi) {
+    spi = _spi;
+    if (nCS >= 0) {
+        pinMode(nCS, OUTPUT);
+        digitalWrite(nCS, HIGH);
+    }
+    spi->begin();
+};
+
+
+
+
+float MT6835::getCurrentAngle(){
+    return readRawAngle21() / (float)MT6835_CPR * _2PI;
+};
+
+
+
+uint32_t MT6835::readRawAngle21(){
+    uint8_t data[6]; // transact 48 bits
+    data[0] = (MT6835_OP_ANGLE<<4);
+    data[1] = MT6835_REG_ANGLE1;
+    data[2] = 0;
+    data[3] = 0;
+    data[4] = 0;
+    data[5] = 0;
+    spi->beginTransaction(settings);
+    if (nCS >= 0)
+        digitalWrite(nCS, LOW);
+    spi->transfer(data, 6);
+    if (nCS >= 0)
+        digitalWrite(nCS, HIGH);
+    spi->endTransaction();
+    laststatus = data[4]&0x07;
+    return (data[2] << 13) | (data[3] << 5) | (data[4] >> 3);
+};
+
+
+uint8_t MT6835::getStatus(){
+    return laststatus;
+};
+
+uint8_t MT6835::getCalibrationStatus(){
+    uint8_t data[3] = {0};
+    data[0] = 0x31; // read register 0b0011, calibration register 0x113
+    data[1] = 0x13;
+    spi->beginTransaction(settings);
+    if(nCS >= 0)
+        digitalWrite(nCS, LOW);
+    spi->transfer(data, 3);    
+    if(nCS >= 0)
+        digitalWrite(nCS, HIGH);
+    spi->endTransaction();
+    return data[2] >> 6; // Calibration state held in bits 6,7
+}
+
+bool MT6835::setZeroFromCurrentPosition(){
+    MT6835Command cmd;
+    cmd.cmd = MT6835_OP_ZERO;
+    cmd.addr = 0x000;
+    transfer24(&cmd);
+    return cmd.data == MT6835_WRITE_ACK;
+};
+
+
+/**
+ * Wait 6s after calling this method
+ */
+bool MT6835::writeEEPROM(){
+    delay(1); // wait at least 1ms
+    MT6835Command cmd;
+    cmd.cmd = MT6835_OP_PROG;
+    cmd.addr = 0x000;
+    transfer24(&cmd);
+    return cmd.data == MT6835_WRITE_ACK;
+};
+
+
+
+
+
+uint8_t MT6835::getBandwidth(){
+    MT6835Options5 opts = { .reg = readRegister(MT6835_REG_OPTS5) };
+    return opts.bw;
+};
+void MT6835::setBandwidth(uint8_t bw){
+    MT6835Options5 opts = { .reg = readRegister(MT6835_REG_OPTS5) };
+    opts.bw = bw;
+    writeRegister(MT6835_REG_OPTS5, opts.reg);
+};
+
+uint8_t MT6835::getHysteresis(){
+    MT6835Options3 opts = { .reg = getOptions3().reg };
+    return opts.hyst;
+};
+void MT6835::setHysteresis(uint8_t hyst){
+    MT6835Options3 opts = { .reg = getOptions3().reg };
+    opts.hyst = hyst;
+    setOptions3(opts);
+};
+
+uint8_t MT6835::getRotationDirection(){
+    MT6835Options3 opts = { .reg = getOptions3().reg };
+    return opts.rot_dir;
+};
+void MT6835::setRotationDirection(uint8_t dir){
+    MT6835Options3 opts = { .reg = getOptions3().reg };
+    opts.rot_dir = dir;
+    setOptions3(opts);
+};
+
+
+uint16_t MT6835::getABZResolution(){
+    uint8_t hi = readRegister(MT6835_REG_ABZ_RES1);
+    MT6835ABZRes lo = {
+			.reg = readRegister(MT6835_REG_ABZ_RES2)
+	};
+    return (hi << 6) | lo.abz_res_low;
+};
+void MT6835::setABZResolution(uint16_t res){
+    uint8_t hi = (res >> 6);
+    MT6835ABZRes lo = {
+			.reg = readRegister(MT6835_REG_ABZ_RES2)
+	};
+    lo.abz_res_low = (res & 0x3F);
+    writeRegister(MT6835_REG_ABZ_RES1, hi);
+    writeRegister(MT6835_REG_ABZ_RES2, lo.reg);
+};
+
+
+
+bool MT6835::isABZEnabled(){
+    MT6835ABZRes lo = {
+			.reg = readRegister(MT6835_REG_ABZ_RES2)
+	};
+    return lo.abz_off==0;
+};
+void MT6835::setABZEnabled(bool enabled){
+    MT6835ABZRes lo = {
+			.reg = readRegister(MT6835_REG_ABZ_RES2)
+	};
+    lo.abz_off = enabled?0:1;
+    writeRegister(MT6835_REG_ABZ_RES2, lo.reg);
+};
+
+
+
+bool MT6835::isABSwapped(){
+    MT6835ABZRes lo = {
+			.reg = readRegister(MT6835_REG_ABZ_RES2)
+	};
+    return lo.ab_swap==1;
+};
+void MT6835::setABSwapped(bool swapped){
+    MT6835ABZRes lo = {
+			.reg = readRegister(MT6835_REG_ABZ_RES2)
+	};
+    lo.ab_swap = swapped?1:0;
+    writeRegister(MT6835_REG_ABZ_RES2, lo.reg);
+};
+
+
+
+uint16_t MT6835::getZeroPosition(){
+    uint8_t hi = readRegister(MT6835_REG_ZERO1);
+    MT6835Options0 lo = {
+            .reg = readRegister(MT6835_REG_ZERO2)
+    };
+    return (hi << 4) | lo.zero_pos_low;
+};
+void MT6835::setZeroPosition(uint16_t pos){
+    uint8_t hi = (pos >> 4);
+    MT6835Options0 lo = {
+            .reg = readRegister(MT6835_REG_ZERO2)
+    };
+    lo.zero_pos_low = pos & 0x0F;
+    writeRegister(MT6835_REG_ZERO1, hi);
+    writeRegister(MT6835_REG_ZERO2, lo.reg);
+};
+
+
+
+MT6835Options1 MT6835::getOptions1(){
+    MT6835Options1 result = {
+			.reg = readRegister(MT6835_REG_OPTS1)
+	};
+    return result;
+};
+void MT6835::setOptions1(MT6835Options1 opts){
+    writeRegister(MT6835_REG_OPTS1, opts.reg);
+};
+
+
+
+MT6835Options2 MT6835::getOptions2(){
+    MT6835Options2 result = {
+			.reg = readRegister(MT6835_REG_OPTS2)
+	};
+    return result;
+};
+void MT6835::setOptions2(MT6835Options2 opts){
+    MT6835Options2 val = getOptions2();
+    val.nlc_en = opts.nlc_en;
+    val.pwm_fq = opts.pwm_fq;
+    val.pwm_pol = opts.pwm_pol;
+    val.pwm_sel = opts.pwm_sel;
+    writeRegister(MT6835_REG_OPTS2, val.reg);
+};
+
+
+
+MT6835Options3 MT6835::getOptions3(){
+    MT6835Options3 result = {
+			.reg = readRegister(MT6835_REG_OPTS3)
+	};
+    return result;    
+};
+void MT6835::setOptions3(MT6835Options3 opts){
+    MT6835Options3 val = getOptions3();
+    val.rot_dir = opts.rot_dir;
+    val.hyst = opts.hyst;
+    writeRegister(MT6835_REG_OPTS3, val.reg);
+};
+
+
+
+MT6835Options4 MT6835::getOptions4(){
+    MT6835Options4 result = {
+			.reg = readRegister(MT6835_REG_OPTS4)
+	};
+    return result;
+};
+void MT6835::setOptions4(MT6835Options4 opts){
+    MT6835Options4 val = getOptions4();
+    val.gpio_ds = opts.gpio_ds;
+    val.autocal_freq = opts.autocal_freq;
+    writeRegister(MT6835_REG_OPTS4, val.reg);
+};
+
+
+
+uint32_t swap_bytes(uint32_t net)
+{
+    return __builtin_bswap32(net);
+}
+
+
+
+
+
+void MT6835::transfer24(MT6835Command* outValue) {
+    uint32_t buff = swap_bytes(outValue->val);
+    spi->beginTransaction(settings);
+    if (nCS >= 0)
+        digitalWrite(nCS, LOW);
+    spi->transfer(&buff, 3);
+    if (nCS >= 0)
+        digitalWrite(nCS, HIGH);
+    spi->endTransaction();
+    outValue->val = swap_bytes(buff);
+};
+uint8_t MT6835::readRegister(uint16_t reg) {
+    MT6835Command cmd;
+    cmd.cmd = MT6835_OP_READ;
+    cmd.addr = reg;
+    transfer24(&cmd);
+    return cmd.data;
+};
+bool MT6835::writeRegister(uint16_t reg, uint8_t value) {
+    MT6835Command cmd;
+    cmd.cmd = MT6835_OP_WRITE;
+    cmd.addr = reg;
+    cmd.data = value;
+    transfer24(&cmd);
+    return cmd.data == MT6835_WRITE_ACK;
+};

firmware/lib/Arduino-FOC-drivers/src/encoders/mt6835/MT6835.h

@@ -0,0 +1,217 @@
+
+
+#pragma once
+
+
+
+#include "Arduino.h"
+#include "SPI.h"
+
+#define MT6835_OP_READ  0b0011
+#define MT6835_OP_WRITE 0b0110
+#define MT6835_OP_PROG  0b1100
+#define MT6835_OP_ZERO  0b0101
+#define MT6835_OP_ANGLE 0b1010
+
+#define MT6835_CMD_MASK  0b111100000000000000000000
+#define MT6835_ADDR_MASK 0b000011111111111100000000
+#define MT6835_DATA_MASK 0b000000000000000011111111
+
+#define MT6835_CPR 2097152
+
+#define MT6835_STATUS_OVERSPEED 0x01
+#define MT6835_STATUS_WEAKFIELD 0x02
+#define MT6835_STATUS_UNDERVOLT 0x04
+
+#define MT6835_WRITE_ACK 0x55
+
+#define MT6835_REG_USERID 0x001
+
+#define MT6835_REG_ANGLE1 0x003
+#define MT6835_REG_ANGLE2 0x004
+#define MT6835_REG_ANGLE3 0x005
+#define MT6835_REG_ANGLE4 0x006
+
+#define MT6835_REG_ABZ_RES1 0x007
+#define MT6835_REG_ABZ_RES2 0x008
+
+#define MT6835_REG_ZERO1 0x009
+#define MT6835_REG_ZERO2 0x00A
+
+#define MT6835_REG_OPTS0 0x00A
+#define MT6835_REG_OPTS1 0x00B
+#define MT6835_REG_OPTS2 0x00C
+#define MT6835_REG_OPTS3 0x00D
+#define MT6835_REG_OPTS4 0x00E
+#define MT6835_REG_OPTS5 0x011
+
+// NLC table, 192 bytes
+#define MT6835_REG_NLC_BASE 0x013
+
+#define MT6835_REG_CAL_STATUS 0x113
+
+
+union MT6835ABZRes {
+	struct {
+		uint8_t ab_swap:1;
+		uint8_t abz_off:1;
+		uint8_t abz_res_low:6;
+	};
+	uint8_t reg;
+};
+
+
+
+union MT6835Options0 {
+	struct {
+		uint8_t z_pul_wid:3;
+		uint8_t z_edge:1;
+		uint8_t zero_pos_low:4;
+	};
+	uint8_t reg;
+};
+
+
+
+union MT6835Options1 {
+	struct {
+		uint8_t uvw_res:4;
+		uint8_t uvw_off:1;
+		uint8_t uvw_mux:1;
+		uint8_t z_phase:2;
+	};
+	uint8_t reg;
+};
+
+
+
+union MT6835Options2 {
+	struct {
+		uint8_t pwm_sel:3;
+		uint8_t pwm_pol:1;
+		uint8_t pwm_fq:1;
+		uint8_t nlc_en:1;
+		uint8_t reserved:2;
+	};
+	uint8_t reg;
+};
+
+
+
+union MT6835Options3 {
+	struct {
+		uint8_t hyst:3;
+		uint8_t rot_dir:1;
+		uint8_t reserved:4;
+	};
+	uint8_t reg;
+};
+
+
+
+union MT6835Options4 {
+	struct {
+		uint8_t reserved:4;
+		uint8_t autocal_freq:3;
+		uint8_t gpio_ds:1;
+	};
+	uint8_t reg;
+};
+
+
+
+union MT6835Options5 {
+	struct {
+		uint8_t bw:3;
+		uint8_t reserved:5;
+	};
+	uint8_t reg;
+};
+
+
+
+
+union MT6835Command {
+	struct {
+		uint32_t unused:8;
+		uint32_t data:8;
+		uint32_t addr:12;
+		uint32_t cmd:4;
+	};
+	uint32_t val;
+};
+
+
+
+
+#define MT6835_BITORDER MSBFIRST
+
+static SPISettings MT6835SPISettings(1000000, MT6835_BITORDER, SPI_MODE3); // @suppress("Invalid arguments")
+
+
+
+
+class MT6835 {
+public:
+    MT6835(SPISettings settings = MT6835SPISettings, int nCS = -1);
+    virtual ~MT6835();
+
+    virtual void init(SPIClass* _spi = &SPI);
+
+
+	float getCurrentAngle(); // angle in radians, return current value
+
+	uint32_t readRawAngle21(); // up to 21bit precision angle value
+
+    
+    uint8_t getBandwidth();
+    void setBandwidth(uint8_t bw);
+
+    uint8_t getHysteresis();
+    void setHysteresis(uint8_t hyst);
+
+    uint8_t getRotationDirection();
+    void setRotationDirection(uint8_t dir);
+
+    uint16_t getABZResolution();
+    void setABZResolution(uint16_t res);
+
+    bool isABZEnabled();
+    void setABZEnabled(bool enabled);
+
+    bool isABSwapped();
+    void setABSwapped(bool swapped);
+
+    uint16_t getZeroPosition();
+    void setZeroPosition(uint16_t pos);
+
+    MT6835Options1 getOptions1();
+    void setOptions1(MT6835Options1 opts);
+
+    MT6835Options2 getOptions2();
+    void setOptions2(MT6835Options2 opts);
+
+    MT6835Options3 getOptions3();
+    void setOptions3(MT6835Options3 opts);
+
+    MT6835Options4 getOptions4();
+    void setOptions4(MT6835Options4 opts);
+
+	uint8_t getStatus();
+
+	uint8_t getCalibrationStatus();
+
+	bool setZeroFromCurrentPosition();
+	bool writeEEPROM(); // wait 6s after calling this method
+
+private:
+	SPIClass* spi;
+	SPISettings settings;
+	int nCS = -1;
+	uint8_t laststatus = 0;
+
+    void transfer24(MT6835Command* outValue);
+    uint8_t readRegister(uint16_t reg);
+    bool writeRegister(uint16_t reg, uint8_t value);
+
+};

firmware/lib/Arduino-FOC-drivers/src/encoders/mt6835/MagneticSensorMT6835.cpp

@@ -0,0 +1,24 @@
+
+#include "MagneticSensorMT6835.h"
+
+
+MagneticSensorMT6835::MagneticSensorMT6835(int nCS, SPISettings settings) : Sensor(), MT6835(settings, nCS) {
+    // nix
+};
+
+
+MagneticSensorMT6835::~MagneticSensorMT6835() {
+    // nix
+};
+
+
+float MagneticSensorMT6835::getSensorAngle() {
+    return getCurrentAngle();
+};
+
+
+void MagneticSensorMT6835::init(SPIClass* _spi) {
+    this->MT6835::init(_spi);
+    this->Sensor::init();
+};
+

firmware/lib/Arduino-FOC-drivers/src/encoders/mt6835/MagneticSensorMT6835.h

@@ -0,0 +1,17 @@
+
+#pragma once
+
+#include "common/base_classes/Sensor.h"
+#include "./MT6835.h"
+
+class MagneticSensorMT6835 : public Sensor, public MT6835 {
+public:
+	MagneticSensorMT6835(int nCS = -1, SPISettings settings = MT6835SPISettings);
+	virtual ~MagneticSensorMT6835();
+
+    virtual float getSensorAngle() override;
+
+	virtual void init(SPIClass* _spi = &SPI);
+};
+
+

firmware/lib/Arduino-FOC-drivers/src/encoders/mt6835/README.md

@@ -0,0 +1,64 @@
+# MT6835 SimpleFOC driver
+
+Driver for the MagnTek MT6835 precision magnetic rotary encoder.
+
+This sensor features support for up to 21 bit resolution (!) and speeds up to 120,000RPM. While its full precision requires calibration using an external calibration system, it is impressively precise even in uncalibrated state, and it offers an internal self-calibration mode whose precision is somewhere between the other two.
+
+It has ABZ, UVW and SPI interfaces, and this driver is for its SPI inteface. You can use its ABZ interface with our Encoder classes, but due to the high resolution the interrupt-based Encoder might cause high MCU load. You can use its UVW interface with our Hall Sensor classes.
+
+# Hardware setup
+
+Connect the sensor to an SPI bus on your MCU. Pay attention to the voltage levels needed by your PCBs. The nCS line should have a pull-up resistor.
+
+# Software setup
+
+Usage example:
+
+```c++
+#include <Arduino.h>
+
+#include "SimpleFOC.h"
+#include "SimpleFOCDrivers.h"
+
+#include "encoders/mt6835/MagneticSensorMT6835.h"
+
+#define SENSOR_nCS PB6
+
+SPISettings myMT6835SPISettings(1000000, MT6835_BITORDER, SPI_MODE3);
+MagneticSensorMT6835 sensor = MagneticSensorMT6835(SENSOR_nCS, myMT6835SPISettings);
+
+long ts;
+
+void setup() {
+    sensor.init();
+    ts = millis();
+}
+
+void loop() {
+    sensor.update();
+    long now = millis();
+    if (now - ts > 1000) {
+        ts = now;
+        SimpleFOCDebug::print("A: ");
+        SimpleFOCDebug::print(sensor.getAngle());
+        SimpleFOCDebug::print(" V: ");
+        SimpleFOCDebug::println(sensor.getVelocity());
+    }
+    delay(10);
+}
+
+```
+
+Set the zero position:
+
+```c++
+uint16_t pos = sensor.getZeroPosition();    // current value
+sensor.setZeroFromCurrentPosition();        // set zero to current position
+```
+
+Set the ABZ resolution (needed if you want to use ABZ as the default is 1):
+
+```c++
+sensor.setABZResolution(2048);
+```
+

firmware/lib/Arduino-FOC-drivers/src/encoders/sc60228/MagneticSensorSC60228.cpp

@@ -0,0 +1,24 @@
+
+#include "./MagneticSensorSC60228.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+
+MagneticSensorSC60228::MagneticSensorSC60228(int nCS, SPISettings settings) : SC60228(settings, nCS){
+    // nix
+};
+MagneticSensorSC60228::~MagneticSensorSC60228(){ };
+
+
+
+float MagneticSensorSC60228::getSensorAngle(){
+    SC60228Angle angle_data = readRawAngle();
+    float result = ( angle_data.angle / (float)SC60228_CPR ) * _2PI;
+    return result;
+};
+
+
+
+void MagneticSensorSC60228::init(SPIClass* _spi){
+    this->SC60228::init(_spi);
+    this->Sensor::init();
+};

firmware/lib/Arduino-FOC-drivers/src/encoders/sc60228/MagneticSensorSC60228.h

@@ -0,0 +1,20 @@
+#ifndef __MAGNETICSENSORSC60228_H__
+#define __MAGNETICSENSORSC60228_H__
+
+
+#include "common/base_classes/Sensor.h"
+#include "./SC60228.h"
+
+
+class MagneticSensorSC60228 : public Sensor, public SC60228 {
+public:
+	MagneticSensorSC60228(int nCS = -1, SPISettings settings = SC60228SPISettings);
+	virtual ~MagneticSensorSC60228();
+
+    virtual float getSensorAngle() override;
+
+	virtual void init(SPIClass* _spi = &SPI) override;
+};
+
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/sc60228/README.md

@@ -0,0 +1,47 @@
+# SC60228 SimpleFOC driver
+
+Driver for the Semiment 12bit magnetic encoder IC SC60288. According to specs it should support 12 bit accuracy, 10 bit effective accuracy and up to 20kRPM.
+
+Link to [Datasheet](https://semiment.com/wp-content/uploads/2021/04/SC60228_EN-VA1.0.pdf)
+
+The encoder and this driver were tested with a small gimbal motor and MKR1000 (SAMD21 MCU).
+
+## Hardware setup
+
+Connect as per normal for your SPI bus. No special hardware setup is needed to use this driver.
+
+## Software setup
+
+Its actually easier to use than the standard SPI sensor class, because it is less generic:
+
+```c++
+#include "Arduino.h"
+#include "Wire.h"
+#include "SPI.h"
+#include "SimpleFOC.h"
+#include "SimpleFOCDrivers.h"
+#include "encoders/sc60228/MagneticSensorSC60228.h"
+
+#define SENSOR1_CS 5 // some digital pin that you're using as the nCS pin
+MagneticSensorSC60228 sensor1(SENSOR1_CS);
+
+
+void setup() {
+    sensor1.init();
+}
+```
+
+Set some SPI options:
+
+```c++
+SPISettings mySPISettings(1000000, SC60228_BITORDER, SPI_MODE0); // lower speed to 1Mhz
+MagneticSensorSC60228 sensor1(SENSOR1_CS, mySPISettings);
+```
+
+Use another SPI bus:
+
+```c++
+void setup() {
+    sensor1.init(SPI2);
+}
+```

firmware/lib/Arduino-FOC-drivers/src/encoders/sc60228/SC60228.cpp

@@ -0,0 +1,57 @@
+
+#include "./SC60228.h"
+
+
+SC60228::SC60228(SPISettings settings, int nCS) : settings(settings), nCS(nCS) {
+    // nix
+};
+
+SC60228::~SC60228() { };
+
+void SC60228::init(SPIClass* _spi) {
+    spi = _spi;
+	if (nCS>=0)
+		pinMode(nCS, OUTPUT);
+	digitalWrite(nCS, HIGH);
+	spi->begin();
+	readRawAngle();
+};
+
+
+
+SC60228Angle SC60228::readRawAngle(){
+    SC60228Angle result;
+    result.reg = spi_transfer16(0x0000);
+    errorflag = (result.err==1);
+    // TODO check parity
+    // Serial.print("0x");
+    // Serial.print(result.angle, HEX);
+    // Serial.print(" - 0x");
+    // Serial.println(result.reg, HEX);
+    return result;
+};
+
+
+
+bool SC60228::isError() {
+    return errorflag;
+};
+
+
+
+uint16_t SC60228::spi_transfer16(uint16_t outdata){
+    uint16_t result;
+    if (nCS>=0)
+        digitalWrite(nCS, LOW);
+    // min delay here: 250ns
+    spi->beginTransaction(settings);
+    result = spi->transfer16(outdata);
+    // min delay here: clock period / 2
+    spi->endTransaction();
+    if (nCS>=0)
+        digitalWrite(nCS, HIGH);
+    // min delay until next read: 250ns
+    return result;
+};
+
+

firmware/lib/Arduino-FOC-drivers/src/encoders/sc60228/SC60228.h

@@ -0,0 +1,50 @@
+
+#ifndef __SC60228_H__
+#define __SC60228_H__
+
+#include "Arduino.h"
+#include "SPI.h"
+
+
+typedef union {
+    struct {
+        uint16_t parity:1;
+        uint16_t df2:1;
+        uint16_t df1:1;
+        uint16_t err:1;
+        uint16_t angle:12;
+    };
+    uint16_t reg;
+} SC60228Angle;
+
+
+#define SC60228_CPR 4096
+#define SC60228_BITORDER MSBFIRST
+
+static SPISettings SC60228SPISettings(8000000, SC60228_BITORDER, SPI_MODE0); // @suppress("Invalid arguments")
+
+
+
+class SC60228 {
+public:
+    SC60228(SPISettings settings = SC60228SPISettings, int nCS = -1);
+	virtual ~SC60228();
+
+	virtual void init(SPIClass* _spi = &SPI);
+
+    SC60228Angle readRawAngle(); // 12bit angle value + error flag
+    bool isError(); // true if the last call to readRawAngle() returned an error
+
+protected:
+	uint16_t spi_transfer16(uint16_t outdata);
+
+	SPIClass* spi;
+	SPISettings settings;
+	bool errorflag = false;
+	int nCS = -1;
+};
+
+
+
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/smoothing/README.md

@@ -0,0 +1,69 @@
+# Smoothing Sensor
+
+by [@dekutree64](https://github.com/dekutree64)
+
+A SimpleFOC Sensor wrapper implementation which adds angle extrapolation
+
+Please also see our [forum thread](https://community.simplefoc.com/t/smoothingsensor-experimental-sensor-angle-extrapoltion/3105) on this topic.
+
+
+SmoothingSensor is a wrapper class which is inserted inbetween a sensor and motor to provide better quality angle readings from sensors that are low resolution or are slow to communicate. It provides no improvement for sensors that are high resolution and quick to update. It uses the timestamp of the last angle reading from the sensor and the low-pass filtered velocity from the motor to predict what the angle should be at the current time.
+
+
+## Hardware setup
+
+Connect your sensor as usual. Make sure the sensor is working 'normally' i.e. without smoothing first. Once things are working and tuned without smoothing, you can add the SmoothingSensor to see if you get an improvement.
+
+
+## Softwate setup
+
+The SmoothingSensor acts as a wrapper to the actual sensor class. When creating the SmoothingSensor object, provide the real sensor to the constructor of SmoothingSensor. Initialize the real sensor instance as normal. Then link the SmoothingSensor to the motor and call motor.initFOC().
+
+
+```c++
+// BLDC motor & driver instance
+BLDCMotor motor = BLDCMotor(11);
+BLDCDriver3PWM driver = BLDCDriver3PWM(PB4,PC7,PB10,PA9);
+// real sensor instance
+MagneticSensorPWM sensor = MagneticSensorPWM(2, 4, 904);
+// instantiate the smoothing sensor, providing the real sensor as a constructor argument
+SmoothingSensor smooth = SmoothingSensor(sensor, motor);
+
+void doPWM(){sensor.handlePWM();}
+
+void setup() {
+  sensor.init();
+  sensor.enableInterrupt(doPWM);
+  // Link motor to sensor
+  motor.linkSensor(&smooth);
+  // power supply voltage
+  driver.voltage_power_supply = 20;
+  driver.init();
+  motor.linkDriver(&driver);
+  // aligning voltage 
+  motor.voltage_sensor_align = 8;
+  motor.voltage_limit = 20;
+  // set motion control loop to be used
+  motor.controller = MotionControlType::torque;
+
+  // use monitoring with serial 
+  Serial.begin(115200);
+  // comment out if not needed
+  motor.useMonitoring(Serial);
+  motor.monitor_variables =  _MON_VEL; 
+  motor.monitor_downsample = 10; // default 10
+
+  // initialize motor
+  motor.init();
+  motor.initFOC();
+}
+```
+
+
+## Roadmap
+
+Possible future improvements we've thought about:
+
+- Add extrapolation of acceleration as well
+- Switch to open loop control at very low speed with hall sensors, which otherwise move in steps even with smoothing.
+

firmware/lib/Arduino-FOC-drivers/src/encoders/smoothing/SmoothingSensor.cpp

@@ -0,0 +1,57 @@
+#include "SmoothingSensor.h"
+#include "common/foc_utils.h"
+#include "common/time_utils.h"
+
+
+SmoothingSensor::SmoothingSensor(Sensor& s, const FOCMotor& m) : _wrapped(s), _motor(m)
+{
+}
+
+
+void SmoothingSensor::update() {
+  // Update sensor, with optional downsampling of update rate
+  if(sensor_cnt++ >= sensor_downsample) {
+    sensor_cnt = 0;
+    _wrapped.update();
+  }
+
+  // Copy state variables from the sensor
+  angle_prev = _wrapped.angle_prev;
+  angle_prev_ts = _wrapped.angle_prev_ts;
+  full_rotations = _wrapped.full_rotations;
+
+  // Perform angle prediction, using low-pass filtered velocity. But don't advance more than
+  // pi/3 (equivalent to one step of block commutation) from the last true angle reading.
+  float dt = (_micros() - angle_prev_ts) * 1e-6f;
+  angle_prev += _motor.sensor_direction * _constrain(_motor.shaft_velocity * dt, -_PI_3 / _motor.pole_pairs, _PI_3 / _motor.pole_pairs);
+
+  // Apply phase correction if needed
+  if (phase_correction != 0) {
+    if (_motor.shaft_velocity < -0) angle_prev -= _motor.sensor_direction * phase_correction / _motor.pole_pairs;
+    else if (_motor.shaft_velocity > 0) angle_prev += _motor.sensor_direction * phase_correction / _motor.pole_pairs;
+  }
+
+  // Handle wraparound of the projected angle
+  if (angle_prev < 0) full_rotations -= 1, angle_prev += _2PI;
+  else if (angle_prev >= _2PI) full_rotations += 1, angle_prev -= _2PI;
+}
+
+
+float SmoothingSensor::getVelocity() {
+  return _wrapped.getVelocity();
+}
+
+
+int SmoothingSensor::needsSearch() {
+  return _wrapped.needsSearch();
+}
+
+
+float SmoothingSensor::getSensorAngle() {
+  return _wrapped.getSensorAngle();
+}
+
+
+void SmoothingSensor::init() {
+  _wrapped.init();
+}

firmware/lib/Arduino-FOC-drivers/src/encoders/smoothing/SmoothingSensor.h

@@ -0,0 +1,46 @@
+#ifndef SMOOTHING_SENSOR_H
+#define SMOOTHING_SENSOR_H
+
+#include "Arduino.h"
+#include "common/base_classes/FOCMotor.h"
+#include "common/base_classes/Sensor.h"
+
+/**
+  SmoothingSensor is a wrapper class which is inserted inbetween a sensor and motor to provide better
+  quality angle readings from sensors that are low resolution or are slow to communicate. It provides
+  no improvement for sensors that are high resolution and quick to update.
+  It uses the timestamp of the last angle reading from the sensor and the low-pass filtered velocity
+  from the motor to predict what the angle should be at the current time.
+*/
+
+class SmoothingSensor : public Sensor
+{
+  public:
+    /**
+    SmoothingSensor class constructor
+    @param s  Wrapped sensor
+    @param m  Motor that the SmoothingSensor will be linked to
+    */
+    SmoothingSensor(Sensor& s, const FOCMotor& m);
+
+    void update() override;
+    float getVelocity() override;
+    int needsSearch() override;
+
+    // For sensors with slow communication, use these to poll less often
+    unsigned int sensor_downsample = 0; // parameter defining the ratio of downsampling for sensor update
+    unsigned int sensor_cnt = 0; // counting variable for downsampling
+
+    // For hall sensors, the predicted angle is always 0 to 60 electrical degrees ahead of where it would be without
+    // smoothing, so offset it backward by 30 degrees (set this to -PI_6) to get the average in phase with the rotor
+    float phase_correction = 0;
+
+  protected:
+    float getSensorAngle() override;
+    void init() override;
+
+    Sensor& _wrapped;
+    const FOCMotor& _motor;
+};
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/stm32hwencoder/README.md

@@ -0,0 +1,49 @@
+# SimpleFOC Driver for STM32 hardware encoder
+
+This encoder driver uses the STM32 timer hardware to track the A/B impulses, which is much more efficient (and will support higher speeds) than the generic interrupt-based solution used by the standard encoder driver.
+
+Big thank you to @conroy-cheers for originally contributing this code in pull request #114 to the simplefoc repository. Due its hardware-specific nature we moved the code to this drivers repository.
+
+## Warning
+
+This code has not been tested much! Use at your own risk, your milage may vary.
+I have tested on: STM32F401CC/TIM4-PB6,PB7
+
+## Hardware setup
+
+The ABI signals of your encoder will have to be connected to the STM32 MCU in such a way that all signals are connected to the same timer. Consult your STM32 chip's datasheet for more information on which pins connect to which timer.
+
+Not all of the timers support the encoder mode, so check the datasheet which timers can be used.
+
+An excellent option can be to use the STM32CubeIDE software's pin assignment view to quickly check which pins connect to which timer.
+
+Note that the index (I) pin is currently not used.
+
+
+## Software setup
+
+There is no need for interrupt functions or their configuration with this encoder class, so usage is quite simple:
+
+```c++
+#include "Arduino.h"
+#include "Wire.h"
+#include "SPI.h"
+#include "SimpleFOC.h"
+#include "SimpleFOCDrivers.h"
+#include "encoders/stm32hwencoder/STM32HWEncoder.h"
+
+
+#define ENCODER_PPR 500
+#define ENCODER_PIN_A PB6
+#define ENCODER_PIN_B PB7
+#define ENCODER_PIN_I PC13
+
+
+STM32HWEncoder encoder = STM32HWEncoder(ENCODER_PPR, ENCODER_PIN_A, ENCODER_PIN_B, ENCODER_PIN_I);
+
+
+void setup() {
+    encoder.init();
+}
+
+```

firmware/lib/Arduino-FOC-drivers/src/encoders/stm32hwencoder/STM32HWEncoder.cpp

@@ -0,0 +1,78 @@
+#include "STM32HWEncoder.h"
+
+#if defined(_STM32_DEF_)
+
+/*
+  HardwareEncoder(int cpr)
+*/
+STM32HWEncoder::STM32HWEncoder(unsigned int _ppr, int8_t pinA, int8_t pinB, int8_t pinI) {
+    cpr = _ppr * 4; // 4x for quadrature
+    _pinA = digitalPinToPinName(pinA);
+    _pinB = digitalPinToPinName(pinB);
+    _pinI = digitalPinToPinName(pinI);
+}
+
+/*
+  Shaft angle calculation
+*/
+float STM32HWEncoder::getSensorAngle() { 
+    return _2PI * encoder_handle.Instance->CNT / static_cast<float>(cpr);
+}
+
+// getter for index pin
+int STM32HWEncoder::needsSearch() { return false; }
+
+// private function used to determine if encoder has index
+int STM32HWEncoder::hasIndex() { return 0; }
+
+// encoder initialisation of the hardware pins
+void STM32HWEncoder::init() {
+    // GPIO configuration
+    TIM_TypeDef *InstanceA = (TIM_TypeDef *)pinmap_peripheral(_pinA, PinMap_TIM);
+    TIM_TypeDef *InstanceB = (TIM_TypeDef *)pinmap_peripheral(_pinB, PinMap_TIM);
+    if (InstanceA != InstanceB) {
+        initialized = false;
+        return;
+    }
+    pinmap_pinout(_pinA, PinMap_TIM);
+    pinmap_pinout(_pinB, PinMap_TIM);
+
+    // set up timer for encoder
+    encoder_handle.Init.Period = cpr - 1;
+    encoder_handle.Init.Prescaler = 0;
+    encoder_handle.Init.ClockDivision = 0;
+    encoder_handle.Init.CounterMode = TIM_COUNTERMODE_UP;
+    encoder_handle.Init.RepetitionCounter = 0;
+    encoder_handle.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+
+    TIM_Encoder_InitTypeDef encoder_config;
+
+    encoder_config.EncoderMode = TIM_ENCODERMODE_TI12;
+
+    encoder_config.IC1Polarity = TIM_ICPOLARITY_RISING;
+    encoder_config.IC1Selection = TIM_ICSELECTION_DIRECTTI;
+    encoder_config.IC1Prescaler = TIM_ICPSC_DIV1;
+    encoder_config.IC1Filter = 0;
+
+    encoder_config.IC2Polarity = TIM_ICPOLARITY_RISING;
+    encoder_config.IC2Selection = TIM_ICSELECTION_DIRECTTI;
+    encoder_config.IC2Prescaler = TIM_ICPSC_DIV1;
+    encoder_config.IC2Filter = 0;
+
+    encoder_handle.Instance = InstanceA; // e.g. TIM4;
+    enableTimerClock(&encoder_handle);
+
+    if (HAL_TIM_Encoder_Init(&encoder_handle, &encoder_config) != HAL_OK) {
+        initialized = false;
+        return;
+    }
+
+    if (HAL_TIM_Encoder_Start(&encoder_handle, TIM_CHANNEL_1) != HAL_OK) {
+        initialized = false;
+        return;
+    }
+
+    initialized = true;
+}
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/stm32hwencoder/STM32HWEncoder.h

@@ -0,0 +1,36 @@
+
+#pragma once
+
+#include <Arduino.h>
+
+
+#if defined(_STM32_DEF_)
+
+#include <HardwareTimer.h>
+#include "common/base_classes/Sensor.h"
+#include "common/foc_utils.h"
+
+class STM32HWEncoder : public Sensor {
+  public:
+    /**
+    Encoder class constructor
+    @param ppr  impulses per rotation  (cpr=ppr*4)
+    */
+    explicit STM32HWEncoder(unsigned int ppr, int8_t pinA, int8_t pinB, int8_t pinI=-1);
+
+    void init() override;
+    int needsSearch() override;
+    int hasIndex();  // !< function returning 1 if encoder has index pin and 0 if not.
+
+    bool initialized = false;
+    uint32_t cpr;  //!< encoder cpr number
+    PinName _pinA, _pinB, _pinI;
+    
+  protected:
+    float getSensorAngle() override;
+    
+    TIM_HandleTypeDef encoder_handle;
+
+};
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/stm32pwmsensor/README.md

@@ -0,0 +1,46 @@
+
+# STM32MagneticSensorPWM
+
+STM32 MCU specific PWM sensor class. This class uses the STM32's hardware timers to precisely capture the PWM input signal, and doesn't use interrupts or have MCU overhead.
+
+:warning: this code is not yet well tested.
+
+## Setup
+
+Please use an advanced control or general purpose timer pin on your STM32. Connect the sensor's PWM output to the MCU's input pin. Usually, pull-ups or pull-downs are not needed, but check your sensor's datasheet.
+
+:warning: only tested on 16 bit timers. Code changes may be needed to make it work on 32 but timers. Avoid using TIM2 and TIM5 unless you want to test it.
+
+The sensor needs the values `min_ticks` and `max-ticks` to be configured correctly to convert the PWM input into an angle. These values will depend on the sensor, but also on the MCU's timer clock speed.
+
+To print the current tick value, use:
+
+```
+sensor.getDutyCycleTicks();
+```
+
+By rotating the motor through several full turns while printing the ticks to the screen you will be able to determine the correct values empirically.
+
+## Usage
+
+```
+STM32MagneticSensorPWM sensor = STM32MagneticSensorPWM(PB7, 412, 6917); // sample values, yours will be different
+
+void setup() {
+    ...
+    sensor.init();
+    ...
+}
+```
+
+To use alternate function timers, set the PinName directly:
+
+```
+sensor._pin = PB_7_ALT1;  // manually set a PinName to use alternate timer function
+```
+
+PWM sensor may have a slow update time (not more often than once per PWM-period, e.g. at the PWM frequency):
+
+```
+sensor.min_elapsed_time = 0.001; // 1ms minimum sample time for velocity
+```

firmware/lib/Arduino-FOC-drivers/src/encoders/stm32pwmsensor/STM32MagneticSensorPWM.cpp

@@ -0,0 +1,35 @@
+
+#include "./STM32MagneticSensorPWM.h"
+
+#if defined(_STM32_DEF_)
+
+#include "common/foc_utils.h"
+
+
+STM32MagneticSensorPWM::STM32MagneticSensorPWM(int pin, uint32_t _min_ticks, uint32_t _max_ticks) : STM32PWMInput(pin), max_ticks(_max_ticks), min_ticks(_min_ticks) {
+
+};
+
+
+
+STM32MagneticSensorPWM::~STM32MagneticSensorPWM(){};
+
+
+
+void STM32MagneticSensorPWM::init(){
+    initialized = (STM32PWMInput::initialize()==0);
+    if(initialized)
+        Sensor::init();
+};
+
+
+
+float STM32MagneticSensorPWM::getSensorAngle(){
+    uint32_t ticks = getDutyCycleTicks();
+    return (ticks - min_ticks) * _2PI / (max_ticks - min_ticks);
+};
+
+
+
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/stm32pwmsensor/STM32MagneticSensorPWM.h

@@ -0,0 +1,28 @@
+
+#pragma once
+
+
+#include <Arduino.h>
+
+
+#if defined(_STM32_DEF_)
+
+#include "common/base_classes/Sensor.h"
+#include "utilities/stm32pwm/STM32PWMInput.h"
+
+
+class STM32MagneticSensorPWM : public Sensor, public STM32PWMInput {
+    public:
+        STM32MagneticSensorPWM(int pin, uint32_t _min_ticks = 0, uint32_t _max_ticks = 0x0FFF);
+        ~STM32MagneticSensorPWM();
+
+        void init() override;
+
+        uint32_t max_ticks = 0x0FFF;
+        uint32_t min_ticks = 0;
+        bool initialized = false;
+    protected:
+        float getSensorAngle() override;
+};
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/tle5012b/MagneticSensorTLE5012B.cpp

@@ -0,0 +1,18 @@
+
+#include "./MagneticSensorTLE5012B.h"
+
+#if defined(_STM32_DEF_)
+
+MagneticSensorTLE5012B::MagneticSensorTLE5012B(int data, int sck, int nCS) : TLE5012B(data, sck, nCS) { };
+MagneticSensorTLE5012B::~MagneticSensorTLE5012B(){ };
+
+void MagneticSensorTLE5012B::init() {
+    this->TLE5012B::init();
+    this->Sensor::init();
+};
+
+float MagneticSensorTLE5012B::getSensorAngle() {
+    return getCurrentAngle();
+};
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/tle5012b/MagneticSensorTLE5012B.h

@@ -0,0 +1,25 @@
+
+#ifndef __MAGNETIC_SENSOR_TLE5012B_H__
+#define __MAGNETIC_SENSOR_TLE5012B_H__
+
+#include "./STM32TLE5012B.h"
+
+#if defined(_STM32_DEF_)
+
+#include "common/base_classes/Sensor.h"
+
+
+
+
+class MagneticSensorTLE5012B : public Sensor, public TLE5012B {
+    public:
+        MagneticSensorTLE5012B(int data, int sck, int nCS);
+        ~MagneticSensorTLE5012B();
+        virtual void init() override;
+        virtual float getSensorAngle() override;
+};
+
+
+
+#endif
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/tle5012b/README.md

@@ -0,0 +1,91 @@
+# SimpleFOC Driver for TLE5012B encoder for STM32 MCUs
+
+This driver code is compatible with STM32 MCUs. Based loosely on code from [here](https://github.com/xerootg/btt-s42b-simplefoc), with many thanks to @xerootg
+
+## Hardware setup
+
+Connect the data pin of the sensor to the MOSI (COPI) line of the MCU. The SCLK and nCS lines are connected as normal.
+
+## Software setup
+
+Sample code:
+
+```
+#include <Arduino.h>
+#include <SimpleFOC.h>
+#include <SimpleFOCDrivers.h>
+#include "encoders/tle5012b/MagneticSensorTLE5012B.h"
+
+MagneticSensorTLE5012B sensor(PB15,PB13,PB12);
+
+long ts;
+
+void setup() {
+  Serial.begin(115200);
+  while (!Serial) ;
+  delay(2000);
+
+  Serial.println("Initializing sensor...");
+
+  sensor.init();
+
+  Serial.println("Sensor initialized.");
+
+  ts = millis();
+}
+
+void loop() {
+  sensor.update();
+  if (millis() - ts > 1000) {
+    Serial.println(sensor.getAngle(), 3);
+    ts = millis();
+  }
+  delay(1);
+}
+```
+
+## Other MCUs
+
+Infineon provides a driver library compatible with Atmel AVR (Arduino UNO, Nano, etc...) and Infineon XMC MCUs. If you have one of those MCUs, you can use this library in conjunction with the GenericSensor class in SimpleFOC.
+
+```
+#include <SimpleFOC.h>
+#include <TLE5012-ino.hpp>
+
+Tle5012Ino Tle5012Sensor = Tle5012Ino();
+errorTypes checkError = NO_ERROR;
+
+float readMySensor(){
+ // read my sensor
+ // return the angle value in radians in between 0 and 2PI
+ 
+    Tle5012Sensor.getAngleValue(d);
+    d = (d + 180);
+    if ( d + 40.9 > 360 ) {
+        d = (d + 40.9) - 360;
+    }
+    else
+        d = d + 40.9;
+
+    d = d * 0.0174533;
+    return d;
+}
+void initMySensor(){
+  // do the init
+   checkError = Tle5012Sensor.begin();
+}
+
+// empty constructor 
+GenericSensor sensor = GenericSensor(readMySensor, initMySensor);
+
+void setup(){
+...
+  //init sensor and use it further with foc...
+  sensor.init();
+...
+}
+```
+
+Other MCUs are currently not supported. The problem is that the sensor uses SPI in half-duplex mode, which is not supported by Arduino framework. So for each MCU type a custom driver has to be written to get the half-duplex SPI communication to work. There are currently no plans to support other MCUs on our side, but we will gladly accept pull requests :-)
+
+If you can find other libraries for other MCUs, you can use the GenericSensor approach to integrate them.

firmware/lib/Arduino-FOC-drivers/src/encoders/tle5012b/STM32TLE5012B.cpp

@@ -0,0 +1,159 @@
+
+#include "STM32TLE5012B.h"
+
+#if defined(_STM32_DEF_)
+
+
+#include "utility/spi_com.h"
+extern "C" uint32_t spi_getClkFreqInst(SPI_TypeDef *spi_inst);
+
+
+
+TLE5012B::TLE5012B(int data, int sck, int nCS, uint32_t freq) {
+    _data = data;
+    _sck = sck;
+    _nCS = nCS;
+    _freq = freq;
+};
+
+TLE5012B::~TLE5012B() {
+
+};
+
+
+void TLE5012B::init() {
+    pinMode(_nCS, OUTPUT);
+    digitalWrite(_nCS, HIGH);
+
+    // initialize pins
+    GPIO_InitTypeDef gpio;
+    gpio.Pin = digitalPinToBitMask(_data);
+    gpio.Mode = GPIO_MODE_AF_PP;
+    gpio.Speed = GPIO_SPEED_FREQ_HIGH;
+    HAL_GPIO_Init(digitalPinToPort(_data), &gpio);
+    gpio.Pin = digitalPinToBitMask(_sck);
+    gpio.Mode = GPIO_MODE_AF_PP;
+    gpio.Speed = GPIO_SPEED_FREQ_HIGH;
+    HAL_GPIO_Init(digitalPinToPort(_sck), &gpio);
+
+    SPI_TypeDef *spi_data = (SPI_TypeDef*)pinmap_peripheral(digitalPinToPinName(_data), PinMap_SPI_MOSI);
+    SPI_TypeDef *spi_sclk = (SPI_TypeDef*)pinmap_peripheral(digitalPinToPinName(_sck), PinMap_SPI_SCLK);
+    SPI_TypeDef *spi_inst = (SPI_TypeDef*)pinmap_merge_peripheral(spi_data, spi_sclk);
+
+    pinmap_pinout(digitalPinToPinName(_data), PinMap_SPI_MOSI);
+    pinmap_pinout(digitalPinToPinName(_sck), PinMap_SPI_SCLK);
+
+    #if defined SPI1_BASE
+    if (spi_inst == SPI1) {
+        __HAL_RCC_SPI1_CLK_ENABLE();
+        __HAL_RCC_SPI1_FORCE_RESET();
+        __HAL_RCC_SPI1_RELEASE_RESET();
+    }
+    #endif
+
+    #if defined SPI2_BASE
+    if (spi_inst == SPI2) {
+        __HAL_RCC_SPI2_CLK_ENABLE();
+        __HAL_RCC_SPI2_FORCE_RESET();
+        __HAL_RCC_SPI2_RELEASE_RESET();
+    }
+    #endif
+
+    #if defined SPI3_BASE
+    if (spi_inst == SPI3) {
+        __HAL_RCC_SPI3_CLK_ENABLE();
+        __HAL_RCC_SPI3_FORCE_RESET();
+        __HAL_RCC_SPI3_RELEASE_RESET();
+    }
+    #endif
+
+    #if defined SPI4_BASE
+    if (spi_inst == SPI4) {
+        __HAL_RCC_SPI4_CLK_ENABLE();
+        __HAL_RCC_SPI4_FORCE_RESET();
+        __HAL_RCC_SPI4_RELEASE_RESET();
+    }
+    #endif
+
+    #if defined SPI5_BASE
+    if (spi_inst == SPI5) {
+        __HAL_RCC_SPI5_CLK_ENABLE();
+        __HAL_RCC_SPI5_FORCE_RESET();
+        __HAL_RCC_SPI5_RELEASE_RESET();
+    }
+    #endif
+
+    #if defined SPI6_BASE
+    if (spi_inst == SPI6) {
+        __HAL_RCC_SPI6_CLK_ENABLE();
+        __HAL_RCC_SPI6_FORCE_RESET();
+        __HAL_RCC_SPI6_RELEASE_RESET();
+    }
+    #endif
+
+    _spi.Instance = spi_inst;
+    _spi.Init.Direction = SPI_DIRECTION_1LINE;
+    _spi.Init.Mode = SPI_MODE_MASTER;
+    _spi.Init.DataSize = SPI_DATASIZE_8BIT;
+    _spi.Init.CLKPolarity = SPI_POLARITY_LOW;
+    _spi.Init.CLKPhase = SPI_PHASE_2EDGE;
+    _spi.Init.NSS = SPI_NSS_SOFT;
+    _spi.Init.FirstBit = SPI_FIRSTBIT_MSB;
+    _spi.Init.CRCCalculation = SPI_CRCCALCULATION_DISABLE;
+    _spi.Init.CRCPolynomial = 7;
+    _spi.Init.TIMode = SPI_TIMODE_DISABLE;
+    #if defined(SPI_NSS_PULSE_DISABLE)
+    _spi.Init.NSSPMode = SPI_NSS_PULSE_DISABLE;
+    #endif
+
+    uint32_t spi_freq = spi_getClkFreqInst(spi_inst);
+    if (_freq >= (spi_freq / SPI_SPEED_CLOCK_DIV2_MHZ)) {
+        _spi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_2;
+    } else if (_freq >= (spi_freq / SPI_SPEED_CLOCK_DIV4_MHZ)) {
+        _spi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_4;
+    } else if (_freq >= (spi_freq / SPI_SPEED_CLOCK_DIV8_MHZ)) {
+        _spi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_8;
+    } else if (_freq >= (spi_freq / SPI_SPEED_CLOCK_DIV16_MHZ)) {
+        _spi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_16;
+    } else if (_freq >= (spi_freq / SPI_SPEED_CLOCK_DIV32_MHZ)) {
+        _spi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_32;
+    } else if (_freq >= (spi_freq / SPI_SPEED_CLOCK_DIV64_MHZ)) {
+        _spi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_64;
+    } else if (_freq >= (spi_freq / SPI_SPEED_CLOCK_DIV128_MHZ)) {
+        _spi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_128;
+    } else {
+        _spi.Init.BaudRatePrescaler = SPI_BAUDRATEPRESCALER_256;
+    }
+
+    if (HAL_SPI_Init(&_spi) != HAL_OK) {
+        // setup error
+        Serial.println("TLE5012B setup error");
+    }
+};
+
+uint16_t TLE5012B::readRawAngle() {
+    uint8_t data[4];
+    readBytes(TLE5012B_ANGLE_REG, data, 2);
+    return (((uint16_t)data[0] << 8) | data[1]) & 0x7FFF;
+}; 
+
+
+float TLE5012B::getCurrentAngle() {
+    return ((float)readRawAngle())/TLE5012B_CPR * _2PI;
+}; // angle in radians, return current value
+
+
+void TLE5012B::readBytes(uint16_t reg, uint8_t *data, uint8_t len) {
+    digitalWrite(_nCS, LOW);
+
+    reg |= TLE5012B_READ_REGISTER + (len>>1);
+    uint8_t txbuffer[2] = { (uint8_t)(reg >> 8), (uint8_t)(reg & 0x00FF) };    
+    HAL_SPI_Transmit(&_spi, txbuffer, 2, 100); // TODO check return value for error, timeout
+    //delayMicroseconds(1);
+    HAL_SPI_Receive(&_spi, data, len + 2, 100);
+
+    digitalWrite(_nCS, HIGH);
+};
+
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/encoders/tle5012b/STM32TLE5012B.h

@@ -0,0 +1,45 @@
+#ifndef __TLE5012B_H__
+#define __TLE5012B_H__
+
+#include "Arduino.h"
+
+#if defined(_STM32_DEF_)
+
+
+#define _2PI 6.28318530718f
+#define TLE5012B_CPR 32768.0f
+
+#define TLE5012B_READ_REGISTER 0x8000
+
+enum TLE5012B_Register : uint16_t {
+    TLE5012B_ANGLE_REG = 0x0020,
+    TLE5012B_SPEED_REG = 0x0030
+};
+
+
+
+class TLE5012B {
+    public:
+        TLE5012B(int data, int sck, int nCS, uint32_t freq = 1000000);
+        ~TLE5012B();
+        virtual void init();
+
+	    uint16_t readRawAngle();
+	    float getCurrentAngle(); // angle in radians, return current value
+    private:
+
+        void readBytes(uint16_t reg, uint8_t *data, uint8_t len);
+
+        int _data;
+        int _sck;
+        int _nCS;
+        uint32_t _freq;
+        SPI_HandleTypeDef _spi;
+};
+
+
+
+
+#endif
+
+#endif