ziggi/stm32uav
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

added .gitignore Makefile README.md main.c main.ld mpu6050.c mpu6050.h syscall.c usartu.c usartu.h

Browse Source
This commit is contained in:
Oleg Borodin
2022-09-06 08:39:32 +02:00
commit c8432c1d65
10 changed files with 690 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
.gitignore vendored Normal file
View File

@@ -0,0 +1,7 @@
*.elf
*.bin
*.map
*.geany
*.o
*~
work

81
Makefile Normal file
View File

@@ -0,0 +1,81 @@
#
# Copyright: Oleg Borodin <onborodin@gmail.com> 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

3
README.md Normal file
View File

@@ -0,0 +1,3 @@
## MPU draft

142
main.c Normal file
View File

@@ -0,0 +1,142 @@
/*
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
*/
#include <libopencm3/cm3/nvic.h>
#include <libopencm3/cm3/systick.h>
#include <libopencm3/cm3/scb.h>
#include <libopencm3/stm32/rcc.h>
#include <libopencm3/stm32/gpio.h>
#include <libopencm3/stm32/usart.h>
#include <libopencm3/stm32/i2c.h>
#include <stdlib.h>
#include <stdio.h>
#include <stdbool.h>
#include <string.h>
#include <math.h>
#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;
};
}

68
main.ld Normal file
View File

@@ -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));

179
mpu6050.c Normal file
View File

@@ -0,0 +1,179 @@
/*
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
*/
#include <libopencm3/stm32/i2c.h>
#include <stdint.h>
#include <mpu6050.h>
#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, &reg, 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, &reg, 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;
}

25
mpu6050.h Normal file
View File

@@ -0,0 +1,25 @@
/*
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
*/
#include <stdint.h>
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);

159
syscall.c Normal file
View File

@@ -0,0 +1,159 @@
/*
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
*/
#include <libopencm3/stm32/usart.h>
#include <stdlib.h>
#include <reent.h>
#include <sys/stat.h>
#include <sys/errno.h>
#include <errno.h>
#include <usartu.h>
#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;
}

19
usartu.c Normal file
View File

@@ -0,0 +1,19 @@
/*
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
*/
#include <libopencm3/stm32/usart.h>
#include <stdlib.h>
#include <stdio.h>
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);
}

7
usartu.h Normal file
View File

@@ -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