From f4b59acce0766d23571c77465825fd5f70b6fa44 Mon Sep 17 00:00:00 2001 From: Oleg Borodin Date: Tue, 6 Sep 2022 08:39:50 +0200 Subject: [PATCH] Merge mculoop --- mculoop/.gitignore | 7 ++ mculoop/Makefile | 81 ++++++++++++++++++++ mculoop/README.md | 3 + mculoop/main.c | 142 +++++++++++++++++++++++++++++++++++ mculoop/main.ld | 68 +++++++++++++++++ mculoop/mpu6050.c | 179 +++++++++++++++++++++++++++++++++++++++++++++ mculoop/mpu6050.h | 25 +++++++ mculoop/syscall.c | 159 ++++++++++++++++++++++++++++++++++++++++ mculoop/usartu.c | 19 +++++ mculoop/usartu.h | 7 ++ 10 files changed, 690 insertions(+) create mode 100644 mculoop/.gitignore create mode 100644 mculoop/Makefile create mode 100644 mculoop/README.md create mode 100644 mculoop/main.c create mode 100644 mculoop/main.ld create mode 100644 mculoop/mpu6050.c create mode 100644 mculoop/mpu6050.h create mode 100644 mculoop/syscall.c create mode 100644 mculoop/usartu.c create mode 100644 mculoop/usartu.h diff --git a/mculoop/.gitignore b/mculoop/.gitignore new file mode 100644 index 0000000..3ca5774 --- /dev/null +++ b/mculoop/.gitignore @@ -0,0 +1,7 @@ +*.elf +*.bin +*.map +*.geany +*.o +*~ +work diff --git a/mculoop/Makefile b/mculoop/Makefile new file mode 100644 index 0000000..ec8a3d3 --- /dev/null +++ b/mculoop/Makefile @@ -0,0 +1,81 @@ +# +# 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+= mpu6050.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/mculoop/README.md b/mculoop/README.md new file mode 100644 index 0000000..fea908c --- /dev/null +++ b/mculoop/README.md @@ -0,0 +1,3 @@ + +## MPU draft + \ No newline at end of file diff --git a/mculoop/main.c b/mculoop/main.c new file mode 100644 index 0000000..79db649 --- /dev/null +++ b/mculoop/main.c @@ -0,0 +1,142 @@ + +/* + * Copyright 2022 Oleg Borodin + */ + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "usartu.h" +#include "mpu6050.h" + +uint32_t g_sys_tick_counter; + +static void _delay(uint32_t n) { + for (int i = 0; i < n * 925; i++) + __asm__("nop"); +} + +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); + +} + +static void usart_setup(void) { + usart_disable(USART1); + nvic_disable_irq(NVIC_USART1_IRQ); + + gpio_mode_setup(GPIOA, GPIO_MODE_AF, GPIO_PUPD_NONE, GPIO9 | GPIO10); + gpio_set_af(GPIOA, GPIO_AF7, GPIO9 | GPIO10); + gpio_set_output_options(GPIOA, GPIO_OTYPE_PP, GPIO_OSPEED_100MHZ, GPIO9 | GPIO10); + + //usart_set_baudrate(USART1, 115200); + //usart_set_baudrate(USART1, 230400); + usart_set_baudrate(USART1, 460800); + + usart_set_databits(USART1, 8); + usart_set_stopbits(USART1, USART_STOPBITS_1); + usart_set_parity(USART1, USART_PARITY_NONE); + usart_set_flow_control(USART1, USART_FLOWCONTROL_NONE); + usart_set_mode(USART1, USART_MODE_TX_RX); + + usart_disable_rx_interrupt(USART1); + + usart_enable(USART1); +} + +const uint32_t systic_freq = 100 * 1000; + +static void systick_setup(void) { + g_sys_tick_counter = 0; + + + gpio_mode_setup(GPIOB, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, GPIO6); + gpio_set_output_options(GPIOB, GPIO_OTYPE_PP, GPIO_OSPEED_100MHZ, GPIO6); + + systick_set_frequency(systic_freq, rcc_ahb_frequency); + systick_interrupt_enable(); + systick_counter_enable(); +} + + +static void i2c_setup(void) { + gpio_mode_setup(GPIOB, GPIO_MODE_AF, GPIO_PUPD_PULLUP, GPIO8 | GPIO9); + gpio_set_output_options(GPIOB, GPIO_OTYPE_OD, GPIO_OSPEED_100MHZ, GPIO8 | GPIO9); + gpio_set_af(GPIOB, GPIO_AF4, GPIO8 | GPIO9); + + i2c_reset(I2C1); + i2c_peripheral_disable(I2C1); + i2c_set_speed(I2C1, i2c_speed_fm_400k, I2C_CR2_FREQ_36MHZ); + + i2c_peripheral_enable(I2C1); +} + +void sys_tick_handler(void) { + g_sys_tick_counter++; + //gpio_toggle(GPIOB, GPIO6); +} + +uint32_t sys_tick_counter(void) { + uint32_t val = g_sys_tick_counter; + return val; +} + +void ahrs_update(float gx, float gy, float gz, float ax, float ay, float az) { + +} + +int main(void) { + + _delay(100); + clock_setup(); + usart_setup(); + + i2c_setup(); + systick_setup(); + + mpu_t mpu; + mpu_setup(&mpu, I2C1, 0x68); + mpu_calibrate(&mpu, 5000); + printf("==== start ====\r\n"); + + mpu_value_t val; + + uint32_t old_time = 0; + uint32_t new_time = 0; + + float freq = 100.0f; + uint32_t delta = 0.0f; + + while (true) { + mpu_read(&mpu, &val); + gpio_toggle(GPIOB, GPIO6); + + ahrs_update(val.gx, val.gy, val.gz, val.ax, val.ay, val.az); + + printf("%12.3f\r\n", freq); + + new_time = g_sys_tick_counter; + delta = (float)(old_time - new_time); + if (delta != 0.0f) { + freq = (float)systic_freq / (float)(new_time - old_time); + //printf("%12.3f\r\n", freq); + } + old_time = new_time; + }; +} diff --git a/mculoop/main.ld b/mculoop/main.ld new file mode 100644 index 0000000..7e40cf7 --- /dev/null +++ b/mculoop/main.ld @@ -0,0 +1,68 @@ + +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)); diff --git a/mculoop/mpu6050.c b/mculoop/mpu6050.c new file mode 100644 index 0000000..61fc0eb --- /dev/null +++ b/mculoop/mpu6050.c @@ -0,0 +1,179 @@ +/* + * Copyright 2022 Oleg Borodin + */ + + +#include +#include + +#include + +#define MPU_REG_CONFIG 0x1A +#define MPU_REG_GYRO_CONFIG 0x1B +#define MPU_REG_ACCEL_CONFIG 0x1C + +#define MPU_REG_ACCEL_XOUT_H 0x3B +#define MPU_REG_ACCEL_XOUT_L 0x3C +#define MPU_REG_ACCEL_YOUT_H 0x3D +#define MPU_REG_ACCEL_YOUT_L 0x3E +#define MPU_REG_ACCEL_ZOUT_H 0x3F +#define MPU_REG_ACCEL_ZOUT_L 0x40 +#define MPU_REG_TEMP_OUT_H 0x41 +#define MPU_REG_TEMP_OUT_L 0x42 +#define MPU_REG_GYRO_XOUT_H 0x43 +#define MPU_REG_GYRO_XOUT_L 0x44 +#define MPU_REG_GYRO_YOUT_H 0x45 +#define MPU_REG_GYRO_YOUT_L 0x46 +#define MPU_REG_GYRO_ZOUT_H 0x47 +#define MPU_REG_GYRO_ZOUT_L 0x48 + +#define MPU_REG_PWR_MGMT_1 0x6B +#define MPU_REG_PWR_MGMT_2 0x6C + +/* GYRO_CONFIG 0x1B */ +#define MPU_GYRO_FS_BASE 3 +#define MPU_GYRO_FS_LEN 2 + +#define MPU_GYRO_FS_250 0 +#define MPU_GYRO_FS_500 1 +#define MPU_GYRO_FS_1000 2 +#define MPU_GYRO_FS_2000 3 + +/* ACCEL_CONFIG 0x1C */ +#define MPU_ACCEL_FS_BASE 3 +#define MPU_ACCEL_FS_LEN 2 +#define MPU_ACCEL_FS_2 0 +#define MPU_ACCEL_FS_4 1 +#define MPU_ACCEL_FS_8 2 +#define MPU_ACCEL_FS_16 3 + +#define MPU_GYRO_LSB_250 131.0f +#define MPU_GYRO_LSB_500 65.5f +#define MPU_GYRO_LSB_1000 32.8f +#define MPU_GYRO_LSB_2000 16.4f + +#define MPU_ACCEL_LSB_2 16384.0f +#define MPU_ACCEL_LSB_4 8192.0f +#define MPU_ACCEL_LSB_8 4096.0f +#define MPU_ACCEL_LSB_16 2048.0f + + +//static uint8_t i2cdev_read_reg8(uint32_t i2c, uint8_t addr, uint8_t reg); +static void i2cdev_write_reg8(uint32_t i2c, uint8_t addr, uint8_t reg, uint8_t value); +static void i2cdev_read_seq8(uint32_t i2c, uint8_t addr, uint8_t reg, uint8_t* buffer, uint8_t size); + +static void i2cdev_write_reg8(uint32_t i2c, uint8_t addr, uint8_t reg, uint8_t value) { + uint8_t buffer[2]; + buffer[0] = reg; + buffer[1] = value; + i2c_transfer7(i2c, addr, buffer, 2, NULL, 0); +} + +//static void i2cdev_reg_setbit(uint32_t i2c, uint8_t addr, uint8_t reg, uint8_t mask) { +// uint8_t buffer[2]; +// buffer[0] = reg; +// buffer[1] = 0x00; +// i2c_transfer7(i2c, addr, &buffer[0], 1, &buffer[1], 1); +// buffer[1] |= mask; +// i2c_transfer7(i2c, addr, buffer, 2, NULL, 0); +//} + +//static uint8_t i2cdev_read_reg8(uint32_t i2c, uint8_t addr, uint8_t reg) { +// uint8_t val; +// i2c_transfer7(i2c, addr, ®, 1, &val, 1); +// return val; +//} + + +static void i2cdev_read_seq8(uint32_t i2c, uint8_t addr, uint8_t reg, uint8_t* buffer, uint8_t size) { + i2c_transfer7(i2c, addr, ®, 1, buffer, size); +} + + +void mpu_setup(mpu_t* mpu, uint32_t i2c, uint8_t addr) { + + mpu->bus = i2c; + mpu->addr = addr; + + mpu->err.ax = 0; + mpu->err.ay = 0; + mpu->err.az = 0; + mpu->err.gx = 0; + mpu->err.gy = 0; + mpu->err.gz = 0; + + i2cdev_write_reg8(i2c, addr, MPU_REG_PWR_MGMT_1, 0x00); + i2cdev_write_reg8(i2c, addr, MPU_REG_GYRO_CONFIG, MPU_GYRO_FS_250 << MPU_GYRO_FS_BASE); + i2cdev_write_reg8(i2c, addr, MPU_REG_ACCEL_CONFIG, MPU_ACCEL_FS_2 << MPU_ACCEL_FS_BASE); +} + +static void mpu_rawread(mpu_t* mpu, mpu_value_t* val) { + + uint8_t buffer[14]; + i2cdev_read_seq8(mpu->bus, mpu->addr, MPU_REG_ACCEL_XOUT_H, (uint8_t*)buffer, 14); + + int16_t ax = (((int16_t) buffer[0]) << 8) | buffer[1]; + int16_t ay = (((int16_t) buffer[2]) << 8) | buffer[3]; + int16_t az = (((int16_t) buffer[4]) << 8) | buffer[5]; + + int16_t gx = (((int16_t) buffer[8]) << 8) | buffer[9]; + int16_t gy = (((int16_t) buffer[10]) << 8) | buffer[11]; + int16_t gz = (((int16_t) buffer[12]) << 8) | buffer[13]; + + val->ax = (float)ax / MPU_ACCEL_LSB_2; + val->ay = (float)ay / MPU_ACCEL_LSB_2; + val->az = (float)az / MPU_ACCEL_LSB_2; + + val->gx = (float)gx / MPU_GYRO_LSB_250; + val->gy = (float)gy / MPU_GYRO_LSB_250; + val->gz = (float)gz / MPU_GYRO_LSB_250; + + //val->gx *= 0.0174533f; + //val->gy *= 0.0174533f; + //val->gz *= 0.0174533f; + +} + +void mpu_calibrate(mpu_t* mpu, int count) { + mpu_value_t val; + val.ax = 0; + val.ay = 0; + val.az = 0; + val.gx = 0; + val.gy = 0; + val.gz = 0; + + for (int i = 0; i < count; i++) { + mpu_rawread(mpu, &val); + + mpu->err.ax += val.ax; + mpu->err.ay += val.ay; + mpu->err.az += val.az; + + mpu->err.gx += val.gx; + mpu->err.gy += val.gy; + mpu->err.gz += val.gz; + } + + mpu->err.ax /= count; + mpu->err.ay /= count; + mpu->err.az /= count; + + mpu->err.gx /= count; + mpu->err.gy /= count; + mpu->err.gz /= count; +} + + +void mpu_read(mpu_t* mpu, mpu_value_t* val) { + mpu_rawread(mpu, val); + + val->ax -= mpu->err.ax; + val->ay -= mpu->err.ay; + val->az -= mpu->err.az; + + val->gx -= mpu->err.gx; + val->gy -= mpu->err.gy; + val->gz -= mpu->err.gz; + +} diff --git a/mculoop/mpu6050.h b/mculoop/mpu6050.h new file mode 100644 index 0000000..5ff6752 --- /dev/null +++ b/mculoop/mpu6050.h @@ -0,0 +1,25 @@ +/* + * Copyright 2022 Oleg Borodin + */ + + +#include + +typedef struct { + float ax; + float ay; + float az; + float gx; + float gy; + float gz; +} mpu_value_t; + +typedef struct { + uint32_t bus; + uint8_t addr; + mpu_value_t err; +} mpu_t; + +void mpu_setup(mpu_t* mpu, uint32_t i2c, uint8_t addr); +void mpu_calibrate(mpu_t* mpu, int count); +void mpu_read(mpu_t* mpu, mpu_value_t* val); diff --git a/mculoop/syscall.c b/mculoop/syscall.c new file mode 100644 index 0000000..91b4299 --- /dev/null +++ b/mculoop/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/mculoop/usartu.c b/mculoop/usartu.c new file mode 100644 index 0000000..29ac7b4 --- /dev/null +++ b/mculoop/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/mculoop/usartu.h b/mculoop/usartu.h new file mode 100644 index 0000000..90fd757 --- /dev/null +++ b/mculoop/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