diff --git a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
index e632eff..4762cb1 100644
--- a/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
+++ b/src/encoders/stm32hwencoder/STM32HWEncoder.cpp
@@ -19,6 +19,14 @@ STM32HWEncoder::STM32HWEncoder(unsigned int _ppr, int pinA, int pinB, int pinI)
 // function returns encoder handle
 TIM_HandleTypeDef STM32HWEncoder::getEncoderTimerHandle() { return encoder_handle; }
 
+#if STM32_CORE_VERSION_MAJOR >= 3
+void STM32HWEncoder::IndexInterrupt()
+{
+    encoder_handle.Instance->CNT = 0; // reset counter
+    index_found = true;
+}
+#endif
+
 /*
   Shaft angle calculation
 */
@@ -95,7 +103,11 @@ void STM32HWEncoder::init() {
     encoder_config.IC2Filter = 0;
 
     encoder_handle.Instance = InstanceA; // e.g. TIM4;
+    #if STM32_CORE_VERSION_MAJOR >= 3
+    enableTimerClock(encoder_handle.Instance);
+    #else
     enableTimerClock(&encoder_handle);
+    #endif
 
     if (HAL_TIM_Encoder_Init(&encoder_handle, &encoder_config) != HAL_OK) {
         initialized = false;
@@ -125,10 +137,14 @@ void STM32HWEncoder::init() {
     // on index pulse
     if(hasIndex())
     {
+        #if STM32_CORE_VERSION_MAJOR >= 3
+        attachInterruptParam(digitalPinToInterrupt(pinNametoDigitalPin(_pinI)), (void (*)(void*))&STM32HWEncoder::IndexInterrupt, index_polarity, this);
+        #else
         attachInterrupt(digitalPinToInterrupt(pinNametoDigitalPin(_pinI)), [this]() {
             encoder_handle.Instance->CNT = 0; // reset counter
             index_found = true;
         }, index_polarity);
+        #endif
     }
 
     if (HAL_TIM_Encoder_Start(&encoder_handle, TIM_CHANNEL_1) != HAL_OK) {
diff --git a/src/encoders/stm32hwencoder/STM32HWEncoder.h b/src/encoders/stm32hwencoder/STM32HWEncoder.h
index 417934c..ec96713 100644
--- a/src/encoders/stm32hwencoder/STM32HWEncoder.h
+++ b/src/encoders/stm32hwencoder/STM32HWEncoder.h
@@ -29,13 +29,21 @@ class STM32HWEncoder : public Sensor {
     uint32_t cpr;  //!< encoder cpr number
     PinName _pinA, _pinB, _pinI;
     bool index_found;
-    uint32_t index_polarity = RISING;
+    #if STM32_CORE_VERSION_MAJOR >= 3
+    PinStatus index_polarity = RISING;
+    #else
+    int index_polarity = RISING;
+    #endif
     // Use TIM_ENCODERMODE_CLOCKPLUSDIRECTION_X1 for step/dir counting (pinA ch1 dir, pinB ch2 step)
     uint32_t encoder_mode = TIM_ENCODERMODE_TI12; // Must be set before calling init
     
   protected:
     float getSensorAngle() override;
     
+    #if STM32_CORE_VERSION_MAJOR >= 3
+    void IndexInterrupt();
+    #endif
+    
     TIM_HandleTypeDef encoder_handle;
 
 };
diff --git a/src/utilities/stm32math/STM32G4CORDICTrigFunctions.cpp b/src/utilities/stm32math/STM32G4CORDICTrigFunctions.cpp
index 2095ee8..b65dc81 100644
--- a/src/utilities/stm32math/STM32G4CORDICTrigFunctions.cpp
+++ b/src/utilities/stm32math/STM32G4CORDICTrigFunctions.cpp
@@ -9,7 +9,6 @@
 #include "stm32g4xx_ll_rcc.h"
 #include "stm32g4xx_ll_bus.h"
 #include "common/foc_utils.h"
-#include "arm_math.h"
 
 CORDIC_HandleTypeDef thisCordic;
 
@@ -46,37 +45,37 @@ bool SimpleFOC_CORDIC_Config(void){
     return true;
 };
 
+#define float_to_q1_31(in) ((uint32_t)(int32_t)(in * 0x80000000))
+#define q31_to_float(in) ((float)((int32_t)in) / 0x80000000)
 
 
 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;
+    CORDIC->WDATA = float_to_q1_31(a / _PI);
+    uint32_t out_cos = CORDIC->RDATA; // read cosine result
+    float out_sin = q31_to_float(CORDIC->RDATA); // read sine result
+    return out_sin;
 }
 
 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;
+    CORDIC->WDATA = float_to_q1_31(a / _PI);
+    float out_cos = q31_to_float(CORDIC->RDATA); // read cosine result
+    uint32_t out_sin = CORDIC->RDATA; // read sine result
+    return out_cos;
 }
 
 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;
+    CORDIC->WDATA = float_to_q1_31(a / _PI);
+    *c = q31_to_float(CORDIC->RDATA); // read cosine result
+    *s = q31_to_float(CORDIC->RDATA); // read sine result
 }