Hi folks,
I wrote a OpenGL program which simulates an Artifical Horizon, which is a instrument in an aircraft cockpit.
I have a gyro sensor attached to the Serial port on my computer which gives me orientation data (Euler angles) in 3D.
I had a crack at using the sensor data to control the artifical horizon I made, but it didnt like the repeated calls to the GetData() function that I copied from the software SDK, and after a few seconds the program would crash with:
Unhandled Exeption: Access Violation.
I put the GetData function in DrawGLScene part of my code. (GetData calls two other functions to do with setting up the Serial connection).
I will past the code from the SDK below, and would very much appreciate it if someone could show me how to get the euler angles from the matrix into OpenGl without crashing my program.
I am still a bit green at all this, but quite enjoying it, albeit a steep learning curve!
Thanks
-Al
#include <conio.h> // Needed for _kbhit function
#include <stdio.h> // Needed for printf etc
#include <objbase.h> // Needed for COM functionality
// import functions in MT object
#include "IMTObj.h"
// GUIDs of MT object
#include "IMTObj_i.c"
// return values for MT_GetOrientation function
#define MT_NEWDATA 1
#define MT_NODATA 2
#define MT_NOSENSORID 3
#define MT_INCOMPLETE 4
#define MT_CHECKSUMERROR 5
#define MT_NOPORT 6
#define MT_NOCALIBVALUES 7
#define MT_POWERLOSS 8
// output possiblities for MT object
#define MT_LOGQUATERNION 0
#define MT_LOGEULER 1
#define MT_LOGROTMATRIX 2
// Global pointer to the MTObj COM Interface
IMotionTracker* pMT;
// Output data format
short g_nMode = MT_LOGEULER;
void SetupFilter()
{
// Set MTObj COM object options
short bLogCalibratedData = FALSE;
// Set MTObj COM object variables
float fGain = 1.0;
short nCorInterval = 1;
float fRho = 1.0;
short nPortNumber;
printf("Enter COM port: ");
scanf("%d", &nPortNumber);
// Create instance of MTObj COM object
printf("Create instance of MotionTracker object...");
HRESULT hRes = CoCreateInstance(CLSID_MotionTracker, NULL, CLSCTX_SERVER, IID_IMotionTracker, (void**) &pMT);
if (FAILED(hRes))
{
printf("Error %x in CoCreateInstance for MT object!",hRes);
return;
}
else
printf("done
");
printf("Setting filter parameters...");
// Optional settings
pMT->MT_SetCalibratedOutput(bLogCalibratedData);
// Set Gain, Correction interval and Rho
pMT->MT_SetFilterSettings(fGain,nCorInterval,fRho);
// Required settings
pMT->MT_SetOutputMode(g_nMode);
// Set COM port number where MotionTracker is attached
pMT->MT_SetCOMPort(nPortNumber);
printf("done
");
}
BOOL GetData()
{
float fOrientationData[9] = {0};
VARIANT OrientationBuffer;
void* pDest;
short nNew = 0;
BOOL bNewData = FALSE;
pMT->MT_GetOrientationData(&nNew, &OrientationBuffer);
if (nNew == MT_NEWDATA)
{
// Check if array is not empty
if (OrientationBuffer.vt != VT_EMPTY)
{
// Retrieve pointer to array data
HRESULT hr = SafeArrayAccessData(OrientationBuffer.parray, &pDest);
// One dimensional array. Get the bounds for the array.
if (SUCCEEDED(hr))
{
__try{
// Copy data from the VARIANT array to the local fData array
memcpy(fOrientationData,pDest,(OrientationBuffer.parray->rgsabound->cElements * sizeof(float)));
bNewData = TRUE;
}
__except(GetExceptionCode() == STATUS_ACCESS_VIOLATION){
bNewData = FALSE;
}
SafeArrayUnaccessData(OrientationBuffer.parray); // Invalidate pointer
// Variant must be cleared. This also destroys the SafeArray
VariantClear(&OrientationBuffer);
// fOrientationData now contains orientation data is bNewData = true
// Can be logged to file or written to screen (see below)
if (g_nMode == MT_LOGEULER)
{
printf("%f %f %f
",fOrientationData[0],fOrientationData[1],fOrientationData[2]);
}
bNewData = FALSE;
return TRUE;
}
else
return FALSE;
}
else
return FALSE;
}
else if (nNew != 0)
{
// Check if error was reported by MotionTracker object
switch(nNew) {
case MT_NODATA:
printf("No Data On COM Port
");
break;
case MT_NOSENSORID:
printf("No Sensor ID Received From Sensor
");
break;
case MT_INCOMPLETE:
printf("Incomplete Data Received (Connection Lost)
");
break;
case MT_CHECKSUMERROR:
printf("Checksum Error
");
break;
case MT_NOPORT:
printf("COM Port Could Not Be Opened
");
break;
case MT_NOCALIBVALUES:
printf("XMU File With Calibration Data Could Not Be Read or
MTS Data With Calibration Data Not Set
");
break;
case MT_POWERLOSS:
printf("Power Supply To The Sensor Was Probably Interupted
");
break;
}
return FALSE;
}
else
return TRUE;
}
int main(int argc, char* argv[])
{
// Initialize COM library
printf("Initialize COM library...");
if (CoInitialize(NULL) != S_OK)
printf("Failed to initialize COM library");
else
printf("done
");
// Create filter instance and set filter parameters
SetupFilter();
// Start processing by MotionTracker object
printf("Start processing by the MotionTracker object..");
pMT->MT_StartProcess();
printf("done
");
// Keep receiving data as long as no error is received
// Diconnect the MotionTracker to stop the program nicely
BOOL bKeepRunning = TRUE;
while (bKeepRunning == TRUE && !_kbhit())
bKeepRunning = GetData();
// Stop processing by MotionTracker object
printf("Stopping filter...");
pMT->MT_StopProcess();
printf("done
");
// Release and clean up MotionTracker object
printf("Release MotionTracker object...");
if (pMT != NULL)
{
pMT->Release();
pMT = NULL;
printf("done
");
}
// Uninitialize COM library
printf("Uninitialize COM library...");
CoUninitialize();
printf("done
");
return 1;
}