From 9b03b5d9e9a298714a90b18f2842c639c21bb693 Mon Sep 17 00:00:00 2001
From: Richard Unger <richard.unger@nexum.com>
Date: Sat, 15 Apr 2023 00:21:35 +0200
Subject: [PATCH 01/17] fix errors in MT6835 driver code

---
 src/encoders/mt6835/MT6835.cpp | 16 +++++++++++--
 src/encoders/mt6835/MT6835.h   | 41 ++++++++++++++++++----------------
 2 files changed, 36 insertions(+), 21 deletions(-)

diff --git a/src/encoders/mt6835/MT6835.cpp b/src/encoders/mt6835/MT6835.cpp
index c885308..3d96b98 100644
--- a/src/encoders/mt6835/MT6835.cpp
+++ b/src/encoders/mt6835/MT6835.cpp
@@ -46,10 +46,14 @@ uint32_t MT6835::readRawAngle21(){
     spi->endTransaction();
     if (nCS >= 0)
         digitalWrite(nCS, HIGH);
+    laststatus = data[4]&0x07;
     return (data[2] << 13) | (data[3] << 5) | (data[4] >> 3);
 };
 
 
+uint8_t MT6835::getStatus(){
+    return laststatus;
+};
 
 
 bool MT6835::setZeroFromCurrentPosition(){
@@ -237,17 +241,25 @@ void MT6835::setOptions4(MT6835Options4 opts){
 
 
 
+uint32_t swap_bytes(uint32_t net)
+{
+    return __builtin_bswap32(net);
+}
+
+
 
 
 
 void MT6835::transfer24(MT6835Command* outValue) {
+    uint32_t buff = swap_bytes(outValue->val);
     if (nCS >= 0)
         digitalWrite(nCS, LOW);
     spi->beginTransaction(settings);
-    spi->transfer(outValue, 3);
+    spi->transfer(&buff, 3);
     spi->endTransaction();
     if (nCS >= 0)
         digitalWrite(nCS, HIGH);
+    outValue->val = swap_bytes(buff);
 };
 uint8_t MT6835::readRegister(uint16_t reg) {
     MT6835Command cmd;
@@ -258,7 +270,7 @@ uint8_t MT6835::readRegister(uint16_t reg) {
 };
 bool MT6835::writeRegister(uint16_t reg, uint8_t value) {
     MT6835Command cmd;
-    cmd.cmd = MT6835_OP_READ;
+    cmd.cmd = MT6835_OP_WRITE;
     cmd.addr = reg;
     cmd.data = value;
     transfer24(&cmd);
diff --git a/src/encoders/mt6835/MT6835.h b/src/encoders/mt6835/MT6835.h
index f29043e..5751901 100644
--- a/src/encoders/mt6835/MT6835.h
+++ b/src/encoders/mt6835/MT6835.h
@@ -53,9 +53,9 @@
 
 union MT6835ABZRes {
 	struct {
-		uint8_t abz_res_low:6;
-		uint8_t abz_off:1;
 		uint8_t ab_swap:1;
+		uint8_t abz_off:1;
+		uint8_t abz_res_low:6;
 	};
 	uint8_t reg;
 };
@@ -64,9 +64,9 @@ union MT6835ABZRes {
 
 union MT6835Options0 {
 	struct {
-		uint8_t zero_pos_low:4;
-		uint8_t z_edge:1;
 		uint8_t z_pul_wid:3;
+		uint8_t z_edge:1;
+		uint8_t zero_pos_low:4;
 	};
 	uint8_t reg;
 };
@@ -75,10 +75,10 @@ union MT6835Options0 {
 
 union MT6835Options1 {
 	struct {
-		uint8_t z_phase:2;
-		uint8_t uvw_mux:1;
-		uint8_t uvw_off:1;
 		uint8_t uvw_res:4;
+		uint8_t uvw_off:1;
+		uint8_t uvw_mux:1;
+		uint8_t z_phase:2;
 	};
 	uint8_t reg;
 };
@@ -87,11 +87,11 @@ union MT6835Options1 {
 
 union MT6835Options2 {
 	struct {
-		uint8_t reserved:2;
-		uint8_t nlc_en:1;
-		uint8_t pwm_fq:1;
-		uint8_t pwm_pol:1;
 		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;
 };
@@ -100,9 +100,9 @@ union MT6835Options2 {
 
 union MT6835Options3 {
 	struct {
-		uint8_t reserved:4;
-		uint8_t rot_dir:1;
 		uint8_t hyst:3;
+		uint8_t rot_dir:1;
+		uint8_t reserved:4;
 	};
 	uint8_t reg;
 };
@@ -111,9 +111,9 @@ union MT6835Options3 {
 
 union MT6835Options4 {
 	struct {
-		uint8_t gpio_ds:1;
-		uint8_t autocal_freq:3;
 		uint8_t reserved:4;
+		uint8_t autocal_freq:3;
+		uint8_t gpio_ds:1;
 	};
 	uint8_t reg;
 };
@@ -122,8 +122,8 @@ union MT6835Options4 {
 
 union MT6835Options5 {
 	struct {
-		uint8_t reserved:5;
 		uint8_t bw:3;
+		uint8_t reserved:5;
 	};
 	uint8_t reg;
 };
@@ -133,10 +133,10 @@ union MT6835Options5 {
 
 union MT6835Command {
 	struct {
-		uint32_t cmd:4;
-		uint32_t addr:12;
-		uint32_t data:8;
 		uint32_t unused:8;
+		uint32_t data:8;
+		uint32_t addr:12;
+		uint32_t cmd:4;
 	};
 	uint32_t val;
 };
@@ -197,6 +197,8 @@ class MT6835 {
     MT6835Options4 getOptions4();
     void setOptions4(MT6835Options4 opts);
 
+	uint8_t getStatus();
+
 	bool setZeroFromCurrentPosition();
 	bool writeEEPROM(); // wait 6s after calling this method
 
@@ -204,6 +206,7 @@ class MT6835 {
 	SPIClass* spi;
 	SPISettings settings;
 	int nCS = -1;
+	uint8_t laststatus = 0;
 
     void transfer24(MT6835Command* outValue);
     uint8_t readRegister(uint16_t reg);

From d47243d724dbfc3ca8cad8251998d80661a6997e Mon Sep 17 00:00:00 2001
From: Richard Unger <richard.unger@nexum.com>
Date: Sat, 15 Apr 2023 00:30:43 +0200
Subject: [PATCH 02/17] fix compile of STM32HWEncoder w stm32 2.5

---
 src/encoders/stm32hwencoder/STM32HWEncoder.h | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/encoders/stm32hwencoder/STM32HWEncoder.h b/src/encoders/stm32hwencoder/STM32HWEncoder.h
index 2ada65b..edff139 100644
--- a/src/encoders/stm32hwencoder/STM32HWEncoder.h
+++ b/src/encoders/stm32hwencoder/STM32HWEncoder.h
@@ -51,13 +51,13 @@ class STM32HWEncoder : public Sensor {
 
     TIM_HandleTypeDef encoder_handle;
 
-    static constexpr u_int16_t overflow_margin = 20000;
-    u_int16_t rotations_per_overflow;
-    u_int16_t ticks_per_overflow;
+    static constexpr uint16_t overflow_margin = 20000;
+    uint16_t rotations_per_overflow;
+    uint16_t ticks_per_overflow;
 
     volatile int32_t overflow_count;
-    volatile u_int16_t count;  //!< current pulse counter
-    volatile u_int16_t prev_count;
+    volatile uint16_t count;  //!< current pulse counter
+    volatile uint16_t prev_count;
     volatile int32_t prev_overflow_count;
 
     // velocity calculation variables

From eeb3b371cf685a123f8521b1dba23db7c30bc495 Mon Sep 17 00:00:00 2001
From: Richard Unger <richard.unger@nexum.com>
Date: Sun, 7 May 2023 23:15:08 +0200
Subject: [PATCH 03/17] added STM32SpeedDirCommander

---
 README.md                                     |  30 ++++--
 src/comms/stm32speeddir/README.md             |  58 ++++++++++
 .../stm32speeddir/STM32SpeedDirInput.cpp      |  29 +++++
 src/comms/stm32speeddir/STM32SpeedDirInput.h  |  31 ++++++
 src/utilities/stm32pwm/README.md              |  73 +++++++++++++
 src/utilities/stm32pwm/STM32PWMInput.cpp      | 100 ++++++++++++++++++
 src/utilities/stm32pwm/STM32PWMInput.h        |  27 +++++
 7 files changed, 338 insertions(+), 10 deletions(-)
 create mode 100644 src/comms/stm32speeddir/README.md
 create mode 100644 src/comms/stm32speeddir/STM32SpeedDirInput.cpp
 create mode 100644 src/comms/stm32speeddir/STM32SpeedDirInput.h
 create mode 100644 src/utilities/stm32pwm/README.md
 create mode 100644 src/utilities/stm32pwm/STM32PWMInput.cpp
 create mode 100644 src/utilities/stm32pwm/STM32PWMInput.h

diff --git a/README.md b/README.md
index e1b5572..74ccc02 100644
--- a/README.md
+++ b/README.md
@@ -10,17 +10,11 @@ The intent is to keep the core of SimpleFOC clean, and thus easy to maintain, un
 
 ## New Release
 
-v1.0.3 - Released March 2023, for Simple FOC 2.3.0
-
-What's changed since 1.0.2?
-- New Sensor: MT6835
-- Fixed bugs
-
-What's changed since 1.0.1?
-- Calibrated sensor by @MarethyuPrefect
-- New Sensors: MT6701, MA330, MT6816
-- Fixed bugs
+v1.0.4 - Released May 2023, for Simple FOC 2.3.0
 
+What's changed since 1.0.3?
+- New Input: STM32SpeedDirCommander
+- Fixed MT6835 driver bugs
 
 ## What is included
 
@@ -49,6 +43,11 @@ What is here? See the sections below. Each driver or function should come with i
 ### Communications
 
  - [I2CCommander I2C driver](src/comms/i2c/) - I2C communications protocol and drivers for both controller and target devices. 
+ - [STM32 SpeedDir Input](src/comms/stm32speeddir/) - Control target velocity with PWM speed and direction inputs
+
+### Utilities
+
+ - [STM32 PWM Input driver](src/utilities/stm32pwm/) - PWM Input driver for STM32 MCUs. Accurately measure PWM inputs with zero MCU overhead.
 
 
 ## How to use
@@ -74,3 +73,14 @@ If you do so, please be sure to adhere to and include the [LICENSE](https://gith
 ## Further Documentation
 
 Find out more information about the Arduino SimpleFOC project on the [docs website](https://docs.simplefoc.com/) 
+
+## Release History
+
+What's changed since 1.0.2?
+- New Sensor: MT6835
+- Fixed bugs
+
+What's changed since 1.0.1?
+- Calibrated sensor by @MarethyuPrefect
+- New Sensors: MT6701, MA330, MT6816
+- Fixed bugs
diff --git a/src/comms/stm32speeddir/README.md b/src/comms/stm32speeddir/README.md
new file mode 100644
index 0000000..1e36a5a
--- /dev/null
+++ b/src/comms/stm32speeddir/README.md
@@ -0,0 +1,58 @@
+
+# STM32SpeedDirInput
+
+Control SimpleFOC using PWM speed and direction inputs.
+
+Based on STM32 timer PWM-Input capabilities, which means this can only be used on STM32 MCUs. It can cover a wide range of PWM frequencies, and runs without MCU overhead in the timer hardware.
+
+
+
+## Setup
+
+The PWM speed input should be connected to either channel 1 or channel 2 of a general purpose or advanced control timer on your STM32 MCU. Suitable timers are:
+- Advanced control timers: TIM1, TIM8
+- General purpose timers (32 bit): TIM2, TIM5
+- General purpose timers (16 bit): TIM3, TIM4, TIM9, TIM12
+If in doubt, check in STM32CubeIDE and see if the PWM-Input mode can be enabled (under "Combined Channels") for the timer.
+
+The optional direction input can be connected to any GPIO pin. By default a high level direction input is associated with a positive velocity value, while a low level direction input results in a negative velocity value. To reverse this, set the option `dir_positive_high = false`
+
+The direction input is optional - if not provided, you can control the direction from software using the `direction` field.
+
+The velocity values returned are in the range `min_speed` to `max_speed`, while the input PWM duty cycle should lie within the range `min_pwm` to `max_pwm`. Actual input values smaller than `min_pwm` will be treated as `min_pwm`, values larger than `max_pwm` will be treated as `max_pwm`. The behaviour for 100% or 0% duty cycles is undefined, and they should be avoided.
+
+## Usage
+
+Use it like this:
+
+```c++
+#include "comms/stm32speeddir/STM32SpeedDirInput.h"
+
+// some example pins - the speed pin has to be on channel 1 or 2 of a timer
+#define PIN_SPEED PC6
+#define PIN_DIRECTION PB8
+
+STM32SpeedDirInput speed_dir = STM32SpeedDirInput(PIN_SPEED, PIN_DIRECTION);
+
+float target = 0.0f;
+
+void setup(){
+    ...
+
+    speed_dir.min_speed = 10.0f;    // 10rad/s min speed
+    speed_dir.max_speed = 100.0f;   // 100rad/s max speed
+    speed_dir.min_pwm = 5.0f;       // 5% min duty cycle
+    speed_dir.max_pwm = 95.0f;      // 95% max duty cycle
+    speed_dir.init();
+
+    ...
+}
+
+
+void loop(){
+    target = speed_dir.getTargetVelocity();
+    motor.move(target);
+    motor.loopFOC();
+}
+
+```
\ No newline at end of file
diff --git a/src/comms/stm32speeddir/STM32SpeedDirInput.cpp b/src/comms/stm32speeddir/STM32SpeedDirInput.cpp
new file mode 100644
index 0000000..5176e8b
--- /dev/null
+++ b/src/comms/stm32speeddir/STM32SpeedDirInput.cpp
@@ -0,0 +1,29 @@
+
+#include "./STM32SpeedDirInput.h"
+
+#if defined(_STM32_DEF_)
+
+STM32SpeedDirInput::STM32SpeedDirInput(int pin_speed, int pin_dir) : STM32PWMInput(pin_speed) {
+    _pin_speed = pin_speed;
+    _pin_dir = pin_dir;
+};
+
+STM32SpeedDirInput::~STM32SpeedDirInput(){};
+
+int STM32SpeedDirInput::init(){
+    pinMode(_pin_dir, INPUT);
+    return STM32PWMInput::init();
+};
+
+
+float STM32SpeedDirInput::getTargetVelocity(){
+    if (_pin_dir != NOT_SET)
+        direction = digitalRead(_pin_dir);
+    float speed = getDutyCyclePercent();
+    speed = constrain(speed, min_pwm, max_pwm);
+    speed = (speed - min_pwm)/(max_pwm - min_pwm) * (max_speed - min_speed) + min_speed;
+    return (direction == dir_positive_high) ? speed : -speed;
+};
+
+
+#endif
\ No newline at end of file
diff --git a/src/comms/stm32speeddir/STM32SpeedDirInput.h b/src/comms/stm32speeddir/STM32SpeedDirInput.h
new file mode 100644
index 0000000..3a3647b
--- /dev/null
+++ b/src/comms/stm32speeddir/STM32SpeedDirInput.h
@@ -0,0 +1,31 @@
+
+#pragma once
+
+#include "Arduino.h"
+
+#if defined(_STM32_DEF_)
+
+#include "common/foc_utils.h"
+#include "utilities/stm32pwm/STM32PWMInput.h"
+
+class STM32SpeedDirInput : public STM32PWMInput {
+    public:
+        STM32SpeedDirInput(int pin_speed, int pin_dir = NOT_SET);
+        ~STM32SpeedDirInput();
+
+        int init();
+        float getTargetVelocity();
+
+        float min_speed = 0;            // min speed in rad/s
+        float max_speed = 100;          // max speed in rad/s
+        float min_pwm = 5.0;            // min duty cycle in %
+        float max_pwm = 95.0;           // max duty cycle in %
+        bool dir_positive_high = true;  // true if the direction pin is high when the motor is spinning in the positive direction
+        bool direction = true;          // direction of rotation, default positive
+    protected:
+        int _pin_speed;
+        int _pin_dir;
+};
+
+
+#endif
\ No newline at end of file
diff --git a/src/utilities/stm32pwm/README.md b/src/utilities/stm32pwm/README.md
new file mode 100644
index 0000000..20a641b
--- /dev/null
+++ b/src/utilities/stm32pwm/README.md
@@ -0,0 +1,73 @@
+
+# STM32 Utilities - PWM Input
+
+Using the `STM32PWMInput` class you can precisely read a PWM input signal on STM32 MCUs by using their timer's PWM-Input mode. This happens entirely in the timer hardware with zero MCU overhead, and is very precise.
+
+## Setup
+
+Connect the PWM input to either channel 1 or channel 2 of a general purpose or advanced control timer supporting PWM-Input mode.
+
+This should include the following timers:
+
+- Advanced control timers: TIM1, TIM8
+- General purpose timers (32 bit): TIM2, TIM5
+- General purpose timers (16 bit): TIM3, TIM4, TIM9, TIM12
+
+The best to use are the 32 bit general purpose timers, although TIM1 may also be interesting because on some MCUs it can be clocked at a higher rate than the other timers.
+
+## Usage
+
+```
+  STM32PWMInput pwmInput = STM32PWMInput(PA15);
+  pwmInput.init();
+
+  uint32_t periodTicks = pwmInput.getPeriodTicks();
+  uint32_t dutyTicks = pwmInput.getDutyCycleTicks();
+  float dutyPercent = pwmInput.getDutyCyclePercent();
+```
+
+## Input signal
+
+The input signal should be a PWM signal (square wave) with a duty cycle that is >0% and <100%. The 
+
+The behaviour if the input is not a square wave is not defined, although the MCU will continue to sample the input, and resumes correct measurement when the square wave input is restored.
+
+## Performance
+
+The range of PWM frequencies that can be measured and the resolution with which they can be sampled depends on the following:
+
+- timer clock speed - normally this is your MCU speed, but it can be lower, or in some cases even higher. 
+- timer prescaler - can divide the timer clock.
+
+The sample resolution for a PWM input signal of frequency _Fp_ is given by
+
+_R = Fc / Fp_
+
+Where _R_ is the resolution in ticks, and _Fc_ is the timer tick frequency (timer clock with prescaler).
+
+This sample resolution is equal to the length of the PWM period in ticks. The timer needs to sample both the duty cycle and the full period, so the timer can't sample signals whose period would cause it to overflow. 
+
+On a 16 bit timer this means the period (and sample resolution) must be less than 65536 ticks, while on a 32 bit timer it must be less than 4294967296 ticks. This places a lower limit on the PWM frequencies that can be sampled.
+
+On the other side, the minimum duty cycle that can be reliably captured is limited by the duration of one timer tick, e.g. the minimum on-time that can be measured reliably is equal to two timer ticks in duration.
+
+So as the frequency increases, the resolution decreases, and the minimum duty cycle increases.
+
+For example, on a 64MHz STM32 MCU, using TIM3 (16 bit) you could capture a PWM signal at 10kHz with a resolution of 6400 ticks. This fits in the 16 bit timer, so no problem. Assuming the signal is perfectly stable (it usually isn't) that would be 12 bits of accuracy on your input. 
+
+If you lowered the PWM input frequency to 1kHz, you'd get a 64000 range - just fitting in the timer. But now the accuracy is greatly increased - almost 16 bits!
+
+If you increased the PWM frequency to 10Mhz, the resolution would be just 6 ticks, and the resulting accuracy very low - just 2 bits, with a minimum duty cycle of 33% and a maximum of 66%.
+
+Of course you could vary the 10MHz signal faster than the 1kHz one - so choosing the PWM input frequency is a tradeoff between accuracy and control bandwidth...
+
+Note: clock configuration is out of scope for this class. Set your clocks up in advance.
+
+
+
+## Roadmap
+
+- Support setting of the filtering function, this could help a lot against noise on the input
+- Support setting of the timer prescaler (not the clock prescaler!)
+- Support setting of the downsample function, this could help increase accuracy
+- Support choosing of the alternate timers on pins with more than one
diff --git a/src/utilities/stm32pwm/STM32PWMInput.cpp b/src/utilities/stm32pwm/STM32PWMInput.cpp
new file mode 100644
index 0000000..6f18722
--- /dev/null
+++ b/src/utilities/stm32pwm/STM32PWMInput.cpp
@@ -0,0 +1,100 @@
+
+#include "./STM32PWMInput.h"
+
+#if defined(_STM32_DEF_)
+
+
+
+STM32PWMInput::STM32PWMInput(int pin){
+    _pin = pin;
+};
+
+
+STM32PWMInput::~STM32PWMInput(){};
+
+
+
+
+
+int STM32PWMInput::init(){
+    PinName pinN = digitalPinToPinName(_pin);
+    pinmap_pinout(pinN, PinMap_TIM);
+    uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pinN, PinMap_TIM));
+    timer.Instance = (TIM_TypeDef *)pinmap_peripheral(pinN, PinMap_TIM);
+    timer.Init.Prescaler = 0;
+    timer.Init.CounterMode = TIM_COUNTERMODE_UP;
+    timer.Init.Period = 4.294967295E9;    // TODO max period, depends on which timer is used - 32 bits or 16 bits
+    timer.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+    timer.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+    if (HAL_TIM_Base_Init(&timer) != HAL_OK) {
+        return -1;
+    }
+    TIM_ClockConfigTypeDef clkCfg = { .ClockSource=TIM_CLOCKSOURCE_INTERNAL };
+    if (HAL_TIM_ConfigClockSource(&timer, &clkCfg) != HAL_OK) {
+        return -2;
+    }
+    if (HAL_TIM_IC_Init(&timer) != HAL_OK) {
+        return -3;
+    }
+
+    TIM_SlaveConfigTypeDef slCfg = {
+        .SlaveMode = TIM_SLAVEMODE_RESET,
+        .InputTrigger = (channel==1)?TIM_TS_TI1FP1:TIM_TS_TI2FP2,
+        .TriggerPolarity = TIM_INPUTCHANNELPOLARITY_RISING,
+        .TriggerPrescaler = TIM_ICPSC_DIV1,
+        .TriggerFilter = 0
+    };
+    if (HAL_TIM_SlaveConfigSynchro(&timer, &slCfg) != HAL_OK) {
+        return -4;
+    }
+    TIM_IC_InitTypeDef icCfg = {
+        .ICPolarity = TIM_INPUTCHANNELPOLARITY_RISING,
+        .ICSelection = (channel==1)?TIM_ICSELECTION_DIRECTTI:TIM_ICSELECTION_INDIRECTTI,
+        .ICPrescaler = TIM_ICPSC_DIV1,
+        .ICFilter = 0
+    };
+    if (HAL_TIM_IC_ConfigChannel(&timer, &icCfg, TIM_CHANNEL_1) != HAL_OK) {
+        return -5;
+    }
+    icCfg.ICPolarity = TIM_INPUTCHANNELPOLARITY_FALLING;
+    icCfg.ICSelection = (channel==1)?TIM_ICSELECTION_INDIRECTTI:TIM_ICSELECTION_DIRECTTI;
+    if (HAL_TIM_IC_ConfigChannel(&timer, &icCfg, TIM_CHANNEL_2) != HAL_OK) {
+        return -6;
+    }
+    TIM_MasterConfigTypeDef mCfg = {
+        .MasterOutputTrigger = TIM_TRGO_RESET,
+        .MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE
+    };
+    if (HAL_TIMEx_MasterConfigSynchronization(&timer, &mCfg) != HAL_OK) {
+        return -7;
+    }
+    if (HAL_TIM_IC_Start(&timer, TIM_CHANNEL_1)!=HAL_OK) {
+        return -8;
+    }
+    if (HAL_TIM_IC_Start(&timer, TIM_CHANNEL_2)!=HAL_OK) {
+        return -9;
+    }
+    timer.Instance->CR1 |= TIM_CR1_CEN;
+    return 0;
+};
+
+
+float STM32PWMInput::getDutyCyclePercent(){
+    uint32_t period = getPeriodTicks();
+    if (period<1) return 0.0f;
+    return getDutyCycleTicks() / (float)period * 100.0f;
+};
+
+
+uint32_t STM32PWMInput::getDutyCycleTicks(){
+    return timer.Instance->CCR2;
+};
+
+
+uint32_t STM32PWMInput::getPeriodTicks(){
+    return timer.Instance->CCR1;
+};
+
+
+
+#endif
diff --git a/src/utilities/stm32pwm/STM32PWMInput.h b/src/utilities/stm32pwm/STM32PWMInput.h
new file mode 100644
index 0000000..c589987
--- /dev/null
+++ b/src/utilities/stm32pwm/STM32PWMInput.h
@@ -0,0 +1,27 @@
+#pragma once
+
+#include "Arduino.h"
+
+#if defined(_STM32_DEF_)
+
+
+class STM32PWMInput {
+    public:
+        STM32PWMInput(int pin);
+        ~STM32PWMInput();
+
+        int init();
+
+        float getDutyCyclePercent();
+        uint32_t getDutyCycleTicks();
+        uint32_t getPeriodTicks();
+    protected:
+        int _pin;
+        TIM_HandleTypeDef timer;
+};
+
+
+
+
+
+#endif
\ No newline at end of file

From 11489d87a0ee9a64defbc2752f9e48c543c80f86 Mon Sep 17 00:00:00 2001
From: Richard Unger <richard.unger@nexum.com>
Date: Wed, 10 May 2023 19:58:15 +0200
Subject: [PATCH 04/17] improvements to STM32 PWM input

---
 src/comms/stm32speeddir/STM32SpeedDirInput.cpp |  2 +-
 src/utilities/stm32pwm/STM32PWMInput.cpp       | 11 +++++------
 src/utilities/stm32pwm/STM32PWMInput.h         |  4 ++--
 3 files changed, 8 insertions(+), 9 deletions(-)

diff --git a/src/comms/stm32speeddir/STM32SpeedDirInput.cpp b/src/comms/stm32speeddir/STM32SpeedDirInput.cpp
index 5176e8b..4bc8bee 100644
--- a/src/comms/stm32speeddir/STM32SpeedDirInput.cpp
+++ b/src/comms/stm32speeddir/STM32SpeedDirInput.cpp
@@ -12,7 +12,7 @@ STM32SpeedDirInput::~STM32SpeedDirInput(){};
 
 int STM32SpeedDirInput::init(){
     pinMode(_pin_dir, INPUT);
-    return STM32PWMInput::init();
+    return STM32PWMInput::initialize();
 };
 
 
diff --git a/src/utilities/stm32pwm/STM32PWMInput.cpp b/src/utilities/stm32pwm/STM32PWMInput.cpp
index 6f18722..5d04b4c 100644
--- a/src/utilities/stm32pwm/STM32PWMInput.cpp
+++ b/src/utilities/stm32pwm/STM32PWMInput.cpp
@@ -6,7 +6,7 @@
 
 
 STM32PWMInput::STM32PWMInput(int pin){
-    _pin = pin;
+    _pin = digitalPinToPinName(pin);
 };
 
 
@@ -16,11 +16,10 @@ STM32PWMInput::~STM32PWMInput(){};
 
 
 
-int STM32PWMInput::init(){
-    PinName pinN = digitalPinToPinName(_pin);
-    pinmap_pinout(pinN, PinMap_TIM);
-    uint32_t channel = STM_PIN_CHANNEL(pinmap_function(pinN, PinMap_TIM));
-    timer.Instance = (TIM_TypeDef *)pinmap_peripheral(pinN, PinMap_TIM);
+int STM32PWMInput::initialize(){
+    pinmap_pinout(_pin, PinMap_TIM);
+    uint32_t channel = STM_PIN_CHANNEL(pinmap_function(_pin, PinMap_TIM));
+    timer.Instance = (TIM_TypeDef *)pinmap_peripheral(_pin, PinMap_TIM);
     timer.Init.Prescaler = 0;
     timer.Init.CounterMode = TIM_COUNTERMODE_UP;
     timer.Init.Period = 4.294967295E9;    // TODO max period, depends on which timer is used - 32 bits or 16 bits
diff --git a/src/utilities/stm32pwm/STM32PWMInput.h b/src/utilities/stm32pwm/STM32PWMInput.h
index c589987..5c220ff 100644
--- a/src/utilities/stm32pwm/STM32PWMInput.h
+++ b/src/utilities/stm32pwm/STM32PWMInput.h
@@ -10,13 +10,13 @@ class STM32PWMInput {
         STM32PWMInput(int pin);
         ~STM32PWMInput();
 
-        int init();
+        int initialize();
 
         float getDutyCyclePercent();
         uint32_t getDutyCycleTicks();
         uint32_t getPeriodTicks();
     protected:
-        int _pin;
+        PinName _pin;
         TIM_HandleTypeDef timer;
 };
 

From 47fd048efe1b288af66a9f96131273a8d766bb34 Mon Sep 17 00:00:00 2001
From: Richard Unger <richard.unger@nexum.com>
Date: Wed, 10 May 2023 19:58:51 +0200
Subject: [PATCH 05/17] added a STM32MagneticSensorPWM

---
 src/encoders/stm32pwmsensor/README.md         | 46 +++++++++++++++++++
 .../stm32pwmsensor/STM32MagneticSensorPWM.cpp | 35 ++++++++++++++
 .../stm32pwmsensor/STM32MagneticSensorPWM.h   | 28 +++++++++++
 3 files changed, 109 insertions(+)
 create mode 100644 src/encoders/stm32pwmsensor/README.md
 create mode 100644 src/encoders/stm32pwmsensor/STM32MagneticSensorPWM.cpp
 create mode 100644 src/encoders/stm32pwmsensor/STM32MagneticSensorPWM.h

diff --git a/src/encoders/stm32pwmsensor/README.md b/src/encoders/stm32pwmsensor/README.md
new file mode 100644
index 0000000..fd98be9
--- /dev/null
+++ b/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
+```
diff --git a/src/encoders/stm32pwmsensor/STM32MagneticSensorPWM.cpp b/src/encoders/stm32pwmsensor/STM32MagneticSensorPWM.cpp
new file mode 100644
index 0000000..3c3e1d6
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/src/encoders/stm32pwmsensor/STM32MagneticSensorPWM.h b/src/encoders/stm32pwmsensor/STM32MagneticSensorPWM.h
new file mode 100644
index 0000000..3e5d335
--- /dev/null
+++ b/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
\ No newline at end of file

From d7cd0c151d769029235bf031228fb19f20d37b9b Mon Sep 17 00:00:00 2001
From: Richard Unger <richard.unger@nexum.com>
Date: Wed, 10 May 2023 19:59:12 +0200
Subject: [PATCH 06/17] improvements to STM32HardwareEncoder

---
 .../stm32hwencoder/STM32HWEncoder.cpp         | 66 ++++++++-----------
 src/encoders/stm32hwencoder/STM32HWEncoder.h  |  9 ++-
 2 files changed, 32 insertions(+), 43 deletions(-)

diff --git a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
index f911e00..029e3df 100644
--- a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
+++ b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
@@ -21,9 +21,9 @@ STM32HWEncoder::STM32HWEncoder(unsigned int _ppr, int8_t pinA, int8_t pinB, int8
     prev_timestamp = getCurrentMicros();
     pulse_timestamp = getCurrentMicros();
 
-    _pinA = pinA;
-    _pinB = pinB;
-    _pinI = pinI;
+    _pinA = digitalPinToPinName(pinA);
+    _pinB = digitalPinToPinName(pinB);
+    _pinI = digitalPinToPinName(pinI);
 }
 
 
@@ -76,9 +76,7 @@ int32_t STM32HWEncoder::getFullRotations() {
 float STM32HWEncoder::getVelocity() {
     // sampling time calculation
     float dt = (pulse_timestamp - prev_timestamp) * 1e-6f;
-    // quick fix for strange cases (micros overflow)
-    if (dt <= 0 || dt > 0.5f)
-        dt = 1e-3f;
+    if (dt < min_elapsed_time) return velocity; // don't update velocity if deltaT is too small
 
     // time from last impulse
     int32_t overflow_diff = overflow_count - prev_overflow_count;
@@ -87,7 +85,8 @@ float STM32HWEncoder::getVelocity() {
     float pulse_per_second = dN / dt;
 
     // velocity calculation
-    return pulse_per_second / (static_cast<float>(cpr)) * _2PI;
+    velocity = pulse_per_second / (static_cast<float>(cpr)) * _2PI;
+    return velocity;
 }
 
 // getter for index pin
@@ -99,33 +98,25 @@ int STM32HWEncoder::hasIndex() { return 0; }
 // encoder initialisation of the hardware pins
 // and calculation variables
 void STM32HWEncoder::init() {
+    // counter setup
+    overflow_count = 0;
+    count = 0;
+    prev_count = 0;
+    prev_overflow_count = 0;
+
     // overflow handling
     rotations_per_overflow = 0xFFFF / cpr;
     ticks_per_overflow = cpr * rotations_per_overflow;
 
-    // set up GPIO
-    GPIO_InitTypeDef gpio;
-
-    PinName pinA = digitalPinToPinName(_pinA);
-    TIM_TypeDef *InstanceA = (TIM_TypeDef *)pinmap_peripheral(pinA, PinMap_PWM);
-    gpio.Pin = digitalPinToBitMask(_pinA);
-    gpio.Mode = GPIO_MODE_AF_PP;
-    gpio.Pull = GPIO_NOPULL;
-    gpio.Speed = GPIO_SPEED_FREQ_MEDIUM;
-    #ifndef STM32F1xx_HAL_GPIO_H
-    gpio.Alternate = pinmap_function(pinA, PinMap_PWM);
-    #endif
-    HAL_GPIO_Init(digitalPinToPort(_pinA), &gpio);
-
-    // lets assume pinB is on the same timer as pinA... otherwise it can't work but the API currently doesn't allow us to fail gracefully
-    gpio.Pin = digitalPinToBitMask(_pinB);
-    gpio.Mode = GPIO_MODE_AF_PP;
-    gpio.Pull = GPIO_NOPULL;
-    gpio.Speed = GPIO_SPEED_FREQ_MEDIUM;
-    #ifndef STM32F1xx_HAL_GPIO_H
-    gpio.Alternate = pinmap_function(digitalPinToPinName(_pinB), PinMap_PWM);
-    #endif
-    HAL_GPIO_Init(digitalPinToPort(_pinB), &gpio);
+    // GPIO configuration
+    TIM_TypeDef *InstanceA = (TIM_TypeDef *)pinmap_peripheral(_pinA, PinMap_PWM);
+    TIM_TypeDef *InstanceB = (TIM_TypeDef *)pinmap_peripheral(_pinB, PinMap_PWM);
+    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 = ticks_per_overflow - 1;
@@ -152,19 +143,18 @@ void STM32HWEncoder::init() {
     encoder_handle.Instance = InstanceA; // e.g. TIM4;
     enableTimerClock(&encoder_handle);
     if (HAL_TIM_Encoder_Init(&encoder_handle, &encoder_config) != HAL_OK) {
-        _Error_Handler(__FILE__, __LINE__);
+        initialized = false;
+        return;
     }
 
-    HAL_TIM_Encoder_Start(&encoder_handle, TIM_CHANNEL_1);
-
-    // counter setup
-    overflow_count = 0;
-    count = 0;
-    prev_count = 0;
-    prev_overflow_count = 0;
+    if (HAL_TIM_Encoder_Start(&encoder_handle, TIM_CHANNEL_1) != HAL_OK) {
+        initialized = false;
+        return;
+    }
 
     prev_timestamp = getCurrentMicros();
     pulse_timestamp = getCurrentMicros();
+    initialized = true;
 }
 
 #endif
\ No newline at end of file
diff --git a/src/encoders/stm32hwencoder/STM32HWEncoder.h b/src/encoders/stm32hwencoder/STM32HWEncoder.h
index edff139..3030941 100644
--- a/src/encoders/stm32hwencoder/STM32HWEncoder.h
+++ b/src/encoders/stm32hwencoder/STM32HWEncoder.h
@@ -1,6 +1,5 @@
 
-#ifndef STM32_HARDWARE_ENCODER_H
-#define STM32_HARDWARE_ENCODER_H
+#pragma once
 
 #include <Arduino.h>
 
@@ -44,6 +43,9 @@ class STM32HWEncoder : public Sensor {
      */
     int needsSearch() override;
 
+    bool initialized = false;
+    PinName _pinA, _pinB, _pinI;
+    
   private:
     int hasIndex();  // !< function returning 1 if encoder has index pin and 0 if not.
 
@@ -62,9 +64,6 @@ class STM32HWEncoder : public Sensor {
 
     // velocity calculation variables
     volatile int32_t pulse_timestamp, prev_timestamp;
-
-    int8_t _pinA, _pinB, _pinI;
 };
 
-#endif
 #endif
\ No newline at end of file

From 03391c2bfa15bc3153d0f27637d7d3427a6dfa2a Mon Sep 17 00:00:00 2001
From: Richard Unger <richard.unger@nexum.com>
Date: Sat, 13 May 2023 00:06:01 +0200
Subject: [PATCH 07/17] add setZero capability to AS5047U

---
 src/encoders/as5047u/AS5047U.cpp | 30 ++++++++++++++++++++++++++++--
 src/encoders/as5047u/AS5047U.h   |  1 +
 2 files changed, 29 insertions(+), 2 deletions(-)

diff --git a/src/encoders/as5047u/AS5047U.cpp b/src/encoders/as5047u/AS5047U.cpp
index 60224d1..7a79a3d 100644
--- a/src/encoders/as5047u/AS5047U.cpp
+++ b/src/encoders/as5047u/AS5047U.cpp
@@ -264,13 +264,39 @@ void AS5047U::enableUVW(bool enable){
 
 
 
+
+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 = nop()
+	};
+	return ((result&0x00FF)<<6) | posL.zposl;
+}
+
+
+
 uint16_t AS5047U::setZero(uint16_t value){
-	// TODO implement me!
-	return 0;
+	uint16_t command = AS5047U_ZPOSL_REG | AS5047U_RW;
+	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	command = AS5047U_ZPOSL_REG;
+	AS5047UZPosL posL = {
+			.reg = spi_transfer16(command)
+	};
+	posL.zposl = value&0x003F;
+	spi_transfer16(posL.reg);
+	command = AS5047U_ZPOSM_REG;
+	spi_transfer16(command);
+	spi_transfer16((value>>6)&0x00FF);
+	delayMicroseconds(50);
+	return getZero();
 }
 
 
 
+
 uint16_t AS5047U::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&AS5047U_RESULT_MASK;
diff --git a/src/encoders/as5047u/AS5047U.h b/src/encoders/as5047u/AS5047U.h
index b8fc908..eebb87b 100644
--- a/src/encoders/as5047u/AS5047U.h
+++ b/src/encoders/as5047u/AS5047U.h
@@ -206,6 +206,7 @@ class AS5047U {
 	void enableUVW(bool enable);
 
 	uint16_t setZero(uint16_t);
+	uint16_t getZero();
 
 private:
 

From 51e5254774e00ea642c09b58b576c8791e3742be Mon Sep 17 00:00:00 2001
From: Richard Unger <richard.unger@nexum.com>
Date: Sat, 13 May 2023 00:24:04 +0200
Subject: [PATCH 08/17] use _micros()

---
 src/encoders/stm32hwencoder/STM32HWEncoder.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
index 029e3df..d6e86ad 100644
--- a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
+++ b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
@@ -36,7 +36,7 @@ void STM32HWEncoder::update() {
     count = encoder_handle.Instance->CNT;
 
     prev_timestamp = pulse_timestamp;
-    pulse_timestamp = getCurrentMicros();
+    pulse_timestamp = _micros(); // micros() rollover is handled in velocity calculation
 
     prev_overflow_count = overflow_count;
     if (prev_count > (ticks_per_overflow - overflow_margin) &&
@@ -76,6 +76,7 @@ int32_t STM32HWEncoder::getFullRotations() {
 float STM32HWEncoder::getVelocity() {
     // sampling time calculation
     float dt = (pulse_timestamp - prev_timestamp) * 1e-6f;
+    // this also handles the moment when micros() rolls over
     if (dt < min_elapsed_time) return velocity; // don't update velocity if deltaT is too small
 
     // time from last impulse

From 0476f73d020462c9526d12a015fc6bd1bccdafa7 Mon Sep 17 00:00:00 2001
From: Richard Unger <richard.unger@nexum.com>
Date: Sat, 13 May 2023 00:24:30 +0200
Subject: [PATCH 09/17] add code example to MT6835 readme

---
 src/encoders/mt6835/README.md | 48 ++++++++++++++++++++++++++++++-----
 1 file changed, 42 insertions(+), 6 deletions(-)

diff --git a/src/encoders/mt6835/README.md b/src/encoders/mt6835/README.md
index 11adaad..66cbb33 100644
--- a/src/encoders/mt6835/README.md
+++ b/src/encoders/mt6835/README.md
@@ -1,7 +1,5 @@
 # MT6835 SimpleFOC driver
 
-:warning: untested
-
 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.
@@ -17,12 +15,50 @@ Connect the sensor to an SPI bus on your MCU. Pay attention to the voltage level
 Usage example:
 
 ```c++
-#include "Arduino.h"
-#include "Wire.h"
+#include <Arduino.h>
+
 #include "SimpleFOC.h"
 #include "SimpleFOCDrivers.h"
-#include "encoders/MT6835/MagneticSensorMT6835.h"
 
-// TODO code example
+#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);
 ```
 

From a749c5ef16872d974ab4805c0d28a6f6a58a7a92 Mon Sep 17 00:00:00 2001
From: Richard Unger <richard.unger@nexum.com>
Date: Tue, 16 May 2023 00:02:50 +0200
Subject: [PATCH 10/17] fix setting ABZ resolution in MT6835

---
 src/encoders/mt6835/MT6835.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/encoders/mt6835/MT6835.cpp b/src/encoders/mt6835/MT6835.cpp
index 3d96b98..aeaa200 100644
--- a/src/encoders/mt6835/MT6835.cpp
+++ b/src/encoders/mt6835/MT6835.cpp
@@ -120,11 +120,11 @@ uint16_t MT6835::getABZResolution(){
     return (hi << 6) | lo.abz_res_low;
 };
 void MT6835::setABZResolution(uint16_t res){
-     uint8_t hi = (res >> 2);
+    uint8_t hi = (res >> 6);
     MT6835ABZRes lo = {
 			.reg = readRegister(MT6835_REG_ABZ_RES2)
 	};
-    lo.abz_res_low = res & 0x3F;
+    lo.abz_res_low = (res & 0x3F);
     writeRegister(MT6835_REG_ABZ_RES1, hi);
     writeRegister(MT6835_REG_ABZ_RES2, lo.reg);
 };

From de0cfea3ba31ccd82cc877f3eb078b3b82a117d2 Mon Sep 17 00:00:00 2001
From: Richard Unger <richard.unger@nexum.com>
Date: Tue, 16 May 2023 00:04:03 +0200
Subject: [PATCH 11/17] improvements to STM32HWEncoder

---
 src/encoders/stm32hwencoder/STM32HWEncoder.cpp | 17 ++++++++++-------
 1 file changed, 10 insertions(+), 7 deletions(-)

diff --git a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
index d6e86ad..6464314 100644
--- a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
+++ b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
@@ -39,12 +39,14 @@ void STM32HWEncoder::update() {
     pulse_timestamp = _micros(); // micros() rollover is handled in velocity calculation
 
     prev_overflow_count = overflow_count;
-    if (prev_count > (ticks_per_overflow - overflow_margin) &&
-        prev_count <= ticks_per_overflow && count < overflow_margin)
-        ++overflow_count;
-    if (prev_count >= 0 && prev_count < overflow_margin &&
-        count >= (ticks_per_overflow - overflow_margin))
+    // if (prev_count > (ticks_per_overflow - overflow_margin) && count < overflow_margin)
+    //     ++overflow_count;
+    // if (prev_count < overflow_margin && count >= (ticks_per_overflow - overflow_margin))
+    //     --overflow_count;
+    if (prev_count < count && (count - prev_count > ticks_per_overflow / 2))
         --overflow_count;
+    if (prev_count > count && (prev_count - count > ticks_per_overflow / 2))
+        ++overflow_count;
 }
 
 
@@ -110,8 +112,8 @@ void STM32HWEncoder::init() {
     ticks_per_overflow = cpr * rotations_per_overflow;
 
     // GPIO configuration
-    TIM_TypeDef *InstanceA = (TIM_TypeDef *)pinmap_peripheral(_pinA, PinMap_PWM);
-    TIM_TypeDef *InstanceB = (TIM_TypeDef *)pinmap_peripheral(_pinB, PinMap_PWM);
+    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;
@@ -143,6 +145,7 @@ void STM32HWEncoder::init() {
 
     encoder_handle.Instance = InstanceA; // e.g. TIM4;
     enableTimerClock(&encoder_handle);
+
     if (HAL_TIM_Encoder_Init(&encoder_handle, &encoder_config) != HAL_OK) {
         initialized = false;
         return;

From 31559561d7b5c71e9b1f98e4426e425189f680e0 Mon Sep 17 00:00:00 2001
From: Richard Unger <richard.unger@nexum.com>
Date: Sat, 20 May 2023 00:27:41 +0200
Subject: [PATCH 12/17] make _pin public - set alternate timers by PinName

---
 src/utilities/stm32pwm/STM32PWMInput.h | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/utilities/stm32pwm/STM32PWMInput.h b/src/utilities/stm32pwm/STM32PWMInput.h
index 5c220ff..4a1a04d 100644
--- a/src/utilities/stm32pwm/STM32PWMInput.h
+++ b/src/utilities/stm32pwm/STM32PWMInput.h
@@ -15,8 +15,9 @@ class STM32PWMInput {
         float getDutyCyclePercent();
         uint32_t getDutyCycleTicks();
         uint32_t getPeriodTicks();
-    protected:
+        
         PinName _pin;
+    protected:
         TIM_HandleTypeDef timer;
 };
 

From 9abb188bed1286b00d7cddc2b3980086c7c6ff12 Mon Sep 17 00:00:00 2001
From: Richard Unger <richard.unger@nexum.com>
Date: Sat, 20 May 2023 00:29:27 +0200
Subject: [PATCH 13/17] AS5047: settings, ABI settings, zero setting

---
 src/encoders/as5047/AS5047.cpp | 106 +++++++++++++++++++++++++++++++--
 src/encoders/as5047/AS5047.h   |  28 +++++++++
 2 files changed, 128 insertions(+), 6 deletions(-)

diff --git a/src/encoders/as5047/AS5047.cpp b/src/encoders/as5047/AS5047.cpp
index 905d1b6..9455775 100644
--- a/src/encoders/as5047/AS5047.cpp
+++ b/src/encoders/as5047/AS5047.cpp
@@ -85,14 +85,14 @@ AS5047Settings1 AS5047::readSettings1(){
 
 void AS5047::writeSettings1(AS5047Settings1 settings){
 	uint16_t command = AS5047_SETTINGS1_REG;  // set r=0, result is 0x0018
-	/*uint16_t cmdresult =*/ spi_transfer16(command);
-	/*cmdresult =*/ spi_transfer16(settings.reg);
+	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
-	/*uint16_t cmdresult =*/ spi_transfer16(command);
+	spi_transfer16(command);
 	AS5047Settings2 result = {
 			.reg = nop()
 	};
@@ -100,6 +100,14 @@ AS5047Settings2 AS5047::readSettings2(){
 }
 
 
+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);
@@ -112,21 +120,96 @@ AS5047Diagnostics AS5047::readDiagnostics(){
 
 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();
-	settings.uvw_abi = enable?0:1;
+	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){
-	// TODO implement me!
-	return 0;
+	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;
 }
 
 
@@ -135,6 +218,17 @@ uint16_t AS5047::nop(){
 	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);
diff --git a/src/encoders/as5047/AS5047.h b/src/encoders/as5047/AS5047.h
index 59b33a3..e3cc80c 100644
--- a/src/encoders/as5047/AS5047.h
+++ b/src/encoders/as5047/AS5047.h
@@ -21,6 +21,7 @@ union AS5047Diagnostics {
 		uint16_t cof:1;
 		uint16_t magh:1;
 		uint16_t magl:1;
+		uint16_t unused:4;
 	};
 	uint16_t reg;
 };
@@ -29,6 +30,7 @@ union AS5047Diagnostics {
 union AS5047ZPosM {
 	struct {
 		uint16_t zposm:8;
+		uint16_t unused:8;
 	};
 	uint16_t reg;
 };
@@ -39,6 +41,7 @@ union AS5047ZPosL {
 		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;
 };
@@ -54,6 +57,7 @@ union AS5047Settings1 {
 		uint16_t abibin:1;
 		uint16_t dataselect:1;
 		uint16_t pwmon:1;
+		uint16_t unused:8;
 	};
 	uint16_t reg;
 };
@@ -64,6 +68,7 @@ union AS5047Settings2 {
 		uint16_t uvwpp:3;
 		uint16_t hys:2;
 		uint16_t abires:3;
+		uint16_t unused:8;
 	};
 	uint16_t reg;
 };
@@ -76,6 +81,21 @@ struct AS5047Error {
 };
 
 
+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
@@ -120,10 +140,18 @@ class AS5047 {
 	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:
 

From 87b8d9e1b35d13dde251a0722b9f791b352e7d48 Mon Sep 17 00:00:00 2001
From: Richard Unger <richard.unger@nexum.com>
Date: Sat, 20 May 2023 00:30:19 +0200
Subject: [PATCH 14/17] AS5047U: settings, zero setting, 24bit writes+CRC

---
 src/encoders/as5047u/AS5047U.cpp | 97 ++++++++++++++++++++------------
 src/encoders/as5047u/AS5047U.h   |  4 +-
 2 files changed, 65 insertions(+), 36 deletions(-)

diff --git a/src/encoders/as5047u/AS5047U.cpp b/src/encoders/as5047u/AS5047U.cpp
index 7a79a3d..481fbaf 100644
--- a/src/encoders/as5047u/AS5047U.cpp
+++ b/src/encoders/as5047u/AS5047U.cpp
@@ -52,7 +52,7 @@ uint16_t AS5047U::readCorrectedAngle(){
 uint16_t AS5047U::readMagnitude(){
 	uint16_t command = AS5047U_MAGNITUDE_REG | AS5047U_RW;
 	/*uint16_t cmdresult =*/ spi_transfer16(command);
-	uint16_t result = nop();
+	uint16_t result = nop16();
 	return result;
 }
 
@@ -60,7 +60,7 @@ uint16_t AS5047U::readMagnitude(){
 uint16_t AS5047U::readVelocity(){
 	uint16_t command = AS5047U_VELOCITY_REG | AS5047U_RW;
 	/*uint16_t cmdresult =*/ spi_transfer16(command);
-	uint16_t result = nop();
+	uint16_t result = nop16();
 	return result;
 }
 
@@ -78,7 +78,7 @@ 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 = nop();
+	result.reg = nop16();
 	return result;
 }
 
@@ -87,7 +87,7 @@ 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 = nop()
+			.reg = nop16()
 	};
 	return result;
 }
@@ -96,9 +96,7 @@ AS5047USettings1 AS5047U::readSettings1(){
 
 
 void AS5047U::writeSettings1(AS5047USettings1 settings){
-	uint16_t command = AS5047U_SETTINGS1_REG;  // set r=0, result is 0x0018
-	/*uint16_t cmdresult =*/ spi_transfer16(command);
-	/*cmdresult =*/ spi_transfer16(settings.reg);
+	writeRegister24(AS5047U_SETTINGS1_REG, settings.reg);
 }
 
 
@@ -108,7 +106,7 @@ 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 = nop()
+			.reg = nop16()
 	};
 	return result;
 }
@@ -118,9 +116,7 @@ AS5047USettings2 AS5047U::readSettings2(){
 
 
 void AS5047U::writeSettings2(AS5047USettings2 settings){
-	uint16_t command = AS5047U_SETTINGS2_REG;
-	/*uint16_t cmdresult =*/ spi_transfer16(command);
-	/*cmdresult =*/ spi_transfer16(settings.reg);
+	writeRegister24(AS5047U_SETTINGS2_REG, settings.reg);
 }
 
 
@@ -131,7 +127,7 @@ AS5047USettings3 AS5047U::readSettings3(){
 	uint16_t command = AS5047U_SETTINGS3_REG | AS5047U_RW;
 	/*uint16_t cmdresult =*/ spi_transfer16(command);
 	AS5047USettings3 result = {
-			.reg = nop()
+			.reg = nop16()
 	};
 	return result;
 }
@@ -141,9 +137,7 @@ AS5047USettings3 AS5047U::readSettings3(){
 
 
 void AS5047U::writeSettings3(AS5047USettings3 settings){
-	uint16_t command = AS5047U_SETTINGS3_REG;
-	/*uint16_t cmdresult =*/ spi_transfer16(command);
-	/*cmdresult =*/ spi_transfer16(settings.reg);
+	writeRegister24(AS5047U_SETTINGS3_REG, settings.reg);
 }
 
 
@@ -153,7 +147,7 @@ AS5047UDiagnostics AS5047U::readDiagnostics(){
 	uint16_t command = AS5047U_DIAGNOSTICS_REG | AS5047U_RW;
 	/*uint16_t cmdresult =*/ spi_transfer16(command);
 	AS5047UDiagnostics result = {
-			.reg = nop()
+			.reg = nop16()
 	};
 	return result;
 }
@@ -164,7 +158,7 @@ AS5047UDiagnostics AS5047U::readDiagnostics(){
 uint8_t AS5047U::readAGC(){
 	uint16_t command = AS5047U_AGC_REG | AS5047U_RW;
 	/*uint16_t cmdresult =*/ spi_transfer16(command);
-	uint16_t result = nop();
+	uint16_t result = nop16();
 	return result & 0x00FF;
 };
 
@@ -173,7 +167,7 @@ uint8_t AS5047U::readAGC(){
 uint8_t AS5047U::readECCCHK(){
 	uint16_t command = AS5047U_ECCCHK_REG | AS5047U_RW;
 	/*uint16_t cmdresult =*/ spi_transfer16(command);
-	uint16_t result = nop();
+	uint16_t result = nop16();
 	return result & 0x007F;
 };
 
@@ -184,7 +178,7 @@ AS5047UDisableSettings AS5047U::readDisableSettings(){
 	uint16_t command = AS5047U_DISABLE_REG | AS5047U_RW;
 	/*uint16_t cmdresult =*/ spi_transfer16(command);
 	AS5047UDisableSettings result = {
-			.reg = nop()
+			.reg = nop16()
 	};
 	return result;
 };
@@ -192,9 +186,7 @@ AS5047UDisableSettings AS5047U::readDisableSettings(){
 
 
 void AS5047U::writeDisableSettings(AS5047UDisableSettings settings){
-	uint16_t command = AS5047U_DISABLE_REG;
-	/*uint16_t cmdresult =*/ spi_transfer16(command);
-	/*cmdresult =*/ spi_transfer16(settings.reg);
+	writeRegister24(AS5047U_DISABLE_REG, settings.reg);
 };
 
 
@@ -203,7 +195,7 @@ AS5047UECCSettings AS5047U::readECCSettings(){
 	uint16_t command = AS5047U_ECC_REG | AS5047U_RW;
 	/*uint16_t cmdresult =*/ spi_transfer16(command);
 	AS5047UECCSettings result = {
-			.reg = nop()
+			.reg = nop16()
 	};
 	return result;
 };
@@ -211,9 +203,7 @@ AS5047UECCSettings AS5047U::readECCSettings(){
 
 
 void AS5047U::writeECCSettings(AS5047UECCSettings settings){
-	uint16_t command = AS5047U_ECC_REG;
-	/*uint16_t cmdresult =*/ spi_transfer16(command);
-	/*cmdresult =*/ spi_transfer16(settings.reg);
+	writeRegister24(AS5047U_ECC_REG, settings.reg);
 };
 
 
@@ -271,7 +261,7 @@ uint16_t AS5047U::getZero(){
 	command = AS5047U_ZPOSL_REG | AS5047U_RW;
 	uint16_t result = spi_transfer16(command);
 	AS5047UZPosL posL = {
-			.reg = nop()
+			.reg = nop16()
 	};
 	return ((result&0x00FF)<<6) | posL.zposl;
 }
@@ -281,23 +271,19 @@ uint16_t AS5047U::getZero(){
 uint16_t AS5047U::setZero(uint16_t value){
 	uint16_t command = AS5047U_ZPOSL_REG | AS5047U_RW;
 	/*uint16_t cmdresult =*/ spi_transfer16(command);
-	command = AS5047U_ZPOSL_REG;
 	AS5047UZPosL posL = {
-			.reg = spi_transfer16(command)
+			.reg = nop16()
 	};
 	posL.zposl = value&0x003F;
-	spi_transfer16(posL.reg);
-	command = AS5047U_ZPOSM_REG;
-	spi_transfer16(command);
-	spi_transfer16((value>>6)&0x00FF);
-	delayMicroseconds(50);
+	writeRegister24(AS5047U_ZPOSL_REG, posL.reg);
+	writeRegister24(AS5047U_ZPOSM_REG, (value>>6)&0x00FF);
 	return getZero();
 }
 
 
 
 
-uint16_t AS5047U::nop(){
+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;
 }
@@ -317,4 +303,45 @@ uint16_t AS5047U::spi_transfer16(uint16_t outdata) {
 	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] = { (reg>>8)&0x3F, reg&0xFF, 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];
+}
\ No newline at end of file
diff --git a/src/encoders/as5047u/AS5047U.h b/src/encoders/as5047u/AS5047U.h
index eebb87b..cb9cf10 100644
--- a/src/encoders/as5047u/AS5047U.h
+++ b/src/encoders/as5047u/AS5047U.h
@@ -210,8 +210,10 @@ class AS5047U {
 
 private:
 
-	uint16_t nop();
+	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;

From 2aeb0f057c955bb368612733a741d9b4e071d723 Mon Sep 17 00:00:00 2001
From: Richard Unger <richard.unger@nexum.com>
Date: Sat, 20 May 2023 20:19:18 +0200
Subject: [PATCH 15/17] fix ESP32 compiler problem

---
 src/encoders/as5047u/AS5047U.cpp | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/encoders/as5047u/AS5047U.cpp b/src/encoders/as5047u/AS5047U.cpp
index 481fbaf..bfe70be 100644
--- a/src/encoders/as5047u/AS5047U.cpp
+++ b/src/encoders/as5047u/AS5047U.cpp
@@ -317,7 +317,10 @@ uint8_t AS5047U::calcCRC(uint16_t data){
 }
 
 uint16_t AS5047U::writeRegister24(uint16_t reg, uint16_t data) {
-	uint8_t buff[3] = { (reg>>8)&0x3F, reg&0xFF, calcCRC(reg) };
+	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);

From 51b1cbebb8ff45ec47dac10f245135d410ef5740 Mon Sep 17 00:00:00 2001
From: Richard Unger <richard.unger@nexum.com>
Date: Thu, 25 May 2023 18:51:59 +0200
Subject: [PATCH 16/17] big simplification to STM32HWEncoder

---
 .../stm32hwencoder/STM32HWEncoder.cpp         | 92 +------------------
 src/encoders/stm32hwencoder/STM32HWEncoder.h  | 43 +--------
 2 files changed, 8 insertions(+), 127 deletions(-)

diff --git a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
index 6464314..0525e3f 100644
--- a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
+++ b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
@@ -6,90 +6,17 @@
   HardwareEncoder(int cpr)
 */
 STM32HWEncoder::STM32HWEncoder(unsigned int _ppr, int8_t pinA, int8_t pinB, int8_t pinI) {
-    rotations_per_overflow = 0;
-    ticks_per_overflow = 0;
-
-    overflow_count = 0;
-    count = 0;
-    prev_count = 0;
-    prev_overflow_count = 0;
-    pulse_timestamp = 0;
-
     cpr = _ppr * 4; // 4x for quadrature
-
-    // velocity calculation variables
-    prev_timestamp = getCurrentMicros();
-    pulse_timestamp = getCurrentMicros();
-
     _pinA = digitalPinToPinName(pinA);
     _pinB = digitalPinToPinName(pinB);
     _pinI = digitalPinToPinName(pinI);
 }
 
-
-
-void STM32HWEncoder::update() {
-    // handle overflow of the 16-bit counter here
-    // must be called at least twice per traversal of overflow range
-    // TODO(conroy-cheers): investigate performance impact
-    prev_count = count;
-    count = encoder_handle.Instance->CNT;
-
-    prev_timestamp = pulse_timestamp;
-    pulse_timestamp = _micros(); // micros() rollover is handled in velocity calculation
-
-    prev_overflow_count = overflow_count;
-    // if (prev_count > (ticks_per_overflow - overflow_margin) && count < overflow_margin)
-    //     ++overflow_count;
-    // if (prev_count < overflow_margin && count >= (ticks_per_overflow - overflow_margin))
-    //     --overflow_count;
-    if (prev_count < count && (count - prev_count > ticks_per_overflow / 2))
-        --overflow_count;
-    if (prev_count > count && (prev_count - count > ticks_per_overflow / 2))
-        ++overflow_count;
-}
-
-
-
 /*
   Shaft angle calculation
 */
-float STM32HWEncoder::getSensorAngle() { return getAngle(); }
-
-float STM32HWEncoder::getMechanicalAngle() {
-    return _2PI * (count % static_cast<int>(cpr)) / static_cast<float>(cpr);
-}
-float STM32HWEncoder::getAngle() {
-    return _2PI * (count / static_cast<float>(cpr) +
-                   overflow_count * rotations_per_overflow);
-}
-double STM32HWEncoder::getPreciseAngle() {
-    return _2PI * (count / static_cast<double>(cpr) +
-                   overflow_count * rotations_per_overflow);
-}
-int32_t STM32HWEncoder::getFullRotations() {
-    return count / static_cast<int>(cpr) +
-           overflow_count * rotations_per_overflow;
-}
-
-/*
-  Shaft velocity calculation
-*/
-float STM32HWEncoder::getVelocity() {
-    // sampling time calculation
-    float dt = (pulse_timestamp - prev_timestamp) * 1e-6f;
-    // this also handles the moment when micros() rolls over
-    if (dt < min_elapsed_time) return velocity; // don't update velocity if deltaT is too small
-
-    // time from last impulse
-    int32_t overflow_diff = overflow_count - prev_overflow_count;
-    int32_t dN = (count - prev_count) + (ticks_per_overflow * overflow_diff);
-
-    float pulse_per_second = dN / dt;
-
-    // velocity calculation
-    velocity = pulse_per_second / (static_cast<float>(cpr)) * _2PI;
-    return velocity;
+float STM32HWEncoder::getSensorAngle() { 
+    return _2PI * encoder_handle.Instance->CNT / static_cast<float>(cpr);
 }
 
 // getter for index pin
@@ -99,18 +26,7 @@ int STM32HWEncoder::needsSearch() { return false; }
 int STM32HWEncoder::hasIndex() { return 0; }
 
 // encoder initialisation of the hardware pins
-// and calculation variables
 void STM32HWEncoder::init() {
-    // counter setup
-    overflow_count = 0;
-    count = 0;
-    prev_count = 0;
-    prev_overflow_count = 0;
-
-    // overflow handling
-    rotations_per_overflow = 0xFFFF / cpr;
-    ticks_per_overflow = cpr * rotations_per_overflow;
-
     // GPIO configuration
     TIM_TypeDef *InstanceA = (TIM_TypeDef *)pinmap_peripheral(_pinA, PinMap_TIM);
     TIM_TypeDef *InstanceB = (TIM_TypeDef *)pinmap_peripheral(_pinB, PinMap_TIM);
@@ -122,7 +38,7 @@ void STM32HWEncoder::init() {
     pinmap_pinout(_pinB, PinMap_TIM);
 
     // set up timer for encoder
-    encoder_handle.Init.Period = ticks_per_overflow - 1;
+    encoder_handle.Init.Period = cpr - 1;
     encoder_handle.Init.Prescaler = 0;
     encoder_handle.Init.ClockDivision = 0;
     encoder_handle.Init.CounterMode = TIM_COUNTERMODE_UP;
@@ -156,8 +72,6 @@ void STM32HWEncoder::init() {
         return;
     }
 
-    prev_timestamp = getCurrentMicros();
-    pulse_timestamp = getCurrentMicros();
     initialized = true;
 }
 
diff --git a/src/encoders/stm32hwencoder/STM32HWEncoder.h b/src/encoders/stm32hwencoder/STM32HWEncoder.h
index 3030941..98b14e5 100644
--- a/src/encoders/stm32hwencoder/STM32HWEncoder.h
+++ b/src/encoders/stm32hwencoder/STM32HWEncoder.h
@@ -9,7 +9,6 @@
 #include <HardwareTimer.h>
 #include "common/base_classes/Sensor.h"
 #include "common/foc_utils.h"
-#include "common/time_utils.h"
 
 class STM32HWEncoder : public Sensor {
   public:
@@ -19,51 +18,19 @@ class STM32HWEncoder : public Sensor {
     */
     explicit STM32HWEncoder(unsigned int ppr, int8_t pinA, int8_t pinB, int8_t pinI=-1);
 
-    /** encoder initialise pins */
     void init() override;
-
-    // Encoder configuration
-    unsigned int cpr;  //!< encoder cpr number
-
-    // Abstract functions of the Sensor class implementation
-    /** get current angle (rad) */
-    float getSensorAngle() override;
-    float getMechanicalAngle() override;
-    /**  get current angular velocity (rad/s) */
-    float getVelocity() override;
-    float getAngle() override;
-    double getPreciseAngle() override;
-    int32_t getFullRotations() override;
-    void update() override;
-
-    /**
-     * returns 0 if it does need search for absolute zero
-     * 0 - encoder without index
-     * 1 - ecoder with index
-     */
     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;
     
-  private:
-    int hasIndex();  // !< function returning 1 if encoder has index pin and 0 if not.
-
-    void handleOverflow();
-
+  protected:
+    float getSensorAngle() override;
+    
     TIM_HandleTypeDef encoder_handle;
 
-    static constexpr uint16_t overflow_margin = 20000;
-    uint16_t rotations_per_overflow;
-    uint16_t ticks_per_overflow;
-
-    volatile int32_t overflow_count;
-    volatile uint16_t count;  //!< current pulse counter
-    volatile uint16_t prev_count;
-    volatile int32_t prev_overflow_count;
-
-    // velocity calculation variables
-    volatile int32_t pulse_timestamp, prev_timestamp;
 };
 
 #endif
\ No newline at end of file

From 4ce521f18e98717b50ef96eaf5b0a43a52c5d844 Mon Sep 17 00:00:00 2001
From: Richard Unger <richard.unger@nexum.com>
Date: Thu, 1 Jun 2023 21:20:55 +0200
Subject: [PATCH 17/17] update README and version for release

---
 README.md          | 8 ++++++--
 keywords.txt       | 4 +++-
 library.properties | 2 +-
 3 files changed, 10 insertions(+), 4 deletions(-)

diff --git a/README.md b/README.md
index 74ccc02..4c952d6 100644
--- a/README.md
+++ b/README.md
@@ -10,11 +10,14 @@ The intent is to keep the core of SimpleFOC clean, and thus easy to maintain, un
 
 ## New Release
 
-v1.0.4 - Released May 2023, for Simple FOC 2.3.0
+v1.0.4 - Released June 2023, for Simple FOC 2.3.0
 
 What's changed since 1.0.3?
-- New Input: STM32SpeedDirCommander
+- New Comms/Input: STM32SpeedDirCommander
+- New Utility: STM32PWMInput
 - Fixed MT6835 driver bugs
+- Improved AS5047 driver, fixed bugs
+- Improved AS5047U driver, fixed bugs
 
 ## What is included
 
@@ -39,6 +42,7 @@ What is here? See the sections below. Each driver or function should come with i
  - [MT6816 SPI driver](src/encoders/mt6816/) - SPI driver for the MagnTek MT6816 absolute position magnetic rotary encoder IC.
  - [MT6701 SSI driver](src/encoders/mt6701/) - SSI driver for the MagnTek MT6701 absolute position magnetic rotary encoder IC.
  - [MT6835 SPI driver](src/encoders/mt6835/) - SPI driver for the MagnTek MT6835 21 bit magnetic rotary encoder IC.
+ - [STM32 PWM sensor driver](src/encoders/stm32pwmsensor/) - STM32 native timer-based driver for PWM angle sensors.
 
 ### Communications
 
diff --git a/keywords.txt b/keywords.txt
index e99910a..516cc85 100644
--- a/keywords.txt
+++ b/keywords.txt
@@ -4,6 +4,8 @@ AS5048A     KEYWORD1
 AS5047      KEYWORD1
 AS5145      KEYWORD1
 MA730       KEYWORD1
+MT6835      KEYWORD1
+SC60228     KEYWORD1
 TLE5012B    KEYWORD1
 I2CCommander    KEYWORD1
-
+STM32HWEncoder  KEYWORD1
diff --git a/library.properties b/library.properties
index 0d28aed..d60e428 100644
--- a/library.properties
+++ b/library.properties
@@ -1,5 +1,5 @@
 name=SimpleFOCDrivers
-version=1.0.3
+version=1.0.4
 author=Simplefoc <info@simplefoc.com>
 maintainer=Simplefoc <info@simplefoc.com>
 sentence=A library of supporting drivers for SimpleFOC. Motor drivers chips, encoder chips, current sensing and supporting code.