20k hz

.gitignore

@@ -7,6 +7,8 @@
 *.cppcheck
 Debug/
 Release/
+__pycache__/
+*.pyc
 
 # Prerequisites
 *.d

01-Firmware/.project

@@ -0,0 +1,11 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<projectDescription>
+	<name>01-Firmware</name>
+	<comment></comment>
+	<projects>
+	</projects>
+	<buildSpec>
+	</buildSpec>
+	<natures>
+	</natures>
+</projectDescription>

01-Firmware/B-G431B-ESC1-v2/.project

@@ -24,7 +24,6 @@
 		<nature>org.eclipse.cdt.core.cnature</nature>
 		<nature>com.st.stm32cube.ide.mcu.MCUCubeIdeServicesRevAev2ProjectNature</nature>
 		<nature>com.st.stm32cube.ide.mcu.MCUAdvancedStructureProjectNature</nature>
-		<nature>com.st.stm32cube.ide.mcu.MCUEndUserDisabledTrustZoneProjectNature</nature>
 		<nature>com.st.stm32cube.ide.mcu.MCUSingleCpuProjectNature</nature>
 		<nature>com.st.stm32cube.ide.mcu.MCURootProjectNature</nature>
 		<nature>org.eclipse.cdt.managedbuilder.core.managedBuildNature</nature>

01-Firmware/B-G431B-ESC1-v2/B-G431B-ESC1-v2.ioc

@@ -99,7 +99,7 @@ Dma.ADC2.3.PeriphDataAlignment=DMA_PDATAALIGN_HALFWORD
 ADC1.Rank-0\#ChannelRegularConversion=1
 Mcu.PinsNb=38
 PC11.Locked=true
-ADC1.IPParameters=Rank-0\#ChannelRegularConversion,Channel-0\#ChannelRegularConversion,SamplingTime-0\#ChannelRegularConversion,OffsetNumber-0\#ChannelRegularConversion,NbrOfConversionFlag,Rank-1\#ChannelRegularConversion,Channel-1\#ChannelRegularConversion,SamplingTime-1\#ChannelRegularConversion,OffsetNumber-1\#ChannelRegularConversion,NbrOfConversion,EOCSelection,ExternalTrigConv,Overrun,Rank-2\#ChannelRegularConversion,Channel-2\#ChannelRegularConversion,SamplingTime-2\#ChannelRegularConversion,OffsetNumber-2\#ChannelRegularConversion,Rank-3\#ChannelRegularConversion,Channel-3\#ChannelRegularConversion,SamplingTime-3\#ChannelRegularConversion,OffsetNumber-3\#ChannelRegularConversion,master,Rank-4\#ChannelRegularConversion,Channel-4\#ChannelRegularConversion,SamplingTime-4\#ChannelRegularConversion,OffsetNumber-4\#ChannelRegularConversion
+ADC1.IPParameters=Rank-0\#ChannelRegularConversion,Channel-0\#ChannelRegularConversion,SamplingTime-0\#ChannelRegularConversion,OffsetNumber-0\#ChannelRegularConversion,NbrOfConversionFlag,Rank-1\#ChannelRegularConversion,Channel-1\#ChannelRegularConversion,SamplingTime-1\#ChannelRegularConversion,OffsetNumber-1\#ChannelRegularConversion,NbrOfConversion,EOCSelection,ExternalTrigConv,Overrun,Rank-2\#ChannelRegularConversion,Channel-2\#ChannelRegularConversion,SamplingTime-2\#ChannelRegularConversion,OffsetNumber-2\#ChannelRegularConversion,Rank-3\#ChannelRegularConversion,Channel-3\#ChannelRegularConversion,SamplingTime-3\#ChannelRegularConversion,OffsetNumber-3\#ChannelRegularConversion,master,Rank-4\#ChannelRegularConversion,Channel-4\#ChannelRegularConversion,SamplingTime-4\#ChannelRegularConversion,OffsetNumber-4\#ChannelRegularConversion,DMAContinuousRequests
 PC13.Locked=true
 TIM1.CounterMode=TIM_COUNTERMODE_CENTERALIGNED1
 ADC1.OffsetNumber-0\#ChannelRegularConversion=ADC_OFFSET_NONE
@@ -121,7 +121,7 @@ PA11.Locked=true
 Dma.ADC1.2.MemInc=DMA_MINC_ENABLE
 TIM4.IPParameters=Prescaler,ICPolarity_1
 Dma.ADC1.2.SyncSignalID=NONE
-ADC2.IPParameters=Rank-0\#ChannelRegularConversion,Channel-0\#ChannelRegularConversion,SamplingTime-0\#ChannelRegularConversion,OffsetNumber-0\#ChannelRegularConversion,NbrOfConversionFlag,EOCSelection,ExternalTrigConv,Rank-1\#ChannelRegularConversion,Channel-1\#ChannelRegularConversion,SamplingTime-1\#ChannelRegularConversion,OffsetNumber-1\#ChannelRegularConversion,NbrOfConversion,Overrun,Rank-2\#ChannelRegularConversion,Channel-2\#ChannelRegularConversion,SamplingTime-2\#ChannelRegularConversion,OffsetNumber-2\#ChannelRegularConversion
+ADC2.IPParameters=Rank-0\#ChannelRegularConversion,Channel-0\#ChannelRegularConversion,SamplingTime-0\#ChannelRegularConversion,OffsetNumber-0\#ChannelRegularConversion,NbrOfConversionFlag,EOCSelection,ExternalTrigConv,Rank-1\#ChannelRegularConversion,Channel-1\#ChannelRegularConversion,SamplingTime-1\#ChannelRegularConversion,OffsetNumber-1\#ChannelRegularConversion,NbrOfConversion,Overrun,Rank-2\#ChannelRegularConversion,Channel-2\#ChannelRegularConversion,SamplingTime-2\#ChannelRegularConversion,OffsetNumber-2\#ChannelRegularConversion,DMAContinuousRequests
 PC6.Locked=true
 PA9.Signal=S_TIM1_CH2
 VP_TIM1_VS_ClockSourceINT.Signal=TIM1_VS_ClockSourceINT
@@ -155,6 +155,7 @@ Mcu.Pin32=VP_ADC2_OPAMP_INTERNAL3
 PA9.Locked=true
 NVIC.NonMaskableInt_IRQn=true\:0\:0\:false\:false\:true\:false\:false
 PC10.GPIOParameters=GPIO_Label
+ADC1.DMAContinuousRequests=ENABLE
 PB7.GPIO_PuPdOD=GPIO_NOPULL
 PA13.Mode=Serial_Wire
 ProjectManager.FreePins=false
@@ -209,7 +210,7 @@ PF0-OSC_IN.Signal=RCC_OSC_IN
 Dma.USART2_TX.1.MemDataAlignment=DMA_MDATAALIGN_BYTE
 RCC.I2C2Freq_Value=160000000
 PB0.Signal=OPAMP3_VINP
-TIM1.PeriodNoDither=4999
+TIM1.PeriodNoDither=1999
 Dma.USART2_TX.1.SyncSignalID=NONE
 Mcu.Family=STM32G4
 ProjectManager.MainLocation=Core/Src
@@ -248,12 +249,13 @@ Mcu.IP1=ADC2
 PA12.Signal=TIM1_CH2N
 TIM1.Prescaler=0
 Mcu.UserConstants=
+ADC2.DMAContinuousRequests=ENABLE
 Dma.USART2_RX.0.Priority=DMA_PRIORITY_LOW
 Mcu.ThirdPartyNb=0
 ADC1.Channel-3\#ChannelRegularConversion=ADC_CHANNEL_1
 RCC.HCLKFreq_Value=160000000
 Mcu.IPNb=16
-ProjectManager.PreviousToolchain=
+ProjectManager.PreviousToolchain=STM32CubeIDE
 Dma.ADC2.3.PeriphInc=DMA_PINC_DISABLE
 Mcu.Pin6=PA3
 Mcu.Pin7=PA5
@@ -325,12 +327,13 @@ PC14-OSC32_IN.GPIO_Label=CAN_TERM
 ADC2.Rank-0\#ChannelRegularConversion=1
 PF1-OSC_OUT.Mode=HSE-External-Oscillator
 Dma.Request0=USART2_RX
-NVIC.CORDIC_IRQn=true\:0\:0\:false\:false\:true\:true\:true
+NVIC.CORDIC_IRQn=true\:2\:0\:true\:false\:true\:true\:true
 PC14-OSC32_IN.Locked=true
 PB4.Signal=USART2_RX
 PB3.Locked=true
 PB15.Locked=true
 PA3.Signal=OPAMP1_VINM0
+NVIC.FDCAN1_IT0_IRQn=true\:3\:0\:true\:false\:true\:true\:true
 PB7.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH
 RCC.PLLSourceVirtual=RCC_PLLSOURCE_HSE
 RCC.I2SFreq_Value=160000000
@@ -353,7 +356,7 @@ Dma.ADC2.3.Instance=DMA1_Channel4
 FDCAN1.IPParameters=AutoRetransmission,TransmitPause,NominalSyncJumpWidth,ClockDivider,NominalTimeSeg1,NominalTimeSeg2,DataSyncJumpWidth,DataTimeSeg1,DataTimeSeg2,StdFiltersNbr
 Dma.USART2_TX.1.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
 Dma.USART2_TX.1.RequestNumber=1
-NVIC.USART2_IRQn=true\:0\:0\:false\:false\:true\:true\:true
+NVIC.USART2_IRQn=true\:4\:0\:true\:false\:true\:true\:true
 ADC1.SamplingTime-2\#ChannelRegularConversion=ADC_SAMPLETIME_2CYCLES_5
 Dma.USART2_TX.1.SignalID=NONE
 RCC.FamilyName=M

01-Firmware/B-G431B-ESC1-v2/Core/Inc/as5048a.h

@@ -17,14 +17,15 @@ extern "C" {
 // Support AS5048A PWM interface only
 
 void API_AS5048A_Position_Sensor_Init(TIM_HandleTypeDef * htim);
-void API_AS5048A_Position_Sensor_It(TIM_HandleTypeDef *htim);
+void API_AS5048A_Position_Sensor_It(TIM_HandleTypeDef *htim) __attribute__((section (".ccmram")));
 float API_AS5048A_Position_Sensor_Get_Radians();
-float API_AS5048A_Position_Sensor_Get_Radians_Estimation(uint16_t time_us);
+float API_AS5048A_Position_Sensor_Get_Radians_Estimation(uint16_t time_us) __attribute__((section (".ccmram")));
 float API_AS5048A_Position_Sensor_Get_Multiturn_Radians();
 float API_AS5048A_Position_Sensor_Get_RPS();
 float API_AS5048A_Position_Sensor_Get_DPS();
 uint16_t API_AS5048A_Position_Sensor_Get_Timestamp();
 uint16_t API_AS5048A_Position_Sensor_Get_DeltaTimestamp();
+int16_t API_AS5048A_Position_Sensor_Get_DeltaTimeEstimation();
 uint32_t API_AS5048A_Position_Sensor_Error();
 uint32_t API_AS5048A_Position_Sensor_Error_Counter();
 float API_AS5048A_Position_Sensor_Get_DeltaRad();

01-Firmware/B-G431B-ESC1-v2/Core/Inc/control_table.h

@@ -85,23 +85,23 @@ extern "C" {
 #define REG_PID_TORQUE_CURRENT_KFF_L			0x44	//68 // RESERVED
 #define REG_PID_TORQUE_CURRENT_KFF_H			0x45	//69 // RESERVED
 
-#define REG_CAL_PHASE1_CURRENT_SENSE_MA_L		0x46	//70
-#define REG_CAL_PHASE1_CURRENT_SENSE_MA_H		0x47	//71
-#define REG_CAL_PHASE1_CURRENT_SENSE_OFFSET_L	0x48	//72
-#define REG_CAL_PHASE1_CURRENT_SENSE_OFFSET_H	0x49	//73
+#define REG_CAL_PHASE1_CURRENT_SENSE_MA_L		0x46	//70 // RESERVED
+#define REG_CAL_PHASE1_CURRENT_SENSE_MA_H		0x47	//71 // RESERVED
+#define REG_CAL_PHASE1_CURRENT_SENSE_OFFSET_L	0x48	//72 // RESERVED
+#define REG_CAL_PHASE1_CURRENT_SENSE_OFFSET_H	0x49	//73 // RESERVED
 
-#define REG_CAL_PHASE2_CURRENT_SENSE_MA_L		0x4A	//74
-#define REG_CAL_PHASE2_CURRENT_SENSE_MA_H		0x4B	//75
-#define REG_CAL_PHASE2_CURRENT_SENSE_OFFSET_L	0x4C	//76
-#define REG_CAL_PHASE2_CURRENT_SENSE_OFFSET_H	0x4D	//77
+#define REG_CAL_PHASE2_CURRENT_SENSE_MA_L		0x4A	//74 // RESERVED
+#define REG_CAL_PHASE2_CURRENT_SENSE_MA_H		0x4B	//75 // RESERVED
+#define REG_CAL_PHASE2_CURRENT_SENSE_OFFSET_L	0x4C	//76 // RESERVED
+#define REG_CAL_PHASE2_CURRENT_SENSE_OFFSET_H	0x4D	//77 // RESERVED
 
-#define REG_CAL_PHASE3_CURRENT_SENSE_MA_L		0x4E	//78
-#define REG_CAL_PHASE3_CURRENT_SENSE_MA_H		0x4F	//79
-#define REG_CAL_PHASE3_CURRENT_SENSE_OFFSET_L	0x50	//80
-#define REG_CAL_PHASE3_CURRENT_SENSE_OFFSET_H	0x51	//81
+#define REG_CAL_PHASE3_CURRENT_SENSE_MA_L		0x4E	//78 // RESERVED
+#define REG_CAL_PHASE3_CURRENT_SENSE_MA_H		0x4F	//79 // RESERVED
+#define REG_CAL_PHASE3_CURRENT_SENSE_OFFSET_L	0x50	//80 // RESERVED
+#define REG_CAL_PHASE3_CURRENT_SENSE_OFFSET_H	0x51	//81 // RESERVED
 
-#define REG_CAL_VOLTAGE_SENSOR_L				0x52	//82
-#define REG_CAL_VOLTAGE_SENSOR_H				0x53	//83
+#define REG_CAL_VOLTAGE_SENSOR_L				0x52	//82 // RESERVED
+#define REG_CAL_VOLTAGE_SENSOR_H				0x53	//83 // RESERVED
 
 #define REG_EWMA_ENCODER						0x54	//84
 
@@ -117,11 +117,11 @@ extern "C" {
 #define REG_GOAL_TORQUE_CURRENT_MA_H			0x88 //8
 #define REG_GOAL_FLUX_CURRENT_MA_L				0x89 //9
 #define REG_GOAL_FLUX_CURRENT_MA_H				0x8A //10
-#define REG_GOAL_KP								0x8B //11
-#define REG_GOAL_KD								0x8C //12
-#define REG_GOAL_SYNCHRO_OFFSET_L				0x8D //13
-#define REG_GOAL_SYNCHRO_OFFSET_H				0x8E //14
-#define REG_GOAL_CLOSED_LOOP					0x8F //15
+#define REG_GOAL_POS_KP							0x8B //11
+#define REG_GOAL_POS_KD							0x8C //12
+#define REG_GOAL_VEL_KP							0x8D //13
+#define REG_GOAL_SYNCHRO_OFFSET_L				0x8E //14
+#define REG_GOAL_SYNCHRO_OFFSET_H				0x8F //15
 
 #define REG_PRESENT_POSITION_DEG_L				0x90 //16
 #define REG_PRESENT_POSITION_DEG_H				0x91 //17
@@ -148,6 +148,7 @@ extern "C" {
 #define REG_PROCESSING_TIME 					0xAA //42
 #define REG_FOC_FREQUENCY	 					0xAB //43
 #define REG_PID_FREQUENCY	 					0xAC //44
+#define REG_MLP_FREQUENCY	 					0xAD //45
 
 // SW & HW ERROR
 #define REG_PROTOCOL_CRC_FAIL 					0xB0 //48
@@ -158,78 +159,71 @@ extern "C" {
 
 // REGISTER FACTORY DEFAULT VALUES ////////////////////////////////////////////
 
-#define REG_MODEL_NUMBER_VALUE 				92
-#define REG_VERSION_VALUE 					0
-#define REG_ID_VALUE 						1
+#define REG_MODEL_NUMBER_VALUE 				92		//
+#define REG_VERSION_VALUE 					30		// FOC V3.0
+#define REG_ID_VALUE 						1		// Default ID
 #define REG_BAUD_RATE_VALUE 				3		// 1: 1Mbps
 #define REG_RETURN_DELAY_VALUE 				0		// 0: zero delay
 
-#define REG_MIN_POSITION_DEG_VALUE 			0		// deg
-#define REG_MAX_POSITION_DEG_VALUE 			1000	// deg
-#define REG_MAX_VELOCITY_DPS_VALUE			10000	// dps
-#define REG_MAX_ACCELERATION_DPSS_VALUE		32000	// dpss
-#define REG_MAX_CURRENT_MA_VALUE			3500	// mA
+#define REG_MIN_POSITION_DEG_VALUE 			-800   	// deg
+#define REG_MAX_POSITION_DEG_VALUE 			800		// deg
+#define REG_MAX_VELOCITY_DPS_VALUE			16000	// dps
+#define REG_MAX_ACCELERATION_DPSS_VALUE		0		// dpss
+#define REG_MAX_CURRENT_MA_VALUE			26000	// mA
 
-#define REG_TEMPERATURE_LIMIT_VALUE		 	60		// °C
-#define REG_LOW_VOLTAGE_LIMIT_VALUE		 	6		// V
+#define REG_TEMPERATURE_LIMIT_VALUE		 	70		// °C
+#define REG_LOW_VOLTAGE_LIMIT_VALUE		 	10		// V
 #define REG_HIGH_VOLTAGE_LIMIT_VALUE		28		// V
 
 #define REG_MOVING_THRESHOLD_DPS_VALUE		5		// dps
-#define REG_STATUS_RETURN_LVL_VALUE			2		// TODO : fill comment
-#define REG_ALARM_LED_VALUE					36		// TODO : fill comment
-#define REG_ALARM_SHUTDOWN_VALUE			36		// TODO : fill comment
+#define REG_STATUS_RETURN_LVL_VALUE			0		// TODO : fill comment
+#define REG_ALARM_LED_VALUE					0		// TODO : fill comment
+#define REG_ALARM_SHUTDOWN_VALUE			0		// TODO : fill comment
 
 #define REG_ENCODER_BITS_VALUE				14		//
 #define REG_MOTOR_POLE_PAIRS_VALUE			14		//
 #define REG_MOTOR_SYNCHRO_VALUE				0		// deg
 #define REG_INV_PHASE_VALUE					0		// 0: NORMAL 1: INV
 #define REG_FIELD_WEAKENING_K_VALUE			255		// (1/)255 is minimum value, recommended around (1/)5
 
-#define REG_PID_POSITION_KP_VALUE			30	// k x1
-#define REG_PID_POSITION_KI_VALUE			0	// k x100
-#define REG_PID_POSITION_KD_VALUE			100	// k x1
+#define REG_PID_POSITION_KP_VALUE			0		// k x1
+#define REG_PID_POSITION_KI_VALUE			0		// k x100
+#define REG_PID_POSITION_KD_VALUE			0		// k x1
 
-#define REG_PID_VELOCITY_KP_VALUE			0	// k x1000
-#define REG_PID_VELOCITY_KI_VALUE			0	// k x100000
-#define REG_PID_VELOCITY_KD_VALUE			0	// k x1000
-#define REG_PID_VELOCITY_KFF_VALUE			0	// k x1000
+#define REG_PID_VELOCITY_KP_VALUE			0		// k x1000
+#define REG_PID_VELOCITY_KI_VALUE			0		// k x100000
+#define REG_PID_VELOCITY_KD_VALUE			0		// k x1000
+#define REG_PID_VELOCITY_KFF_VALUE			0		// k x1000
 
 #define REG_PID_ACCELERATION_KFF_VALUE		0		// k x1000
 
-#define REG_PID_FLUX_CURRENT_KP_VALUE		2000	// k x100000
-#define REG_PID_FLUX_CURRENT_KI_VALUE		0		// k x10000000
+#define REG_PID_FLUX_CURRENT_KP_VALUE		10		// k x100000
+#define REG_PID_FLUX_CURRENT_KI_VALUE		10		// k x10000000
 #define REG_PID_FLUX_CURRENT_KFF_VALUE		0		// k x100000
 
-#define REG_PID_TORQUE_CURRENT_KP_VALUE		2000	// k x100000
-#define REG_PID_TORQUE_CURRENT_KI_VALUE		0		// k x10000000
+#define REG_PID_TORQUE_CURRENT_KP_VALUE		10		// k x100000
+#define REG_PID_TORQUE_CURRENT_KI_VALUE		10		// k x10000000
 #define REG_PID_TORQUE_CURRENT_KFF_VALUE	0		// k x100000
 
-#define REG_CAL_PHASE1_CURRENT_SENSE_MA_VALUE	280	// mA x1000
-#define REG_CAL_PHASE2_CURRENT_SENSE_MA_VALUE	280	// mA x1000
-#define REG_CAL_PHASE3_CURRENT_SENSE_MA_VALUE	280	// mA x1000
+#define REG_CAL_PHASE1_CURRENT_SENSE_MA_VALUE	29	// mA x1000
+#define REG_CAL_PHASE2_CURRENT_SENSE_MA_VALUE	29	// mA x1000
+#define REG_CAL_PHASE3_CURRENT_SENSE_MA_VALUE	29	// mA x1000
 
-#define REG_CAL_PHASE1_CURRENT_SENSE_OFFSET_VALUE 2464 // ADC
-#define REG_CAL_PHASE2_CURRENT_SENSE_OFFSET_VALUE 2482 // ADC
-#define REG_CAL_PHASE3_CURRENT_SENSE_OFFSET_VALUE 2485 // ADC
+#define REG_CAL_PHASE1_CURRENT_SENSE_OFFSET_VALUE 2500 // ADC
+#define REG_CAL_PHASE2_CURRENT_SENSE_OFFSET_VALUE 2500 // ADC
+#define REG_CAL_PHASE3_CURRENT_SENSE_OFFSET_VALUE 2500 // ADC
 
 #define REG_CAL_VOLTAGE_SENSOR_VALUE 1000 // V
 
 #define REG_EWMA_ENCODER_VALUE 255
 
-// REGISTER CONTROL MODE VALUES //////////////////////////////////////////////////////
-
-#define REG_CONTROL_MODE_IDLE 0
-#define REG_CONTROL_MODE_POSITION_VELOCITY_TORQUE 1
-// #define REG_CONTROL_MODE_POSITION_VELOCITY_TORQUE_VELOCITY_PROFIL 1  // Deprecated
-// #define REG_CONTROL_MODE_VELOCITY_TORQUE 2 // Deprecated
-// #define REG_CONTROL_MODE_TORQUE 3 // Deprecated
-// #define REG_CONTROL_MODE_VELOCITY_TORQUE_OPEN_LOOP 4 // Deprecated
-
 // REGISTER ERROR VALUES //////////////////////////////////////////////////////
 
 #define HW_ERROR_BIT_VOLTAGE 0
 #define HW_ERROR_BIT_POSITION_SENSOR_STATUS_ERROR 1
 #define HW_ERROR_BIT_POSITION_SENSOR_NOT_RESPONDING 2
+#define HW_ERROR_BIT_POSITION_SENSOR_TIMESTAMP 3
+#define HW_ERROR_BIT_FOC_TIMEOUT 4
 #define HW_ERROR_BIT_OVERLOAD 5
 #define HW_ERROR_BIT_OVERHEATING 6
 

01-Firmware/B-G431B-ESC1-v2/Core/Inc/cordic.h

@@ -13,27 +13,35 @@ extern "C" {
 #endif
 
 #include "stm32g4xx_hal.h"
+#include "math_tool.h"
+
 #include <math.h>
 
-#define TWO_TO_POW_31 2147483648.0f
-#define Q31_TO_FLOAT(x) ( (float)(x)/(TWO_TO_POW_31))
-#define FLOAT_TO_Q31(x) ( (int)( (float)(x)*(float)0x7FFFFFFF ) )
+// CORDIC
+extern CORDIC_HandleTypeDef hcordic;
+
+// source : MJBot https://github.com/mjbots/moteus/blob/main/fw/math.h
+int32_t RadiansToQ31(float) __attribute__((always_inline));
 
-float WRAP_TO_PI(float angle_radians)
+inline int32_t RadiansToQ31(float x)
 {
-	if (angle_radians>=0)
-        return fmodf(angle_radians+M_PI, 2.0f*M_PI)-M_PI;
-    else
-    	return fmodf(angle_radians-M_PI, 2.0f*M_PI)+M_PI;
+  // First we scale, then wrap, and finally convert out.
+  const float scaled = x / M_2PI;
+  // Now we wrap to be from 0 to 1.
+  const int32_t i = (int32_t)(scaled);
+  float mod = scaled - i;
+  if (mod < 0) { mod += 1.0f; }
+
+  return (int32_t)(((mod > 0.5f) ? (mod - 1.0f) : mod) * 4294967296.0f);
 }
 
-int32_t FLOAT_RADIANS_TO_Q31(float angle_radians)  // Q31 have a scaled input with the range [-1 1] mapping to [-pi pi).
+float Q31ToRadians(int32_t) __attribute__((always_inline));
+
+inline float Q31ToRadians(int32_t x)
 {
-	return FLOAT_TO_Q31(WRAP_TO_PI(angle_radians)/M_PI);
+  return (float)(x)/2147483648.0f;
 }
 
-extern CORDIC_HandleTypeDef hcordic;
-
 HAL_StatusTypeDef API_CORDIC_Processor_Init()
 {
 	CORDIC_ConfigTypeDef config = {
@@ -43,25 +51,30 @@ HAL_StatusTypeDef API_CORDIC_Processor_Init()
 			CORDIC_OUTSIZE_32BITS, // q31
 			CORDIC_NBWRITE_1, // ARG2 is 1 default
 			CORDIC_NBREAD_2, // read cosine and sine
-			CORDIC_PRECISION_6CYCLES // better than 10-3
+			CORDIC_PRECISION_5CYCLES // better than 10-3
 	};
 	return HAL_CORDIC_Configure(&hcordic, &config);
 }
 
-HAL_StatusTypeDef API_CORDIC_Processor_Update(float theta, float * c, float * s); // __attribute__((section (".ccmram")));
+HAL_StatusTypeDef API_CORDIC_Processor_Update(float theta_rad, float * c, float * s) __attribute__((always_inline));
 
-HAL_StatusTypeDef API_CORDIC_Processor_Update(float theta, float * c, float * s)
+inline HAL_StatusTypeDef API_CORDIC_Processor_Update(float theta_rad, float * c, float * s)
 {
-	static int32_t InBuff[1] = {0};
-	static int32_t OutBuff[2] = {0,0};
-	InBuff[0] = FLOAT_RADIANS_TO_Q31(theta);
-	HAL_StatusTypeDef result = HAL_CORDIC_Calculate(&hcordic,InBuff,OutBuff,1,10);
+	int32_t InBuff = RadiansToQ31(theta_rad);
+	int32_t OutBuff[2] = {0,0};
+	HAL_StatusTypeDef result = HAL_CORDIC_Calculate(&hcordic,&InBuff,OutBuff,1,0);
 	if(HAL_OK==result)
 	{
-		if(c!=0)
-			*c = Q31_TO_FLOAT(OutBuff[0]);
-		if(s!=0)
-			*s = Q31_TO_FLOAT(OutBuff[1]);
+		*c = Q31ToRadians(OutBuff[0]);
+		*s = Q31ToRadians(OutBuff[1]);
+	}
+	else
+	{
+		//regs[REG_PROTOCOL_CRC_FAIL]++; // DEBUG
+		// TODO hardware error !
+		// TODO hardware error !
+		// TODO hardware error !
+		// TODO hardware error !
 	}
 	return result;
 }