diff --git a/mculoop/Makefile b/mculoop/Makefile index ec8a3d3..070253c 100644 --- a/mculoop/Makefile +++ b/mculoop/Makefile @@ -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 $@ diff --git a/mculoop/geometry.c b/mculoop/geometry.c new file mode 100644 index 0000000..6d6fb93 --- /dev/null +++ b/mculoop/geometry.c @@ -0,0 +1,68 @@ +/* + * Copyright 2022 Oleg Borodin + */ + + +#include +#include +#include + +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); + } +} diff --git a/mculoop/geometry.h b/mculoop/geometry.h new file mode 100644 index 0000000..1659334 --- /dev/null +++ b/mculoop/geometry.h @@ -0,0 +1,41 @@ +/* + * Copyright 2022 Oleg Borodin + */ + +#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 diff --git a/mculoop/i2cdev.c b/mculoop/i2cdev.c new file mode 100644 index 0000000..932ba8d --- /dev/null +++ b/mculoop/i2cdev.c @@ -0,0 +1,44 @@ +/* + * Copyright 2022 Oleg Borodin + */ + + +#include +#include +#include + + +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, ®, 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, ®, 1, buffer, size); +} diff --git a/mculoop/i2cdev.h b/mculoop/i2cdev.h new file mode 100644 index 0000000..f60c0ba --- /dev/null +++ b/mculoop/i2cdev.h @@ -0,0 +1,18 @@ +/* + * Copyright 2022 Oleg Borodin + */ + +#ifndef I2CDEV_H_QWERTY +#define I2CDEV_H_QWERTY + +#include + +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 diff --git a/mculoop/madgwick.c b/mculoop/madgwick.c new file mode 100644 index 0000000..352268c --- /dev/null +++ b/mculoop/madgwick.c @@ -0,0 +1,87 @@ +/* + * Copyright 2022 Oleg Borodin + */ + + +#include +#include + +#include + +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; +} diff --git a/mculoop/madgwick.h b/mculoop/madgwick.h new file mode 100644 index 0000000..8f30f4d --- /dev/null +++ b/mculoop/madgwick.h @@ -0,0 +1,13 @@ +/* + * Copyright 2022 Oleg Borodin + */ + + +#ifndef MADGWIC_H_QWERTY +#define MADGWIC_H_QWERTY + +#include + +void madgwick(double dt, quaternion_t* q, imuvec_t* m); + +#endif diff --git a/mculoop/main.c b/mculoop/main.c index 85744ef..cf71c14 100644 --- a/mculoop/main.c +++ b/mculoop/main.c @@ -18,15 +18,19 @@ #include #include -#include "usartu.h" -#include "mpu6050.h" +#include +#include +#include +#include + +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; }; } diff --git a/mculoop/mpu6050.c b/mculoop/mpu6050.c index 33c3bca..5deb6a0 100644 --- a/mculoop/mpu6050.c +++ b/mculoop/mpu6050.c @@ -2,11 +2,11 @@ * Copyright 2022 Oleg Borodin */ - #include #include #include +#include #include #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, ®, 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, ®, 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; } diff --git a/mculoop/mpu6050.h b/mculoop/mpu6050.h index 5454d42..0811cea 100644 --- a/mculoop/mpu6050.h +++ b/mculoop/mpu6050.h @@ -4,22 +4,17 @@ #include +#include 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);