74 lines
1.3 KiB
C
74 lines
1.3 KiB
C
#include "stm32g431xx.h"
|
|
#include "imu.h"
|
|
#include "imu_processing.h"
|
|
#include "timer.h"
|
|
#include "attitude.h"
|
|
#include "radio_receiver.h"
|
|
#include "motors.h"
|
|
#include "pid.h"
|
|
|
|
|
|
imu_scaled_t imu;
|
|
attitude_t attitude;
|
|
rc_channels rx_chs_raw;
|
|
rc_channels rx_chs_normalized;
|
|
control_channels_t ctrl_chs;
|
|
|
|
void delay_ms(uint32_t ms);
|
|
|
|
int main(void)
|
|
{
|
|
__enable_irq();
|
|
|
|
RCC->AHB2ENR |= RCC_AHB2ENR_GPIOCEN;
|
|
GPIOC->MODER &= ~(3 << (13 * 2));
|
|
GPIOC->MODER |= 1 << (13 * 2);
|
|
GPIOC->OTYPER &= ~(1 << 13);
|
|
GPIOC->PUPDR &= ~(3 << (13 * 2));
|
|
GPIOC->BSRR = 1 << (13 + 16);
|
|
|
|
delay_ms(200);
|
|
|
|
NVIC_SetPriority(TIM6_DAC_IRQn, 1);
|
|
NVIC_SetPriority(LPUART1_IRQn, 0);
|
|
|
|
i2c_gpio_init();
|
|
i2c1_init();
|
|
icm_init();
|
|
imu_processing_init();
|
|
tim6_init();
|
|
|
|
imu_calibrate();
|
|
|
|
receiver_init();
|
|
|
|
motors_init();
|
|
|
|
while (1)
|
|
{
|
|
attitude_update(&attitude, &imu);
|
|
|
|
receiver_update(&rx_chs_raw);
|
|
rx_chs_normalized = normalize_channels(rx_chs_raw);
|
|
|
|
attitude_pid_update(&ctrl_chs, &rx_chs_normalized, &attitude, &imu);
|
|
|
|
if (rx_chs_normalized.rc_armed)
|
|
{
|
|
motors_set_throttle_mix(rx_chs_normalized.rc_throttle, &ctrl_chs, rx_chs_normalized.rc_armed);
|
|
}
|
|
else
|
|
{
|
|
motors_turn_off();
|
|
}
|
|
}
|
|
}
|
|
|
|
void delay_ms(uint32_t ms)
|
|
{
|
|
for (uint32_t i = 0; i < ms * 4000; i++);
|
|
}
|
|
|
|
|
|
|