136 lines
2.3 KiB
C
136 lines
2.3 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_raw_t imu_raw;
|
|
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 imu_callback(uint8_t* buf);
|
|
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_accel_calibrate();
|
|
|
|
imu_tim6_init(500);
|
|
|
|
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 (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 = 0;
|
|
|
|
motors_set_throttle_mix(
|
|
rx_chs_normalized.rc_throttle,
|
|
&ctrl_chs,
|
|
rx_chs_normalized.rc_armed
|
|
);
|
|
}
|
|
else
|
|
{
|
|
motors_turn_off();
|
|
allowed_calib = 1;
|
|
}
|
|
}
|
|
}
|
|
|
|
void imu_callback(uint8_t* buf)
|
|
{
|
|
imu_raw.ax = (buf[0] << 8) | buf[1];
|
|
imu_raw.ay = (buf[2] << 8) | buf[3];
|
|
imu_raw.az = (buf[4] << 8) | buf[5];
|
|
|
|
imu_raw.gx = (buf[6] << 8) | buf[7];
|
|
imu_raw.gy = (buf[8] << 8) | buf[9];
|
|
imu_raw.gz = (buf[10] << 8) | buf[11];
|
|
|
|
imu_process_raw(&imu, &imu_raw, &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);
|
|
}
|
|
|
|
void TIM6_DAC_IRQHandler()
|
|
{
|
|
if (TIM6->SR & TIM_SR_UIF)
|
|
{
|
|
TIM6->SR &= ~TIM_SR_UIF;
|
|
|
|
i2c_read_async(ICM_ADDR, 0x2D, 12, imu_callback);
|
|
control_update_flag = 1;
|
|
}
|
|
}
|
|
|
|
void delay_ms(uint32_t ms)
|
|
{
|
|
for (uint32_t i = 0; i < ms * 4000; i++);
|
|
}
|
|
|
|
|
|
|