Add gyro and mag calibration to oc gyro sensor

oc_sensors/gyro/.cproject

@@ -22,7 +22,7 @@
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_coreid.853150589" name="Core" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_coreid" useByScannerDiscovery="false" value="0" valueType="string"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board.1581564930" name="Board" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board" useByScannerDiscovery="false" value="genericBoard" valueType="string"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.296468547" name="Defaults" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults" useByScannerDiscovery="false" value="com.st.stm32cube.ide.common.services.build.inputs.revA.1.0.6 || Debug || true || Executable || com.st.stm32cube.ide.mcu.gnu.managedbuild.option.toolchain.value.workspace || STM32G030K8Tx || 0 || 0 || arm-none-eabi- || ${gnu_tools_for_stm32_compiler_path} || ../Core/Inc | ../Drivers/STM32G0xx_HAL_Driver/Inc | ../Drivers/STM32G0xx_HAL_Driver/Inc/Legacy | ../Drivers/CMSIS/Device/ST/STM32G0xx/Include | ../Drivers/CMSIS/Include ||  ||  || USE_HAL_DRIVER | STM32G030xx | USE_FULL_LL_DRIVER ||  || Drivers | Core/Startup | Core ||  ||  || ${workspace_loc:/${ProjName}/STM32G030K8TX_FLASH.ld} || true || NonSecure ||  || secure_nsclib.o ||  || None ||  ||  || " valueType="string"/>
-							<option id="com.st.stm32cube.ide.mcu.debug.option.cpuclock.250007437" name="Cpu clock frequence" superClass="com.st.stm32cube.ide.mcu.debug.option.cpuclock" useByScannerDiscovery="false" value="16" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.debug.option.cpuclock.250007437" name="Cpu clock frequence" superClass="com.st.stm32cube.ide.mcu.debug.option.cpuclock" useByScannerDiscovery="false" value="32" valueType="string"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat.821420940" name="Use float with printf from newlib-nano (-u _printf_float)" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.nanoprintffloat" useByScannerDiscovery="false" value="false" valueType="boolean"/>
 							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform.2121127138" isAbstract="false" osList="all" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform"/>
 							<builder buildPath="${workspace_loc:/oc_ahrs_project}/Debug" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder.992949292" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder"/>
@@ -102,7 +102,7 @@
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_coreid.1812847181" name="Core" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_coreid" useByScannerDiscovery="false" value="0" valueType="string"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board.914997677" name="Board" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.target_board" useByScannerDiscovery="false" value="genericBoard" valueType="string"/>
 							<option id="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults.356337517" name="Defaults" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.option.defaults" useByScannerDiscovery="false" value="com.st.stm32cube.ide.common.services.build.inputs.revA.1.0.6 || Release || false || Executable || com.st.stm32cube.ide.mcu.gnu.managedbuild.option.toolchain.value.workspace || STM32G030K8Tx || 0 || 0 || arm-none-eabi- || ${gnu_tools_for_stm32_compiler_path} || ../Core/Inc | ../Drivers/STM32G0xx_HAL_Driver/Inc | ../Drivers/STM32G0xx_HAL_Driver/Inc/Legacy | ../Drivers/CMSIS/Device/ST/STM32G0xx/Include | ../Drivers/CMSIS/Include ||  ||  || USE_HAL_DRIVER | STM32G030xx | USE_FULL_LL_DRIVER ||  || Drivers | Core/Startup | Core ||  ||  || ${workspace_loc:/${ProjName}/STM32G030K8TX_FLASH.ld} || true || NonSecure ||  || secure_nsclib.o ||  || None ||  ||  || " valueType="string"/>
-							<option id="com.st.stm32cube.ide.mcu.debug.option.cpuclock.1228995116" name="Cpu clock frequence" superClass="com.st.stm32cube.ide.mcu.debug.option.cpuclock" useByScannerDiscovery="false" value="16" valueType="string"/>
+							<option id="com.st.stm32cube.ide.mcu.debug.option.cpuclock.1228995116" name="Cpu clock frequence" superClass="com.st.stm32cube.ide.mcu.debug.option.cpuclock" useByScannerDiscovery="false" value="32" valueType="string"/>
 							<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform.2073175144" isAbstract="false" osList="all" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.targetplatform"/>
 							<builder buildPath="${workspace_loc:/oc_ahrs_project}/Release" id="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder.1545953281" keepEnvironmentInBuildfile="false" managedBuildOn="true" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.builder"/>
 							<tool id="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler.1663509245" name="MCU GCC Assembler" superClass="com.st.stm32cube.ide.mcu.gnu.managedbuild.tool.assembler">

oc_sensors/gyro/Core/Inc/oc_gyro.h

@@ -89,6 +89,10 @@
 
 /* Other constants */
 #define SAMPLE_RATE 			200		/* For 200Hz ODR of GYRO */
+#define GYRO_CALIB_CYCLES 		(SAMPLE_RATE*2)
+#define MAG_CALIB_MINMAX_THRS 10000
+#define MAG_CALIB_POS_THRS 0.9
+#define MAG_CALIB_POS_COUNT 6
 
  /* Bitfield holding all status variables */
 typedef struct {
@@ -98,9 +102,12 @@ typedef struct {
 	 * is available. Gyro availability triggers Fusion calculations... */
 	uint8_t Gyro_Data_Available : 1;
 	uint8_t Gyro_Timer_Running : 1;
+	uint8_t Gyro_Calibrated : 1;
+	uint8_t Mag_Calibrated : 1;
 	float Gyro_Time_Delta;
 	uint32_t Measure_Count;
 	uint32_t Gyro_Count;
+	uint32_t Gyro_Calib_Count;
 
 } oc_gyro_status; 
 
@@ -123,6 +130,7 @@ typedef struct {
 	int16_t axes[3];
 	int16_t min[3];
 	int16_t max[3];
+	uint8_t pos[MAG_CALIB_POS_COUNT];
 } oc_gyro_calibration;
 
 typedef struct {

oc_sensors/gyro/Core/Src/FusionOffset.c

@@ -22,7 +22,7 @@
 /**
  * @brief Timeout in seconds.
  */
-#define TIMEOUT (5)
+#define TIMEOUT (3)
 
 /**
  * @brief Threshold in degrees per second.

oc_sensors/gyro/Core/Src/main.c

@@ -146,6 +146,9 @@ void USART2_Rec_Cal(void) {
 							&& rx_buff[1] > S_MODE_INIT
 							&& rx_buff[1] < S_MODE_COUNT) {
 							sensor_state = rx_buff[1];
+							Status.Gyro_Calibrated = 0;
+							Status.Mag_Calibrated = 0;
+							Status.Should_Send_UART = 1;
 						}
 					} else if (msg_subtype == MTYPE_COMMAND_WRITE) {
 
@@ -320,7 +323,7 @@ void SystemClock_Config(void)
   RCC_OscInitStruct.PLL.PLLM = RCC_PLLM_DIV1;
   RCC_OscInitStruct.PLL.PLLN = 8;
   RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
-  RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV8;
+  RCC_OscInitStruct.PLL.PLLR = RCC_PLLR_DIV4;
   if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
   {
     Error_Handler();
@@ -334,7 +337,7 @@ void SystemClock_Config(void)
   RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
   RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV1;
 
-  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_0) != HAL_OK)
+  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1) != HAL_OK)
   {
     Error_Handler();
   }
@@ -356,7 +359,7 @@ static void MX_I2C2_Init(void)
 
   /* USER CODE END I2C2_Init 1 */
   hi2c2.Instance = I2C2;
-  hi2c2.Init.Timing = 0x00300718;
+  hi2c2.Init.Timing = 0x00601133;
   hi2c2.Init.OwnAddress1 = 0;
   hi2c2.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
   hi2c2.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
@@ -444,7 +447,7 @@ static void MX_TIM14_Init(void)
 
   /* USER CODE END TIM14_Init 1 */
   htim14.Instance = TIM14;
-  htim14.Init.Prescaler = 16 - 1;
+  htim14.Init.Prescaler = 32 - 1;
   htim14.Init.CounterMode = TIM_COUNTERMODE_UP;
   htim14.Init.Period = 65535;
   htim14.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
@@ -475,7 +478,7 @@ static void MX_TIM16_Init(void)
 
   /* USER CODE END TIM16_Init 1 */
   htim16.Instance = TIM16;
-  htim16.Init.Prescaler = 4000-1;
+  htim16.Init.Prescaler = 8000-1;
   htim16.Init.CounterMode = TIM_COUNTERMODE_UP;
   htim16.Init.Period = 40-1;
   htim16.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
@@ -507,7 +510,7 @@ static void MX_TIM17_Init(void)
 
   /* USER CODE END TIM17_Init 1 */
   htim17.Instance = TIM17;
-  htim17.Init.Prescaler = 16000-1;
+  htim17.Init.Prescaler = 32000-1;
   htim17.Init.CounterMode = TIM_COUNTERMODE_UP;
   htim17.Init.Period = 65535;
   htim17.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;

oc_sensors/gyro/Core/Src/oc_gyro.c

@@ -8,26 +8,28 @@
 #include "main.h"
 #include "oc_gyro.h"
 
+#define max(a,b) \
+({ __typeof__ (a) _a = (a); \
+   __typeof__ (b) _b = (b); \
+ _a > _b ? _a : _b; })
+
+#define min(a,b) \
+({ __typeof__ (a) _a = (a); \
+   __typeof__ (b) _b = (b); \
+ _a > _b ? _b : _a; })
+
 extern I2C_HandleTypeDef hi2c2;
 extern SPI_HandleTypeDef hspi1;
 extern TIM_HandleTypeDef htim14;
 
 /* Fusion-algo variables */
 FusionAhrs ahrs;
-FusionVector latest_gyro;
 FusionVector latest_gyro_cal;
 FusionVector latest_mag;
 FusionVector latest_acc;
 FusionOffset offset;
 
-volatile oc_gyro_status Status = {
-	.Should_Send_UART = 0,
-	.Gyro_Timer_Running = 0,
-	.Gyro_Data_Available = 0,
-	.Gyro_Count = 0,
-	.Measure_Count = 0,
-	.Gyro_Time_Delta = 0
-};
+volatile oc_gyro_status Status = {0};
 
 oc_gyro_data Data = {
 	.MAG = {},
@@ -40,10 +42,14 @@ oc_gyro_fusion Fusion = {
 
 oc_gyro_calibration HardIronCal = {
 	.axes = {260, 929, -610},
-	.min = {10000, 10000, 10000},
-	.max = {-10000, -10000, -10000}
+	.min = {MAG_CALIB_MINMAX_THRS, MAG_CALIB_MINMAX_THRS, MAG_CALIB_MINMAX_THRS},
+	.max = {-MAG_CALIB_MINMAX_THRS, -MAG_CALIB_MINMAX_THRS, -MAG_CALIB_MINMAX_THRS},
+	.pos = {0}
 };
 
+FusionVector GyroCalSum = {0};
+FusionVector GyroCal = {0};
+
 static void Write_To_SPI1_Sync(uint8_t reg_addr, uint8_t* p_write,
 	const uint8_t bytes_to_write);
 static void Read_From_SPI1_Async(uint8_t reg_addr, uint8_t* p_read,
@@ -164,6 +170,7 @@ void oc_gyro_init() {
 	FusionOffsetInitialise(&offset, SAMPLE_RATE);
 	FusionAhrsInitialise(&ahrs);				/* NOTE: Changes with FS! */
 }
+
 void oc_gyro_fusion_step(sensor_mode sensor_state) {
 	FusionVector accelerometer = { .axis = {
 				.x = Get_Accel_Value_In_G(Data.ICM + 1),
@@ -176,21 +183,83 @@ void oc_gyro_fusion_step(sensor_mode sensor_state) {
 			.y = Get_Gyro_Value_In_DPS(Data.ICM + 9),
 			.z = Get_Gyro_Value_In_DPS(Data.ICM + 11)
 	} };
-	latest_gyro = gyroscope;
 
 	/* Magnetometer readings can be in arbitrary units... */
 	/* First casting to int16_t and then to float is needed! */
 	FusionVector magnetometer = { .axis = {
-			.x = (float)(int16_t)(((Data.MAG[1] << 8) | Data.MAG[0]) - HardIronCal.axes[0]),
-			.y = (float)(int16_t)(((Data.MAG[3] << 8) | Data.MAG[2]) - HardIronCal.axes[1]),
-			.z = (float)(int16_t)(((Data.MAG[5] << 8) | Data.MAG[4]) - HardIronCal.axes[2])
+			.x = (float)(int16_t)(((Data.MAG[1] << 8) | Data.MAG[0])),
+			.y = (float)(int16_t)(((Data.MAG[3] << 8) | Data.MAG[2])),
+			.z = (float)(int16_t)(((Data.MAG[5] << 8) | Data.MAG[4]))
 	} };
 
-	latest_acc = accelerometer;
-	latest_mag = magnetometer;
 	/* Update gyroscope offset */
-	gyroscope = FusionOffsetUpdate(&offset, gyroscope);
-	latest_gyro_cal = gyroscope;
+	if (sensor_state == S_MODE_GYRO_CALIB) {
+		if (!Status.Gyro_Calibrated) {
+			for (int i=0; i < 3; i++) {
+				GyroCalSum.array[i] += gyroscope.array[i];
+			}
+			Status.Gyro_Calib_Count++;
+			if (Status.Gyro_Calib_Count >= GYRO_CALIB_CYCLES) {
+				Status.Gyro_Calibrated = 1;
+				Status.Gyro_Calib_Count = 0;
+				FusionOffsetInitialise(&offset, SAMPLE_RATE);
+				for (int i=0; i < 3; i++) {
+					GyroCal.array[i] = GyroCalSum.array[i] / GYRO_CALIB_CYCLES;
+					GyroCalSum.array[i] = 0;
+				}
+			}
+		}
+	} else {
+		for (int i=0; i < 3; i++) {
+			gyroscope.array[i] -= GyroCal.array[i];
+		}
+		gyroscope = FusionOffsetUpdate(&offset, gyroscope);
+		latest_gyro_cal = gyroscope;
+	}
+
+	if (sensor_state == S_MODE_MAG_CALIB) {
+		if (!Status.Mag_Calibrated) {
+			for (int i=0; i < 3; i++) {
+				HardIronCal.max[i] = max(HardIronCal.max[i], magnetometer.array[i]);
+				HardIronCal.min[i] = min(HardIronCal.min[i], magnetometer.array[i]);
+			}
+			FusionVector earth_acc = FusionAhrsGetEarthAcceleration(&ahrs);
+			for (int i=0; i < 3; i++) {
+				if (accelerometer.array[i] > MAG_CALIB_POS_THRS) {
+					HardIronCal.pos[2*i] = 1;
+				}
+				if (accelerometer.array[i] < -MAG_CALIB_POS_THRS) {
+					HardIronCal.pos[2*i + 1] = 1;
+				}
+			}
+			uint8_t pos_count = 0;
+			for (int i=0; i < MAG_CALIB_POS_COUNT; i++) {
+				if (HardIronCal.pos[i]) {
+					pos_count++;
+				}
+			}
+			if (pos_count == MAG_CALIB_POS_COUNT) {
+				Status.Mag_Calibrated = 1;
+				for (int i=0; i < 3; i++) {
+					HardIronCal.axes[i] = (HardIronCal.max[i] + HardIronCal.min[i])/2;
+				}
+				//reset arrays for next calib
+				for (int i=0; i < 3; i++) {
+					HardIronCal.max[i] = -MAG_CALIB_MINMAX_THRS;
+					HardIronCal.min[i] = MAG_CALIB_MINMAX_THRS;
+				}
+				for (int i=0; i < MAG_CALIB_POS_COUNT; i++) {
+					HardIronCal.pos[i] = 0;
+				}
+			}
+		}
+	} else {
+		for (int i=0; i < 3; i++) {
+			magnetometer.array[i] -= HardIronCal.axes[i];
+		}
+
+	}
+
 	/* Update fusion estimation
 	 * (Status.Gyro_Time_Delta is set in ICM data availability interrupt)
 	 * */
@@ -199,22 +268,18 @@ void oc_gyro_fusion_step(sensor_mode sensor_state) {
 			Status.Gyro_Time_Delta);
 	}
 	else {
-		FusionVector no_magnetometer = { .axis = {
-								.x = 0,
-								.y = 0,
-								.z = 0
-		} };
-		FusionAhrsUpdate(&ahrs, gyroscope, accelerometer, no_magnetometer,
-			Status.Gyro_Time_Delta);
+		FusionAhrsUpdateNoMagnetometer(&ahrs, gyroscope, accelerometer,
+				Status.Gyro_Time_Delta);
 	}
 	/* Get quaternion */
 	const FusionQuaternion q = FusionAhrsGetQuaternion(&ahrs);
 	const FusionEuler euler = FusionQuaternionToEuler(q);
+	latest_acc = accelerometer;
+	latest_mag = magnetometer;
 	memcpy(&Fusion.quaternion, &q, sizeof(FusionQuaternion));
 	memcpy(&Fusion.euler, &euler, sizeof(FusionEuler));
-
-
 }
+
 uint8_t oc_gyro_choose_data(uint16_t* data_send, sensor_mode sensor_state) {
 	uint8_t data_len = 0;
 	switch (sensor_state) {
@@ -245,11 +310,11 @@ uint8_t oc_gyro_choose_data(uint16_t* data_send, sensor_mode sensor_state) {
 			data_len = 18;
 			break;
 		case S_MODE_GYRO_CALIB:
-			data_send[0] = 0;
+			data_send[0] = Status.Gyro_Calibrated;
 			data_len = 2;
 			break;
 		case S_MODE_MAG_CALIB:
-			data_send[0] = 0;
+			data_send[0] = Status.Mag_Calibrated;
 			data_len = 2;
 			break;
 		default:

oc_sensors/gyro/gyro.ioc

@@ -80,7 +80,7 @@ GPIO.groupedBy=Group By Peripherals
 I2C2.Analog_Filter=I2C_ANALOGFILTER_DISABLE
 I2C2.I2C_Speed_Mode=I2C_Fast
 I2C2.IPParameters=Analog_Filter,Timing,I2C_Speed_Mode
-I2C2.Timing=0x00300718
+I2C2.Timing=0x00601133
 KeepUserPlacement=false
 Mcu.CPN=STM32G030K8T6
 Mcu.Family=STM32G0
@@ -239,31 +239,31 @@ ProjectManager.UAScriptAfterPath=
 ProjectManager.UAScriptBeforePath=
 ProjectManager.UnderRoot=true
 ProjectManager.functionlistsort=1-SystemClock_Config-RCC-false-HAL-false,2-MX_GPIO_Init-GPIO-false-HAL-true,3-MX_DMA_Init-DMA-false-HAL-true,4-MX_I2C2_Init-I2C2-false-HAL-true,5-MX_USART2_UART_Init-USART2-false-LL-true,6-MX_SPI1_Init-SPI1-false-HAL-true,7-MX_TIM14_Init-TIM14-false-HAL-true,8-MX_TIM16_Init-TIM16-false-HAL-true,9-MX_TIM17_Init-TIM17-false-HAL-true
-RCC.ADCFreq_Value=16000000
-RCC.AHBFreq_Value=16000000
-RCC.APBFreq_Value=16000000
-RCC.APBTimFreq_Value=16000000
-RCC.CortexFreq_Value=16000000
+RCC.ADCFreq_Value=32000000
+RCC.AHBFreq_Value=32000000
+RCC.APBFreq_Value=32000000
+RCC.APBTimFreq_Value=32000000
+RCC.CortexFreq_Value=32000000
 RCC.EXTERNAL_CLOCK_VALUE=12288000
-RCC.FCLKCortexFreq_Value=16000000
+RCC.FCLKCortexFreq_Value=32000000
 RCC.FamilyName=M
-RCC.HCLKFreq_Value=16000000
+RCC.HCLKFreq_Value=32000000
 RCC.HSE_VALUE=8000000
 RCC.HSI_VALUE=16000000
-RCC.I2C1Freq_Value=16000000
-RCC.I2S1Freq_Value=16000000
+RCC.I2C1Freq_Value=32000000
+RCC.I2S1Freq_Value=32000000
 RCC.IPParameters=ADCFreq_Value,AHBFreq_Value,APBFreq_Value,APBTimFreq_Value,CortexFreq_Value,EXTERNAL_CLOCK_VALUE,FCLKCortexFreq_Value,FamilyName,HCLKFreq_Value,HSE_VALUE,HSI_VALUE,I2C1Freq_Value,I2S1Freq_Value,LSCOPinFreq_Value,LSE_VALUE,LSI_VALUE,MCO1PinFreq_Value,PLLPoutputFreq_Value,PLLR,PLLRCLKFreq_Value,PWRFreq_Value,SYSCLKFreq_VALUE,SYSCLKSource,USART1Freq_Value,VCOInputFreq_Value,VCOOutputFreq_Value
 RCC.LSCOPinFreq_Value=32000
 RCC.LSE_VALUE=32768
 RCC.LSI_VALUE=32000
-RCC.MCO1PinFreq_Value=16000000
+RCC.MCO1PinFreq_Value=32000000
 RCC.PLLPoutputFreq_Value=64000000
-RCC.PLLR=RCC_PLLR_DIV8
-RCC.PLLRCLKFreq_Value=16000000
-RCC.PWRFreq_Value=16000000
-RCC.SYSCLKFreq_VALUE=16000000
+RCC.PLLR=RCC_PLLR_DIV4
+RCC.PLLRCLKFreq_Value=32000000
+RCC.PWRFreq_Value=32000000
+RCC.SYSCLKFreq_VALUE=32000000
 RCC.SYSCLKSource=RCC_SYSCLKSOURCE_PLLCLK
-RCC.USART1Freq_Value=16000000
+RCC.USART1Freq_Value=32000000
 RCC.VCOInputFreq_Value=16000000
 RCC.VCOOutputFreq_Value=128000000
 SH.GPXTI1.0=GPIO_EXTI1
@@ -275,18 +275,18 @@ SH.GPXTI6.ConfNb=1
 SPI1.BaudRatePrescaler=SPI_BAUDRATEPRESCALER_16
 SPI1.CLKPhase=SPI_PHASE_2EDGE
 SPI1.CLKPolarity=SPI_POLARITY_HIGH
-SPI1.CalculateBaudRate=1000.0 KBits/s
+SPI1.CalculateBaudRate=2.0 MBits/s
 SPI1.Direction=SPI_DIRECTION_2LINES
 SPI1.IPParameters=VirtualType,Mode,Direction,CalculateBaudRate,BaudRatePrescaler,CLKPolarity,CLKPhase
 SPI1.Mode=SPI_MODE_MASTER
 SPI1.VirtualType=VM_MASTER
 TIM14.IPParameters=Prescaler
-TIM14.Prescaler=16 - 1
+TIM14.Prescaler=32 - 1
 TIM16.IPParameters=Prescaler,Period
 TIM16.Period=40-1
-TIM16.Prescaler=4000-1
+TIM16.Prescaler=8000-1
 TIM17.IPParameters=Prescaler
-TIM17.Prescaler=16000-1
+TIM17.Prescaler=32000-1
 USART2.BaudRate=baud_rate
 USART2.IPParameters=VirtualMode-Asynchronous,BaudRate,OneBitSampling
 USART2.IPParametersWithoutCheck=BaudRate

oc_sensors/rgb_opto/rgb_opto Debug.launch

@@ -40,7 +40,7 @@
     <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.stlink_txt_serial_number" value=""/>
     <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlink.watchdog_config" value="none"/>
     <booleanAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkenable_rtos" value="false"/>
-    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkrestart_configurations" value="{&quot;fVersion&quot;:1,&quot;fItems&quot;:[{&quot;fDisplayName&quot;:&quot;Reset&quot;,&quot;fIsSuppressible&quot;:false,&quot;fResetAttribute&quot;:&quot;Software system reset&quot;,&quot;fResetStrategies&quot;:[{&quot;fDisplayName&quot;:&quot;Software system reset&quot;,&quot;fLaunchAttribute&quot;:&quot;system_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Hardware reset&quot;,&quot;fLaunchAttribute&quot;:&quot;hardware_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset hardware\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Core reset&quot;,&quot;fLaunchAttribute&quot;:&quot;core_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset core\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;None&quot;,&quot;fLaunchAttribute&quot;:&quot;no_reset&quot;,&quot;fGdbCommands&quot;:[],&quot;fCmdOptions&quot;:[&quot;-g&quot;]}],&quot;fGdbCommandGroup&quot;:{&quot;name&quot;:&quot;Additional commands&quot;,&quot;commands&quot;:[]},&quot;fStartApplication&quot;:true}]}"/>
+    <stringAttribute key="com.st.stm32cube.ide.mcu.debug.stlinkrestart_configurations" value="{&quot;fVersion&quot;:1,&quot;fItems&quot;:[{&quot;fDisplayName&quot;:&quot;Reset&quot;,&quot;fIsSuppressible&quot;:false,&quot;fResetAttribute&quot;:&quot;Software system reset&quot;,&quot;fResetStrategies&quot;:[{&quot;fDisplayName&quot;:&quot;Software system reset&quot;,&quot;fLaunchAttribute&quot;:&quot;system_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset\r\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Hardware reset&quot;,&quot;fLaunchAttribute&quot;:&quot;hardware_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset hardware\r\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;Core reset&quot;,&quot;fLaunchAttribute&quot;:&quot;core_reset&quot;,&quot;fGdbCommands&quot;:[&quot;monitor reset core\r\n&quot;],&quot;fCmdOptions&quot;:[&quot;-g&quot;]},{&quot;fDisplayName&quot;:&quot;None&quot;,&quot;fLaunchAttribute&quot;:&quot;no_reset&quot;,&quot;fGdbCommands&quot;:[],&quot;fCmdOptions&quot;:[&quot;-g&quot;]}],&quot;fGdbCommandGroup&quot;:{&quot;name&quot;:&quot;Additional commands&quot;,&quot;commands&quot;:[]},&quot;fStartApplication&quot;:true}]}"/>
     <booleanAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.enableRtosProxy" value="false"/>
     <stringAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyCustomProperties" value=""/>
     <stringAttribute key="com.st.stm32cube.ide.mcu.rtosproxy.rtosProxyDriver" value="threadx"/>