Skip to content
Permalink
Browse files

Version 5.6 - cleaned up the variable aircraft state driven filter, a…

…dded VSI to the vertical instrument
  • Loading branch information...
spencerdynojet committed Apr 11, 2019
1 parent 5695154 commit a6d25393f6660770a3cf38f9be6f3a531cb3b7b1
Showing with 95 additions and 70 deletions.
  1. +1 −1 ExEFIS/ExEFIS.cpp
  2. +35 −52 ExEFIS/SKFilter.cpp
  3. +19 −15 ExEFIS/SKFilter.h
  4. +3 −0 ExEFIS/panelWidget.cpp
  5. +31 −2 ExEFIS/vertical_instrument.cpp
  6. +6 −0 ExEFIS/vertical_instrument.h
@@ -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.4 - Added settings file and Attitude Adjust - added attitude offset for Filter ");
qDebug("Version 5.6 - Added VSI to vertical instruments ");

system("cat /proc/cpuinfo | grep Serial | cut -d ' ' -f 2");
system("gpio mode 26 out");
@@ -70,12 +70,12 @@ SKFilter::SKFilter()
_initCounter = 0;
_initialized = false;

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

magbufferindex = 0;
}
@@ -335,9 +335,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 = (abs(dmag[0]) < 0.02f) ? true : false;
axisstill.magpitch = (abs(dmag[1]) < 0.02f) ? true : false;
axisstill.magyaw = (abs(dmag[2]) < 0.04f) ? true : false;
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;

magPrev[0] = _magEuler[0];
magPrev[1] = _magEuler[1];
@@ -357,9 +360,12 @@ bool SKFilter::update(float gx, float gy, float gz, float ax, float ay, float az
integral[2] = ((gyroPrev[2] * _dt) + ((gzz - gyroPrev[2]) * 0.5f * _dt));

//Set the axis still flags
axisstill.gyroroll = abs(integral[0]) < 0.05f ? true : false;
axisstill.gyropitch = abs(integral[1]) < 0.05f ? true : false;
axisstill.gyroyaw = abs(integral[2]) < 0.05f ? true : false;
axisstill.gyroroll = 100 - (abs(integral[0])*AXISSTILLGYROSCALAR);
axisstill.gyropitch = 100 - (abs(integral[1])*AXISSTILLGYROSCALAR);
axisstill.gyroyaw = 100 - (abs(integral[2])*AXISSTILLGYROSCALAR);
if (axisstill.gyroroll < 0) axisstill.gyroroll = 0;
if (axisstill.gyropitch < 0) axisstill.gyropitch = 0;
if (axisstill.gyroyaw < 0) axisstill.gyroyaw = 0;

//First Calc Roll Angle
_gyroEuler[0] = _Euler[0] + integral[0];
@@ -408,18 +414,18 @@ bool SKFilter::update(float gx, float gy, float gz, float ax, float ay, float az
{
_accelEuler[0] = atan(-ay_ / az_);
_accelEuler[1] = atan(-ax_ / az_);
//_accelEuler[2] = atan(ay / ax);
_accelEuler[2] = atan(-ay_ / ax_);

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



axisstill.accelroll = 100 - (abs(_accelEuler[0] - accelPrev[0]) * AXISSTILLACCELSCALAR);
axisstill.accelpitch = 100 - (abs(_accelEuler[1] - accelPrev[1]) * AXISSTILLACCELSCALAR);
axisstill.accelyaw = 100 - (abs(_accelEuler[2] - accelPrev[2]) * AXISSTILLACCELSCALAR);
if (axisstill.accelroll < 0)axisstill.accelroll = 0;
if (axisstill.accelpitch < 0)axisstill.accelpitch = 0;
if (axisstill.accelyaw < 0)axisstill.accelyaw = 0;

//_Euler[0] = constrainAngle360(_Euler[0]);
//_Euler[1] = constrainAngle360(_Euler[1]);
@@ -429,14 +435,12 @@ bool SKFilter::update(float gx, float gy, float gz, float ax, float ay, float az
//accelPrev[2] = _accelEuler[2];
}



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] - attitudeOffset) < 0.05 && !acState.turning;
acState.ballcentered = 1.0f - abs(ay_);// < 0.07f ? true : false;
acState.pitchstill = (axisstill.accelpitch * 0.0033f) + (axisstill.magpitch * 0.0033f) + (axisstill.gyropitch * 0.0033f);// ? true : false;
acState.turning = 1.0f - ((axisstill.gyroyaw * 0.005) + (axisstill.magyaw*0.005));
acState.wingslevel = (-acState.turning) + acState.ballcentered;
acState.oneG = 1.0f - (abs(1.0f - totalmag));
acState.pitchlevel = (acState.oneG * 0.25) + (acState.pitchstill* 0.25) + (.25f - abs(_accelEuler[1] - attitudeOffset)) + (.25 - (.25f*acState.turning));


if (acState.pitchlevel)
@@ -448,33 +452,12 @@ bool SKFilter::update(float gx, float gy, float gz, float ax, float ay, float az
float fg[3] = { 1.0f, 1.0f, 1.0f };
float fm[3] = { 0.0f, 0.0f, 0.0f };
float fa[3] = { 0.0f, 0.0f, 0.0f };

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

if (!acState.turning && acState.oneG)
{
fg[0] = 0.0f;
fa[0] = 1.0f;
}
fg[1] = 1.0f - acState.pitchlevel;
fa[1] = acState.pitchlevel;

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

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

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

@@ -3,27 +3,31 @@
#include <sys/time.h>
#include <math.h>

#define AXISSTILLACCELSCALAR 10000
#define AXISSTILLMAGSCALAR 500
#define AXISSTILLGYROSCALAR 600

typedef struct
{
bool magpitch;
bool magyaw;
bool magroll;
bool gyropitch;
bool gyroyaw;
bool gyroroll;
bool accelpitch;
bool accelyaw;
bool accelroll;
int magpitch;
int magyaw;
int magroll;
int gyropitch;
int gyroyaw;
int gyroroll;
int accelpitch;
int accelyaw;
int accelroll;
} AXIS_STILL;

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

class SKFilter
@@ -48,6 +48,8 @@ panelWidget::panelWidget(QWidget *parent, QFile* f, knobs *k)
h1 = new horizon_instrument(this, e, s, settings.horizonOffset);
//r1 = new round_instrument(this);
vi1 = new vertical_instrument(this, Qt::black);
vi1->showSecondary = false;
vi1->showSecondaryValue = false;

int altvals[10] = { 0, 1000, 2000, 3000, 4000, 5000, 6000, 7000, 8000, 9000 };
vi2 = new vertical_instrument(this, Qt::black);
@@ -119,6 +121,7 @@ void panelWidget::onTimer(void)
dg->value = data.heading;
vi1->setValue(data.airspeed);
vi2->setValue(data.altitude);
vi2->setSecondaryValue((int)data.vsi);
ss->setValue(data.slip);
r->setWingsLevel(adhr->getAcState().wingslevel);
/*
@@ -48,9 +48,9 @@ void vertical_instrument::setupInstrument(int* vals, int numVals)
void vertical_instrument::paintEvent(QPaintEvent * /* event */)
{
/* Calculate the size of the rectangle for the whole instrument*/
int tall = height() * 0.8f;
int tall = height() * 0.9f;
int wide = width() * 0.8f;
int y = height() * 0.1f;
int y = height() * 0.05f;
int x = width() * 0.1f;
/* This is the rectangle for the outline of the instrument*/
QRect rect(x, y, wide, tall);
@@ -61,6 +61,14 @@ void vertical_instrument::paintEvent(QPaintEvent * /* event */)
painter.setBrush(QColor::fromRgb(169, 169, 169, 100));
painter.drawRect(rect);

if (showSecondary)
{
float ratio = (float)secondaryValue / (float)secondaryScale;
QRect secondaryRect(x, (tall / 2) +y, wide, (-ratio * (tall / 2)));
painter.setBrush(QColor::fromRgb(98, 52, 94, 200));
painter.drawRect(secondaryRect);
}

/* vert is the height of each rectangle for values*/
int vert = tall / vertical_divs / 2;
bool found = false;
@@ -104,6 +112,10 @@ void vertical_instrument::paintEvent(QPaintEvent * /* event */)
QPoint bottomBRight(x + wide, y + tall);
QRect bottomRect(bottomTLeft, bottomBRight);

QPoint topTLeft(x, y);
QPoint topBRight(x + wide, y + vert);
QRect topRect(topTLeft, topBRight);

QFont font = painter.font();
font.setPointSize(tall/20);
font.setWeight(QFont::DemiBold);
@@ -134,6 +146,17 @@ void vertical_instrument::paintEvent(QPaintEvent * /* event */)
painter.drawText(bottomRect, Qt::AlignCenter, QString::number(setting / pow(10, settingPrec), 'f', settingPrec));
pen.setWidth(1);
}

if (showSecondaryValue)
{
int val = secondaryValue / 10.0f;
painter.setPen(Qt::black);
painter.setBrush(QColor::fromRgb(0, 0, 0, 150));
painter.drawRect(topRect);
painter.setPen(Qt::white);
painter.drawText(topRect, Qt::AlignCenter, QString::number(val*10, 'f', 0));
pen.setWidth(1);
}
}

void vertical_instrument::change(void)
@@ -187,3 +210,9 @@ int vertical_instrument::getValue(void)
{
return (value);
}


void vertical_instrument::setSecondaryValue(int val)
{
secondaryValue = val;
}
@@ -13,6 +13,8 @@ public :
vertical_instrument(QWidget *parent, QColor c);
void setValue(int val);
int getValue(void);
void setSecondaryValue(int val);
int getSecondaryValue(void);
void setupInstrument(int* vals, int numVals);
void setSetting(float stg);
void setEditMode(bool emode);
@@ -24,6 +26,8 @@ public :
int setting = 0;
int settingPrec = 2;
bool showSetting = false;
bool showSecondary = true;
bool showSecondaryValue = true;

public slots :
void onBlinkTimer(void);
@@ -39,6 +43,8 @@ public slots :

int vertical_divs = 4;
int value = 81;
int secondaryValue = 670;
int secondaryScale = 2000;



0 comments on commit a6d2539

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