Skip to content
Permalink
Browse files

Got VSI working, updates some rotary settings and modified the ahrs f…

…ilter
  • Loading branch information...
spencerdynojet committed Apr 5, 2019
1 parent ea328b5 commit 9df5016076a9816a73de53ca435d69d9f4d48b56
@@ -83,7 +83,7 @@ void DiagWidget::setKnobs(knobs *k)
void DiagWidget::pollTimerExp(void)
{
AHRS_DATA data;
int status = adhr->getDataSet(&data);
// int status = adhr->getDataSet(&data);

textEditadhrs->setText(
QString("Altitude") + '\r' + '\n' +
@@ -10,7 +10,7 @@
<Transport>None</Transport>
</BuildHost>
<DeploymentHost>
<HostName>raspberrypi</HostName>
<HostName>raspberrypi.local</HostName>
<Transport>SSH</Transport>
<UserName>pi</UserName>
</DeploymentHost>
@@ -35,7 +35,6 @@
</FileMasks>
<TransferNewFilesOnly>true</TransferNewFilesOnly>
<IncludeSubdirectories>true</IncludeSubdirectories>
<SelectedDirectories />
<DeleteDisappearedFiles>false</DeleteDisappearedFiles>
<ApplyGlobalExclusionList>true</ApplyGlobalExclusionList>
</MainSourceTransferCommand>
@@ -69,6 +69,12 @@ void RotaryEncoder::eventHandler(int gpio, int level, unsigned tick)
/* sum == 14 || sum == 7 value -- */
if (/*sum == 0b1110 || */ sum == 0b0111 /* || sum == 0b0001 || sum == 0b1000 */) enc->value--;
}
if (enc->config == 4)
{

if (/*sum == 0b1101 || sum == 0b0100 || */ sum == 0b0010 || sum == 0b1011) enc->value++;
if (/*sum == 0b1110 || */ sum == 0b0111 || sum == 0b0001 /*|| sum == 0b1000*/) enc->value--;
}


// printf("sum %d value %d \n", sum, enc->value);
@@ -30,6 +30,8 @@ SKFilter::SKFilter()
{
valid = false;

attitudeOffset = 0;

//Bank, Pitch and Yaw
_Euler[0] = 0;
_Euler[1] = 0;
@@ -434,7 +436,7 @@ bool SKFilter::update(float gx, float gy, float gz, float ax, float ay, float az
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;
acState.pitchlevel = acState.oneG && acState.pitchstill && abs(_accelEuler[1] - attitudeOffset) < 0.02 && !acState.turning;


if (acState.pitchlevel)
@@ -453,10 +455,10 @@ bool SKFilter::update(float gx, float gy, float gz, float ax, float ay, float az
fa[1] = 1.0f;
}
//Think about a slow level turn
if (acState.oneG && acState.pitchstill)
if(acState.oneG && acState.pitchstill && acState.wingslevel)
{
//fg[1] = 0.0f;
//fa[1] = 1.0f;
fg[1] = 0.0f;
fa[1] = 1.0f;
}

if (!acState.turning && acState.oneG)
@@ -505,3 +507,9 @@ float SKFilter::constrainAngle360(float dta) {
return dta;
}



void SKFilter::setAttitudeOffset(float offset_rads)
{
attitudeOffset = offset_rads;
}
@@ -35,6 +35,8 @@ 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);

void setAttitudeOffset(float offset_rads);

imu::Vector<3> getEuler(void);
AC_STATE acState;
@@ -68,6 +70,8 @@ class SKFilter

bool gyroUpdated_, accelUpdated_, magUpdated_;

float attitudeOffset;

float constrainAngle180(float dta);
float constrainAngle360(float dta);

@@ -16,7 +16,10 @@

static AHRS_CAL cal;
static AHRS_CAL defaultcal;
//float vsibuffer[VSIBUFFERSIZE];
static AIR_DATAPOINT airDataBuffer[AIRDATABUFFERSIZE];
static float vsiBuffer[VSIDATABUFFERSIZE];
static int airDataBufferIndex;
static int vsiDataBufferIndex;
/**********************************************************************************************//**
* Module: adhrs::adhrs
***************************************************************************************************
@@ -40,19 +43,8 @@ adhrs::adhrs(AHRS_CAL* pCalibration, bool domagtest, bool showmagvectors)
pCalibration->magBias, pCalibration->magScale, pCalibration->axisremap);
performMagTest = domagtest;
showMagVectors = showmagvectors;

float alt = altitude::getAltitudeFt(staticpress->getPressure(), 29.92);

for (int i = 0; i < ALTBUFFERSIZE; i++)
{
altbuffer[i].altitude = alt;
altbuffer[i].speed = 0.0f;
// vsibuffer[i] = 0.0f;
}

altbufferindex = 0;
//vsibufferindex = 0;

airDataBufferIndex = 0;
vsiDataBufferIndex = 0;
}

void adhrs::Init(void)
@@ -97,43 +89,11 @@ int adhrs::getDataSet(AHRS_DATA* data)
{
error = 0;
if (!error)
{
/*
float aa = altbuffer[altbufferindex].altitude = altitude::getAltitudeFt(staticpress->getPressure(), altimeterSetting);
float ss = altbuffer[altbufferindex].speed = airspeed::getIASMph(airspeed->getPressure());
timeval tp;
int v = gettimeofday(&tp, NULL);
altbuffer[altbufferindex].seconds = tp.tv_sec;
altbuffer[altbufferindex].useconds = tp.tv_usec;
altbufferindex++;
if (altbufferindex > ALTBUFFERSIZE) altbufferindex = 0;
float vsi = 0;
for (int i = 0; i < ALTBUFFERSIZE; i++)
{
if (i > 0)
{
int secs = altbuffer[i].seconds - altbuffer[i-1].seconds;
float _dt = secs * 1000000.0f + (altbuffer[i].useconds - altbuffer[i - 1].useconds);
_dt /= 60000000.0f; //calc dt in minutes
vsi += ((altbuffer[i].altitude - altbuffer[i - 1].altitude) / _dt);
}
}
vsibuffer[vsibufferindex] = (vsi / (ALTBUFFERSIZE - 1));
vsibufferindex++;
if (vsibufferindex > VSIBUFFERSIZE) vsibufferindex = 0;
vsi = 0;
for (int i = 0; i < VSIBUFFERSIZE; i++)
{
vsi += vsibuffer[i];
}
*/
data->altitude = altitude::getAltitudeFt(staticpress->getPressure(), altimeterSetting);
data->vsi = 0;//vsi / (VSIBUFFERSIZE);
data->airspeed = airspeed::getIASMph(airspeed->getPressure());
{
AIR_DATAPOINT point = adhrs::processAirData(staticpress->getPressure(), airspeed->getPressure(), altimeterSetting);
data->altitude = point.altitude;
data->vsi = adhrs::processVSI();
data->airspeed = point.speed;
data->heading = hrs->getHeading() * (180.0f / M_PI);
data->roll = hrs->getRoll() * (180.0f / M_PI);
data->pitch = hrs->getPitch() * (180.0f / M_PI);
@@ -531,3 +491,71 @@ void adhrs::setSteerToSettings(int* steerto)
{
hrs->setSteerCard(steerto);
}


void adhrs::setAttitudeOffset(int offset_deg)
{
hrs->setAttitudeOffset((float)(offset_deg*M_PI/180.0f));
}


AIR_DATAPOINT adhrs::processAirData(float pStatic, float pPitot, float altSetting)
{
AIR_DATAPOINT point;

timeval tp;
int v = gettimeofday(&tp, NULL);

int airDataBufferIndexPrev = (airDataBufferIndex - 1) >= 0 ? airDataBufferIndex - 1 : AIRDATABUFFERSIZE;


airDataBuffer[airDataBufferIndex].altitude = altitude::getAltitudeFt(pStatic, altSetting);
airDataBuffer[airDataBufferIndex].speed = airspeed::getIASMph(pPitot);
airDataBuffer[airDataBufferIndex].seconds = tp.tv_sec;
airDataBuffer[airDataBufferIndex].useconds = tp.tv_usec;

int secs = airDataBuffer[airDataBufferIndex].seconds - airDataBuffer[airDataBufferIndexPrev].seconds;
float _dt = secs * 1000000.0f + (airDataBuffer[airDataBufferIndex].useconds - airDataBuffer[airDataBufferIndexPrev].useconds);
_dt /= 60000000.0f;

float altdiff = airDataBuffer[airDataBufferIndex].altitude - airDataBuffer[airDataBufferIndexPrev].altitude;

airDataBuffer[airDataBufferIndex].vsi = (altdiff / _dt);


point.altitude = airDataBuffer[airDataBufferIndex].altitude;
point.speed = airDataBuffer[airDataBufferIndex].speed;

airDataBufferIndex++;
if (airDataBufferIndex >= AIRDATABUFFERSIZE) {
airDataBufferIndex = 0;
}

return point;
}


float adhrs::processVSI(void)
{
float vsi = 0;
for (int i = 0; i < AIRDATABUFFERSIZE; i++)
{
vsi += airDataBuffer[i].vsi;
}

vsi /= (AIRDATABUFFERSIZE);

vsiBuffer[vsiDataBufferIndex] = vsi;

vsiDataBufferIndex++;
if (vsiDataBufferIndex >= VSIDATABUFFERSIZE) vsiDataBufferIndex = 0;

vsi = 0;
for (int i = 0; i < VSIDATABUFFERSIZE; i++)
{
vsi += vsiBuffer[i];
}
vsi /= VSIDATABUFFERSIZE;

return vsi;
}
@@ -21,10 +21,8 @@
#define PRESSURE_MAX_VALID 30.0f
#define PRESSURE_MIN_VALID -30.0f

//Define the buffer size for the altimeter and VSI
#define ALTBUFFERSIZE 50
#define VSIBUFFERSIZE 50

#define AIRDATABUFFERSIZE 40
#define VSIDATABUFFERSIZE 30

typedef struct
{
@@ -41,6 +39,7 @@ typedef struct
{
float speed;
float altitude;
float vsi;
int seconds;
int useconds;
}AIR_DATAPOINT;
@@ -84,7 +83,12 @@ class adhrs
AC_STATE getAcState(void);
void setAltimeterSetting(int setting, int settingPrec);
void setSteerToSettings(int* steerto);
void setAttitudeOffset(int offset_deg);
static AHRS_CAL* processCalibrationFile(QFile* file);

static AIR_DATAPOINT processAirData(float pStatic, float pPitot, float altSetting);
static float processVSI(void);




@@ -101,9 +105,7 @@ class adhrs
static void calfile_process_line(QByteArray &line);
static bool calfile_validate( void );

AIR_DATAPOINT altbuffer[ALTBUFFERSIZE];
int altbufferindex;
//int vsibufferindex;



};
@@ -3,5 +3,6 @@ class altitude
{
public:
static float getAltitudeFt(float pressurePSI, float setting);

};

@@ -534,3 +534,9 @@ void mpudriver::setSteerCard(int* steerto)
}

}


void mpudriver::setAttitudeOffset(float offset_rad)
{
filter.setAttitudeOffset(offset_rad);
}
@@ -37,6 +37,7 @@ class mpudriver
int Init(bool doSelfTest, bool doCalibration, bool doMagCalibration, bool _showmagvectors);

void setSteerCard(int* steerto);
void setAttitudeOffset(float offset_rad);

imu::Vector<3> GetEuler(int* status);
imu::Vector<3> GetAccelerometer(int*status);
@@ -60,6 +60,8 @@ panelWidget::panelWidget(QWidget *parent, QFile* f, knobs *k)
dg = new directional_gyro(this);
dg->value = 7;
r = new reticle(this);
r->setSetting(settings.horizonOffset);
h1->setVerticalOffset(r->setting);

sw = new StatusWidget(this);
mw = new MenuWidget(this);
@@ -181,12 +183,14 @@ void panelWidget::onTimer(void)
r->toggleEditMode();
r->setSetting(h1->verticalOffset);
knob->right->setValue(r->setting);
adhr->setAttitudeOffset(r->setting);
}
if (mode == 6)
{
r->toggleEditMode();
h1->setVerticalOffset(r->setting);
settings.horizonOffset = r->setting;
adhr->setAttitudeOffset(r->setting);
saveSettingsFile();
}
//Show the menu
@@ -272,6 +276,7 @@ void panelWidget::setADHRS(adhrs* a)
//strange almost "multithreaded" application for a bit...
adhr->setAltimeterSetting(settings.altimeterSetting, vi2->settingPrec);
adhr->setSteerToSettings(settings.steerCard);
adhr->setAttitudeOffset(r->setting);
adhr->Init();
}

@@ -296,7 +301,7 @@ void panelWidget::onDebugTimer(void)
unsigned char offsets[22];
char caldata[4];
AHRS_DATA data;
int status = adhr->getDataSet(&data);
// int status = adhr->getDataSet(&data);

printf("!!! Orientation !!!");
printf("Pitch %3.2f", data.pitch);
@@ -31,8 +31,8 @@ public :
void setSetting(int arg1)
{
int tempsetting = arg1;
while (tempsetting > 360) tempsetting -= 360;
while (tempsetting < 0) tempsetting += 360;
if (tempsetting > 20)tempsetting = 20;
if (tempsetting < -20) tempsetting = -20;
setting = tempsetting;
}

0 comments on commit 9df5016

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