add Robert Mahony correlator

mculoop/main.c

@@ -60,12 +60,11 @@ static void usart_setup(void) {
     usart_enable(USART1);
 }
 
-const uint32_t systic_freq = 100 * 1000;
+const uint32_t systic_freq = 50 * 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);
 
@@ -89,7 +88,6 @@ static void i2c_setup(void) {
 
 void sys_tick_handler(void) {
     g_sys_tick_counter++;
-    //gpio_toggle(GPIOB, GPIO6);
 }
 
 uint32_t sys_tick_counter(void) {
@@ -97,10 +95,175 @@ uint32_t sys_tick_counter(void) {
     return val;
 }
 
-void ahrs_update(float gx, float gy, float gz, float ax, float ay, float az) {
+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);
 }
 
+void ahrs_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;
+    double _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
+
+    // Rate of change of quaternion from gyroscope
+    qDot1 = 0.5f * (-q1*gx - q2*gy - q3*gz);
+    qDot2 = 0.5f * ( q0*gx + q2*gz - q3*gy);
+    qDot3 = 0.5f * ( q0*gy - q1*gz + q3*gx);
+    qDot4 = 0.5f * ( q0*gz + q1*gy - q2*gx);
+
+    // 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;
+
+        // Auxiliary variables to avoid repeated arithmetic
+        _2q0 = 2.0f * q0;
+        _2q1 = 2.0f * q1;
+        _2q2 = 2.0f * q2;
+        _2q3 = 2.0f * q3;
+        _4q0 = 4.0f * q0;
+        _4q1 = 4.0f * q1;
+        _4q2 = 4.0f * q2;
+        _8q1 = 8.0f * q1;
+        _8q2 = 8.0f * q2;
+        q0q0 = q0 * q0;
+        q1q1 = q1 * q1;
+        q2q2 = q2 * q2;
+        q3q3 = q3 * q3;
+
+        // Gradient decent algorithm corrective step
+        s0 =     _4q0*q2q2    + _2q2*ax +    _4q0*q1q1 - _2q1*ay;
+        s1 =     _4q1*q3q3    - _2q3*ax + 4.0f*q0q0*q1 - _2q0*ay - _4q1 + _8q1*q1q1 + _8q1*q2q2 + _4q1*az;
+        s2 = 4.0f*q0q0*q2     + _2q0*ax +    _4q2*q3q3 - _2q3*ay - _4q2 + _8q2*q1q1 + _8q2*q2q2 + _4q2*az;
+        s3 = 4.0f*q1q1*q3     - _2q1*ax + 4.0f*q2q2*q3 - _2q2*ay;
+
+        recipNorm = invSqrt(s0*s0 + s1*s1 + s2*s2 + s3*s3);     // normalise step magnitude
+        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.5f * (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.0f * (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;
+}
+
+
 int main(void) {
 
     _delay(100);
@@ -110,33 +273,40 @@ int main(void) {
     i2c_setup();
     systick_setup();
 
-    mpu_t mpu;
-    mpu_setup(&mpu, I2C1, 0x68);
-    mpu_calibrate(&mpu, 5000);
     printf("==== start ====\r\n");
 
-    mpu_value_t val;
+    mpu_t mpu;
+    mpu_setup(&mpu, I2C1, 0x68);
+    _delay(10);
+    mpu_calibrate(&mpu, 20000);
+    printf("==== mpu ====\r\n");
 
-    uint32_t old_time = 0;
+    mpu_value_t mval;
-    uint32_t new_time = 0;
 
-    float freq = 100.0f;
+    uint32_t prev_ts = 0, last_ts = 0;
-    uint32_t delta = 0.0f;
 
+    quaternion_t q;
+    quaternion_init(&q);
+
+    int i = 0;
     while (true) {
-            mpu_read(&mpu, &val);
+        mpu_read(&mpu, &mval);
+
         gpio_toggle(GPIOB, GPIO6);
+        //if ((i % 350) == 0) {
+        //    printf("gx=%8.4f gy=%8.4f gz=%8.4f ax=%8.4f ay=%8.4f az=%8.4f \r\n", mval.gx, mval.gy, mval.gz, mval.ax, mval.ay, mval.az);
+        //}
+        i++;
+        last_ts = g_sys_tick_counter;
+        float dt = (float)(last_ts - prev_ts) / (float)systic_freq;
+        prev_ts = last_ts;
 
-            ahrs_update(val.gx, val.gy, val.gz, val.ax, val.ay, val.az);
+        ahrs_update(dt, &q, &mval);
 
-            printf("%12.3f\r\n", freq);
+        angle_t a = quaternion2xyz(&q);
+        angle_degress(&a);
+
+        printf("dt=%.6f y=%10.4f x=%10.4f z=%10.4f  \r\n", dt, a.y, a.x, a.z);
 
-        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;
     };
 }

mculoop/mpu6050.c

@@ -5,13 +5,17 @@
 
 #include <libopencm3/stm32/i2c.h>
 #include <stdint.h>
+#include <math.h>
 
 #include <mpu6050.h>
 
+#define MPU_REG_SMPLRT_DIV       0x19
+
 #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
@@ -57,6 +61,38 @@
 #define MPU_ACCEL_LSB_8      4096.0f
 #define MPU_ACCEL_LSB_16     2048.0f
 
+/* PWR_MGMT_1 0x6B */
+#define MPU_PWR1_DEVICE_RESET_BIT   7
+#define MPU_PWR1_SLEEP_BIT          6
+#define MPU_PWR1_CYCLE_BIT          5
+#define MPU_PWR1_TEMP_DIS_BIT       3
+
+#define MPU_PWR1_CLKSEL_BASE        0
+#define MPU_PWR1_CLKSEL_LEN         3
+
+#define MPU_PWR1_CLKSEL_INTERNAL          0
+#define MPU_PWR1_CLKSEL_PLL_XGYRO         1
+#define MPU_PWR1_CLKSEL_PLL_YGYRO         2
+#define MPU_PWR1_CLKSEL_PLL_ZGYRO         3
+#define MPU_PWR1_CLKSEL_PLL_EXT32K        4
+#define MPU_PWR1_CLKSEL_PLL_EXT19M        5
+#define MPU_PWR1_CLKSEL_KEEP_RESET        7
+
+/* PWR_MGMT_2 0x6C */
+#define MPU_PWR2_LP_WAKE_CTRL_BASE      6
+#define MPU_PWR2_LP_WAKE_CTRL_LEN       2
+
+#define MPU_PWR2_WAKE_FREQ_1P25         0
+#define MPU_PWR2_WAKE_FREQ_2P5          1
+#define MPU_PWR2_WAKE_FREQ_5            2
+#define MPU_PWR2_WAKE_FREQ_10           3
+
+#define MPU_PWR2_STBY_XA_BIT            5
+#define MPU_PWR2_STBY_YA_BIT            4
+#define MPU_PWR2_STBY_ZA_BIT            3
+#define MPU_PWR2_STBY_XG_BIT            2
+#define MPU_PWR2_STBY_YG_BIT            1
+#define MPU_PWR2_STBY_ZG_BIT            0
 
 //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);
@@ -90,6 +126,12 @@ static void i2cdev_read_seq8(uint32_t i2c, uint8_t addr, uint8_t reg, uint8_t* b
 }
 
 
+#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) {
 
     mpu->bus    = i2c;
@@ -102,35 +144,42 @@ void mpu_setup(mpu_t* mpu, uint32_t i2c, uint8_t addr) {
     mpu->err.gy = 0;
     mpu->err.gz = 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");
+
     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);
+    i2cdev_write_reg8(i2c, addr, MPU_REG_GYRO_CONFIG, MPU_GYRO_FS << MPU_GYRO_FS_BASE);
+    i2cdev_write_reg8(i2c, addr, MPU_REG_ACCEL_CONFIG, MPU_ACCEL_FS << MPU_ACCEL_FS_BASE);
+    i2cdev_write_reg8(i2c, addr, MPU_REG_SMPLRT_DIV, 4);
 }
 
+
 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 ax = (((int16_t)buffer[0]) << 8) | buffer[1];
-    int16_t ay = (((int16_t) buffer[2]) << 8) | buffer[3];
+    int16_t ay = (((int16_t)buffer[2]) << 8) | buffer[3];
-    int16_t az = (((int16_t) buffer[4]) << 8) | buffer[5];
+    int16_t az = (((int16_t)buffer[4]) << 8) | buffer[5];
 
-    int16_t gx = (((int16_t) buffer[8]) << 8) | buffer[9];
+    int16_t gx = (((int16_t)buffer[8]) << 8) | buffer[9];
-    int16_t gy = (((int16_t) buffer[10]) << 8) | buffer[11];
+    int16_t gy = (((int16_t)buffer[10]) << 8) | buffer[11];
-    int16_t gz = (((int16_t) buffer[12]) << 8) | buffer[13];
+    int16_t gz = (((int16_t)buffer[12]) << 8) | buffer[13];
 
-    val->ax = (float)ax / MPU_ACCEL_LSB_2;
+    val->ax = (float)ax / (float)MPU_ACCEL_LSB;
-    val->ay = (float)ay / MPU_ACCEL_LSB_2;
+    val->ay = (float)ay / (float)MPU_ACCEL_LSB;
-    val->az = (float)az / MPU_ACCEL_LSB_2;
+    val->az = (float)az / (float)MPU_ACCEL_LSB;
 
-    val->gx = (float)gx / MPU_GYRO_LSB_250;
+    val->gx = (float)gx / (float)MPU_GYRO_LSB;
-    val->gy = (float)gy / MPU_GYRO_LSB_250;
+    val->gy = (float)gy / (float)MPU_GYRO_LSB;
-    val->gz = (float)gz / MPU_GYRO_LSB_250;
+    val->gz = (float)gz / (float)MPU_GYRO_LSB;
 
-    //val->gx *= 0.0174533f;
+    val->gx *= M_PI / 180.0f;
-    //val->gy *= 0.0174533f;
+    val->gy *= M_PI / 180.0f;
-    //val->gz *= 0.0174533f;
+    val->gz *= M_PI / 180.0f;
 
 }
 
@@ -146,32 +195,16 @@ void mpu_calibrate(mpu_t* mpu, int count) {
     for (int i = 0; i < count; i++) {
         mpu_rawread(mpu, &val);
 
-        mpu->err.ax += val.ax;
+        mpu->err.gx += (float)val.gx / count;
-        mpu->err.ay += val.ay;
+        mpu->err.gy += (float)val.gy / count;
-        mpu->err.az += val.az;
+        mpu->err.gz += (float)val.gz / count;
-
-        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;