98 lines
1.6 KiB
C
98 lines
1.6 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"
|
|
|
|
|
|
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;
|
|
|
|
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_tim6_init();
|
|
imu_processing_init();
|
|
|
|
imu_calibrate();
|
|
|
|
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 (imu_update_flag)
|
|
{
|
|
imu_update_flag = 0;
|
|
|
|
imu_read_scaled(&imu);
|
|
|
|
Vector3 gyro = {imu.gx, imu.gy, imu.gz};
|
|
Vector3 accel = {imu.ax, imu.ay, imu.az};
|
|
|
|
IRS_update(&irs, &gyro, &accel, IMU_DT);
|
|
}
|
|
|
|
if (pid_update_flag)
|
|
{
|
|
pid_update_flag = 0;
|
|
|
|
attitude_controller_update(
|
|
&ctrl_chs,
|
|
&rx_chs_normalized,
|
|
&irs.q,
|
|
&irs.gyro
|
|
);
|
|
|
|
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++);
|
|
}
|
|
|
|
|
|
|