diff --git a/mainloop/.gitignore b/mainloop/.gitignore deleted file mode 100644 index 3ca5774..0000000 --- a/mainloop/.gitignore +++ /dev/null @@ -1,7 +0,0 @@ -*.elf -*.bin -*.map -*.geany -*.o -*~ -work diff --git a/mainloop/README.md b/mainloop/README.md deleted file mode 100644 index 5dea59b..0000000 --- a/mainloop/README.md +++ /dev/null @@ -1,5 +0,0 @@ - -## Drafts around UAV system - -UAV - unmanned aircraft velocity - diff --git a/mainloop/main.c b/mainloop/main.c deleted file mode 100644 index 2124c24..0000000 --- a/mainloop/main.c +++ /dev/null @@ -1,321 +0,0 @@ - -/* - * Copyright 2022 Oleg Borodin - */ - -#include -#include -#include - -#include -#include -#include -#include -#include - - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - - - -const uint32_t g_systick_freq = 50 * 1000; -uint32_t g_sys_tick_counter; - -static void clock_setup(void) { - rcc_clock_setup_pll(&rcc_hse_8mhz_3v3[RCC_CLOCK_3V3_168MHZ]); - rcc_periph_clock_enable(RCC_GPIOA); - rcc_periph_clock_enable(RCC_GPIOB); - rcc_periph_clock_enable(RCC_USART1); - rcc_periph_clock_enable(RCC_I2C1); - rcc_periph_clock_enable(RCC_TIM2); - rcc_periph_clock_enable(RCC_TIM5); -} - -static void usart_setup(uint32_t usart, uint32_t gpioport, uint32_t gpiopins, uint32_t baudrate) { - usart_disable(usart); - - gpio_mode_setup(gpioport, GPIO_MODE_AF, GPIO_PUPD_NONE, gpiopins); - gpio_set_af(gpioport, GPIO_AF7, gpiopins); - gpio_set_output_options(gpioport, GPIO_OTYPE_PP, GPIO_OSPEED_100MHZ, gpiopins); - - usart_set_baudrate(usart, baudrate); - usart_set_databits(usart, 8); - usart_set_stopbits(usart, USART_STOPBITS_1); - usart_set_parity(usart, USART_PARITY_NONE); - usart_set_flow_control(usart, USART_FLOWCONTROL_NONE); - usart_set_mode(usart, USART_MODE_TX_RX); - usart_disable_rx_interrupt(usart); - - usart_enable(usart); -} - -static void i2c_setup(uint32_t i2c, uint32_t gpioport, uint32_t gpiopins) { - gpio_mode_setup(gpioport, GPIO_MODE_AF, GPIO_PUPD_PULLUP, gpiopins); - gpio_set_output_options(gpioport, GPIO_OTYPE_OD, GPIO_OSPEED_100MHZ, gpiopins); - gpio_set_af(gpioport, GPIO_AF4, gpiopins); - - i2c_reset(i2c); - i2c_peripheral_disable(i2c); - i2c_set_speed(i2c, i2c_speed_fm_400k, I2C_CR2_FREQ_36MHZ); - i2c_peripheral_enable(i2c); -} - - -static void systick_setup(uint32_t systic_freq) { - g_sys_tick_counter = 0; - - systick_set_frequency(systic_freq, rcc_ahb_frequency); - systick_interrupt_enable(); - systick_counter_enable(); -} - - -void sys_tick_handler(void) { - g_sys_tick_counter++; -} - -uint32_t sys_tick_counter(void) { - uint32_t val = g_sys_tick_counter; - return val; -} - - -#define PWM100 9999 -#define PWM150 6666 -#define PWM200 4999 -#define PWM250 3999 -#define PWM300 3333 -#define PWM330 3030 - -static void timer_mode_init(uint32_t timer) { - - int prescale = rcc_ahb_frequency / (2 * 1000 * 1000) - 1; - int period = PWM330; - - timer_disable_counter(timer); - timer_set_mode(timer, TIM_CR1_CKD_CK_INT, TIM_CR1_CMS_EDGE, TIM_CR1_DIR_UP); - timer_disable_preload(timer); - timer_continuous_mode(timer); - timer_set_prescaler(timer, prescale); - timer_set_period(timer, period); - timer_set_repetition_counter(timer, 0); - timer_enable_break_main_output(timer); - timer_enable_counter(timer); -} - -static void timer_channel_init(uint32_t timer, uint32_t channel) { - - timer_disable_oc_output(timer, channel); - timer_set_oc_value(timer, channel, 0); - timer_disable_oc_clear(timer, channel); - timer_enable_oc_preload(timer, channel); - timer_set_oc_slow_mode(timer, channel); - timer_set_oc_mode(timer, channel, TIM_OCM_PWM1); - - timer_set_oc_polarity_high(timer, channel); - timer_set_oc_idle_state_set(timer, channel); - - timer_set_oc_value(timer, channel, 20000); - timer_enable_oc_output(timer, channel); -} - -static void timer_channel_setratio(uint32_t timer, uint32_t channel, uint32_t ratio) { - uint32_t period = TIM_ARR(timer); - uint32_t value = (period * ratio) / 1000; - timer_set_oc_value(timer, channel, value); -} - -static void timer_gpio_setup(uint32_t gpio_port, uint32_t gpio_af, uint32_t gpio_pin) { - gpio_mode_setup(gpio_port, GPIO_MODE_AF, GPIO_PUPD_NONE, gpio_pin); - gpio_set_af(gpio_port, gpio_af, gpio_pin); - gpio_set_output_options(gpio_port, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ, gpio_pin); -} - - -typedef struct { - double start; - double freq; -} systimer_t; - -void systimer_init(systimer_t* timer, double freq, double start) { - timer->start = start; - timer->freq = freq; -} - -double systimer_apply(systimer_t* timer, double timestamp) { - double difftime = (timestamp - timer->start) / timer->freq; - timer->start = timestamp; - return difftime; -} - -typedef struct { - imu_t imu; - imuvec_t ival; - - systimer_t systimer; - uint32_t timer; - - lpf3_t lpfax; - lpf3_t lpfay; - lpf3_t lpfaz; - - lpf3_t lpfgx; - lpf3_t lpfgy; - lpf3_t lpfgz; - - quaternion_t q; - eulerangle_t a; - - eulerangle_t da; - - pidcont_t px; - pidcont_t py; - pidcont_t pz; - - mixer_t mix; - double outx; - double outy; -} uav_t; - - -void uav_init(uav_t* uav) { - - clock_setup(); - usart_setup(USART1, GPIOA, GPIO9 | GPIO10, 460800); - - i2c_setup(I2C1, GPIOB, GPIO8 | GPIO9); - - systick_setup(g_systick_freq); - - uav->timer = TIM2; - timer_mode_init(uav->timer); - - timer_channel_init(uav->timer, TIM_OC1); - timer_channel_init(uav->timer, TIM_OC2); - timer_channel_init(uav->timer, TIM_OC3); - timer_channel_init(uav->timer, TIM_OC4); - - timer_channel_setratio(uav->timer, TIM_OC1, 50); - timer_channel_setratio(uav->timer, TIM_OC2, 50); - timer_channel_setratio(uav->timer, TIM_OC3, 50); - timer_channel_setratio(uav->timer, TIM_OC4, 50); - - timer_gpio_setup(GPIOA, GPIO_AF1, GPIO0); - timer_gpio_setup(GPIOA, GPIO_AF1, GPIO1); - timer_gpio_setup(GPIOA, GPIO_AF1, GPIO2); - timer_gpio_setup(GPIOA, GPIO_AF1, GPIO3); - - imu_setup(&(uav->imu), I2C1, 0x68); - imu_calibrate(&(uav->imu), 100); - - double ak = 50.0; - - lpf3_init(&(uav->lpfgx), ak); - lpf3_init(&(uav->lpfgy), ak); - lpf3_init(&(uav->lpfgz), ak); - - lpf3_init(&(uav->lpfax), ak); - lpf3_init(&(uav->lpfay), ak); - lpf3_init(&(uav->lpfaz), ak); - - systimer_init(&(uav->systimer), (double)g_systick_freq, (double)g_sys_tick_counter); - - quaternion_init(&(uav->q)); - - eulerangle_init(&(uav->a)); - eulerangle_init(&(uav->da)); - - pidcont_init(&(uav->px)); - pidcont_init(&(uav->py)); - pidcont_init(&(uav->pz)); - - double kp = 0.0; - double ki = 400.0; - double kd = 0.0; - - pidcont_setup(&(uav->px), kp, ki, kd); - pidcont_setup(&(uav->py), kp, ki, kd); - pidcont_setup(&(uav->pz), kp, ki, kd); - - mixer_init(&(uav->mix)); - - mixer_iset(&(uav->mix), 0, &(uav->da.x)); - mixer_iset(&(uav->mix), 1, &(uav->da.y)); - - mixer_oset(&(uav->mix), 0, &(uav->outx)); - mixer_oset(&(uav->mix), 1, &(uav->outy)); - - mixer_rset(&(uav->mix), 0, 0, 0, 0.7); - mixer_rset(&(uav->mix), 1, 1, 1, 0.7); - mixer_rset(&(uav->mix), 2, 0, 1, -0.7); - mixer_rset(&(uav->mix), 3, 1, 0, 0.7); - -} - - -void uav_loop(uav_t* uav) { - - while (true) { - imu_getvec(&(uav->imu), &(uav->ival)); - - double dt = systimer_apply(&(uav->systimer), g_sys_tick_counter); - - uav->ival.gx = lpf3_apply(&(uav->lpfgx), uav->ival.gx, dt); - uav->ival.gy = lpf3_apply(&(uav->lpfgy), uav->ival.gy, dt); - uav->ival.gz = lpf3_apply(&(uav->lpfgz), uav->ival.gz, dt); - - uav->ival.ax = lpf3_apply(&(uav->lpfax), uav->ival.ax, dt); - uav->ival.ay = lpf3_apply(&(uav->lpfay), uav->ival.ay, dt); - uav->ival.az = lpf3_apply(&(uav->lpfaz), uav->ival.az, dt); - - quaternion_madgwick(&(uav->q), &(uav->ival), dt); - quaternion_toeuler(&(uav->q), &(uav->a)); - - eulerangle_todegress(&(uav->a)); - - uav->da.x = pidcont_apply(&(uav->px), 0, uav->a.x, dt); - uav->da.y = pidcont_apply(&(uav->py), 0, uav->a.y, dt); - uav->da.z = pidcont_apply(&(uav->pz), 0, uav->a.z, dt); - - printf("dt=%.6f %8.3f %8.3f %8.3f\r\n", dt, uav->da.x, uav->da.y, uav->da.z); - - mixer_apply(&(uav->mix)); - - double pmin = -90.0; - double pmax = 90.0; - - double omin = 150.0; - double omax = 850.0; - - uint32_t out1 = (uint32_t)mapval(pmin, pmax, omin, omax, uav->outx, true); - uint32_t out2 = (uint32_t)mapval(pmin, pmax, omin, omax, uav->outy, false); - - //printf("dt=%.6f %lu <-pitch=%8.3f roll=%8.3f yaw=%8.3f\r\n", dt, out1, a.y, a.x, a.z); - - //double pitch = pidcont_apply(&p, 0, a.pitch, dt); - - timer_channel_setratio(uav->timer, TIM_OC1, out1); - timer_channel_setratio(uav->timer, TIM_OC2, out2); - - }; -} - - -int main(void) { - uav_t uav; - uav_init(&uav); - uav_loop(&uav); - return 0; -}