PDA

View Full Version : OpenGL program to connect to Orientation sensor (Euler Angles)



Al Grant
07-07-2006, 05:21 PM
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", &amp;nPortNumber);

// Create instance of MTObj COM object
printf("Create instance of MotionTracker object...");
HRESULT hRes = CoCreateInstance(CLSID_MotionTracker, NULL, CLSCTX_SERVER, IID_IMotionTracker, (void**) &amp;pMT);
if (FAILED(hRes))
{
printf("Error %x in CoCreateInstance for MT object!",hRes);
return;
}
else
printf("done\n\n");


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\n\n");
}

BOOL GetData()
{
float fOrientationData[9] = {0};
VARIANT OrientationBuffer;
void* pDest;
short nNew = 0;

BOOL bNewData = FALSE;

pMT->MT_GetOrientationData(&amp;nNew, &amp;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, &amp;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.p array->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(&amp;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\n",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\n\n");
break;
case MT_NOSENSORID:
printf("No Sensor ID Received From Sensor\n\n");
break;
case MT_INCOMPLETE:
printf("Incomplete Data Received (Connection Lost)\n\n");
break;
case MT_CHECKSUMERROR:
printf("Checksum Error\n\n");
break;
case MT_NOPORT:
printf("COM Port Could Not Be Opened\n\n");
break;
case MT_NOCALIBVALUES:
printf("XMU File With Calibration Data Could Not Be Read or \nMTS Data With Calibration Data Not Set\n\n");
break;
case MT_POWERLOSS:
printf("Power Supply To The Sensor Was Probably Interupted\n\n");
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\n\n");

// 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\n\n");

// Keep receiving data as long as no error is received
// Diconnect the MotionTracker to stop the program nicely
BOOL bKeepRunning = TRUE;
while (bKeepRunning == TRUE &amp;&amp; !_kbhit())
bKeepRunning = GetData();

// Stop processing by MotionTracker object
printf("Stopping filter...");
pMT->MT_StopProcess();
printf("done\n\n");

// Release and clean up MotionTracker object
printf("Release MotionTracker object...");
if (pMT != NULL)
{
pMT->Release();

pMT = NULL;

printf("done\n\n");
}

// Uninitialize COM library
printf("Uninitialize COM library...");
CoUninitialize();
printf("done\n\n");

return 1;
}

Komat
07-08-2006, 03:53 AM
Use the debugger to find out where the crash happens.

Are you certain that the OrientationBuffer always contains a array which has the format you expect: at least one dimension which contains at most 9 elements? While array having zero dimensions would be likely handled by the __except clause, more than 9 elements would corrupt the stack and crash outside the block protected by the __try.

The OrientationBuffer variant is not cleared in all cases (some of the error ones).

Btw the expression in __except statement is ugly since it relies on values of EXCEPTION_EXECUTE_HANDLER and EXCEPTION_CONTINUE_SEARCH constants.

Al Grant
07-08-2006, 04:01 AM
Yeah, I didnt write the code - its hard to follow.

The problem is when I get the exeption it says something to the effect that it doesnt relate to a specific piece of code so i cant see where excately the exception is occuring!

-Al