CORDIC trig functions for STM32G4

Author: Richard Unger

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);
+
+```

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

src/utilities/stm32math/STM32G4CORDICTrigFunctions.h

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