126 lines
2.0 KiB
C
126 lines
2.0 KiB
C
#include "stm32g431xx.h"
|
|
#include "imu.h"
|
|
#include "imu_processing.h"
|
|
#include "IRS.h"
|
|
#include "attitude.h"
|
|
#include "radio_receiver.h"
|
|
#include "motors.h"
|
|
#include "pid.h"
|
|
#include "lidar.h"
|
|
|
|
void TIM6_DAC_IRQHandler();
|
|
|
|
static uint8_t irs_update_flag = 0;
|
|
static uint8_t control_update_flag = 0;
|
|
static uint8_t allowed_calib = 0;
|
|
|
|
imu_scaled_t imu;
|
|
IRS irs;
|
|
attitude_t attitude;
|
|
rc_channels rx_chs_raw;
|
|
rc_channels rx_chs_normalized;
|
|
control_channels_t ctrl_chs;
|
|
lidar_data lidar;
|
|
|
|
Vector3 euler;
|
|
|
|
void delay_ms(uint32_t ms);
|
|
|
|
int main(void)
|
|
{
|
|
__enable_irq();
|
|
|
|
NVIC_SetPriority(TIM6_DAC_IRQn, 2);
|
|
NVIC_SetPriority(USART3_IRQn, 1);
|
|
NVIC_SetPriority(LPUART1_IRQn, 0);
|
|
|
|
imu_pow_init();
|
|
i2c_gpio_init();
|
|
i2c1_init();
|
|
imu_init();
|
|
|
|
imu_processing_init();
|
|
//imu_calibrate();
|
|
|
|
imu_tim6_init();
|
|
|
|
IRS_init(&irs);
|
|
|
|
attitude_init(&attitude);
|
|
|
|
receiver_init();
|
|
|
|
motors_init();
|
|
|
|
while (1)
|
|
{
|
|
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);
|
|
}
|
|
|
|
euler = QuatToEuler(&irs.q);
|
|
|
|
if (control_update_flag)
|
|
{
|
|
control_update_flag = 0;
|
|
|
|
attitude_controller_update(
|
|
&ctrl_chs,
|
|
&rx_chs_normalized,
|
|
&irs.q,
|
|
&irs.gyro
|
|
);
|
|
}
|
|
|
|
if (rx_chs_normalized.rc_armed)
|
|
{
|
|
allowed_calib = 1;
|
|
|
|
motors_set_throttle_mix(
|
|
rx_chs_normalized.rc_throttle,
|
|
&ctrl_chs,
|
|
rx_chs_normalized.rc_armed
|
|
);
|
|
}
|
|
else
|
|
{
|
|
motors_turn_off();
|
|
allowed_calib = 0;
|
|
}
|
|
}
|
|
}
|
|
|
|
void TIM6_DAC_IRQHandler()
|
|
{
|
|
if (TIM6->SR & TIM_SR_UIF)
|
|
{
|
|
TIM6->SR &= ~TIM_SR_UIF;
|
|
irs_update_flag = 1;
|
|
control_update_flag = 1;
|
|
}
|
|
}
|
|
|
|
void delay_ms(uint32_t ms)
|
|
{
|
|
for (uint32_t i = 0; i < ms * 4000; i++);
|
|
}
|
|
|
|
|
|
|