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

added geometry.* i2cdev.* madgwick.*

Browse Source
This commit is contained in:
Oleg Borodin
2022-09-07 12:48:01 +02:00
parent f85bc21e21
commit 736f7bd509
10 changed files with 383 additions and 402 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
mculoop/Makefile
View File

@@ -4,7 +4,7 @@
.SECONDARY:
CFLAGS+= -I. -Os -DSTM32F4 #-std=c99
CFLAGS+= -I. -O2 -DSTM32F4 #-std=c99
CFLAGS+= -mfloat-abi=hard
CFLAGS+= -mcpu=cortex-m4
CFLAGS+= -mthumb
@@ -31,6 +31,9 @@ OBJS+= main.o
OBJS+= syscall.o
OBJS+= usartu.o
OBJS+= mpu6050.o
OBJS+= geometry.o
OBJS+= madgwick.o
OBJS+= i2cdev.o
main.elf: $(OBJS)
$(TARGET)-gcc $(^F) $(LDFLAGS) -o $@

68
mculoop/geometry.c Normal file
View File

@@ -0,0 +1,68 @@
/*
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
*/
#include <stdio.h>
#include <math.h>
#include <geometry.h>
void eulerangle_init(eulerangle_t* a) {
a->z = 0.0;
a->y = 0.0;
a->x = 0.0;
}
void eulerangle_todegress(eulerangle_t* a) {
a->z *= (180.0 / M_PI);
a->y *= (180.0 / M_PI);
a->x *= (180.0 / M_PI);
}
void eulerangle_toradians(eulerangle_t* a) {
a->z *= (M_PI / 180.0);
a->y *= (M_PI / 180.0);
a->x *= (M_PI / 180.0);
}
void eulerangle_norm(eulerangle_t* a) {
double n = sqrt(a->x*a->x + a->y*a->y + a->z*a->z);
a->x *= n;
a->y *= n;
a->z *= n;
}
void quaternion_init(quaternion_t* q) {
q->w = 1.0f;
q->x = 0.0f;
q->y = 0.0f;
q->z = 0.0f;
}
void quaternion_toeuler(quaternion_t* q, eulerangle_t* a) {
double x = q->x;
double y = q->y;
double z = q->z;
double w = q->w;
double t0 = (x + z)*(x - z); // x^2-z^2
double t1 = (w + y)*(w - y); // w^2-y^2
double xx = 0.5 * (t0 + t1); // 1/2 x of x'
double xy = x*y + w*z; // 1/2 y of x'
double xz = w*y - x*z; // 1/2 z of x'
double t = xx*xx + xy*xy; // cos(theta)^2
double yz = 2.0 * (y*z + w*x); // z of y'
a->z = atan2(xy, xx); // yaw (psi)
a->y = atan(xz /sqrt(t)); // pitch (theta)
if (t != 0) {
a->x = atan2(yz, t1 - t0);
} else {
a->x = (2.0*atan2(x, w) - copysign(1.0, xz)*a->z);
}
}

41
mculoop/geometry.h Normal file
View File

@@ -0,0 +1,41 @@
/*
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
*/
#ifndef GEOMETRY_H_QWERTY
#define GEOMETRY_H_QWERTY
typedef struct {
union { double z; double yaw; };
union { double y; double pitch; };
union { double x; double roll; };
} eulerangle_t;
typedef struct quaternion_s {
double w;
double x;
double y;
double z;
} quaternion_t;
typedef struct {
double ax;
double ay;
double az;
double gx;
double gy;
double gz;
} imuvec_t;
void eulerangle_init(eulerangle_t* a);
void eulerangle_norm(eulerangle_t* a);
void eulerangle_todegress(eulerangle_t* a);
void eulerangle_toradians(eulerangle_t* a);
void quaternion_init(quaternion_t* q);
void quaternion_toeuler(quaternion_t* q, eulerangle_t* a);
#endif

44
mculoop/i2cdev.c Normal file
View File

@@ -0,0 +1,44 @@
/*
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
*/
#include <libopencm3/stm32/i2c.h>
#include <stdint.h>
#include <math.h>
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);
}
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;
}
void i2cdev_reg_setbits(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);
}
void i2cdev_reg_cleanbits(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);
}
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);
}

18
mculoop/i2cdev.h Normal file
View File

@@ -0,0 +1,18 @@
/*
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
*/
#ifndef I2CDEV_H_QWERTY
#define I2CDEV_H_QWERTY
#include <stdint.h>
void i2cdev_write_reg8(uint32_t i2c, uint8_t addr, uint8_t reg, uint8_t value);
uint8_t i2cdev_read_reg8(uint32_t i2c, uint8_t addr, uint8_t reg);
void i2cdev_read_seq8(uint32_t i2c, uint8_t addr, uint8_t reg, uint8_t* buffer, uint8_t size);
void i2cdev_reg_setbits(uint32_t i2c, uint8_t addr, uint8_t reg, uint8_t mask);
void i2cdev_reg_cleanbits(uint32_t i2c, uint8_t addr, uint8_t reg, uint8_t mask);
#endif

87
mculoop/madgwick.c Normal file
View File

@@ -0,0 +1,87 @@
/*
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
*/
#include <stdio.h>
#include <math.h>
#include <geometry.h>
static double inv_sqrt(double x) {
return 1.0 / sqrt(x);
}
void madgwick(double dt, quaternion_t* q, imuvec_t* m) {
double q0 = q->w;
double q1 = q->x;
double q2 = q->y;
double q3 = q->z; // quaternion of sensor frame relative to auxiliary frame
double gx = m->gx;
double gy = m->gy;
double gz = m->gz;
double ax = m->ax;
double ay = m->ay;
double az = m->az;
double beta = 0.1f; // 2 * proportional gain (Kp)
double recipNorm;
double s0, s1, s2, s3;
double qDot1, qDot2, qDot3, qDot4;
// Rate of change of quaternion from gyroscope
qDot1 = 0.5 * (-q1*gx - q2*gy - q3*gz);
qDot2 = 0.5 * ( q0*gx + q2*gz - q3*gy);
qDot3 = 0.5 * ( q0*gy - q1*gz + q3*gx);
qDot4 = 0.5 * ( q0*gz + q1*gy - q2*gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if (!((ax == 0.0) && (ay == 0.0) && (az == 0.0))) {
// Normalise accelerometer measurement
recipNorm = inv_sqrt(ax*ax + ay*ay + az*az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Gradient decent algorithm corrective step
s0 = 4.0*q0*q2*q2 + 2.0*q2*ax + 4.0*q0*q1*q1 - 2.0*q1*ay;
s1 = 4.0*q1*q3*q3 - 2.0*q3*ax + 4.0*q0*q0*q1 - 2.0*q0*ay - 4.0*q1 + 8.0*q1*q1*q1 + 8.0*q1*q2*q2 + 4.0*q1*az;
s2 = 4.0*q0*q0*q2 + 2.0*q0*ax + 4.0*q2*q3*q3 - 2.0*q3*ay - 4.0*q2 + 8.0*q2*q1*q1 + 8.0*q2*q2*q2 + 4.0*q2*az;
s3 = 4.0*q1*q1*q3 - 2.0*q1*ax + 4.0*q2*q2*q3 - 2.0*q2*ay;
// Normalise step magnitude
recipNorm = inv_sqrt(s0*s0 + s1*s1 + s2*s2 + s3*s3);
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * dt;
q1 += qDot2 * dt;
q2 += qDot3 * dt;
q3 += qDot4 * dt;
// Normalise quaternion
recipNorm = inv_sqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
q->w = q0;
q->x = q1;
q->y = q2;
q->z = q3;
}

13
mculoop/madgwick.h Normal file
View File

@@ -0,0 +1,13 @@
/*
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
*/
#ifndef MADGWIC_H_QWERTY
#define MADGWIC_H_QWERTY
#include <geometry.h>
void madgwick(double dt, quaternion_t* q, imuvec_t* m);
#endif

365
mculoop/main.c
View File

@@ -18,15 +18,19 @@
#include <string.h>
#include <math.h>
#include "usartu.h"
#include "mpu6050.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 * 925; i++)
__asm__("nop");
}
//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]);
@@ -37,57 +41,49 @@ static void clock_setup(void) {
}
static void usart_setup(void) {
usart_disable(USART1);
nvic_disable_irq(NVIC_USART1_IRQ);
static void usart_setup(uint32_t usart, uint32_t gpioport, uint32_t gpiopins, uint32_t baudrate) {
usart_disable(usart);
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);
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(USART1, 115200);
//usart_set_baudrate(USART1, 230400);
usart_set_baudrate(USART1, 460800);
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_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);
usart_enable(usart);
}
const uint32_t systic_freq = 100 * 1000;
static void systick_setup(void) {
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);
//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);
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);
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) {
@@ -95,278 +91,20 @@ uint32_t sys_tick_counter(void) {
return val;
}
typedef struct angle_s {
double z; // yaw
double y; // pitch
double x; // roll
} angle_t;
typedef struct quaternion_s {
double w;
double x;
double y;
double z;
} quaternion_t;
double invSqrt(double x) {
return 1.0f / sqrt(x);
}
volatile float twoKp = (2.0f * 0.5f); // 2 * proportional gain (Kp)
volatile float twoKi = (2.0f * 0.0f); // 2 * integral gain (Ki)
volatile float integralFBx = 0.0f;
volatile float integralFBy = 0.0f;
volatile float integralFBz = 0.0f;
void quaternion_update_mahony(double dt, quaternion_t* q, mpu_value_t* m) {
double q0 = q->w;
double q1 = q->x;
double q2 = q->y;
double q3 = q->z; // quaternion of sensor frame relative to auxiliary frame
double gx = m->gx;
double gy = m->gy;
double gz = m->gz;
double ax = m->ax;
double ay = m->ay;
double az = m->az;
double recipNorm;
double halfvx, halfvy, halfvz;
double halfex, halfey, halfez;
double qa, qb, qc;
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if(!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax*ax + ay*ay + az*az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Estimated direction of gravity and vector perpendicular to magnetic flux
halfvx = q1*q3 - q0*q2;
halfvy = q0*q1 + q2*q3;
halfvz = q0*q0 - 0.5f + q3*q3;
// Error is sum of cross product between estimated and measured direction of gravity
halfex = (ay*halfvz - az*halfvy);
halfey = (az*halfvx - ax*halfvz);
halfez = (ax*halfvy - ay*halfvx);
// Compute and apply integral feedback if enabled
if(twoKi > 0.0f) {
integralFBx += twoKi*halfex*dt; // integral error scaled by Ki
integralFBy += twoKi*halfey*dt;
integralFBz += twoKi*halfez*dt;
gx += integralFBx; // apply integral feedback
gy += integralFBy;
gz += integralFBz;
}
else {
integralFBx = 0.0f; // prevent integral windup
integralFBy = 0.0f;
integralFBz = 0.0f;
}
// Apply proportional feedback
gx += twoKp*halfex;
gy += twoKp*halfey;
gz += twoKp*halfez;
}
// Integrate rate of change of quaternion
gx *= (0.5f*dt); // pre-multiply common factors
gy *= (0.5f*dt);
gz *= (0.5f*dt);
qa = q0;
qb = q1;
qc = q2;
q0 += (-qb*gx - qc*gy - q3*gz);
q1 += ( qa*gx + qc*gz - q3*gy);
q2 += ( qa*gy - qb*gz + q3*gx);
q3 += ( qa*gz + qb*gy - qc*gx);
// Normalise quaternion
recipNorm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
q->w = q0;
q->x = q1;
q->y = q2;
q->z = q3;
}
void quaternion_update(double dt, quaternion_t* q, mpu_value_t* m) {
double q0 = q->w;
double q1 = q->x;
double q2 = q->y;
double q3 = q->z; // quaternion of sensor frame relative to auxiliary frame
double gx = m->gx;
double gy = m->gy;
double gz = m->gz;
double ax = m->ax;
double ay = m->ay;
double az = m->az;
double beta = 0.1f; // 2 * proportional gain (Kp)
double recipNorm;
double s0, s1, s2, s3;
double qDot1, qDot2, qDot3, qDot4;
// Rate of change of quaternion from gyroscope
qDot1 = 0.5 * (-q1*gx - q2*gy - q3*gz);
qDot2 = 0.5 * ( q0*gx + q2*gz - q3*gy);
qDot3 = 0.5 * ( q0*gy - q1*gz + q3*gx);
qDot4 = 0.5 * ( q0*gz + q1*gy - q2*gx);
// Compute feedback only if accelerometer measurement valid (avoids NaN in accelerometer normalisation)
if (!((ax == 0.0) && (ay == 0.0) && (az == 0.0))) {
// Normalise accelerometer measurement
recipNorm = invSqrt(ax*ax + ay*ay + az*az);
ax *= recipNorm;
ay *= recipNorm;
az *= recipNorm;
// Gradient decent algorithm corrective step
s0 = 4.0*q0*q2*q2 + 2.0*q2*ax + 4.0*q0*q1*q1 - 2.0*q1*ay;
s1 = 4.0*q1*q3*q3 - 2.0*q3*ax + 4.0*q0*q0*q1 - 2.0*q0*ay - 4.0*q1 + 8.0*q1*q1*q1 + 8.0*q1*q2*q2 + 4.0*q1*az;
s2 = 4.0*q0*q0*q2 + 2.0*q0*ax + 4.0*q2*q3*q3 - 2.0*q3*ay - 4.0*q2 + 8.0*q2*q1*q1 + 8.0*q2*q2*q2 + 4.0*q2*az;
s3 = 4.0*q1*q1*q3 - 2.0*q1*ax + 4.0*q2*q2*q3 - 2.0*q2*ay;
// Normalise step magnitude
recipNorm = invSqrt(s0*s0 + s1*s1 + s2*s2 + s3*s3);
s0 *= recipNorm;
s1 *= recipNorm;
s2 *= recipNorm;
s3 *= recipNorm;
// Apply feedback step
qDot1 -= beta * s0;
qDot2 -= beta * s1;
qDot3 -= beta * s2;
qDot4 -= beta * s3;
}
// Integrate rate of change of quaternion to yield quaternion
q0 += qDot1 * dt;
q1 += qDot2 * dt;
q2 += qDot3 * dt;
q3 += qDot4 * dt;
// Normalise quaternion
recipNorm = invSqrt(q0*q0 + q1*q1 + q2*q2 + q3*q3);
q0 *= recipNorm;
q1 *= recipNorm;
q2 *= recipNorm;
q3 *= recipNorm;
q->w = q0;
q->x = q1;
q->y = q2;
q->z = q3;
}
double radian2degrees(double radians) {
return radians * (180.0f / M_PI);
}
double degress2radian(double degress) {
return degress * (M_PI / 180.0f);
}
void quaternion_init(quaternion_t* q) {
q->w = 1.0f;
q->x = 0.0f;
q->y = 0.0f;
q->z = 0.0f;
}
void angle_init(angle_t* a) {
a->z = 0.0f;
a->y = 0.0f;
a->x = 0.0f;
}
void angle_degress(angle_t* a) {
a->z *= (180.0f / M_PI);
a->y *= (180.0f / M_PI);
a->x *= (180.0f / M_PI);
}
double sgn(double x) {
return copysignf(1.f,x);
}
angle_t quaternion2xyz(quaternion_t* q) {
angle_t a;
double x = q->x;
double y = q->y;
double z = q->z;
double w = q->w;
double t0 = (x + z)*(x - z); // x^2-z^2
double t1 = (w + y)*(w - y); // w^2-y^2
double xx = 0.5 * (t0 + t1); // 1/2 x of x'
double xy = x*y + w*z; // 1/2 y of x'
double xz = w*y - x*z; // 1/2 z of x'
double t = xx*xx + xy*xy; // cos(theta)^2
double yz = 2.0 * (y*z + w*x); // z of y'
a.z = atan2(xy, xx); // yaw (psi)
a.y = atan(xz /sqrt(t)); // pitch (theta)
if (t != 0) {
a.x = atan2(yz, t1 - t0);
} else {
a.x = (2.0 * atan2(x, w) - sgn(xz) * a.z);
}
return a;
}
void angle_norm(angle_t* a) {
double n = sqrt(a->x*a->x + a->y*a->y + a->z*a->z);
a->x *= n;
a->y *= n;
a->z *= n;
}
int main(void) {
_delay(100);
clock_setup();
usart_setup();
usart_setup(USART1, GPIOA, GPIO9 | GPIO10, 460800);
i2c_setup(I2C1, GPIOB, GPIO8 | GPIO9);
systick_setup(g_systick_freq);
i2c_setup();
systick_setup();
imu_t imu;
printf("==== imu initialize ====\r\n");
imu_setup(&imu, I2C1, 0x68);
imu_calibrate(&imu, 20000);
printf("==== imu started ====\r\n");
mpu_t mpu;
printf("==== mpu initialize ====\r\n");
mpu_setup(&mpu, I2C1, 0x68);
_delay(100);
mpu_calibrate(&mpu, 20000);
printf("==== mpu started ====\r\n");
mpu_value_t mval;
imuvec_t mval;
quaternion_t q;
quaternion_init(&q);
@@ -374,19 +112,24 @@ int main(void) {
uint32_t last_ts = 0;
printf("==== main loop ====\r\n");
while (true) {
mpu_read(&mpu, &mval);
imu_getvec(&imu, &mval);
last_ts = g_sys_tick_counter;
double dt = (float)(last_ts - prev_ts) / (float)systic_freq;
double dt = (float)(last_ts - prev_ts) / (float)g_systick_freq;
prev_ts = last_ts;
quaternion_update(dt, &q, &mval);
madgwick(dt, &q, &mval);
angle_t a = quaternion2xyz(&q);
angle_degress(&a);
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;
};
}

117
mculoop/mpu6050.c
View File

@@ -2,11 +2,11 @@
* Copyright 2022 Oleg Borodin <borodin@unix7.org>
*/
#include <libopencm3/stm32/i2c.h>
#include <stdint.h>
#include <math.h>
#include <i2cdev.h>
#include <mpu6050.h>
#define MPU_REG_SMPLRT_DIV 0x19
@@ -94,66 +94,20 @@
#define MPU_PWR2_STBY_YG_BIT 1
#define MPU_PWR2_STBY_ZG_BIT 0
//static void i2cdev_reg_setbits(uint32_t i2c, uint8_t addr, uint8_t reg, uint8_t mask);
//static void i2cdev_reg_cleanbits(uint32_t i2c, uint8_t addr, uint8_t reg, uint8_t mask);
//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 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_reg_setbits(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 void i2cdev_reg_cleanbits(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 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);
}
#define MPU_GYRO_LSB MPU_GYRO_LSB_1000
#define MPU_GYRO_FS MPU_GYRO_FS_1000
#define MPU_ACCEL_LSB MPU_ACCEL_LSB_16
#define MPU_ACCEL_FS MPU_ACCEL_FS_16
void mpu_setup(mpu_t* mpu, uint32_t i2c, uint8_t addr) {
void imu_setup(imu_t* imu, uint32_t i2c, uint8_t addr) {
mpu->bus = i2c;
mpu->addr = addr;
imu->bus = i2c;
imu->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;
imu->gxe = 0;
imu->gye = 0;
imu->gze = 0;
//i2cdev_write_reg8(i2c, addr, MPU_REG_PWR_MGMT_1, 1 << MPU_PWR1_DEVICE_RESET_BIT);
//for (int i = 0; i < 10000; i++) __asm__("nop");
@@ -166,10 +120,10 @@ void mpu_setup(mpu_t* mpu, uint32_t i2c, uint8_t addr) {
}
static void mpu_rawread(mpu_t* mpu, mpu_value_t* val) {
static void imu_rawread(imu_t* imu, imuvec_t* val) {
uint8_t buffer[14];
i2cdev_read_seq8(mpu->bus, mpu->addr, MPU_REG_ACCEL_XOUT_H, (uint8_t*)buffer, 14);
i2cdev_read_seq8(imu->bus, imu->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];
@@ -193,8 +147,8 @@ static void mpu_rawread(mpu_t* mpu, mpu_value_t* val) {
}
void mpu_calibrate(mpu_t* mpu, int count) {
mpu_value_t val;
void imu_calibrate(imu_t* imu, int loops) {
imuvec_t val;
val.ax = 0;
val.ay = 0;
val.az = 0;
@@ -202,26 +156,41 @@ void mpu_calibrate(mpu_t* mpu, int count) {
val.gy = 0;
val.gz = 0;
for (int i = 0; i < count; i++) {
mpu_rawread(mpu, &val);
mpu->err.gx += val.gx / (double)count;
mpu->err.gy += val.gy / (double)count;
mpu->err.gz += val.gz / (double)count;
mpu->err.ax += val.ax / (double)count;
mpu->err.ay += val.ay / (double)count;
for (int i = 0; i < loops; i++) {
imu_rawread(imu, &val);
imu->gxe += val.gx / (double)loops;
imu->gye += val.gy / (double)loops;
imu->gze += val.gz / (double)loops;
}
}
void mpu_read(mpu_t* mpu, mpu_value_t* val) {
mpu_rawread(mpu, val);
void imu_gettilt(imu_t* imu, int loops, eulerangle_t* a) {
imuvec_t val;
val.ax = 0;
val.ay = 0;
val.az = 0;
val.gx = 0;
val.gy = 0;
val.gz = 0;
val->gx -= mpu->err.gx;
val->gy -= mpu->err.gy;
val->gz -= mpu->err.gz;
double ax = 0;
double ay = 0;
double az = 0;
val->ax -= mpu->err.ax;
val->ay -= mpu->err.ay;
val->az -= mpu->err.az;
for (int i = 0; i < loops; i++) {
imu_rawread(imu, &val);
ax += val.ax / (double)loops;
ay += val.ay / (double)loops;
az += val.az / (double)loops;
}
a->x = atan(ax / sqrt(ay*ay + az*az));
a->y = atan(ay / sqrt(ax*ax + az*az));
a->z = atan(az / sqrt(ax*ax + ay*ay));
}
void imu_getvec(imu_t* imu, imuvec_t* val) {
imu_rawread(imu, val);
val->gx -= imu->gxe;
val->gy -= imu->gye;
val->gz -= imu->gze;
}

27
mculoop/mpu6050.h
View File

@@ -4,22 +4,17 @@
#include <stdint.h>
#include <geometry.h>
typedef struct {
double ax;
double ay;
double az;
double gx;
double gy;
double gz;
} mpu_value_t;
uint32_t bus;
uint8_t addr;
double gxe;
double gye;
double gze;
} imu_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);
void imu_setup(imu_t* imu, uint32_t i2c, uint8_t addr);
void imu_calibrate(imu_t* imu, int count);
void imu_gettilt(imu_t* imu, int loops, eulerangle_t* a);
void imu_getvec(imu_t* imu, imuvec_t* val);