Микроконтроллер переведён на работу на 170 МГц

Source/BSP/Inc/imu.h

@@ -36,6 +36,21 @@ typedef struct
 	int16_t gx, gy, gz; // lsb
 } imu_raw_t;
 
+/*typedef struct I2C_Request
+{
+    uint8_t addr;
+    uint8_t reg;
+    uint8_t *buf;
+    uint8_t len;
+
+    void (*callback)(uint8_t*);
+
+    struct I2C_Request* next;
+} I2C_Request;
+
+static I2C_Request* i2c_head = 0;
+static uint8_t i2c_busy = 0;*/
+
 static void (*i2c_callback)(uint8_t* buf) = 0;
 
 void imu_pow_init();
@@ -50,7 +65,10 @@ void imu_tim6_init(const uint16_t freq);
 
 void i2c_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len);
 
-uint8_t i2c_read_async(uint8_t addr, uint8_t reg, uint8_t len, void (*cb)(uint8_t*));
+/*void i2c_enqueue(I2C_Request* req);
+void i2c_start_next();*/
+
+void i2c_read_async(uint8_t addr, uint8_t reg, uint8_t len, void (*cb)(uint8_t*));
 
 void i2c_write(uint8_t addr, uint8_t reg, uint8_t data);
 

Source/BSP/Inc/imu_processing.h

@@ -5,7 +5,7 @@
 
 
 #define ACCEL_SENS_SCALE_FACTOR		4096.0f
-#define GYRO_SENS_SCALE_FACTOR		16.4f
+#define GYRO_SENS_SCALE_FACTOR		16.384f
 #define PI							          3.14159265359f
 #define DEG2RAD						        PI / 180.0f
 

Source/BSP/Src/imu.c

@@ -1,5 +1,9 @@
 #include "imu.h"
 
+/*static I2C_Request* current_req = 0;
+static uint8_t i2c_buf[16];
+static uint8_t i2c_index = 0;*/
+
 void imu_pow_init()
 {
 	RCC->AHB2ENR |= RCC_AHB2ENR_GPIOCEN;
@@ -103,25 +107,75 @@ void i2c_read(uint8_t addr, uint8_t reg, uint8_t* buf, uint8_t len)
 	I2C1->ICR |= I2C_ICR_STOPCF;
 }
 
-uint8_t i2c_read_async(uint8_t addr, uint8_t reg, uint8_t len, void (*cb)(uint8_t*))
+/*void i2c_enqueue(I2C_Request* req)
 {
-	if (i2c_busy) return 0;
+    req->next = 0;
 
-	i2c_busy = 1;
+    __disable_irq();
 
-	i2c_addr = addr;
-	i2c_reg = reg;
-	i2c_len = len;
-	i2c_callback = cb;
+    if (!i2c_head)
+    {
+        i2c_head = req;
+    }
+    else
+    {
+        I2C_Request* cur = i2c_head;
+        while (cur->next) cur = cur->next;
+        cur->next = req;
+    }
+
+    __enable_irq();
+
+    // если I2C свободен — стартуем
+    if (!i2c_busy)
+    {
+        NVIC_SetPendingIRQ(I2C1_EV_IRQn);
+    }
+}
+
+void i2c_start_next()
+{
+    if (!i2c_head)
+    {
+        i2c_busy = 0;
+        return;
+    }
+
+    i2c_busy = 1;
+
+    current_req = i2c_head;
+    i2c_head = i2c_head->next;
+
+    i2c_index = 0;
+
+    // сначала пишем регистр
+    I2C1->CR2 = (current_req->addr << 1) |
+                (1 << I2C_CR2_NBYTES_Pos) |
+                I2C_CR2_START;
+}*/
+
+void i2c_read_async(uint8_t addr, uint8_t reg, uint8_t len, void (*cb)(uint8_t*))
+{
+  if (i2c_busy) return; // ❗ ВАЖНО
+
+  i2c_busy = 1;
+
+  i2c_addr = addr;
+  i2c_reg = reg;
+  i2c_len = len;
+  i2c_callback = cb;
   
-  // включаем прерывания
-	I2C1->CR1 |= I2C_CR1_TXIE | I2C_CR1_RXIE | I2C_CR1_TCIE | I2C_CR1_STOPIE;
+  I2C1->CR1 |= I2C_CR1_TXIE |
+           I2C_CR1_RXIE |
+           I2C_CR1_TCIE |
+           I2C_CR1_STOPIE;
+
   NVIC_EnableIRQ(I2C1_EV_IRQn);
 
-	// старт записи регистра
-	I2C1->CR2 = (addr << 1) | (1 << I2C_CR2_NBYTES_Pos) | I2C_CR2_START;
-
-	return 1;
+  // старт записи регистра
+  I2C1->CR2 = (addr << 1) |
+              (1 << I2C_CR2_NBYTES_Pos) |
+              I2C_CR2_START;
 }
 
 void i2c_write(uint8_t addr, uint8_t reg, uint8_t data)
@@ -143,47 +197,51 @@ void i2c_write(uint8_t addr, uint8_t reg, uint8_t data)
 
 void I2C1_EV_IRQHandler()
 {
-	static uint8_t index = 0;
-	static uint8_t stage = 0; // 0 = send reg, 1 = read
+  uint32_t isr = I2C1->ISR;
 
-	// TX — отправляем регистр
-	if (I2C1->ISR & I2C_ISR_TXIS)
-	{
-		I2C1->TXDR = i2c_reg;
-	}
+  // TXIS — отправляем регистр
+  if (isr & I2C_ISR_TXIS)
+  {
+    I2C1->TXDR = i2c_reg;
+  }
 
-	// переход к чтению
-	if (I2C1->ISR & I2C_ISR_TC)
-	{
-		I2C1->CR2 = (i2c_addr << 1) |
-		            I2C_CR2_RD_WRN |
-		            (i2c_len << I2C_CR2_NBYTES_Pos) |
-		            I2C_CR2_AUTOEND |
-		            I2C_CR2_START;
+  // TC — запускаем чтение
+  else if (isr & I2C_ISR_TC)
+  {
+    I2C1->CR2 = (i2c_addr << 1) |
+                I2C_CR2_RD_WRN |
+                (i2c_len << I2C_CR2_NBYTES_Pos) |
+                I2C_CR2_AUTOEND |
+                I2C_CR2_START;
+  }
 
-		index = 0;
-		stage = 1;
-	}
+  // RXNE — читаем байты
+  else if (isr & I2C_ISR_RXNE)
+  {
+    static uint8_t index = 0;
+    i2c_buf[index++] = I2C1->RXDR;
 
-	// чтение данных
-	if (I2C1->ISR & I2C_ISR_RXNE)
-	{
-		i2c_buf[index++] = I2C1->RXDR;
-	}
+    if (index >= i2c_len)
+        index = 0;
+  }
 
-	// завершение
-	if (I2C1->ISR & I2C_ISR_STOPF)
-	{
-		I2C1->ICR |= I2C_ICR_STOPCF;
+  // STOP — завершение
+  else if (isr & I2C_ISR_STOPF)
+  {
+    I2C1->ICR |= I2C_ICR_STOPCF;
 
-		i2c_busy = 0;
+    i2c_busy = 0;
     
-    // отключаем IRQ
-    I2C1->CR1 &= ~(I2C_CR1_TXIE | I2C_CR1_RXIE | I2C_CR1_TCIE | I2C_CR1_STOPIE);
+    I2C1->CR1 |= I2C_CR1_TXIE |
+             I2C_CR1_RXIE |
+             I2C_CR1_TCIE |
+             I2C_CR1_STOPIE;
 
-		if (i2c_callback)
-			i2c_callback(i2c_buf);
-	}
+    NVIC_EnableIRQ(I2C1_EV_IRQn);
+
+    if (i2c_callback)
+        i2c_callback(i2c_buf);
+  }
 }
 
 void imu_read_raw(imu_raw_t* data)

Source/Core/Inc/system_stm32g4xx.h

@@ -85,6 +85,7 @@ extern const uint8_t  APBPrescTable[8];     /*!< APB prescalers table values */
 
 extern void SystemInit(void);
 extern void SystemCoreClockUpdate(void);
+extern void SystemClock_Config();
 /**
   * @}
   */

Source/Core/Src/system_stm32g4xx.c

@@ -282,4 +282,64 @@ void SystemCoreClockUpdate(void)
   * @}
   */
 
+void SystemClock_Config() // STM32G431CBT6
+{
+  // 1. Включить тактирование для интерфейса управления питанием (PWR)
+  // Это действие необходимо совершать одним из первых.
+  RCC->APB1ENR1 |= RCC_APB1ENR1_PWREN;
+  
+  // 2. Установить задержку Flash ПЕРЕД любым увеличением частоты.
+  // При переключении на PLL 170 МГц и Vcore Range 1 требуется 4 цикла ожидания.
+  // Безопаснее установить это значение заранее, пока система работает на низкой частоте HSI.
+  MODIFY_REG(FLASH->ACR, FLASH_ACR_LATENCY, FLASH_ACR_LATENCY_4WS);
+  
+  // 3. Включить prefetch buffer, instruction cache и data cache для максимальной производительности.
+  FLASH->ACR |= FLASH_ACR_PRFTEN | FLASH_ACR_ICEN | FLASH_ACR_DCEN;
+  
+  // 4. Включить и дождаться готовности HSI (16 МГц).
+  // Это важно, даже если он уже включен по умолчанию, для явного контроля.
+  RCC->CR |= RCC_CR_HSION;
+  while(!(RCC->CR & RCC_CR_HSIRDY));
+  
+  // 5. Настроить масштабирование напряжения на Range 1 (High-performance).
+  // Это необходимо для работы на высоких частотах.
+  // ВАЖНО: Делать это ДО включения PLL и переключения на него.
+  MODIFY_REG(PWR->CR1, PWR_CR1_VOS, PWR_CR1_VOS_0);
+  // Ожидаем готовности регулятора напряжения (ухода флага VOSF).
+  while ((PWR->SR2 & PWR_SR2_VOSF) != 0);
+  
+  // 6. Включить режим Range 1 Boost для частот > 150 МГц.
+  // Согласно документации (Reference Manual), это нужно делать, когда система
+  // тактируется от HSI/HSE, ДО включения PLL.
+  PWR->CR5 |= PWR_CR5_R1MODE;
+  
+  // 7. Убедиться, что PLL выключен, перед его настройкой.
+  RCC->CR &= ~RCC_CR_PLLON;
+  while(RCC->CR & RCC_CR_PLLRDY);
+  
+  // 8. Настроить PLL для получения 170 МГц от HSI.
+  // SYSCLK = (HSI / M) * N / R = (16МГц / 4) * 85 / 2 = 170 МГц
+  // VCO = (HSI / M) * N = 4МГц * 85 = 340 МГц (в допустимом диапазоне 64..344 МГц)
+  RCC->PLLCFGR = (RCC_PLLCFGR_PLLSRC_HSI |        // Источник: HSI (16 МГц)
+                  (3 << RCC_PLLCFGR_PLLM_Pos) |   // Предделитель M = 4 (записывается 3)
+                  (85 << RCC_PLLCFGR_PLLN_Pos) |  // Множитель N = 85
+                  RCC_PLLCFGR_PLLREN);            // Включить главный выход PLL 'R'
+                                                  // PLLR divider = 2 (по умолчанию, запись 0)
+  
+  // 9. Включить PLL и дождаться его готовности.
+  RCC->CR |= RCC_CR_PLLON;
+  while(!(RCC->CR & RCC_CR_PLLRDY));
+  
+  // 10. Переключить системные часы (SYSCLK) на PLL.
+  MODIFY_REG(RCC->CFGR, RCC_CFGR_SW, RCC_CFGR_SW_PLL);
+  // Ожидаем подтверждения, что система действительно переключилась на PLL.
+  while((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_PLL);
+  
+  // 11. Обновить глобальную переменную с частотой ядра.
+  // Это необходимо для корректной работы функций HAL/CMSIS (например, для настройки SysTick).
+  SystemCoreClock = 170000000UL;
+}
+
+
+
 

Source/INS/IRS.c

@@ -28,7 +28,7 @@ void IRS_update(IRS* irs, float dt)
   // /gyro update
 	
 	// accel update
-	//restoreQuat(irs);
+	// restoreQuat(irs);
 	
 	// /accel update
 }

Source/main.c

@@ -25,26 +25,12 @@ control_channels_t ctrl_chs;
 
 Vector3 euler;
 
-volatile uint32_t us_ticks = 0;
-
-void SysTick_Handler()
-{
-	us_ticks++;
-}
-
-float micros()
-{
-	return (float)us_ticks;
-}
-
-static float last_time = 0;
-
 void imu_callback(uint8_t* buf);
 void delay_ms(uint32_t ms);
 
 int main(void)
 {
-  last_time = micros();
+  SystemClock_Config(); // 170 MHz
   
 	__enable_irq();
 	
@@ -75,25 +61,6 @@ int main(void)
 	  receiver_update(&rx_chs_raw);
 	  rx_chs_normalized = normalize_channels(rx_chs_raw);
 		
-		/*if (irs_update_flag)
-		{
-		  irs_update_flag = 0;
-			
-			imu_read_scaled(&imu, &allowed_calib);
-			
-			irs.gyro.x = imu.gx;
-			irs.gyro.y = imu.gy;
-			irs.gyro.z = imu.gz;
-			
-			irs.accel.x = imu.ax;
-			irs.accel.y = imu.ay;
-			irs.accel.z = imu.az;
-		
-		  IRS_update(&irs, IMU_DT);
-		}*/
-		
-		
-		
 		if (control_update_flag)
 		{
       control_update_flag = 0;
@@ -136,11 +103,6 @@ void imu_callback(uint8_t* buf)
 
 	imu_process_raw(&imu, &imu_raw, &allowed_calib);
 
-	float now = micros();
-	float dt = (now - last_time) * 1e-6f;
-  if (dt <= 0.0f || dt > 0.1f) dt = IMU_DT;
-	last_time = now;
-
 	irs.gyro.x = imu.gx;
 	irs.gyro.y = imu.gy;
 	irs.gyro.z = imu.gz;
@@ -159,6 +121,7 @@ void TIM6_DAC_IRQHandler()
 	if (TIM6->SR & TIM_SR_UIF)
 	{
 		TIM6->SR &= ~TIM_SR_UIF;
+    
     i2c_read_async(ICM_ADDR, 0x2D, 12, imu_callback);
 		control_update_flag = 1;
 	}