try to fix submodule

firmware/lib/Arduino-FOC-drivers/src/utilities/PreciseAngle.cpp

@@ -0,0 +1,113 @@
+/*
+ * PreciseAngle.cpp
+ *
+ *  Created on: 1 May 2021
+ *      Author: runger
+ */
+
+#include <utilities/PreciseAngle.h>
+#include <common/foc_utils.h>
+
+PreciseAngle::PreciseAngle() : angle(0), rotations(0)  {}
+
+PreciseAngle::~PreciseAngle() {}
+
+PreciseAngle::PreciseAngle(const PreciseAngle &other) : angle(other.angle), rotations(other.rotations) {}
+
+
+PreciseAngle::PreciseAngle(uint16_t count, int32_t rotations) : angle(count), rotations(rotations) {}
+
+
+PreciseAngle::PreciseAngle(float radians) {
+	rotations = radians/_2PI;
+	angle = (radians - (rotations * _2PI)) * cpr / _2PI;
+}
+
+PreciseAngle::PreciseAngle(double radians) {
+	rotations = radians/_2PI;
+	angle = (radians - (rotations * _2PI)) * cpr / _2PI;
+}
+
+// returns the angle mod 2PI, i.e. the angle of the motor shaft, in radians
+float PreciseAngle::getShaftAngle() {
+	return angle * _2PI / cpr;
+}
+
+
+// returns the angle mod 2PI, i.e. the angle of the motor shaft, in radians
+int32_t PreciseAngle::getRotations() {
+	return rotations;
+}
+
+// returns the angle mod 2PI, i.e. the angle of the motor shaft, in radians
+uint16_t PreciseAngle::getShaftTicks() {
+	return angle;
+}
+
+
+// returns the total angle, including rotations, in radians
+float PreciseAngle::asFloat() {
+	return (angle * _2PI / (float)cpr) + ((float)rotations * _2PI);
+}
+
+
+// returns the total angle, including rotations, in radians, as double precision
+double PreciseAngle::asDouble() {
+	return (double)angle * _2PI / cpr + (double)rotations * _2PI;
+}
+
+
+// returns the total angle, including rotations, in ticks, as 64 bit integer
+int64_t PreciseAngle::asTicks() {
+	return (int64_t)angle + (int64_t)rotations * cpr;
+}
+
+
+
+
+PreciseAngle PreciseAngle::operator+(const PreciseAngle &other) {
+	PreciseAngle result;
+	result.rotations = rotations + other.rotations;
+	uint32_t temp = angle + other.angle;
+	if (temp>=cpr) {
+		result.angle = temp-cpr;
+		result.rotations+=1;
+	}
+	else
+		result.angle = temp;
+	return result;
+}
+
+
+PreciseAngle PreciseAngle::operator-(const PreciseAngle &other) {
+	PreciseAngle result;
+	result.rotations = rotations - other.rotations;
+	int32_t temp = angle - other.angle;
+	if (temp<0) {
+		result.angle = temp+cpr;
+		result.rotations-=1;
+	}
+	else
+		result.angle = temp;
+	return result;
+}
+
+
+// velocity in rad/s
+float PreciseAngle::velocity(const PreciseAngle &previous, uint32_t microseconds) {
+	PreciseAngle diff = (*this - previous);
+	return diff.asFloat() * (1e6/(float)microseconds);
+}
+
+
+
+void PreciseAngle::update(uint16_t current_angle){
+	int32_t diff = current_angle - angle;
+	if (abs(diff)>cpr_overflow_check){
+		rotations += diff>0?-1:1;
+	}
+	angle = current_angle;
+}
+
+
+

firmware/lib/Arduino-FOC-drivers/src/utilities/PreciseAngle.h

@@ -0,0 +1,57 @@
+/*
+ * PreciseAngle.h
+ *
+ *  Created on: 1 May 2021
+ *      Author: runger
+ */
+
+#ifndef LIBRARIES_ARDUNIO_FOC_DRIVERS_SRC_UTILITIES_PRECISEANGLE_H_
+#define LIBRARIES_ARDUNIO_FOC_DRIVERS_SRC_UTILITIES_PRECISEANGLE_H_
+
+#include <stdint.h>
+
+class PreciseAngle {
+public:
+	PreciseAngle();
+	virtual ~PreciseAngle();
+	PreciseAngle(const PreciseAngle &other);
+	PreciseAngle(uint16_t count, int32_t rotations);
+	PreciseAngle(float radians);
+	PreciseAngle(double radians);
+	//PreciseAngle& operator=(const PreciseAngle &other);
+
+	// returns the angle mod 2PI, i.e. the angle of the motor shaft, in radians
+	float getShaftAngle();
+	// number of complete motor rotations
+	int32_t getRotations();
+	uint16_t getShaftTicks();
+
+	// returns the total angle, including rotations, in radians
+	float asFloat();
+	// returns the total angle, including rotations, in radians, as double precision
+	double asDouble();
+	// returns the total angle, including rotations, in ticks, as 64 bit integer
+	int64_t asTicks();
+
+
+	PreciseAngle operator+(const PreciseAngle &other);
+	PreciseAngle operator-(const PreciseAngle &other);
+
+	// velocity in rad/s
+	float velocity(const PreciseAngle &previous, uint32_t microseconds);
+
+	// update the angle with a new ticks value. this will increment the rotations
+	// if necessary, using a simple algorithm to detect overflows.
+	// if the motor turns too much between calls to update, the overflow could be missed
+	// and the rotations counted incorrectly.
+	void update(uint16_t current_angle);
+
+
+	const static uint16_t cpr = 16384;
+	const static uint16_t cpr_overflow_check = 13107; //cpr*0.8;
+protected:
+	uint16_t angle;
+	int32_t rotations;
+};
+
+#endif /* LIBRARIES_ARDUNIO_FOC_DRIVERS_SRC_UTILITIES_PRECISEANGLE_H_ */

firmware/lib/Arduino-FOC-drivers/src/utilities/README.md

@@ -0,0 +1,25 @@
+# Utilities
+
+Some classes and functions you may find useful when working with SimpleFOC.
+
+### PreciseAngle
+
+:warning: work in progress! only velocity mode has been tested.
+
+SimpleFOC generally uses floats for calculations. This preserves compatibility with older MCUs which only support 32bit FP, and
+can't do double precision, and ensures the maximum compatibilty for the library.
+
+When older MCUs become irrelevant, this may change, but in the meantime there is a problem representing the angles as floats.
+On the one hand they have to be precise enough to accurately capture the electric position of the motor's rotor with respect to 
+the stator, so meybe even less than 0.1deg. Without this precision you can't do FOC. On the other hand, we need to support many
+rotations of the motor shaft - a motor turning at 1000RPM for 1h will be at 60000 rotations or approx. 378,991.118radians.
+Expressing these large numbers while also maintaining the required precision is too much for the humble float.
+
+PreciseAngle works in conjunction with PreciseXXXSensor classes [like this one](../encoders/as5048a/PreciseMagneticSensorAS5048A.h).
+It solves the float problem by representing the angle as seperate components: angle within rotation and number of rotations, both as
+integers. It maintains the complete precision of the sensor over a rotation range of +/- 2^31, i.e. about 2 billion rotations, before
+overflowing. 
+
+Even at 50000RPM (which would be a lot for SimpleFOC!) that's enough for about 30 days of continuous rotation. At 1000RPM it's enough 
+for 4 years.
+

firmware/lib/Arduino-FOC-drivers/src/utilities/stm32math/README.md

@@ -0,0 +1,41 @@
+
+# SimpleFOC math functions for STM32 CORDIC
+
+Accellerated trigonometry functions are provided for STM32 MCUs having a CORDIC peripheral. The CORDIC hardware can compute sine and cosine values in just a few processor cycles.
+
+## Usage
+
+To use it, set the build-flag `-DHAL_CORDIC_MODULE_ENABLED` for the CORDIC to be enabled in the framework.
+
+Then, include the following in your code:
+
+```c++
+#include <Arduino.h>
+#include "SimpleFOC.h"
+#include "SimpleFOCDrivers.h"
+#include "utilities/stm32math/STM32G4CORDICTrigFunctions.h"
+
+void setup() {
+    ...
+
+    SimpleFOC_CORDIC_Config();      // initialize the CORDIC
+
+    ...
+}
+
+```
+
+That's it. The faster trig functions will be used automatically by the SimpleFOC code.
+
+You can use them yourself in your own code too:
+
+```c++
+
+float angle = 0.5f; // in radians
+float s = _sin(angle);
+float c = _cos(angle);
+
+// get both sine and cosine in one operation
+_sincos(angle, &s, &c);
+
+```

firmware/lib/Arduino-FOC-drivers/src/utilities/stm32math/STM32G4CORDICTrigFunctions.cpp

@@ -0,0 +1,83 @@
+
+#include "./STM32G4CORDICTrigFunctions.h"
+
+#ifdef HAL_CORDIC_MODULE_ENABLED
+
+#include "Arduino.h"
+//#include "stm32g4xx_hal_cordic.h"
+#include "stm32g4xx_ll_cordic.h"
+#include "stm32g4xx_ll_rcc.h"
+#include "stm32g4xx_ll_bus.h"
+#include "common/foc_utils.h"
+#include "arm_math.h"
+
+CORDIC_HandleTypeDef thisCordic;
+
+bool SimpleFOC_CORDIC_Config(void){
+    //__HAL_RCC_CORDIC_CLK_ENABLE();
+    // CORDIC_ConfigTypeDef sConfig;
+    // thisCordic.Instance = CORDIC;
+    // if (HAL_CORDIC_Init(&thisCordic) != HAL_OK) {
+    //     Error_Handler();
+    //     return false;
+    // }
+
+    // sConfig.Function = CORDIC_FUNCTION_COSINE;  
+    // sConfig.Precision = CORDIC_PRECISION_6CYCLES;
+    // sConfig.Scale = CORDIC_SCALE_0;
+    // sConfig.NbWrite = CORDIC_NBWRITE_1;
+    // sConfig.NbRead = CORDIC_NBREAD_2;
+    // sConfig.InSize = CORDIC_INSIZE_32BITS;
+    // sConfig.OutSize = CORDIC_OUTSIZE_32BITS;
+    // if (HAL_CORDIC_Configure(&thisCordic, &sConfig) != HAL_OK) {
+    //     /* Channel Configuration Error */
+    //     Error_Handler();
+    //     return false;
+    // }
+
+    LL_AHB1_GRP1_EnableClock(LL_AHB1_GRP1_PERIPH_CORDIC);
+    LL_CORDIC_Config(CORDIC, LL_CORDIC_FUNCTION_COSINE, /* cosine function */
+                   LL_CORDIC_PRECISION_6CYCLES,       /* max precision for q1.31 cosine */
+                   LL_CORDIC_SCALE_0,                 /* no scale */
+                   LL_CORDIC_NBWRITE_1,               /* One input data: angle. Second input data (modulus) is 1 after cordic reset */
+                   LL_CORDIC_NBREAD_2,                /* Two output data: cosine, then sine */
+                   LL_CORDIC_INSIZE_32BITS,           /* q1.31 format for input data */
+                   LL_CORDIC_OUTSIZE_32BITS);         /* q1.31 format for output data */
+    return true;
+};
+
+
+
+float _sin(float a) {
+    a = fmod(a, _2PI);
+    if (a>_PI) a -= _2PI;
+    if (a<-_PI) a += _2PI;
+    CORDIC->WDATA = (q31_t)((a / _PI) * 0x80000000);
+    q31_t out_cos = (int32_t)CORDIC->RDATA; // read cosine result
+    q31_t out_sin = (int32_t)CORDIC->RDATA; // read sine result
+    return (float)out_sin / (float)0x80000000;
+}
+
+float _cos(float a) {
+    a = fmod(a, _2PI);
+    if (a>_PI) a -= _2PI;
+    if (a<-_PI) a += _2PI;    
+    CORDIC->WDATA = (q31_t)((a / _PI) * 0x80000000);
+    q31_t out_cos = (int32_t)CORDIC->RDATA; // read cosine result
+    q31_t out_sin = (int32_t)CORDIC->RDATA; // read sine result
+    return (float)out_cos / (float)0x80000000;
+}
+
+void _sincos(float a, float* s, float* c) {
+    a = fmod(a, _2PI);
+    if (a>_PI) a -= _2PI;
+    if (a<-_PI) a += _2PI;    
+    CORDIC->WDATA = (q31_t)((a / _PI) * 0x80000000);
+    q31_t out_cos = (int32_t)CORDIC->RDATA; // read cosine result
+    q31_t out_sin = (int32_t)CORDIC->RDATA; // read sine result
+    *c = (float)out_cos / (float)0x80000000;
+    *s = (float)out_sin / (float)0x80000000;
+}
+
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/utilities/stm32math/STM32G4CORDICTrigFunctions.h

@@ -0,0 +1,10 @@
+
+#pragma once
+
+#ifdef HAL_CORDIC_MODULE_ENABLED
+
+
+bool SimpleFOC_CORDIC_Config(void);
+
+
+#endif

firmware/lib/Arduino-FOC-drivers/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

firmware/lib/Arduino-FOC-drivers/src/utilities/stm32pwm/STM32PWMInput.cpp

@@ -0,0 +1,108 @@
+
+#include "./STM32PWMInput.h"
+
+#if defined(_STM32_DEF_)
+
+
+
+STM32PWMInput::STM32PWMInput(int pin){
+    _pin = digitalPinToPinName(pin);
+};
+
+
+STM32PWMInput::~STM32PWMInput(){};
+
+
+
+
+
+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
+    timer.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
+    timer.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
+    if (channel!=1 && channel!=2) // only channels 1 & 2 supported
+        return -10;
+    useChannel2 = (channel==2);// remember the channel
+    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 = (channel==1)?TIM_INPUTCHANNELPOLARITY_RISING:TIM_INPUTCHANNELPOLARITY_FALLING,
+        .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 = (channel==1)?TIM_INPUTCHANNELPOLARITY_FALLING:TIM_INPUTCHANNELPOLARITY_RISING;
+    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(){
+    if (useChannel2)
+        return timer.Instance->CCR1;
+    else
+        return timer.Instance->CCR2;
+};
+
+
+uint32_t STM32PWMInput::getPeriodTicks(){
+    if (useChannel2)
+        return timer.Instance->CCR2;
+    else
+        return timer.Instance->CCR1;
+};
+
+
+
+#endif

firmware/lib/Arduino-FOC-drivers/src/utilities/stm32pwm/STM32PWMInput.h

@@ -0,0 +1,29 @@
+#pragma once
+
+#include "Arduino.h"
+
+#if defined(_STM32_DEF_)
+
+
+class STM32PWMInput {
+    public:
+        STM32PWMInput(int pin);
+        ~STM32PWMInput();
+
+        int initialize();
+
+        float getDutyCyclePercent();
+        uint32_t getDutyCycleTicks();
+        uint32_t getPeriodTicks();
+        
+        PinName _pin;
+    protected:
+        TIM_HandleTypeDef timer;
+        bool useChannel2 = false;
+};
+
+
+
+
+
+#endif