vadyschka01
139 lines
3.8 KiB
C
139 lines
3.8 KiB
C
#include "stm32g4xx.h"
|
|
#include "imu.h"
|
|
#include "motors.h"
|
|
#include "dsp_manager.h"
|
|
#include "dshot2.h"
|
|
|
|
// 1. ПЕРЕМЕННЫЕ
|
|
volatile uint8_t imu_flag = 0;
|
|
|
|
uint32_t m1_speed = 0;
|
|
uint32_t m2_speed = 0;
|
|
uint32_t m3_speed = 0;
|
|
uint32_t m4_speed = 0;
|
|
|
|
#pragma pack(push, 1)
|
|
typedef struct {
|
|
uint8_t header[2];
|
|
int16_t gx;
|
|
int16_t filt_gx;
|
|
uint16_t notch1_hz;
|
|
uint16_t notch2_hz;
|
|
uint16_t notch3_hz;
|
|
} Telemetry_t;
|
|
#pragma pack(pop)
|
|
|
|
// 2. ПРОТОТИПЫ
|
|
void SystemClock_Config_160MHz(void);
|
|
void UART2_Init_921600(void);
|
|
void TIM7_Init_1000Hz(void);
|
|
void UART_SendPacket(Telemetry_t *p);
|
|
|
|
// 3. ОСНОВНОЙ ЦИКЛ
|
|
int main(void) {
|
|
SystemClock_Config_160MHz();
|
|
Motors_Init();
|
|
Set_Motors(900);
|
|
for (int i = 0; i < 20000000; i++) __NOP(); // Пауза для ESC
|
|
|
|
I2C1_Init();
|
|
UART2_Init_921600();
|
|
IMU_Init();
|
|
IMU_Calibrate();
|
|
|
|
DSP_Init(); // Инициализация нашего анализатора Фурье
|
|
|
|
TIM7_Init_1000Hz();
|
|
__enable_irq();
|
|
|
|
Telemetry_t pkt;
|
|
pkt.header[0] = 0xAA; pkt.header[1] = 0xBB;
|
|
|
|
while (1) {
|
|
if (imu_flag) {
|
|
imu_flag = 0;
|
|
|
|
static unsigned short dshot_erpm[4];
|
|
DSHOT_GetERPM(dshot_erpm, 0);
|
|
DSP_SetMotorErpm(dshot_erpm, 7);
|
|
|
|
IMU_ReadRawData();
|
|
//Motors_PollTelemetry();
|
|
|
|
//сохраняем во временную переменную
|
|
int16_t gx_val = raw_gx;
|
|
DSP_AddSample((float32_t)gx_val);
|
|
|
|
if (fft_ready) {
|
|
DSP_Process();
|
|
}
|
|
|
|
float32_t filt_gx_value = filt_gx;
|
|
pkt.gx = gx_val;
|
|
pkt.filt_gx = (int16_t)filt_gx_value;
|
|
pkt.notch1_hz = notch_report_hz[0];
|
|
pkt.notch2_hz = notch_report_hz[1];
|
|
pkt.notch3_hz = notch_report_hz[2];
|
|
UART_SendPacket(&pkt);
|
|
|
|
|
|
uint32_t m1 = m1_speed;
|
|
uint32_t m2 = m2_speed;
|
|
uint32_t m3 = m3_speed;
|
|
uint32_t m4 = m4_speed;
|
|
Set_Motor_Individual(m1, m2, m3, m4);
|
|
}
|
|
}
|
|
}
|
|
|
|
// 4. РЕАЛИЗАЦИЯ ФУНКЦИЙ
|
|
|
|
void SystemClock_Config_160MHz(void) {
|
|
RCC->CR |= RCC_CR_HSION;
|
|
while(!(RCC->CR & RCC_CR_HSIRDY));
|
|
RCC->PLLCFGR = (RCC_PLLCFGR_PLLSRC_HSI | (3 << RCC_PLLCFGR_PLLM_Pos) |
|
|
(80 << RCC_PLLCFGR_PLLN_Pos) | (0 << RCC_PLLCFGR_PLLR_Pos));
|
|
RCC->CR |= RCC_CR_PLLON;
|
|
RCC->PLLCFGR |= RCC_PLLCFGR_PLLREN;
|
|
while(!(RCC->CR & RCC_CR_PLLRDY));
|
|
FLASH->ACR |= (4 << FLASH_ACR_LATENCY_Pos);
|
|
RCC->CFGR |= RCC_CFGR_SW_PLL;
|
|
while((RCC->CFGR & RCC_CFGR_SWS) != RCC_CFGR_SWS_PLL);
|
|
SystemCoreClockUpdate();
|
|
}
|
|
|
|
void UART2_Init_921600(void) {
|
|
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOBEN;
|
|
RCC->APB1ENR1 |= RCC_APB1ENR1_USART2EN;
|
|
RCC->CCIPR |= (1 << RCC_CCIPR_USART2SEL_Pos);
|
|
GPIOB->MODER &= ~(GPIO_MODER_MODE3 | GPIO_MODER_MODE4);
|
|
GPIOB->MODER |= (GPIO_MODER_MODE3_1 | GPIO_MODER_MODE4_1);
|
|
GPIOB->AFR[0] &= ~((0xF << 12) | (0xF << 16));
|
|
GPIOB->AFR[0] |= ((7 << 12) | (7 << 16));
|
|
USART2->BRR = 174;
|
|
USART2->CR1 = USART_CR1_TE | USART_CR1_UE;
|
|
}
|
|
|
|
void UART_SendPacket(Telemetry_t *p) {
|
|
uint8_t *ptr = (uint8_t*)p;
|
|
for (uint16_t i = 0; i < sizeof(Telemetry_t); i++) {
|
|
while (!(USART2->ISR & USART_ISR_TXE));
|
|
USART2->TDR = ptr[i];
|
|
}
|
|
}
|
|
|
|
void TIM7_Init_1000Hz(void) {
|
|
RCC->APB1ENR1 |= RCC_APB1ENR1_TIM7EN;
|
|
TIM7->PSC = 16000 - 1;
|
|
TIM7->ARR = 10 - 1;
|
|
TIM7->DIER |= TIM_DIER_UIE;
|
|
NVIC_EnableIRQ(TIM7_DAC_IRQn);
|
|
TIM7->CR1 |= TIM_CR1_CEN;
|
|
}
|
|
|
|
extern "C" void TIM7_DAC_IRQHandler(void) {
|
|
if (TIM7->SR & TIM_SR_UIF) {
|
|
TIM7->SR = 0;
|
|
imu_flag = 1;
|
|
}
|
|
} |