Save OC AHRS last gyro and mag calib to flash

oc_sensors/gyro/Core/Inc/oc_gyro.h

@@ -12,6 +12,8 @@
 #include "oc_sensor_modes.h"
 
 
+#define FLASH_GYRO_CALIB_START_ADD 0x0800F800
+#define FLASH_MEM_CONTROL_NUM 12345
  /* MAG I2C address and register addresses */
 #define MAG_I2C_ADDR			(uint16_t)(0x0D<<1)
 #define MAG_DATA_START_REG		0x00
@@ -150,6 +152,8 @@ void oc_gyro_init(void);
 void oc_gyro_fusion_reset(void);
 void oc_gyro_fusion_step(sensor_mode sensor_state);
 uint8_t oc_gyro_choose_data(uint16_t* data_send, sensor_mode sensor_state);
+void oc_gyro_flash_save_calib(float x, float y, float z, float xm, float ym, float zm);
+void oc_gyro_flash_load_calib(void);
 void oc_gyro_read_mag_data(void);
 void oc_gyro_read_icm_data(void);
 void oc_gyro_read_icm_done(void);

oc_sensors/gyro/Core/Src/main.c

@@ -241,6 +241,7 @@ int main(void)
   oc_gyro_init();
   // Nack timer
   HAL_TIM_Base_Start(&htim17);
+  oc_gyro_flash_load_calib();
   /* USER CODE END 2 */
 
   /* Infinite loop */

oc_sensors/gyro/Core/Src/oc_gyro.c

@@ -214,6 +214,8 @@ void oc_gyro_fusion_step(sensor_mode sensor_state) {
 					GyroCal.array[i] = GyroCalSum.array[i] / GYRO_CALIB_CYCLES;
 					GyroCalSum.array[i] = 0;
 				}
+				oc_gyro_flash_save_calib(GyroCal.axis.x, GyroCal.axis.y, GyroCal.axis.z,
+						HardIronCal.axes[0], HardIronCal.axes[1], HardIronCal.axes[2]);
 			}
 		}
 	} else {
@@ -258,6 +260,8 @@ void oc_gyro_fusion_step(sensor_mode sensor_state) {
 				for (int i=0; i < MAG_CALIB_POS_COUNT; i++) {
 					HardIronCal.pos[i] = 0;
 				}
+				oc_gyro_flash_save_calib(GyroCal.axis.x, GyroCal.axis.y, GyroCal.axis.z,
+						HardIronCal.axes[0], HardIronCal.axes[1], HardIronCal.axes[2]);
 			}
 		}
 	} else {
@@ -340,6 +344,79 @@ uint8_t oc_gyro_choose_data(uint16_t* data_send, sensor_mode sensor_state) {
 	return data_len;
 }
 
+void oc_gyro_flash_load_calib(void) {
+	// Define pointers to the memory locations where data was stored
+	uint64_t calib_data1 = *(uint64_t *)FLASH_GYRO_CALIB_START_ADD;
+	uint64_t calib_data2 = *(uint64_t *)(FLASH_GYRO_CALIB_START_ADD + 8);
+	uint64_t calib_data_m1 = *(uint64_t *)(FLASH_GYRO_CALIB_START_ADD+16);
+	uint64_t calib_data_m2 = *(uint64_t *)(FLASH_GYRO_CALIB_START_ADD + 24);
+
+	float x, y, z, xm, ym, zm;
+
+	uint32_t x_bits = (uint32_t)(calib_data1 >> 32);
+	uint32_t y_bits = (uint32_t)(calib_data1 & 0xFFFFFFFF);
+	uint32_t z_bits = (uint32_t)(calib_data2 >> 32);
+	uint32_t control_num = (uint32_t)(calib_data2 & 0xFFFFFFFF);
+	uint32_t xm_bits = (uint32_t)(calib_data_m1 >> 32);
+	uint32_t ym_bits = (uint32_t)(calib_data_m1 & 0xFFFFFFFF);
+	uint32_t zm_bits = (uint32_t)(calib_data_m2 >> 32);
+	uint32_t control_num_m = (uint32_t)(calib_data_m2 & 0xFFFFFFFF);
+	// Convert the bit patterns back to float
+	memcpy(&x, &x_bits, sizeof(x));
+	memcpy(&y, &y_bits, sizeof(y));
+	memcpy(&z, &z_bits, sizeof(z));
+	memcpy(&xm, &xm_bits, sizeof(xm));
+	memcpy(&ym, &ym_bits, sizeof(ym));
+	memcpy(&zm, &zm_bits, sizeof(zm));
+
+	if (control_num == FLASH_MEM_CONTROL_NUM && control_num_m == FLASH_MEM_CONTROL_NUM) {
+		GyroCal.axis.x = x;
+		GyroCal.axis.y = y;
+		GyroCal.axis.z = z;
+		HardIronCal.axes[0] = xm;
+		HardIronCal.axes[1] = ym;
+		HardIronCal.axes[2] = zm;
+	} else {
+		oc_gyro_flash_save_calib(0, 0, 0, 0, 0, 0);
+	}
+
+
+	//FLASH_CALIB_START_ADD
+}
+
+void oc_gyro_flash_save_calib(float x, float y, float z, float xm, float ym, float zm) {
+	uint32_t x_bits, y_bits, z_bits, xm_bits, ym_bits, zm_bits;
+	memcpy(&x_bits, &x, sizeof(x));
+	memcpy(&y_bits, &y, sizeof(y));
+	memcpy(&z_bits, &z, sizeof(z));
+	memcpy(&xm_bits, &xm, sizeof(xm));
+	memcpy(&ym_bits, &ym, sizeof(ym));
+	memcpy(&zm_bits, &zm, sizeof(zm));
+	uint64_t calib_data1 = ((uint64_t)x_bits << 32) | (uint64_t)y_bits;
+	uint64_t calib_data2 = ((uint64_t)z_bits << 32) | FLASH_MEM_CONTROL_NUM;
+	uint64_t calib_data_m1 = ((uint64_t)xm_bits << 32) | (uint64_t)ym_bits;
+	uint64_t calib_data_m2 = ((uint64_t)zm_bits << 32) | FLASH_MEM_CONTROL_NUM;
+
+	static FLASH_EraseInitTypeDef EraseInitStruct;
+	uint32_t PAGEError;
+	EraseInitStruct.TypeErase   = FLASH_TYPEERASE_PAGES;
+	EraseInitStruct.NbPages = 1;
+	EraseInitStruct.Banks = FLASH_BANK_1;
+	EraseInitStruct.Page = 31;
+	__disable_irq();
+	HAL_FLASH_Unlock();
+	if (HAL_FLASHEx_Erase(&EraseInitStruct, &PAGEError) != HAL_OK)
+	{
+	    HAL_FLASH_GetError ();
+	}
+	/*Write into flash*/
+	HAL_StatusTypeDef status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, FLASH_GYRO_CALIB_START_ADD, calib_data1);
+	status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, FLASH_GYRO_CALIB_START_ADD+8, calib_data2);
+	status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, FLASH_GYRO_CALIB_START_ADD+16, calib_data_m1);
+	status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_DOUBLEWORD, FLASH_GYRO_CALIB_START_ADD+24, calib_data_m2);
+	HAL_FLASH_Lock();
+	__enable_irq();
+}
 
 void oc_gyro_read_mag_data()
 {

oc_sensors/laser_distance/Core/Src/stm32g0xx_it.c

@@ -196,7 +196,11 @@ void USART1_IRQHandler(void)
 		USART1_Rec_Cal();
 		USART1->RQR |= USART_RQR_RXFRQ;
 	}
-
+	// Overrun Error
+	if (USART1->ISR & USART_ISR_ORE) {
+		USART1->ICR = USART_ICR_ORECF;
+		LL_USART_EnableIT_RXNE(USART1);
+	}
   /* USER CODE END USART1_IRQn 0 */
   /* USER CODE BEGIN USART1_IRQn 1 */