From c8432c1d657fbfcde27477f5977ae58cc92794ed Mon Sep 17 00:00:00 2001 From: Oleg Borodin Date: Tue, 6 Sep 2022 08:39:32 +0200 Subject: [PATCH] added .gitignore Makefile README.md main.c main.ld mpu6050.c mpu6050.h syscall.c usartu.c usartu.h --- .gitignore | 7 +++ Makefile | 81 ++++++++++++++++++++++++ README.md | 3 + main.c | 142 ++++++++++++++++++++++++++++++++++++++++++ main.ld | 68 ++++++++++++++++++++ mpu6050.c | 179 +++++++++++++++++++++++++++++++++++++++++++++++++++++ mpu6050.h | 25 ++++++++ syscall.c | 159 +++++++++++++++++++++++++++++++++++++++++++++++ usartu.c | 19 ++++++ usartu.h | 7 +++ 10 files changed, 690 insertions(+) create mode 100644 .gitignore create mode 100644 Makefile create mode 100644 README.md create mode 100644 main.c create mode 100644 main.ld create mode 100644 mpu6050.c create mode 100644 mpu6050.h create mode 100644 syscall.c create mode 100644 usartu.c create mode 100644 usartu.h diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..3ca5774 --- /dev/null +++ b/.gitignore @@ -0,0 +1,7 @@ +*.elf +*.bin +*.map +*.geany +*.o +*~ +work diff --git a/Makefile b/Makefile new file mode 100644 index 0000000..ec8a3d3 --- /dev/null +++ b/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/README.md b/README.md new file mode 100644 index 0000000..fea908c --- /dev/null +++ b/README.md @@ -0,0 +1,3 @@ + +## MPU draft + \ No newline at end of file diff --git a/main.c b/main.c new file mode 100644 index 0000000..79db649 --- /dev/null +++ b/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/main.ld b/main.ld new file mode 100644 index 0000000..7e40cf7 --- /dev/null +++ b/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/mpu6050.c b/mpu6050.c new file mode 100644 index 0000000..61fc0eb --- /dev/null +++ b/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/mpu6050.h b/mpu6050.h new file mode 100644 index 0000000..5ff6752 --- /dev/null +++ b/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/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