diff --git a/mainloop/Makefile b/Makefile similarity index 100% rename from mainloop/Makefile rename to Makefile diff --git a/mainloop/OUTPUT.txt b/OUTPUT.txt similarity index 100% rename from mainloop/OUTPUT.txt rename to OUTPUT.txt diff --git a/README.md b/README.md index 53dba96..a1e5105 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,11 @@ -### A little code for experiments -![](/draft02-gen.svg "Draft") +## Drafts around UAV system, a little code for experiments + +UAV - unmanned aircraft velocity -![](/stm32stand.jpg "Stand") + + +![](/docs/draft02-gen.svg "Draft") + +![](/docs/stm32stand.jpg "Stand") diff --git a/draft02-gen.dia b/docs/draft02-gen.dia similarity index 100% rename from draft02-gen.dia rename to docs/draft02-gen.dia diff --git a/draft02-gen.svg b/docs/draft02-gen.svg similarity index 100% rename from draft02-gen.svg rename to docs/draft02-gen.svg diff --git a/mpu60x0-pm.pdf b/docs/mpu60x0-pm.pdf similarity index 100% rename from mpu60x0-pm.pdf rename to docs/mpu60x0-pm.pdf diff --git a/stm32-f4ve-schematic.pdf b/docs/stm32-f4ve-schematic.pdf similarity index 100% rename from stm32-f4ve-schematic.pdf rename to docs/stm32-f4ve-schematic.pdf diff --git a/stm32f103xx.pdf b/docs/stm32f103xx.pdf similarity index 100% rename from stm32f103xx.pdf rename to docs/stm32f103xx.pdf diff --git a/stm32f4xx-rm.pdf b/docs/stm32f4xx-rm.pdf similarity index 100% rename from stm32f4xx-rm.pdf rename to docs/stm32f4xx-rm.pdf diff --git a/stm32stand.jpg b/docs/stm32stand.jpg similarity index 100% rename from stm32stand.jpg rename to docs/stm32stand.jpg diff --git a/mainloop/filter.c b/filter.c similarity index 100% rename from mainloop/filter.c rename to filter.c diff --git a/mainloop/filter.h b/filter.h similarity index 100% rename from mainloop/filter.h rename to filter.h diff --git a/flashrw/config.c b/flashrw/config.c index 0f8e970..8a734e8 100644 --- a/flashrw/config.c +++ b/flashrw/config.c @@ -8,7 +8,6 @@ #include #include - void config_init(config_t* c) { c->gz = 0x00; c->gy = 0x00; diff --git a/mainloop/geometry.c b/geometry.c similarity index 100% rename from mainloop/geometry.c rename to geometry.c diff --git a/mainloop/geometry.h b/geometry.h similarity index 100% rename from mainloop/geometry.h rename to geometry.h diff --git a/mainloop/i2cdev.c b/i2cdev.c similarity index 100% rename from mainloop/i2cdev.c rename to i2cdev.c diff --git a/mainloop/i2cdev.h b/i2cdev.h similarity index 100% rename from mainloop/i2cdev.h rename to i2cdev.h diff --git a/main.c b/main.c new file mode 100644 index 0000000..2124c24 --- /dev/null +++ b/main.c @@ -0,0 +1,321 @@ + +/* + * 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; +} diff --git a/mainloop/main.ld b/main.ld similarity index 100% rename from mainloop/main.ld rename to main.ld diff --git a/mainloop/main.c b/mainloop/main.c index 233e827..2124c24 100644 --- a/mainloop/main.c +++ b/mainloop/main.c @@ -99,7 +99,7 @@ uint32_t sys_tick_counter(void) { #define PWM300 3333 #define PWM330 3030 -static void timer_init(uint32_t timer) { +static void timer_mode_init(uint32_t timer) { int prescale = rcc_ahb_frequency / (2 * 1000 * 1000) - 1; int period = PWM330; @@ -115,7 +115,7 @@ static void timer_init(uint32_t timer) { timer_enable_counter(timer); } -static void tc_init(uint32_t timer, uint32_t channel) { +static void timer_channel_init(uint32_t timer, uint32_t channel) { timer_disable_oc_output(timer, channel); timer_set_oc_value(timer, channel, 0); @@ -131,7 +131,7 @@ static void tc_init(uint32_t timer, uint32_t channel) { timer_enable_oc_output(timer, channel); } -static void tc_setratio(uint32_t timer, uint32_t channel, uint32_t ratio) { +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); @@ -178,6 +178,12 @@ typedef struct { 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; @@ -190,26 +196,27 @@ void uav_init(uav_t* uav) { 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); - uav->timer = TIM2; - timer_init(uav->timer); - - tc_init(uav->timer, TIM_OC1); - tc_init(uav->timer, TIM_OC2); - tc_init(uav->timer, TIM_OC3); - tc_init(uav->timer, TIM_OC4); - - tc_setratio(uav->timer, TIM_OC1, 10); - tc_setratio(uav->timer, TIM_OC2, 30); - tc_setratio(uav->timer, TIM_OC3, 50); - tc_setratio(uav->timer, TIM_OC4, 70); - imu_setup(&(uav->imu), I2C1, 0x68); imu_calibrate(&(uav->imu), 100); @@ -227,10 +234,25 @@ void uav_init(uav_t* uav) { 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->a.x)); - mixer_iset(&(uav->mix), 1, &(uav->a.y)); + 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)); @@ -240,12 +262,6 @@ void uav_init(uav_t* uav) { mixer_rset(&(uav->mix), 2, 0, 1, -0.7); mixer_rset(&(uav->mix), 3, 1, 0, 0.7); - //pidcont_t p; - //pidcont_init(&p); - - //double kp = 0; - //double ki = 4000.0; - //pidcont_setup(&p, kp, ki, 0); } @@ -269,9 +285,13 @@ void uav_loop(uav_t* uav) { eulerangle_todegress(&(uav->a)); - mixer_apply(&(uav->mix)); + 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 outx=%8.3f outy=%8.3f\r\n", dt, uav->outx, uav->outy); + 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; @@ -282,13 +302,13 @@ void uav_loop(uav_t* uav) { 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); - tc_setratio(uav->timer, TIM_OC1, out1); - tc_setratio(uav->timer, TIM_OC2, out2); - //printf("dt=%.6f %lu <-pitch=%8.3f roll=%8.3f yaw=%8.3f\r\n", dt, out1, a.y, a.x, a.z); - //double out = pidcont_apply(&p, 0, a.pitch, dt); - //printf("dt=%.6f pitch=%8.3f out=%10.3f %10.6f \r\n", dt, a.pitch, out, p.integ); + //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); + }; } diff --git a/mainloop/misc.c b/misc.c similarity index 100% rename from mainloop/misc.c rename to misc.c diff --git a/mainloop/misc.h b/misc.h similarity index 100% rename from mainloop/misc.h rename to misc.h diff --git a/mainloop/mixer.c b/mixer.c similarity index 100% rename from mainloop/mixer.c rename to mixer.c diff --git a/mainloop/mixer.h b/mixer.h similarity index 100% rename from mainloop/mixer.h rename to mixer.h diff --git a/mainloop/mpu6050.c b/mpu6050.c similarity index 100% rename from mainloop/mpu6050.c rename to mpu6050.c diff --git a/mainloop/mpu6050.h b/mpu6050.h similarity index 100% rename from mainloop/mpu6050.h rename to mpu6050.h diff --git a/mainloop/pidcont.c b/pidcont.c similarity index 100% rename from mainloop/pidcont.c rename to pidcont.c diff --git a/mainloop/pidcont.h b/pidcont.h similarity index 100% rename from mainloop/pidcont.h rename to pidcont.h diff --git a/sbus/.gitignore b/sbus/.gitignore new file mode 100644 index 0000000..3ca5774 --- /dev/null +++ b/sbus/.gitignore @@ -0,0 +1,7 @@ +*.elf +*.bin +*.map +*.geany +*.o +*~ +work diff --git a/sbus/Makefile b/sbus/Makefile new file mode 100644 index 0000000..0b6f60b --- /dev/null +++ b/sbus/Makefile @@ -0,0 +1,82 @@ +# +# Copyright: Oleg Borodin 2018 +# + +.SECONDARY: + +CFLAGS+= -I. -Os -DSTM32F4 #-std=c99 +CFLAGS+= -mfloat-abi=hard +CFLAGS+= -mcpu=cortex-m4 +CFLAGS+= -mthumb +CFLAGS+= -fno-common -ffunction-sections -fdata-sections +CFLAGS+= -g -gdwarf-2 +CFLAGS+= -Wall + + +LDFLAGS+= ${CFLAGS} +LDFLAGS+= --static +#LDFLAGS+= -nostartfiles +LDFLAGS+= -T main.ld + +LDFLAGS+= -Wl,-Map=main.map +LDFLAGS+= -Wl,--cref -Wl,--gc-sections +LDFLAGS+= -lopencm3_stm32f4 +LDFLAGS+= -Wl,--start-group -lc -lm -lgcc -lnosys -Wl,--end-group + +TARGET= arm-eabi + +all: main.bin + +OBJS+= main.o +OBJS+= syscall.o +OBJS+= usartu.o +OBJS+= sbus.o + + +main.elf: $(OBJS) + $(TARGET)-gcc $(^F) $(LDFLAGS) -o $@ + $(TARGET)-size --format=berkeley $@ + +%.o: %.c + $(TARGET)-gcc $(CFLAGS) -c -o $@ $< + +%.o: %.S + $(TARGET)-gcc $(CFLAGS) -c -o $@ $< + +%.bin: %.elf + $(TARGET)-objcopy -O binary $< $@ + +%.elf: %.o + $(TARGET)-gcc $(^F) $(LDFLAGS) -o $@ + $(TARGET)-size --format=berkeley $@ + +clean: + rm -f *.i *.o *.elf *.bin *.map *~ *.hex *.d *.s + +flash: main.bin + @openocd \ + -c 'puts "--- START --------------------"' \ + -f 'interface/stlink.cfg' \ + -f 'target/stm32f4x.cfg' \ + -c 'puts "--- INIT --------------------"' \ + -c "init" \ + -c "reset halt" \ + -c 'puts "--- WRITE --------------------"' \ + -c "flash write_image erase $< 0x08000000"\ + -c 'puts "--- VERIFY --------------------"' \ + -c "verify_image $<" \ + -c 'puts "--- RESET --------------------"' \ + -c "reset" \ + -c 'puts "--- DONE --------------------"' \ + -c "shutdown" + +debug: main.bin + @openocd \ + -c 'puts "--- START --------------------"' \ + -f 'interface/stlink.cfg' \ + -f 'target/stm32f4x.cfg' \ + -c 'puts "--- INIT --------------------"' \ + -c "init" \ + -c "halt" \ + -c "poll" +#EOF diff --git a/sbus/main.c b/sbus/main.c new file mode 100644 index 0000000..9fd1e5f --- /dev/null +++ b/sbus/main.c @@ -0,0 +1,91 @@ + +/* + * Copyright 2022 Oleg Borodin + */ + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include + + +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_GPIOD); + rcc_periph_clock_enable(RCC_USART1); + rcc_periph_clock_enable(RCC_USART2); +} + +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 sbus_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); +} + + +int main(void) { + + clock_setup(); + usart_setup(USART1, GPIOA, GPIO9 | GPIO10, 460800); + sbus_setup(USART2, GPIOD, GPIO6, 115200); + + sbus_t bus; + sbus_init(&bus); + while (true) { + uint8_t ibyte = usart_recv_blocking(USART2); + bool ready = sbus_recv(&bus, ibyte); + //if (ready) { + // for (int i = 0; i < sbus_chcount(&bus); i++) { + // printf("%2d=%4d ", i, sbus_getch(&bus, i)); + // } + // printf(" xxx \r\n"); + //} + } + + +} diff --git a/sbus/main.ld b/sbus/main.ld new file mode 100644 index 0000000..49c4b7e --- /dev/null +++ b/sbus/main.ld @@ -0,0 +1,71 @@ + +MEMORY { + FLASH (rx) : ORIGIN = 0x08000000, LENGTH = 512K + SRAM (rwx) : ORIGIN = 0x20000000, LENGTH = 128K +} + +EXTERN (vector_table) + +ENTRY(reset_handler) + +SECTIONS +{ + .text : { + *(.vectors) + *(.text*) + . = ALIGN(4); + *(.rodata*) + . = ALIGN(4); + } >FLASH + + .preinit_array : { + . = ALIGN(4); + __preinit_array_start = .; + KEEP (*(.preinit_array)) + __preinit_array_end = .; + } >FLASH + + .init_array : { + . = ALIGN(4); + __init_array_start = .; + KEEP (*(SORT(.init_array.*))) + KEEP (*(.init_array)) + __init_array_end = .; + } >FLASH + + .fini_array : { + . = ALIGN(4); + __fini_array_start = .; + KEEP (*(.fini_array)) + KEEP (*(SORT(.fini_array.*))) + __fini_array_end = .; + } >FLASH + + . = ALIGN(4); + _etext = .; + + .data : { + _data = .; + *(.data*) + . = ALIGN(4); + _edata = .; + } >SRAM AT >FLASH + + _data_loadaddr = LOADADDR(.data); + + .bss : { + *(.bss*) + *(COMMON) + . = ALIGN(4); + _ebss = .; + } >SRAM + + + . = ALIGN(4); + _end = .; +} + +PROVIDE(_stack = ORIGIN(SRAM) + LENGTH(SRAM)); + +PROVIDE(_heap = _end ); +PROVIDE(_eheap = ORIGIN(SRAM) + LENGTH(SRAM)); diff --git a/sbus/sbus.c b/sbus/sbus.c new file mode 100644 index 0000000..1deaed6 --- /dev/null +++ b/sbus/sbus.c @@ -0,0 +1,55 @@ +/* + * Copyright 2022 Oleg Borodin + */ + + + +#include +#include +#include +#include + +#include + +#define SBUS_STARTFR_BYTE 0x0F + +void sbus_init(sbus_t* bus) { + memset(bus->iframe, 0, sizeof(bus->iframe)); + memset(bus->rcvalue, 0, sizeof(bus->rcvalue)); + bus->findex = 0; +} + + +bool sbus_recv(sbus_t* bus, uint8_t ibyte) { + + printf("0x%02x ", ibyte); + + if(bus->findex == 0 && ibyte != SBUS_STARTFR_BYTE) { + return false; + } + if(bus->findex < SBUS_FRAMESIZE) { + bus->iframe[bus->findex] = ibyte; + bus->findex++; + + //printf("0x%02x ", ibyte); + + return false; + } + + if(bus->findex == SBUS_FRAMESIZE) { + bus->findex = 0; + //printf("0x%02x\r\n", bus->iframe[0]); + } + return false; +} + +int sbus_chcount(sbus_t* bus) { + return SBUS_CHANNELS; +} + +int16_t sbus_getch(sbus_t* bus, int num) { + if (num < SBUS_CHANNELS) { + return (int16_t)bus->rcvalue[num]; + } + return WRONG_RCVAL; +} diff --git a/sbus/sbus.h b/sbus/sbus.h new file mode 100644 index 0000000..0282cdc --- /dev/null +++ b/sbus/sbus.h @@ -0,0 +1,24 @@ +/* + * Copyright 2022 Oleg Borodin + */ + + +#ifndef SBUS_H_QWERTY +#define SBUS_H_QWERTY + +#define SBUS_FRAMESIZE 25 +#define SBUS_CHANNELS 18 +#define WRONG_RCVAL -1 + +typedef struct { + uint8_t iframe[SBUS_FRAMESIZE]; + uint16_t rcvalue[SBUS_CHANNELS]; + int findex; +} sbus_t; + +void sbus_init(sbus_t* bus); +bool sbus_recv(sbus_t* bus, uint8_t ibyte); +int sbus_chcount(sbus_t* bus); +int16_t sbus_getch(sbus_t* bus, int num); + +#endif diff --git a/mainloop/syscall.c b/sbus/syscall.c similarity index 100% rename from mainloop/syscall.c rename to sbus/syscall.c diff --git a/mainloop/usartu.c b/sbus/usartu.c similarity index 100% rename from mainloop/usartu.c rename to sbus/usartu.c diff --git a/mainloop/usartu.h b/sbus/usartu.h similarity index 100% rename from mainloop/usartu.h rename to sbus/usartu.h diff --git a/syscall.c b/syscall.c new file mode 100644 index 0000000..91b4299 --- /dev/null +++ b/syscall.c @@ -0,0 +1,159 @@ +/* + * Copyright 2022 Oleg Borodin + */ + + +#include + +#include +#include +#include +#include +#include + +#include + +#undef errno +extern int errno; + +char *__env[1] = { 0 }; + +char **environ = __env; + +int _execve(char *name, char **argv, char **env) { + errno = ENOMEM; + return -1; +} + +int _fork(void) { + errno = EAGAIN; + return -1; +} + +int _getpid(void) { + return 1; +} + +int _kill(int pid, int sig) { + errno = EINVAL; + return -1; +} + +void _exit(int i) { + while (1); +} + +int _isatty(int file) { + return 1; +} + +int _fstat(int file, struct stat *st) { + st->st_mode = S_IFCHR; + return 0; +} + +int _link(char *old, char *new) { + errno = EMLINK; + return -1; +} + +int _lseek(int file, int ptr, int dir) { + return 0; +} + +int _open(const char *name, int flags, int mode) { + return -1; +} + +#define STDIN 0 +#define STDOUT 1 +#define STDERR 3 + + +int _read(int file, char *ptr, int len) { + int i = 0; + while (i < len) { + ptr[i++] = 0; + } + return i; +} + + +int _write(int file, char *ptr, int len) { + int i; + + if ((file == STDOUT) || (file == STDERR)) { + for (i = 0; i < len; i++) { + usart_putc(USART1, ptr[i]); + } + return len; + } + return 0; +} + +int _stat(char *file, struct stat *st) { + st->st_mode = S_IFCHR; + return 0; +} + +int _close(int file) { + return -1; +} + + +int _times(struct tms *buf) { + return -1; +} + +int _unlink(char *name) { + errno = ENOENT; + return -1; +} + +int _wait(int *status) { + errno = ECHILD; + return -1; +} + +void *xxx_sbrk(int incr) { + + extern unsigned char *_end; + static unsigned char *heap = NULL; + unsigned char *prev_heap; + + if (heap == NULL) { + heap = (unsigned char*)&_end; + } + prev_heap = heap; + + heap += incr; + return prev_heap; +} + +register char* stack_ptr __asm__ ("sp"); + +caddr_t __attribute__((weak)) _sbrk (int incr) { + extern char end __asm__ ("_end"); + static char * heap_end; + char * prev_heap_end; + + if (heap_end == NULL) { + heap_end = &end; + } + + prev_heap_end = heap_end; + + if (heap_end + incr > stack_ptr) { +#if 0 + extern void abort (void); + _write (1, "_sbrk: Heap and stack collision\n", 32); + abort (); +#else + errno = ENOMEM; + return (caddr_t) -1; +#endif + } + + heap_end += incr; + return (caddr_t) prev_heap_end; +} diff --git a/usartu.c b/usartu.c new file mode 100644 index 0000000..29ac7b4 --- /dev/null +++ b/usartu.c @@ -0,0 +1,19 @@ +/* + * Copyright 2022 Oleg Borodin + */ + +#include + +#include +#include + +void usart_puts(uint32_t usart, char* str) { + int i = 0; + while (str[i] != 0) { + usart_send_blocking(usart, str[i++]); + } +} + +void usart_putc(uint32_t usart, char c) { + usart_send_blocking(usart, c); +} diff --git a/usartu.h b/usartu.h new file mode 100644 index 0000000..90fd757 --- /dev/null +++ b/usartu.h @@ -0,0 +1,7 @@ +#ifndef _USARTU_H_XYZ +#define _USARTU_H_XYZ + +void usart_puts(uint32_t usart, char * str); +void usart_putc(uint32_t usart, char c); + +#endif