Skip to content

Commit

Permalink
added tilt compensated compass, rev 1 - version 5.7
Browse files Browse the repository at this point in the history
  • Loading branch information
@spencerdynojet
spencerdynojet committed Apr 19, 2019
1 parent a6d2539 commit 28721a2
Show file tree
Hide file tree
Showing 4 changed files with 118 additions and 60 deletions.
2 changes: 1 addition & 1 deletion ExEFIS/ExEFIS.cpp
View file
Expand Up @@ -26,7 +26,7 @@ int main(int argc, char *argv[])

/* Start some String outputs in case you are launching the app with loggint enabled */
qDebug("****************** LOG BEGINS HERE **************************");
qDebug("Version 5.6 - Added VSI to vertical instruments ");
qDebug("Version 5.7 - First attempt at correct tilt compensated compass ");

system("cat /proc/cpuinfo | grep Serial | cut -d ' ' -f 2");
system("gpio mode 26 out");
Expand Down
85 changes: 50 additions & 35 deletions ExEFIS/SKFilter.cpp
View file
Expand Up @@ -3,7 +3,7 @@
#include "math.h"
#include <QDebug>

const int magbuffersize = 500;
const int magbuffersize = 50;
const int accelbuffersize = 50;
const int eulerbuffersize = 100;
static float magbuffer[3][magbuffersize];
Expand Down Expand Up @@ -66,6 +66,10 @@ SKFilter::SKFilter()
accelPrev[1] = 0;
accelPrev[2] = 0;

magOffset[0] = 0;
magOffset[1] = 0;
magOffset[2] = 0;

gyroHeading = 0;
_initCounter = 0;
_initialized = false;
Expand Down Expand Up @@ -270,40 +274,47 @@ bool SKFilter::update(float gx, float gy, float gz, float ax, float ay, float az

float mag_norm = sqrt((mx_*mx_) + (my_*my_) + (mz_*mz_));
float xmag = mx_ / mag_norm;
float ymag = my_ / mag_norm;
float zmag = mz_ / mag_norm;
float ymag = -my_ / mag_norm;
float zmag = -mz_ / mag_norm;

//Adjust for bank angle and pitch
float Roll = _Euler[0];
float Pitch = _Euler[1];
float Yaw = _Euler[2];

//Note that these lines are placeholders - we can do better with the magnetometer I think...
float rollmag = atan2((-ymag*cos(Pitch) + xmag*sin(Pitch)), (zmag*cos(Yaw) + ymag*sin(Yaw)*sin(Pitch) + xmag*sin(Yaw)*cos(Pitch)));
// _magEuler[0] = constrainAngle180(rollmag - M_PI); //need to offset this by pi
// Roll = _magEuler[0];
float Roll = atan2(zmag, ymag);
float Pitch = atan2(-zmag, xmag);
float Yaw = atan2(ymag, xmag);

float pitchmag = -atan2((-xmag*cos(Yaw) + ymag*sin(Yaw)), (zmag*cos(Roll) + xmag*sin(Roll)*sin(Yaw) + ymag*sin(Roll)*cos(Yaw)));
// _magEuler[1] = constrainAngle180(pitchmag - M_PI); //need to offset this by pi
// Pitch = _magEuler[1];
if (acState.wingslevel >= 0.9f && acState.pitchlevel >= 0.9f)
{
magOffset[0] = Roll;
magOffset[1] = Pitch;
magOffset[2] = Yaw;
}

float yawmag = -atan2((-zmag*cos(Roll) + xmag*sin(Roll)), (xmag*cos(Pitch) - zmag*sin(Pitch)*sin(Roll) + ymag*sin(Pitch)*cos(Roll)));
// _magEuler[2] = constrainAngle360(yawmag);

_magEuler[0] = Roll - magOffset[0];
//_magEuler[0] = constrainAngle360(_magEuler[0]);
_magEuler[1] = Pitch - magOffset[1]; // - magOffset[1];
//_magEuler[1] = constrainAngle360(_magEuler[1]);

//float rollmag = atan2((-ymag), (zmag));
_magEuler[0] = constrainAngle180(rollmag - M_PI); //need to offset this by pi
// Roll = _magEuler[0];
X_h = xmag*cos(_Euler[1]) + ymag*sin(-_Euler[0])*sin(_Euler[1]) + zmag*cos(-_Euler[0])*sin(_Euler[1]);
Y_h = ymag*cos(-_Euler[0]) - zmag*sin(-_Euler[0]);
hdg = 0.0f;
hdg = atan2(Y_h, X_h);
// if(X_h < 0) hdg = M_PI - atang;
// if (X_h > 0 && Y_h < 0) hdg = -atan2(Y_h, X_h);
// if (X_h > 0 && Y_h > 0) hdg = (2*M_PI) - atan2(Y_h, X_h);
// if (X_h == 0 && Y_h < 0) hdg = M_PI / 4;
// if (X_h == 0 && Y_h > 0) hdg = 3* M_PI / 4;


// float pitchmag = -atan2((-xmag ), (ymag));
_magEuler[1] = constrainAngle180(pitchmag - M_PI); //need to offset this by pi
// Pitch = _magEuler[1];
_magEuler[2] = constrainAngle360((2*M_PI)-hdg) ;

// float yawmag = -atan2((-zmag), (xmag));
//float txmag = xmag*cos(_Euler[1]) + zmag*sin(_Euler[1]);
//float tymag = ymag*cos(_Euler[0]) + ymag*sin(_Euler[0]);
//_magEuler[2] = constrainAngle360(atan2(tymag, txmag));


// Need to correct these for bank angle

float steer = constrainAngle360(yawmag);
float steer = 0;//constrainAngle360(yawmag);
//_magEuler[2] = constrainAngle360(yawmag);
int i = 1;
bool found = false;
Expand All @@ -313,8 +324,8 @@ bool SKFilter::update(float gx, float gy, float gz, float ax, float ay, float az
}

float ratio = (steer - steerTable[i - 1]) / (steerTable[i] - steerTable[i - 1]);
_magEuler[2] = (abs(forSteerTable[i] - forSteerTable[i - 1])* ratio) + forSteerTable[i - 1];
_magEuler[2] = constrainAngle360(yawmag);//_magEuler[2]);
// _magEuler[2] = (abs(forSteerTable[i] - forSteerTable[i - 1])* ratio) + forSteerTable[i - 1];
// _magEuler[2] = constrainAngle360(yawmag);//_magEuler[2]);

dmagbuffer[0][magbufferindex] = (_magEuler[0] - magPrev[0]) / _dt ;
dmagbuffer[1][magbufferindex] = (_magEuler[1] - magPrev[1]) / _dt ;
Expand All @@ -335,12 +346,12 @@ bool SKFilter::update(float gx, float gy, float gz, float ax, float ay, float az
dmag[2] /= magbuffersize;

//Set the axis still variables for initialization state
axisstill.magroll = 100 - (abs(dmag[0])*AXISSTILLMAGSCALAR);
axisstill.magpitch = 100 - (abs(dmag[1])*AXISSTILLMAGSCALAR);
axisstill.magyaw = 100 - (abs(dmag[2])*AXISSTILLMAGSCALAR);
axisstill.magroll = 100;// - (abs(dmag[0])*AXISSTILLMAGSCALAR);
axisstill.magpitch = 100;// - (abs(dmag[1])*AXISSTILLMAGSCALAR);
axisstill.magyaw = 100;// - (abs(dmag[2])*AXISSTILLMAGSCALAR);
if (axisstill.magroll < 0)axisstill.magroll = 0;
if (axisstill.magpitch < 0)axisstill.magpitch = 0;
if (axisstill.magyaw < 0)axisstill.magyaw = 0;
if (axisstill.magyaw < 0)axisstill.magyaw = 0;

magPrev[0] = _magEuler[0];
magPrev[1] = _magEuler[1];
Expand Down Expand Up @@ -449,15 +460,19 @@ bool SKFilter::update(float gx, float gy, float gz, float ax, float ay, float az
else _Euler_Fixed[1] += 0.05f * _dt;
}

float fg[3] = { 1.0f, 1.0f, 1.0f };
float fm[3] = { 0.0f, 0.0f, 0.0f };
float fg[3] = { 1.0f, 1.0f, 0.0f };
float fm[3] = { 0.0f, 0.0f, 1.0f };
float fa[3] = { 0.0f, 0.0f, 0.0f };

fg[1] = 1.0f - acState.pitchlevel;
fa[1] = acState.pitchlevel;
fa[1] = acState.pitchlevel / 2.0f;
fm[1] = acState.pitchlevel / 2.0f;

fg[0] = 1.0f - (((1.0f - acState.turning) * 0.33f) + (acState.oneG * 0.33f) + (acState.wingslevel*0.33f));
fa[0] = (((1.0f - acState.turning) * 0.33f) + (acState.oneG * 0.33f) + (acState.wingslevel * 0.33f));

fg[2] = 1.0 - ((acState.pitchlevel) * 0.5 + (acState.wingslevel * 0.5));
fm[2] = ((acState.pitchlevel) * 0.5 + (acState.wingslevel * 0.5));

//Don't need the "spring" with the fixed Euler with the current setup

Expand Down
5 changes: 5 additions & 0 deletions ExEFIS/SKFilter.h
View file
Expand Up @@ -46,6 +46,9 @@ class SKFilter
AC_STATE acState;
imu::Vector<3> dmag;
AXIS_STILL axisstill;
float Y_h;
float X_h;
float hdg;
//
float forSteerTable[13] = {0.0f, M_PI / 6, M_PI / 3, M_PI / 2, 2*M_PI / 3, 5*M_PI / 6, M_PI, 7*M_PI / 6, 4*M_PI / 3, 3*M_PI / 2, 5*M_PI / 3, 11*M_PI / 6, 2*M_PI };
float steerTable[13] = {0.0f, M_PI / 6, M_PI / 3, M_PI / 2, 2*M_PI / 3, 5*M_PI / 6, M_PI, 7*M_PI / 6, 4*M_PI / 3, 3*M_PI / 2, 5*M_PI / 3, 11*M_PI / 6, 2*M_PI };
Expand Down Expand Up @@ -89,5 +92,7 @@ class SKFilter
imu::Vector<3> gyroPrev;
imu::Vector<3> magPrev;
imu::Vector<3> accelPrev;

imu::Vector<3> magOffset;
};

86 changes: 62 additions & 24 deletions ExEFIS/mpudriver.cpp
View file
Expand Up @@ -308,7 +308,7 @@ float mpudriver::getHeading(void)
ret += yawbuffer[i];
}
ret /= buffersize;
return ret;
return euler.z();
}

imu::Vector<3> mpudriver::GetAccelerometer(int*status)
Expand Down Expand Up @@ -429,7 +429,7 @@ void mpudriver::RunFilter(void)

if (filter.validate(mappedimudata[0][0], mappedimudata[1][0], mappedimudata[2][0],
mappedimudata[0][1], mappedimudata[1][1], mappedimudata[2][1],
mappedimudata[0][2], mappedimudata[1][2], mappedimudata[2][2]))
mappedimudata[0][2], mappedimudata[2][2], mappedimudata[1][2]))
{

filter.update(mappedimudata[0][0],
Expand All @@ -439,46 +439,54 @@ void mpudriver::RunFilter(void)
mappedimudata[1][1],
mappedimudata[2][1],
mappedimudata[0][2],
mappedimudata[1][2],
mappedimudata[2][2]);
mappedimudata[2][2],
mappedimudata[1][2]);
//filter.getQuaternion(&quatW[quatIndex], &quatX[quatIndex], &quatY[quatIndex], &quatZ[quatIndex]);
}

rollbuffer[quatIndex] = filter.getEuler().x();
pitchbuffer[quatIndex] = filter.getEuler().y();
yawbuffer[quatIndex] = filter.getEuler().z();
euler = filter.getEuler();
rollbuffer[quatIndex] = euler.x();
pitchbuffer[quatIndex] = euler.y();
yawbuffer[quatIndex] = euler.z();
accelbuffer[0][quatIndex] = mappedimudata[0][1];
accelbuffer[1][quatIndex] = mappedimudata[1][1];
accelbuffer[2][quatIndex] = mappedimudata[2][1];

quatIndex++;
if (quatIndex > buffersize) quatIndex = 0;

// last = current;
// MadgwickQuaternionUpdate(az,
// ay,
// ax,
// gz*M_PI / 180.0f,
// gy*M_PI / 180.0f,
// gx*M_PI / 180.0f,
// -mz,
// my,
// mx);


// MahonyQuaternionUpdate(ax, ay, az, gx*M_PI / 180.0f, gy*M_PI / 180.0f, gz*M_PI / 180.0f, mx, my, -mz);
// last = current;
// MadgwickQuaternionUpdate(az,
// ay,
// ax,
// gz*M_PI / 180.0f,
// gy*M_PI / 180.0f,
// gx*M_PI / 180.0f,
// -mz,
// my,
// mx);
// MahonyQuaternionUpdate(
// mappedimudata[0][1],
// mappedimudata[1][1],
// mappedimudata[2][1],
// mappedimudata[0][0],
// mappedimudata[1][0],
// mappedimudata[2][0],
// mappedimudata[0][2],
// mappedimudata[1][2],
// mappedimudata[2][2]);
// MahonyQuaternionUpdate(ax, ay, az, gx*M_PI / 180.0f, gy*M_PI / 180.0f, gz*M_PI / 180.0f, mx, my, -mz);

/* only print once per second*/
if (usec > 250000) {

usec = 0;

if (!showmagvectors)
if (showmagvectors)
{

float qw, qx, qy, qz;

euler = filter.getEuler();

printf("Orientation:, %f, %f, %f, Accelerometer:, %d, %d, %d, Gyro: , %+2.2f, %+2.2f, %+2.2f, Mag:, %+3.3f, %+3.3f, %+3.3f, Quad: %d \n",
euler.x() * 180.0f / M_PI,
euler.y() * 180.0f / M_PI,
Expand All @@ -495,11 +503,23 @@ void mpudriver::RunFilter(void)
mz,
//this is yaw1
0);
#if false
imu::Vector<3> mad = q.toEuler();
printf("Orientation SK:, %f, %f, %f, Orientation Madgwick:, %f, %f, %f, \n",
euler.x() * 180.0f / M_PI,
euler.y() * 180.0f / M_PI,
euler.z() * 180.0f / M_PI,
(mad.x() * 180.0f / M_PI) - 90.0f,
mad.y() * 180.0f / M_PI,
mad.z() * 180.0f / M_PI
);
#endif
// printf("Wings Level, %d \n", filter.wingslevel);
// printf("DMAG[2] = , %+2.4f \n", filter.dmag[2]);
}
else
{
#if false
float mag_norm = sqrt((mx*mx) + (my*my) + (mz*mz));
float xmag = mx / mag_norm;
float ymag = my / mag_norm;
Expand All @@ -508,6 +528,24 @@ void mpudriver::RunFilter(void)
mag_norm,
(-atan2((-zmag), (xmag))* (180.0f / M_PI))
);
#endif
float mag_norm = sqrt((mappedimudata[0][2] * mappedimudata[0][2]) +
(mappedimudata[1][2] * mappedimudata[1][2]) +
(mappedimudata[2][2] * mappedimudata[2][2]));
float xmag = mappedimudata[0][2] / mag_norm;
float ymag = -mappedimudata[2][2] / mag_norm;
float zmag =- mappedimudata[1][2] / mag_norm;
printf("MagVectors:, totalmag, %+2.3f, Magnitudes: , %+2.2f, %+2.2f, %+2.2f, X_h: , %+2.2f, Y_h, %+2.2f, Hdg %+2.2f, Constrained: %+2.2f \n",
mag_norm,
xmag,
ymag,
zmag,
filter.X_h,
filter.Y_h,
filter.hdg,
filter.getEuler().z()
);

}

}
Expand Down

0 comments on commit 28721a2

Please sign in to comment.