136 lines
3.5 KiB
C
136 lines
3.5 KiB
C
|
|
/*
|
|
* 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>
|
|
#include <geometry.h>
|
|
#include <madgwick.h>
|
|
|
|
|
|
const uint32_t g_systick_freq = 100 * 1000;
|
|
uint32_t g_sys_tick_counter;
|
|
|
|
//static void _delay(uint32_t n) {
|
|
// for (int i = 0; i < n * ; 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(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 systick_setup(uint32_t systic_freq) {
|
|
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(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);
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
int main(void) {
|
|
|
|
clock_setup();
|
|
usart_setup(USART1, GPIOA, GPIO9 | GPIO10, 460800);
|
|
i2c_setup(I2C1, GPIOB, GPIO8 | GPIO9);
|
|
systick_setup(g_systick_freq);
|
|
|
|
imu_t imu;
|
|
printf("==== imu initialize ====\r\n");
|
|
imu_setup(&imu, I2C1, 0x68);
|
|
imu_calibrate(&imu, 20000);
|
|
printf("==== imu started ====\r\n");
|
|
|
|
imuvec_t mval;
|
|
quaternion_t q;
|
|
quaternion_init(&q);
|
|
|
|
uint32_t prev_ts = 0;
|
|
uint32_t last_ts = 0;
|
|
|
|
printf("==== main loop ====\r\n");
|
|
|
|
while (true) {
|
|
imu_getvec(&imu, &mval);
|
|
|
|
last_ts = g_sys_tick_counter;
|
|
double dt = (float)(last_ts - prev_ts) / (float)g_systick_freq;
|
|
prev_ts = last_ts;
|
|
|
|
madgwick(dt, &q, &mval);
|
|
|
|
eulerangle_t a;
|
|
quaternion_toeuler(&q, &a);
|
|
eulerangle_todegress(&a);
|
|
|
|
printf("dt=%.6f y=%10.4f x=%10.4f z=%10.4f \r\n", dt, a.y, a.x, a.z);
|
|
|
|
//double b2 = 0.0;
|
|
//double K = 0.01;
|
|
//b2 = b2*(1 - K) + b*K;
|
|
};
|
|
}
|