Skip to content
Permalink
Browse files

Made a few updates to the AHRS to combine state based attitude and re…

…move some of the Magnetometer stuff that was messing up
  • Loading branch information...
spencerdynojet committed Apr 3, 2019
1 parent 4d13169 commit ea328b58af37bb8aa9e7d973fb549b925fd1c805
Showing with 205 additions and 139 deletions.
  1. +1 −1 ExEFIS/ExEFIS.cpp
  2. +10 −2 ExEFIS/Makefile
  3. +60 −49 ExEFIS/SKFilter.cpp
  4. +17 −9 ExEFIS/SKFilter.h
  5. +56 −2 ExEFIS/adhrs.cpp
  6. +21 −2 ExEFIS/adhrs.h
  7. +26 −45 ExEFIS/mpudriver.cpp
  8. +8 −26 ExEFIS/mpudriver.h
  9. +6 −3 ExEFIS/panelWidget.cpp
@@ -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.3 - Added settings file and Attitude Adjust");
qDebug("Version 5.3 - Added settings file and Attitude Adjust ");

system("cat /proc/cpuinfo | grep Serial | cut -d ' ' -f 2");
system("gpio mode 26 out");
@@ -399,6 +399,7 @@ Debug/moc_panelWidget.cpp: horizon_instrument.h \
PiTransfer.h \
ByteTransfer.h \
MPU9250.h \
SKFilter.h \
hsc_pressure.h \
knobs.h \
RotaryEncoder.h \
@@ -422,6 +423,7 @@ Debug/moc_SplashWidget.cpp: adhrs.h \
PiTransfer.h \
ByteTransfer.h \
MPU9250.h \
SKFilter.h \
hsc_pressure.h \
knobs.h \
RotaryEncoder.h \
@@ -441,6 +443,7 @@ Debug/moc_DiagWidget.cpp: adhrs.h \
PiTransfer.h \
ByteTransfer.h \
MPU9250.h \
SKFilter.h \
hsc_pressure.h \
knobs.h \
RotaryEncoder.h \
@@ -470,6 +473,7 @@ Debug/adhrs.o: adhrs.cpp adhrs.h \
PiTransfer.h \
ByteTransfer.h \
MPU9250.h \
SKFilter.h \
hsc_pressure.h \
airspeed.h \
altitude.h
@@ -496,6 +500,7 @@ Debug/ExEFIS.o: ExEFIS.cpp SplashWidget.h \
PiTransfer.h \
ByteTransfer.h \
MPU9250.h \
SKFilter.h \
hsc_pressure.h \
knobs.h \
RotaryEncoder.h \
@@ -521,8 +526,8 @@ Debug/mpudriver.o: mpudriver.cpp mpudriver.h \
PiTransfer.h \
ByteTransfer.h \
MPU9250.h \
quaternionFilters.h \
SKFilter.h
SKFilter.h \
quaternionFilters.h
$(CXX) -c $(CXXFLAGS) $(INCPATH) -o Debug/mpudriver.o mpudriver.cpp

Debug/hsc_pressure.o: hsc_pressure.cpp hsc_pressure.h
@@ -541,6 +546,7 @@ Debug/DiagWidget.o: DiagWidget.cpp DiagWidget.h \
PiTransfer.h \
ByteTransfer.h \
MPU9250.h \
SKFilter.h \
hsc_pressure.h \
knobs.h \
RotaryEncoder.h
@@ -570,6 +576,7 @@ Debug/panelWidget.o: panelWidget.cpp panelWidget.h \
PiTransfer.h \
ByteTransfer.h \
MPU9250.h \
SKFilter.h \
hsc_pressure.h \
knobs.h \
RotaryEncoder.h \
@@ -605,6 +612,7 @@ Debug/SplashWidget.o: SplashWidget.cpp SplashWidget.h \
PiTransfer.h \
ByteTransfer.h \
MPU9250.h \
SKFilter.h \
hsc_pressure.h \
knobs.h \
RotaryEncoder.h \
@@ -67,7 +67,14 @@ SKFilter::SKFilter()
gyroHeading = 0;
_initCounter = 0;
_initialized = false;
wingslevel = false;

acState.wingslevel = false;
acState.pitchlevel = false;
acState.pitchstill = false;
acState.turning = false;
acState.ballcentered = false;
acState.oneG = false;

magbufferindex = 0;
}

@@ -265,32 +272,47 @@ bool SKFilter::update(float gx, float gy, float gz, float ax, float ay, float az
float zmag = mz_ / mag_norm;

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

#if 0 //Note that these lines are placeholders - we can do better with the magnetometer I think...
//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
// _magEuler[0] = constrainAngle180(rollmag - M_PI); //need to offset this by pi
// Roll = _magEuler[0];

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
// _magEuler[1] = constrainAngle180(pitchmag - M_PI); //need to offset this by pi
// Pitch = _magEuler[1];

float yawmag = -atan2((-ymag*cos(Roll) + zmag*sin(Roll)), (xmag*cos(Pitch) + ymag*sin(Pitch)*sin(Roll) + zmag*sin(Pitch)*cos(Roll)));
_magEuler[2] = constrainAngle360(yawmag);
#endif
float rollmag = atan2((-ymag), (zmag));
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);


//float rollmag = atan2((-ymag), (zmag));
_magEuler[0] = constrainAngle180(rollmag - M_PI); //need to offset this by pi
// Roll = _magEuler[0];

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

float yawmag = -atan2((-zmag), (xmag));
_magEuler[2] = constrainAngle360(yawmag);
// float yawmag = -atan2((-zmag), (xmag));

// Need to correct these for bank angle

float steer = constrainAngle360(yawmag);
//_magEuler[2] = constrainAngle360(yawmag);
int i = 1;
bool found = false;
for (i = 1; i < 13 && !found; i++)
{
if (steer < steerTable[i]) found = true;
}

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]);

dmagbuffer[0][magbufferindex] = (_magEuler[0] - magPrev[0]) / _dt ;
dmagbuffer[1][magbufferindex] = (_magEuler[1] - magPrev[1]) / _dt ;
@@ -379,6 +401,7 @@ bool SKFilter::update(float gx, float gy, float gz, float ax, float ay, float az
//then we can assume pitch angle from accelerometer...
float pitchmag = -1.0f;
float rollmag = -1.0f;
float totalmag = -1.0f;
if(accelUpdated_)
{
_accelEuler[0] = atan(-ay_ / az_);
@@ -387,12 +410,13 @@ bool SKFilter::update(float gx, float gy, float gz, float ax, float ay, float az

pitchmag = sqrt((ax_*ax_) + (az_*az_));
rollmag = sqrt((ay_*ay_) + (az_*az_));

totalmag = sqrt((ax_*ax_) + (az_*az_) + (ay_*ay_));

axisstill.accelroll = (abs(_accelEuler[0] - accelPrev[0]) < 0.01f);
axisstill.accelpitch = (abs(_accelEuler[1] - accelPrev[1]) < 0.01f);
axisstill.accelyaw = (abs(_accelEuler[2] - accelPrev[2]) < 0.01f);

ballcentered = abs(ay_) < 0.07f ? true : false;



//_Euler[0] = constrainAngle360(_Euler[0]);
@@ -403,10 +427,17 @@ bool SKFilter::update(float gx, float gy, float gz, float ax, float ay, float az
//accelPrev[2] = _accelEuler[2];
}

wingslevel = (axisstill.magyaw && axisstill.gyroyaw && ballcentered);
pitchlevel = (abs(1.0f - pitchmag) < 0.02) && _accelEuler[1] < 0.02 && (abs(ax_) < 0.02) && axisstill.magpitch && axisstill.accelpitch;

if (pitchlevel)

acState.ballcentered = abs(ay_) < 0.07f ? true : false;
acState.pitchstill = axisstill.accelpitch && axisstill.magpitch && axisstill.gyropitch ? true : false;
acState.turning = !axisstill.gyroyaw || !axisstill.magyaw;
acState.wingslevel = (!acState.turning && acState.ballcentered);
acState.oneG = (abs(1.0f - totalmag) <= 0.012f);
acState.pitchlevel = acState.oneG && acState.pitchstill && abs(_accelEuler[1]) < 0.01 && !acState.turning;


if (acState.pitchlevel)
{
if (_Euler[1] > 0)_Euler_Fixed[1] -= 0.05f * _dt; //move at 0.1 rad / s
else _Euler_Fixed[1] += 0.05f * _dt;
@@ -416,30 +447,30 @@ bool SKFilter::update(float gx, float gy, float gz, float ax, float ay, float az
float fm[3] = { 0.0f, 0.0f, 0.0f };
float fa[3] = { 0.0f, 0.0f, 0.0f };

if (pitchlevel)
if (acState.pitchlevel)
{
fg[1] = 0.0f;
fa[1] = 1.0f;
}

if (axisstill.accelpitch && axisstill.magpitch && axisstill.gyropitch)
//Think about a slow level turn
if (acState.oneG && acState.pitchstill)
{
fg[1] = 0.0f;
fa[1] = 1.0f;
//fg[1] = 0.0f;
//fa[1] = 1.0f;
}

if ((abs(1.0f - rollmag) < 0.02f) && axisstill.gyroyaw && axisstill.magyaw)
if (!acState.turning && acState.oneG)
{
fg[0] = 0.0f;
fa[0] = 1.0f;
}

if (wingslevel)
if (acState.wingslevel)
{
fg[0] = 0.0f;
fa[0] = 1.0f;
fg[2] = 0.0f;
fm[2] = 1.0f;
//fg[2] = 0.2f;
//fm[2] = 0.8f;
}


@@ -448,31 +479,11 @@ bool SKFilter::update(float gx, float gy, float gz, float ax, float ay, float az
_Euler[0] = (fg[0]*_gyroEuler[0]) + (fm[0]*_magEuler[0]) + (fa[0]*_accelEuler[0]); //+ _Euler_Fixed[0];
_Euler[1] = (fg[1]*_gyroEuler[1]) + (fm[1]*_magEuler[1]) + (fa[1]*_accelEuler[1]); //+ _Euler_Fixed[1];
_Euler[2] = (fg[2]*_gyroEuler[2]) + (fm[2]*_magEuler[2]) + (fa[2]*_accelEuler[2]);// + _Euler_Fixed[2];

}
}
}

/* Returns the roll angle, rad */
float SKFilter::getRoll_rad() {
return _Euler.x();
}

/* Returns the pitch angle, rad */
float SKFilter::getPitch_rad() {
return _Euler.y();
}

/* Returns the yaw angle, rad */
float SKFilter::getYaw_rad() {
return constrainAngle180(_Euler.z());
}

/* Returns the heading angle, rad */
float SKFilter::getHeading_rad() {
return _Euler.z();//constrainAngle360(_Euler.z());
}

/* Returns the full 3dof vector*/
imu::Vector<3> SKFilter::getEuler(void)
{
@@ -1,6 +1,7 @@
#pragma once
#include "Vector.h"
#include <sys/time.h>
#include <math.h>

typedef struct
{
@@ -15,6 +16,16 @@ typedef struct
bool accelroll;
} AXIS_STILL;

typedef struct
{
bool wingslevel;
bool pitchlevel;
bool pitchstill;
bool turning;
bool ballcentered;
bool oneG;
} AC_STATE;

class SKFilter
{

@@ -24,17 +35,14 @@ class SKFilter
void setInitializationDuration(int duration);
bool validate(float gx, float gy, float gz, float ax, float ay, float az, float hx, float hy, float hz);
bool update(float gx, float gy, float gz, float ax, float ay, float az, float hx, float hy, float hz);
float getRoll_rad();
float getPitch_rad();
float getYaw_rad();
float getHeading_rad();
int quad = 0;
imu::Vector<3> getEuler(void);
bool wingslevel;
bool ballcentered;
bool pitchlevel;

imu::Vector<3> getEuler(void);
AC_STATE acState;
imu::Vector<3> dmag;
AXIS_STILL axisstill;
//
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 };

private:
bool _initialized = false;

0 comments on commit ea328b5

Please sign in to comment.
You can’t perform that action at this time.