Trinity Devboard PCB V1.0 Firmware. FreeRTOS is setup and the MCU reads IMU data over SPI fand Magnetometer data over I2C, each with a seperate task. Sensordata is then run though MadgwickAHRS and send over USB as serial packet data to use in trinity visualizer. Bare minimum functionality works and is replicated from the first prototype.
This commit is contained in:
@@ -0,0 +1,252 @@
|
||||
//=====================================================================================================
|
||||
// MadgwickAHRS.c
|
||||
//=====================================================================================================
|
||||
//
|
||||
// Implementation of Madgwick's IMU and AHRS algorithms.
|
||||
// See: http://www.x-io.co.uk/node/8#open_source_ahrs_and_imu_algorithms
|
||||
//
|
||||
// Date Author Notes
|
||||
// 29/09/2011 SOH Madgwick Initial release
|
||||
// 02/10/2011 SOH Madgwick Optimised for reduced CPU load
|
||||
// 19/02/2012 SOH Madgwick Magnetometer measurement is normalised
|
||||
//
|
||||
//=====================================================================================================
|
||||
|
||||
//---------------------------------------------------------------------------------------------------
|
||||
// Header files
|
||||
|
||||
#include "MadgwickAHRS.h"
|
||||
#include <math.h>
|
||||
#include <stdio.h>
|
||||
|
||||
//---------------------------------------------------------------------------------------------------
|
||||
// Definitions
|
||||
|
||||
//#define sampleFreq 512.0f // sample frequency in Hz
|
||||
//#define betaDef 0.1f // 2 * proportional gain
|
||||
|
||||
//---------------------------------------------------------------------------------------------------
|
||||
// Variable definitions
|
||||
|
||||
//volatile float beta = betaDef; // 2 * proportional gain (Kp)
|
||||
//volatile float q0 = 1.0f, q1 = 0.0f, q2 = 0.0f, q3 = 0.0f; // quaternion of sensor frame relative to auxiliary frame
|
||||
|
||||
//---------------------------------------------------------------------------------------------------
|
||||
// Function declarations
|
||||
|
||||
float invSqrt(float x);
|
||||
|
||||
//====================================================================================================
|
||||
// Functions
|
||||
|
||||
void MadgwickAHRS_init (MadgwickAHRS_Filter* filter, float beta, float sample_freq) {
|
||||
filter->q[0] = 1.0f; // qw
|
||||
filter->q[1] = 0.0f; // qx
|
||||
filter->q[2] = 0.0f; // qy
|
||||
filter->q[3] = 0.0f; // qz
|
||||
filter->beta = beta;
|
||||
filter->sample_freq = sample_freq;
|
||||
}
|
||||
|
||||
|
||||
//---------------------------------------------------------------------------------------------------
|
||||
// AHRS algorithm update
|
||||
|
||||
void MadgwickAHRS_update(MadgwickAHRS_Filter* filter, float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz) {
|
||||
float q[4] = {filter->q[0], filter->q[1], filter->q[2], filter->q[3]};
|
||||
float recipNorm;
|
||||
float s0, s1, s2, s3;
|
||||
float qDot[4];
|
||||
float hx, hy;
|
||||
float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, q2q2, q2q3, q3q3;
|
||||
|
||||
// Use IMU algorithm if magnetometer measurement invalid (avoids NaN in magnetometer normalisation)
|
||||
if((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) {
|
||||
MadgwickAHRS_update_IMU(filter, gx, gy, gz, ax, ay, az);
|
||||
return;
|
||||
}
|
||||
|
||||
// Rate of change of quaternion from gyroscope
|
||||
qDot[0] = 0.5f * (-q[1] * gx - q[2] * gy - q[3] * gz);
|
||||
qDot[1] = 0.5f * (q[0] * gx + q[2] * gz - q[3] * gy);
|
||||
qDot[2] = 0.5f * (q[0] * gy - q[1] * gz + q[3] * gx);
|
||||
qDot[3] = 0.5f * (q[0] * gz + q[1] * gy - q[2] * 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;
|
||||
|
||||
// Normalise magnetometer measurement
|
||||
recipNorm = invSqrt(mx * mx + my * my + mz * mz);
|
||||
mx *= recipNorm;
|
||||
my *= recipNorm;
|
||||
mz *= recipNorm;
|
||||
|
||||
// Auxiliary variables to avoid repeated arithmetic
|
||||
_2q0mx = 2.0f * q[0] * mx;
|
||||
_2q0my = 2.0f * q[0] * my;
|
||||
_2q0mz = 2.0f * q[0] * mz;
|
||||
_2q1mx = 2.0f * q[1] * mx;
|
||||
_2q0 = 2.0f * q[0];
|
||||
_2q1 = 2.0f * q[1];
|
||||
_2q2 = 2.0f * q[2];
|
||||
_2q3 = 2.0f * q[3];
|
||||
_2q0q2 = 2.0f * q[0] * q[2];
|
||||
_2q2q3 = 2.0f * q[2] * q[3];
|
||||
q0q0 = q[0] * q[0];
|
||||
q0q1 = q[0] * q[1];
|
||||
q0q2 = q[0] * q[2];
|
||||
q0q3 = q[0] * q[3];
|
||||
q1q1 = q[1] * q[1];
|
||||
q1q2 = q[1] * q[2];
|
||||
q1q3 = q[1] * q[3];
|
||||
q2q2 = q[2] * q[2];
|
||||
q2q3 = q[2] * q[3];
|
||||
q3q3 = q[3] * q[3];
|
||||
|
||||
// Reference direction of Earth's magnetic field
|
||||
hx = mx * q0q0 - _2q0my * q[3] + _2q0mz * q[2] + mx * q1q1 + _2q1 * my * q[2] + _2q1 * mz * q[3] - mx * q2q2 - mx * q3q3;
|
||||
hy = _2q0mx * q[3] + my * q0q0 - _2q0mz * q[1] + _2q1mx * q[2] - my * q1q1 + my * q2q2 + _2q2 * mz * q[3] - my * q3q3;
|
||||
_2bx = sqrt(hx * hx + hy * hy);
|
||||
_2bz = -_2q0mx * q[2] + _2q0my * q[1] + mz * q0q0 + _2q1mx * q[3] - mz * q1q1 + _2q2 * my * q[3] - mz * q2q2 + mz * q3q3;
|
||||
_4bx = 2.0f * _2bx;
|
||||
_4bz = 2.0f * _2bz;
|
||||
|
||||
// Gradient decent algorithm corrective step
|
||||
s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - _2bz * q[2] * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q[3] + _2bz * q[1]) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q[2] * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
|
||||
s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q[1] * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + _2bz * q[3] * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q[2] + _2bz * q[0]) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q[3] - _4bz * q[1]) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
|
||||
s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - 4.0f * q[2] * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + (-_4bx * q[2] - _2bz * q[0]) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (_2bx * q[1] + _2bz * q[3]) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + (_2bx * q[0] - _4bz * q[2]) * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
|
||||
s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + (-_4bx * q[3] + _2bz * q[1]) * (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + (-_2bx * q[0] + _2bz * q[2]) * (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + _2bx * q[1] * (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz);
|
||||
recipNorm = invSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); // normalise step magnitude
|
||||
s0 *= recipNorm;
|
||||
s1 *= recipNorm;
|
||||
s2 *= recipNorm;
|
||||
s3 *= recipNorm;
|
||||
|
||||
// Apply feedback step
|
||||
qDot[0] -= filter->beta * s0;
|
||||
qDot[1] -= filter->beta * s1;
|
||||
qDot[2] -= filter->beta * s2;
|
||||
qDot[3] -= filter->beta * s3;
|
||||
}
|
||||
|
||||
// Integrate rate of change of quaternion to yield quaternion
|
||||
q[0] += qDot[0] * (1.0f / filter->sample_freq);
|
||||
q[1] += qDot[1] * (1.0f / filter->sample_freq);
|
||||
q[2] += qDot[2] * (1.0f / filter->sample_freq);
|
||||
q[3] += qDot[3] * (1.0f / filter->sample_freq);
|
||||
|
||||
// Normalise quaternion
|
||||
recipNorm = invSqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
|
||||
q[0] *= recipNorm;
|
||||
q[1] *= recipNorm;
|
||||
q[2] *= recipNorm;
|
||||
q[3] *= recipNorm;
|
||||
|
||||
// Update filter state
|
||||
filter->q[0] = q[0];
|
||||
filter->q[1] = q[1];
|
||||
filter->q[2] = q[2];
|
||||
filter->q[3] = q[3];
|
||||
}
|
||||
|
||||
//---------------------------------------------------------------------------------------------------
|
||||
// IMU algorithm update
|
||||
|
||||
void MadgwickAHRS_update_IMU(MadgwickAHRS_Filter* filter, float gx, float gy, float gz, float ax, float ay, float az) {
|
||||
float q[4] = {filter->q[0], filter->q[1], filter->q[2], filter->q[3]};
|
||||
float recipNorm;
|
||||
float s0, s1, s2, s3;
|
||||
float qDot[4];
|
||||
float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2 ,_8q1, _8q2, q0q0, q1q1, q2q2, q3q3;
|
||||
|
||||
// Rate of change of quaternion from gyroscope
|
||||
qDot[0] = 0.5f * (-q[1] * gx - q[2] * gy - q[3] * gz);
|
||||
qDot[1] = 0.5f * (q[0] * gx + q[2] * gz - q[3] * gy);
|
||||
qDot[2] = 0.5f * (q[0] * gy - q[1] * gz + q[3] * gx);
|
||||
qDot[3] = 0.5f * (q[0] * gz + q[1] * gy - q[2] * 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 * q[0];
|
||||
_2q1 = 2.0f * q[1];
|
||||
_2q2 = 2.0f * q[2];
|
||||
_2q3 = 2.0f * q[3];
|
||||
_4q0 = 4.0f * q[0];
|
||||
_4q1 = 4.0f * q[1];
|
||||
_4q2 = 4.0f * q[2];
|
||||
_8q1 = 8.0f * q[1];
|
||||
_8q2 = 8.0f * q[2];
|
||||
q0q0 = q[0] * q[0];
|
||||
q1q1 = q[1] * q[1];
|
||||
q2q2 = q[2] * q[2];
|
||||
q3q3 = q[3] * q[3];
|
||||
|
||||
// Gradient decent algorithm corrective step
|
||||
s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay;
|
||||
s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * q[1] - _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az;
|
||||
s2 = 4.0f * q0q0 * q[2] + _2q0 * ax + _4q2 * q3q3 - _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az;
|
||||
s3 = 4.0f * q1q1 * q[3] - _2q1 * ax + 4.0f * q2q2 * q[3] - _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
|
||||
qDot[0] -= filter->beta * s0;
|
||||
qDot[1] -= filter->beta * s1;
|
||||
qDot[2] -= filter->beta * s2;
|
||||
qDot[3] -= filter->beta * s3;
|
||||
}
|
||||
|
||||
// Integrate rate of change of quaternion to yield quaternion
|
||||
q[0] += qDot[0] * (1.0f / filter->sample_freq);
|
||||
q[1] += qDot[1] * (1.0f / filter->sample_freq);
|
||||
q[2] += qDot[2] * (1.0f / filter->sample_freq);
|
||||
q[3] += qDot[3] * (1.0f / filter->sample_freq);
|
||||
|
||||
// Normalise quaternion
|
||||
recipNorm = invSqrt(q[0] * q[0] + q[1] * q[1] + q[2] * q[2] + q[3] * q[3]);
|
||||
q[0] *= recipNorm;
|
||||
q[1] *= recipNorm;
|
||||
q[2] *= recipNorm;
|
||||
q[3] *= recipNorm;
|
||||
|
||||
// Update filter state
|
||||
filter->q[0] = q[0];
|
||||
filter->q[1] = q[1];
|
||||
filter->q[2] = q[2];
|
||||
filter->q[3] = q[3];
|
||||
}
|
||||
|
||||
//---------------------------------------------------------------------------------------------------
|
||||
// Fast inverse square-root
|
||||
// See: http://en.wikipedia.org/wiki/Fast_inverse_square_root
|
||||
|
||||
float invSqrt(float x) {
|
||||
float halfx = 0.5f * x;
|
||||
float y = x;
|
||||
long i = *(long*)&y;
|
||||
i = 0x5f3759df - (i>>1);
|
||||
y = *(float*)&i;
|
||||
y = y * (1.5f - (halfx * y * y));
|
||||
return y;
|
||||
}
|
||||
|
||||
//====================================================================================================
|
||||
// END OF CODE
|
||||
//====================================================================================================
|
||||
Reference in New Issue
Block a user