Полный переход на C++

*Чтение IMU и обработка его данных выполняется в точности как в рабочей прошивке. *Определение вращения работает корректно.
2026-04-17 13:40:27 +03:00
parent a3d845df9e
commit 0faafbf089
21 changed files with 247 additions and 1210 deletions
--- a/Source/Devices/Motors.cpp
+++ b/Source/Devices/Motors.cpp
@@ -0,0 +1,219 @@
+#include "Motors.h"
+
+
+//======================Init======================
+void motors_init()
+{
+	RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
+	RCC->APB2ENR |= RCC_APB2ENR_TIM1EN;
+	RCC->APB1ENR1 |= RCC_APB1ENR1_TIM2EN;
+	
+	motor_gpio_tim1_ch3_init();
+	motor_gpio_tim1_ch4_init();
+	motors_tim1_ch3_4_init();
+	
+	motor_gpio_tim2_ch1_init();
+	motor_gpio_tim2_ch2_init();
+	motors_tim2_ch1_2_init();
+}
+
+void motor_gpio_tim1_ch3_init()
+{
+	// set alt function mode PA10
+	GPIOA->MODER &= ~(3 << (10 * 2));
+	GPIOA->MODER |= 2 << (10 * 2);
+	
+	// AF6 for PA10
+	GPIOA->AFR[1] &= ~(0xF << 8);
+	GPIOA->AFR[1] |= 6 << 8;
+	
+	// very high speed
+	GPIOA->OSPEEDR |= 3 << (10 * 2);
+}
+
+void motor_gpio_tim1_ch4_init()
+{
+	// set alt function mode PA11
+	GPIOA->MODER &= ~(3 << (11 * 2));
+	GPIOA->MODER |= 2 << (11 * 2);
+	
+	// AF11 for PA11
+	GPIOA->AFR[1] &= ~(0xF << 12);
+	GPIOA->AFR[1] |= 11 << 12;
+	
+	// very high speed
+	GPIOA->OSPEEDR |= 3 << (11 * 2);
+}
+
+void motors_tim1_ch3_4_init()
+{
+	// PWM mode 1
+	TIM1->CCMR2 &= ~TIM_CCMR2_OC3M;
+	TIM1->CCMR2 |= TIM_CCMR2_OC3M_1 | TIM_CCMR2_OC3M_2;
+	
+	TIM1->CCMR2 &= ~TIM_CCMR2_OC4M;
+	TIM1->CCMR2 |= TIM_CCMR2_OC4M_1 | TIM_CCMR2_OC4M_2;
+	
+	// preload enable
+	TIM1->CCMR2 |= TIM_CCMR2_OC3PE;
+	TIM1->CCMR2 |= TIM_CCMR2_OC4PE;
+	
+	// enable capture/compare 3 output
+	TIM1->CCER |= TIM_CCER_CC3E;
+	TIM1->CCER |= TIM_CCER_CC4E;
+	
+	TIM1->PSC = 16 - 1;
+	TIM1->ARR = 20000 - 1;
+	TIM1->CCR3 = 900;
+	TIM1->CCR4 = 900;
+	
+	// TIM1_ARR is buffered
+	TIM1->CR1 |= TIM_CR1_ARPE;
+	
+	// set main output enable
+	TIM1->BDTR |= TIM_BDTR_MOE;
+	
+	// set update generation
+	TIM1->EGR |= TIM_EGR_UG;
+	
+	// set counter enable
+	TIM1->CR1 |= TIM_CR1_CEN;
+}
+
+void motor_gpio_tim2_ch1_init()
+{
+	// set alt function mode PA0
+	GPIOA->MODER &= ~(3 << (0 * 2));
+	GPIOA->MODER |= 2 << (0 * 2);
+	
+	// AF1 for PA0
+	GPIOA->AFR[0] &= ~(0xF << 0);
+	GPIOA->AFR[0] |= 1 << 0;
+	
+	// very high speed
+	GPIOA->OSPEEDR |= 3 << (0 * 2);
+}
+
+void motor_gpio_tim2_ch2_init()
+{
+	// set alt function mode PA1
+	GPIOA->MODER &= ~(3 << (1 * 2));
+	GPIOA->MODER |= 2 << (1 * 2);
+	
+	// AF1 for PA1
+	GPIOA->AFR[0] &= ~(0xF << 4);
+	GPIOA->AFR[0] |= 1 << 4;
+	
+	// very high speed
+	 GPIOA->OSPEEDR |= 3 << (1 * 2);
+}
+
+void motors_tim2_ch1_2_init()
+{
+	// PWM mode 1
+	TIM2->CCMR1 &= ~TIM_CCMR1_OC1M;
+	TIM2->CCMR1 |= TIM_CCMR1_OC1M_1 | TIM_CCMR1_OC1M_2;
+	
+	TIM2->CCMR1 &= ~TIM_CCMR1_OC2M;
+	TIM2->CCMR1 |= TIM_CCMR1_OC2M_1 | TIM_CCMR1_OC2M_2;
+	
+	// preload enable
+	TIM2->CCMR1 |= TIM_CCMR1_OC1PE;
+	TIM2->CCMR1 |= TIM_CCMR1_OC2PE;
+	
+	// enable capture/compare 3 output
+	TIM2->CCER |= TIM_CCER_CC1E;
+	TIM2->CCER |= TIM_CCER_CC2E;
+	
+	TIM2->PSC = 16 - 1;
+	TIM2->ARR = 20000 - 1;
+	TIM2->CCR1 = 900;
+	TIM2->CCR2 = 900;
+	
+	// TIM2_ARR is buffered
+	TIM2->CR1 |= TIM_CR1_ARPE;
+	
+	// set main output enable
+	TIM2->BDTR |= TIM_BDTR_MOE;
+	
+	// set update generation
+	TIM2->EGR |= TIM_EGR_UG;
+	
+	// set counter enable
+	TIM2->CR1 |= TIM_CR1_CEN;
+}
+//======================/Init======================
+
+
+short T;
+short P;
+short R;
+short Y;
+
+short m1;
+short m2;
+short m3;
+short m4;
+
+void motors_set_throttle_mix(const short throttle, const control_channels_t* chs, const bool armed)
+{
+	T = throttle;
+	P = (int16_t) chs->pitch;
+	R = (int16_t) chs->roll;
+	Y = (int16_t) chs->yaw;
+	
+	m1 = T - P + R - Y;
+	m2 = T - P - R + Y;
+	m3 = T + P + R + Y;
+	m4 = T + P - R - Y;
+	
+	motor_set_throttle(1, m1, armed);
+	motor_set_throttle(2, m2, armed);
+	motor_set_throttle(3, m3, armed);
+	motor_set_throttle(4, m4, armed);
+}
+
+void motor_set_throttle(unsigned char motor_number, unsigned short us, bool armed)
+{
+	if (armed && us < 1050) us = 1050;
+	if (us > 2000) us = 2000;
+	
+	switch (motor_number)
+	{
+	case 1:
+	  TIM1->CCR3 = us;
+	  break;
+	case 2:
+	  TIM2->CCR2 = us;
+	  break;
+	case 3:
+	  TIM1->CCR4 = us;
+	  break;
+	case 4:
+	  TIM2->CCR1 = us;
+	  break;
+	}
+}
+
+void motors_turn_off()
+{
+	motor_set_throttle(1, 900, 0);
+	motor_set_throttle(2, 900, 0);
+	motor_set_throttle(3, 900, 0);
+	motor_set_throttle(4, 900, 0);
+}
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
--- a/Source/Devices/Motors.h
+++ b/Source/Devices/Motors.h
@@ -0,0 +1,21 @@
+#pragma once
+
+#ifndef MOTORS_H
+#define MOTORS_H
+
+#include "pid.h"
+
+void motors_init();
+void motor_gpio_tim1_ch3_init();
+void motor_gpio_tim1_ch4_init();
+void motors_tim1_ch3_4_init();
+
+void motor_gpio_tim2_ch1_init();
+void motor_gpio_tim2_ch2_init();
+void motors_tim2_ch1_2_init();
+
+void motors_set_throttle_mix(const short throttle, const control_channels_t* chs, const bool armed);
+void motor_set_throttle(unsigned char motor_number, unsigned short us, bool armed);
+void motors_turn_off();
+
+#endif
--- a/Source/Devices/RadioReceiver.cpp
+++ b/Source/Devices/RadioReceiver.cpp
@@ -0,0 +1,167 @@
+#include "RadioReceiver.h"
+
+volatile unsigned short sbus_channels[16] = {0};
+volatile unsigned char  sbus_buffer[SBUS_FRAME_SIZE] = {0};
+volatile unsigned char  sbus_index = 0;
+volatile unsigned char  sbus_failsafe = 0;
+volatile unsigned char  sbus_frame_lost = 0;
+volatile bool           sbus_frame_ready = 0;
+
+void receiver_gpio_init()
+{
+	RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;
+	
+	GPIOA->MODER &= ~(3 << (3 * 2));
+	GPIOA->MODER |= 2 << (3 * 2);
+	
+	GPIOA->AFR[0] &= ~(0xF << (3 * 4));
+	GPIOA->AFR[0] |= 12 << (3 * 4);
+	
+	// pull-up
+	GPIOA->PUPDR &= ~(3 << (3 * 2));
+	GPIOA->PUPDR |= 1 << (3 * 2);
+}
+
+void receiver_lpuart_clock_init()
+{
+	RCC->CCIPR &= ~(RCC_CCIPR_LPUART1SEL);
+	RCC->CCIPR |= 1 << RCC_CCIPR_LPUART1SEL_Pos;
+	RCC->APB1ENR2 |= RCC_APB1ENR2_LPUART1EN;
+}
+
+void receiver_uart_init()
+{
+	receiver_lpuart_clock_init();
+	
+	LPUART1->CR1 = 0;
+	LPUART1->CR2 = 0;
+	LPUART1->CR3 = 0;
+	
+	LPUART1->BRR = (256 * 16000000UL) / 100000UL;
+	
+	// parity control enable
+	LPUART1->CR1 |= USART_CR1_PCE | USART_CR1_M0;
+	
+	// word length M = 01 - 9 bit
+	LPUART1->CR1 &= ~USART_CR1_M1;
+	LPUART1->CR1 |= USART_CR1_M0;
+	
+	// even parity
+	LPUART1->CR1 &= ~USART_CR1_PS;
+	
+	// 2 stop bits
+	LPUART1->CR2 &= ~USART_CR2_STOP;
+	LPUART1->CR2 |= 2 << USART_CR2_STOP_Pos;
+	
+	// invertion enabled
+	LPUART1->CR2 |= USART_CR2_RXINV;
+	
+	// receiver enable
+	// interrupt generated whenever ORE = 1 or RXNE = 1
+	LPUART1->CR1 |= USART_CR1_RE | USART_CR1_RXNEIE;
+	
+	// uart enable
+	LPUART1->CR1 |= USART_CR1_UE;
+	
+	NVIC_EnableIRQ(LPUART1_IRQn);
+}
+
+void receiver_init()
+{
+	receiver_gpio_init();
+	receiver_uart_init();
+}
+
+void LPUART1_IRQHandler()
+{ 
+	if (LPUART1->ISR & USART_ISR_RXNE)
+	{
+		uint8_t b = LPUART1->RDR;
+		
+		if (b == SBUS_START_BYTE)
+			sbus_index = 0;
+		
+		if (sbus_index < SBUS_FRAME_SIZE)
+			sbus_buffer[sbus_index++] = b;
+		
+		if (sbus_index == SBUS_FRAME_SIZE)
+		{
+			sbus_index = 0;
+			sbus_frame_ready = 1;
+		}
+	}
+}
+
+void receiver_update(rc_channels* chs)
+{
+	if (!sbus_frame_ready)
+		return;
+	  
+	sbus_frame_ready = 0;
+	
+	if (sbus_buffer[0] != SBUS_START_BYTE)
+		return;
+	
+	sbus_failsafe   = sbus_buffer[23] & (1 << 3);
+	sbus_frame_lost = sbus_buffer[23] & (1 << 2);
+	
+	if (sbus_failsafe || sbus_frame_lost)
+		return;
+	    
+	receiver_parse_frame();
+	
+	chs->rc_roll = sbus_channels[0];
+	chs->rc_pitch = sbus_channels[1];
+	chs->rc_throttle = sbus_channels[2];
+	chs->rc_armed = sbus_channels[4];
+}
+
+void receiver_parse_frame()
+{
+  uint16_t b[22];
+  
+  for (uint8_t i = 0; i < 22; ++i)
+	b[i] = sbus_buffer[i + 1];
+	
+  sbus_channels[0]  = ( b[0]        | (b[1] << 8) )                  & 0x07FF;
+  sbus_channels[1]  = ( (b[1] >> 3) | (b[2] << 5) )                  & 0x07FF;
+  sbus_channels[2]  = ( (b[2] >> 6) | (b[3] << 2) | (b[4] << 10) )   & 0x07FF;
+  sbus_channels[3]  = ( (b[4] >> 1) | (b[5] << 7) )                  & 0x07FF;
+  sbus_channels[4]  = ( (b[5] >> 4) | (b[6] << 4) )                  & 0x07FF;
+  sbus_channels[5]  = ( (b[6] >> 7) | (b[7] << 1) | (b[8] << 9) )    & 0x07FF;
+  sbus_channels[6]  = ( (b[8] >> 2) | (b[9] << 6) )                  & 0x07FF;
+  sbus_channels[7]  = ( (b[9] >> 5) | (b[10] << 3) )                 & 0x07FF;
+	
+  sbus_channels[8]  = ( b[11]       | (b[12] << 8) )                 & 0x07FF;
+  sbus_channels[9]  = ( (b[12] >> 3)| (b[13] << 5) )                 & 0x07FF;
+  sbus_channels[10] = ( (b[13] >> 6)| (b[14] << 2) | (b[15] << 10) ) & 0x07FF;
+  sbus_channels[11] = ( (b[15] >> 1)| (b[16] << 7) )                 & 0x07FF;
+  sbus_channels[12] = ( (b[16] >> 4)| (b[17] << 4) )                 & 0x07FF;
+  sbus_channels[13] = ( (b[17] >> 7)| (b[18] << 1) | (b[19] << 9) )  & 0x07FF;
+  sbus_channels[14] = ( (b[19] >> 2)| (b[20] << 6) )                 & 0x07FF;
+  sbus_channels[15] = ( (b[20] >> 5)| (b[21] << 3) )                 & 0x07FF;
+	
+  sbus_frame_lost = sbus_buffer[23] & (1 << 2);
+  sbus_failsafe   = sbus_buffer[23] & (1 << 3);
+}
+
+rc_channels normalize_channels(rc_channels chs)
+{
+	chs.rc_roll = int_mapping(chs.rc_roll, 240, 1807, -500, 500);
+	chs.rc_pitch = int_mapping(chs.rc_pitch, 240, 1807, -500, 500);
+	chs.rc_throttle = int_mapping(chs.rc_throttle, 240, 1807, 1000, 2000);
+	//chs.rc_yaw = int_mapping(chs.rc_yaw, 240, 1807, -10, 10);
+	chs.rc_armed = bool_mapping_gt(chs.rc_armed, 1500);
+	
+	return chs;
+}
+
+short int_mapping(short x, short in_min, short in_max, short out_min, short out_max)
+{
+	return out_min + (x - in_min) * (out_max - out_min) / (in_max - in_min);
+}
+
+bool bool_mapping_gt(short x, short boundary)
+{
+	return x >= boundary;
+}
--- a/Source/Devices/RadioReceiver.h
+++ b/Source/Devices/RadioReceiver.h
@@ -0,0 +1,36 @@
+#pragma once
+
+#ifndef RADIO_RECEIVER_H
+#define RADIO_RECEIVER_H
+
+#include "stm32g431xx.h"
+#include <stdint.h>
+
+#define SBUS_FRAME_SIZE 25
+#define SBUS_START_BYTE 0X0F
+
+struct rc_channels
+{
+	short rc_roll;      // -500 - 500
+	short rc_pitch;     // -500 - 500
+	short rc_throttle;  // 1000 - 2000
+	short rc_yaw;       // -500 - 500
+	bool rc_armed;      // 0/1
+};
+
+void receiver_gpio_init();
+void receiver_lpuart_clock_init();
+void receiver_uart_init();
+void receiver_init();
+void LPUART1_IRQHandler();
+void receiver_update(rc_channels* chs);
+void receiver_parse_frame();
+rc_channels normalize_channels(rc_channels chs);
+short int_mapping(short x, short in_min, short in_max, short out_min, short out_max);
+bool bool_mapping_gt(short x, short boundary);
+
+
+void led_init();
+void toggle_led();
+
+#endif