dan
/
EM7180_STM32
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

remove software filter functions (Mahony/Madgwick); these are done by the hardware

master
Daniel Peter Chokola 4 years ago
parent
eb4e55c6de
commit
16d2932478
1 changed files with 0 additions and 266 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 266
      Drivers/EM7180/Src/em7180.c

266
Drivers/EM7180/Src/em7180.c
Unescape View File

@ -24,15 +24,6 @@ static bool _passThru; @@ -24,15 +24,6 @@ static bool _passThru;
static float _aRes;
static float _gRes;
static float _mRes;
static uint8_t _Mmode;
static float _fuseROMx;
static float _fuseROMy;
static float _fuseROMz;
static float _q[4];
static float _beta;
static float _deltat;
static float _Kp;
static float _Ki;
/* Function Prototypes */
static void m24512dfm_write_byte(uint8_t device_address, uint8_t data_address1,
@ -753,260 +744,3 @@ static void em7180_read(uint8_t address, uint8_t subAddress, uint8_t count, @@ -753,260 +744,3 @@ static void em7180_read(uint8_t address, uint8_t subAddress, uint8_t count,
{
/* Wire.transfer(address, &subAddress, 1, dest, count); */
}
// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
// (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute
// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
__attribute__((optimize("O3"))) void em7180_update_quat_madgwick(float ax,
float ay,
float az,
float gx,
float gy,
float gz,
float mx,
float my,
float mz)
{
float q1 = _q[0], q2 = _q[1], q3 = _q[2], q4 = _q[3]; // short name local variable for readability
float norm;
float hx, hy, _2bx, _2bz;
float s1, s2, s3, s4;
float qDot1, qDot2, qDot3, qDot4;
// Auxiliary variables to avoid repeated arithmetic
float _2q1mx;
float _2q1my;
float _2q1mz;
float _2q2mx;
float _4bx;
float _4bz;
float _2q1 = 2.0f * q1;
float _2q2 = 2.0f * q2;
float _2q3 = 2.0f * q3;
float _2q4 = 2.0f * q4;
float _2q1q3 = 2.0f * q1 * q3;
float _2q3q4 = 2.0f * q3 * q4;
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
// Normalize accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az);
if(norm == 0.0f)
{
return; // handle NaN
}
norm = 1.0f / norm;
ax *= norm;
ay *= norm;
az *= norm;
// Normalize magnetometer measurement
norm = sqrt(mx * mx + my * my + mz * mz);
if(norm == 0.0f)
{
return; // handle NaN
}
norm = 1.0f / norm;
mx *= norm;
my *= norm;
mz *= norm;
// Reference direction of Earth's magnetic field
_2q1mx = 2.0f * q1 * mx;
_2q1my = 2.0f * q1 * my;
_2q1mz = 2.0f * q1 * mz;
_2q2mx = 2.0f * q2 * mx;
hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3
+ _2q2 * mz * q4
- mx * q3q3 - mx * q4q4;
hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2
+ my * q3q3 + _2q3 * mz * q4
- my * q4q4;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2
+ _2q3 * my * q4
- mz * q3q3
+ mz * q4q4;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;
// Gradient decent algorithm corrective step
s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax)
+ _2q2 * (2.0f * q1q2 + _2q3q4 - ay)
- _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx)
+ (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4)
+ _2bz * (q1q2 + q3q4)
- my)
+ _2bx * q3
* (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay)
- 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az)
+ _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx)
+ (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4)
+ _2bz * (q1q2 + q3q4)
- my)
+ (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4)
+ _2bz * (0.5f - q2q2 - q3q3)
- mz);
s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax)
+ _2q4 * (2.0f * q1q2 + _2q3q4 - ay)
- 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az)
+ (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4)
+ _2bz * (q2q4 - q1q3)
- mx)
+ (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4)
+ _2bz * (q1q2 + q3q4)
- my)
+ (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4)
+ _2bz * (0.5f - q2q2 - q3q3)
- mz);
s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax)
+ _2q3 * (2.0f * q1q2 + _2q3q4 - ay)
+ (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4)
+ _2bz * (q2q4 - q1q3)
- mx)
+ (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4)
+ _2bz * (q1q2 + q3q4)
- my)
+ _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalize step magnitude
norm = 1.0f / norm;
s1 *= norm;
s2 *= norm;
s3 *= norm;
s4 *= norm;
// Compute rate of change of quaternion
qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - _beta * s1;
qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - _beta * s2;
qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - _beta * s3;
qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - _beta * s4;
// Integrate to yield quaternion
q1 += qDot1 * _deltat;
q2 += qDot2 * _deltat;
q3 += qDot3 * _deltat;
q4 += qDot4 * _deltat;
norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalize quaternion
norm = 1.0f / norm;
_q[0] = q1 * norm;
_q[1] = q2 * norm;
_q[2] = q3 * norm;
_q[3] = q4 * norm;
}
// Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and
// measured ones.
void em7180_update_quat_mahony(float ax, float ay, float az, float gx, float gy,
float gz, float mx, float my, float mz)
{
float q1 = _q[0], q2 = _q[1], q3 = _q[2], q4 = _q[3]; // short name local variable for readability
float eInt[3] = { 0.0f, 0.0f, 0.0f }; // vector to hold integral error for Mahony method
float norm;
float hx, hy, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
float pa, pb, pc;
// Auxiliary variables to avoid repeated arithmetic
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;
// Normalize accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az);
if(norm == 0.0f)
{
return; // handle NaN
}
norm = 1.0f / norm; // use reciprocal for division
ax *= norm;
ay *= norm;
az *= norm;
// Normalize magnetometer measurement
norm = sqrt(mx * mx + my * my + mz * mz);
if(norm == 0.0f)
{
return; // handle NaN
}
norm = 1.0f / norm; // use reciprocal for division
mx *= norm;
my *= norm;
mz *= norm;
// Reference direction of Earth's magnetic field
hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4)
+ 2.0f * mz * (q2q4 + q1q3);
hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4)
+ 2.0f * mz * (q3q4 - q1q2);
bx = sqrt((hx * hx) + (hy * hy));
bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2)
+ 2.0f * mz * (0.5f - q2q2 - q3q3);
// Estimated direction of gravity and magnetic field
vx = 2.0f * (q2q4 - q1q3);
vy = 2.0f * (q1q2 + q3q4);
vz = q1q1 - q2q2 - q3q3 + q4q4;
wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);
// Error is cross product between estimated direction and measured direction of gravity
ex = (ay * vz - az * vy) + (my * wz - mz * wy);
ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
if(_Ki > 0.0f)
{
eInt[0] += ex; // accumulate integral error
eInt[1] += ey;
eInt[2] += ez;
}
else
{
eInt[0] = 0.0f; // prevent integral wind up
eInt[1] = 0.0f;
eInt[2] = 0.0f;
}
// Apply feedback terms
gx = gx + _Kp * ex + _Ki * eInt[0];
gy = gy + _Kp * ey + _Ki * eInt[1];
gz = gz + _Kp * ez + _Ki * eInt[2];
// Integrate rate of change of quaternion
pa = q2;
pb = q3;
pc = q4;
q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * _deltat);
q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * _deltat);
q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * _deltat);
q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * _deltat);
// Normalize quaternion
norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
norm = 1.0f / norm;
_q[0] = q1 * norm;
_q[1] = q2 * norm;
_q[2] = q3 * norm;
_q[3] = q4 * norm;
}

Write Preview
Loading…
Cancel
Save