smmt2

smmt2.cpp

#include <stdio.h>		// Needed for printf etc
#include <objbase.h>	// Needed for COM functionality
#include "xsens_cmt_static.h"
#include <conio.h>		// included for _getch and _kbhit
#include <string.h>
#include <time.h>
#include <iostream>
#include <math.h>
#include <cv.h>
#include <highgui.h>
/*#include <opencv2/core/core.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <opencv2/highgui/highgui.hpp>*/

#include "main.h"

using namespace cv;
using namespace std;

// this macro tests for an error and exits the program with a message if there was one
#define EXIT_ON_ERROR(res,comment) if (res != XRV_OK) { printf("Error %d occurred in " comment ": %s\n",res,xsensResultText(res)); exit(1); }

using namespace xsens;

// used to signal that the user initiated the exit, so we do not wait for an extra keypress-
int userQuit = 0;
CmtOutputMode mode;
CmtOutputSettings settings;
unsigned long mtCount = 0;
int screenSensorOffset = 0;
int temperatureOffset = 0;
CmtDeviceId deviceIds[256];
xsens::Cmt3 cmt3;



//int g_slider_position = 0;
//CvCapture* capture2 = NULL;

//void onTrackbarSlide(int current_frame)
//{
//    current_frame = g_slider_position;
//    cvSetCaptureProperty(capture2,CV_CAP_PROP_BRIGHTNESS,current_frame);
//}


/*+-------------------------------------------------------------------------+
  +                        Scale Feature Invariante                         +
  +  Syntax:                                                                +
  +  SIFT()                                                                 +
  + where:                                                                  +
  +    descriptors1 e descriptors2                                          +
  +    keypoints1 e keypoints2                                              +
  +    img11 e img22                                                        +
  +    matches                                                              +
  +                                                                         +
  +  Daniel dos Santos 2012                                                 +
  +-------------------------------------------------------------------------+ */
/*int MySIFT()
{
  int round(float i);

  FILE *fMatch, *ReadImage;
  int first_R, first_G, first_B,
      second_R, second_G, second_B,
	  id_LEFT, id_RIGHT;
  CvScalar pixelVal;
  Mat img11, img22;

  img11 = imread(ss1.str(),CV_LOAD_IMAGE_GRAYSCALE);
  img22 = imread(ss2.str(),CV_LOAD_IMAGE_GRAYSCALE);


    std::cout<< "Calculado os descritores SIFT...\n";

	Ptr<FeatureDetector> featureDetector = FeatureDetector::create("SIFT");
    vector<KeyPoint> keypoints1, keypoints2;

 // Detect the keypoints
    featureDetector->detect(img11, keypoints1); // NOTE: featureDetector is a pointer hence the '->'.
    featureDetector->detect(img22, keypoints2); // NOTE: featureDetector is a pointer hence the '->'.

  //Similarly, we create a smart pointer to the SIFT extractor.
    Ptr<DescriptorExtractor> featureExtractor = DescriptorExtractor::create("SIFT");

  // Compute the 128 dimension SIFT descriptor at each keypoint.
  // Each row in "descriptors" correspond to the SIFT descriptor for each keypoint
     Mat descriptor1, descriptor2;
     featureExtractor->compute(img11, keypoints1, descriptor1);
	 featureExtractor->compute(img22, keypoints2, descriptor2);

// If you would like to draw the detected keypoint just to check
    Mat feat1,feat2;
    drawKeypoints(img11,keypoints1,feat1,Scalar(255, 255, 255),DrawMatchesFlags::DRAW_RICH_KEYPOINTS);
    drawKeypoints(img22,keypoints2,feat2,Scalar(255, 255, 255),DrawMatchesFlags::DRAW_RICH_KEYPOINTS);

    int key1 = keypoints1.size();
    int key2 = keypoints2.size();

    std::cout<< "Encontrando correspondencias...\n";

    vector< vector<DMatch> > matches;
    Ptr<DescriptorMatcher> matcher = DescriptorMatcher::create("BruteForce");
    matcher->knnMatch( descriptor1, descriptor2, matches, 500 );

//look whether the match is inside a defined area of the image
//only 25% of maximum of possible distance
double tresholdDist = 0.25 * sqrt(double(img11.size().height*img11.size().height + img11.size().width*img11.size().width));

vector< DMatch > good_matches2;
good_matches2.reserve(matches.size());  
for (size_t i = 0; i < matches.size(); ++i)
{ 
    for (int j = 0; j < matches[i].size(); j++)
    {
        Point2f from = keypoints1[matches[i][j].queryIdx].pt;
        Point2f to = keypoints2[matches[i][j].trainIdx].pt;

        //calculate local distance for each possible match
        double dist = sqrt((from.x - to.x) * (from.x - to.x) + (from.y - to.y) * (from.y - to.y));

        //save as best match if local distance is in specified area and on same height
        if (dist < tresholdDist && abs(from.y-to.y)<5)
        {
            good_matches2.push_back(matches[i][j]);
            j = matches[i].size();
        }
    }
}


//Draw only "good" matches
    Mat result;
    drawMatches( img11, keypoints1, img22, keypoints2,
                 good_matches2, result, Scalar::all(-1), Scalar::all(-1),
                 vector<char>(), DrawMatchesFlags::NOT_DRAW_SINGLE_POINTS );

    descriptor1.release();
	descriptor2.release();
	feat1.release();
	feat2.release();

    int first_x, first_y,
        second_x, second_y;
    for (unsigned int i = 0; i < good_matches2.size(); ++i)
    {

         Point2f  first = keypoints1[good_matches2[i].queryIdx].pt;
         first_x = int(first.x);
         first_y = int(first.y);
         first_R = img11.at<cv::Vec3b>(first.y,first.x)[2];
         first_G = img11.at<cv::Vec3b>(first_y,first_x)[1];
         first_B = img11.at<cv::Vec3b>(first_y,first_x)[0];
      	 Point2f second = keypoints2[good_matches2[i].trainIdx].pt;
      	 second_x = int(second.x);
         second_y = int(second.y);
         second_R = img22.at<cv::Vec3b>(second_y,second_x)[2];
         second_G = img22.at<cv::Vec3b>(second_y,second_x)[1];
         second_B = img22.at<cv::Vec3b>(second_y,second_x)[0];

      }//end for
       fclose(fMatch);
 // display the result
       namedWindow("SIFT", CV_WINDOW_AUTOSIZE );
       imshow( "Correspondencias obtidas", result );
       waitKey(0); //press any key to quit
  //Deallocation of images and vectors
	matches.clear();
	keypoints1.clear();
	keypoints2.clear();
	img11.release();
	img22.release();
	result.release();

    return 0;
}*/
//-------------------------------------------------------------------------------------------------------------------------


int main(void)
{

	XsensResultValue res = XRV_OK;
	short screenSkipFactor = 10;
	short screenSkipFactorCnt = screenSkipFactor;

	FILE *GNSS_IMU;


	// Set exit function
	atexit(exitFunc);

	// Perform hardware scan
	doHardwareScan();

	// Give user a (short) chance to see hardware scan results
	Sleep(2000);
	
	//clear screen present & get the user output mode selection.
	clrscr();
	getUserInputs();

	// Set device to user input settings
	doMtSettings();
	
	// Wait for first data item(s) to arrive. In production code, you would use a callback function instead (see cmtRegisterCallback function)
	Sleep(20);

	//get the placement offsets, clear the screen and write the fixed headers.
	calcScreenOffset();
	clrscr();
	writeHeaders();

	// vars for sample counter & temp.
	unsigned short sdata;
	double tdata;
	
	//structs to hold data.
	CmtCalData caldata;
	CmtQuat qat_data;
	CmtEuler euler_data;
	CmtMatrix matrix_data;

	// Initialize packet for data
	Packet* packet = new Packet((unsigned short)mtCount,cmt3.isXm());

    int contador_tempo, inicial, coleta;
    
	coleta = 1;
	contador_tempo = inicial = 0;

	GNSS_IMU = fopen("c:/Temp/GNSS_IMU.txt", "w");

    char key;
	bool camera = true;

	//cvNamedWindow("Camera Esquerda", 1);    //Create window
    //CvCapture* capture1 = cvCaptureFromCAM(0);  //Capture using any camera connected to your system

	cvNamedWindow("Camera Direita", 1);    //Create window
    CvCapture* capture2 = cvCaptureFromCAM(1);  //Capture using any camera connected to your system
	//cvSetCaptureProperty(capture2,CV_CAP_PROP_BRIGHTNESS,200);
	
	
	

	///

	int bright =(int) cvGetCaptureProperty(capture2,CV_CAP_PROP_BRIGHTNESS); 
	int contr =(int) cvGetCaptureProperty(capture2,CV_CAP_PROP_CONTRAST); 
	int sat =(int) cvGetCaptureProperty(capture2,CV_CAP_PROP_SATURATION); 
	int gain =(int) cvGetCaptureProperty(capture2,CV_CAP_PROP_GAIN);
	int expos = (int) cvGetCaptureProperty(capture2,CV_CAP_PROP_EXPOSURE);
	///

	//cvCreateTrackbar("brilho","Camera Direita",&contr,100,NULL);

	if (!capture2 )//(!capture1 && !capture2 )
	{
	    cout<<"NENHUMA DAS DUAS CAMERAS FORAM ENCONTRADAS!"<<endl;
		return -1;
	}
	while(!userQuit && res == XRV_OK && 2)// 1 && 2)
	{

		

		  //Mat frame1 = cvQueryFrame(capture1); //Create image frames from capture

			int hue =(int) cvGetCaptureProperty(capture2,CV_CAP_PROP_FPS); 
		  Mat frame2 = cvQueryFrame(capture2); //Create image frames from capture	

		  		cvSetCaptureProperty(capture2,CV_CAP_PROP_BRIGHTNESS,contr);
		  
		  //imshow("Camera Esquerda", frame1);   //Show image frames on created window
		  imshow("Camera Direita", frame2);   //Show image frames on created window
		  
		  //cvMoveWindow("Camera Direita",100,100);
        
		key = cvWaitKey(10);     //Capture Keyboard stroke
        if (char(key) == 27)
	    {
            break;      //If you hit ESC key loop will break.
        }

		cmt3.waitForDataMessage(packet);

		//get sample count, goto position & display.
		sdata = packet->getSampleCounter();
		gotoxy(0,0);
		printf("Contador %05hu\n", sdata);

		if (screenSkipFactorCnt++ == screenSkipFactor) {
			screenSkipFactorCnt = 0;

			for (unsigned int i = 0; i < mtCount; i++) {	
				// Output Temperature
				if ((mode & CMT_OUTPUTMODE_TEMP) != 0) {					
					gotoxy(0,4 + i * screenSensorOffset);	
					tdata = packet->getTemp(i);
					printf("%6.2f", tdata);
				}

				gotoxy(0,5 + temperatureOffset + i * screenSensorOffset);	// Output Calibrated data
				if ((mode & CMT_OUTPUTMODE_CALIB) != 0) {					
					caldata = packet->getCalData(i);
					printf("%6.2f\t%6.2f\t%6.2f" , caldata.m_acc.m_data[0], caldata.m_acc.m_data[1], caldata.m_acc.m_data[2]);

					gotoxy(0,7 + temperatureOffset + i * screenSensorOffset);
					printf("%6.2f\t%6.2f\t%6.2f", caldata.m_gyr.m_data[0], caldata.m_gyr.m_data[1], caldata.m_gyr.m_data[2] );

					gotoxy(0,9 + temperatureOffset + i * screenSensorOffset);
					printf("%6.2f\t%6.2f\t%6.2f",caldata.m_mag.m_data[0], caldata.m_mag.m_data[1], caldata.m_mag.m_data[2]);
					gotoxy(0,13 + temperatureOffset + i * screenSensorOffset);
				}

				if ((mode & CMT_OUTPUTMODE_ORIENT) != 0) {					
					switch(settings & CMT_OUTPUTSETTINGS_ORIENTMODE_MASK) {
					case CMT_OUTPUTSETTINGS_ORIENTMODE_QUATERNION:
						// Output: quaternion
						qat_data = packet->getOriQuat(i);
						printf("%Lf\t%Lf\t%Lf\t%Lf\n", qat_data.m_data[0], qat_data.m_data[1],qat_data.m_data[2],qat_data.m_data[3]);
						if ( (contador_tempo >= inicial-10) && (contador_tempo <= inicial+10) )
	 						  fprintf(GNSS_IMU,"%Lf\t%Lf\t%Lf\t%Lf\"\t", qat_data.m_data[0], qat_data.m_data[1],qat_data.m_data[2],qat_data.m_data[3]);

						break;

					case CMT_OUTPUTSETTINGS_ORIENTMODE_EULER:
						// Output: Euler
						euler_data = packet->getOriEuler(i);
						printf("%f\t%f\t%f\n", euler_data.m_roll,euler_data.m_pitch, euler_data.m_yaw);
						if ( (contador_tempo >= inicial-10) && (contador_tempo <= inicial+10) )
							 fprintf(GNSS_IMU,"%d  %Lf\t%Lf\t%Lf\t", coleta, euler_data.m_roll,euler_data.m_pitch, euler_data.m_yaw);
						break;

					case CMT_OUTPUTSETTINGS_ORIENTMODE_MATRIX:
						// Output: Cosine Matrix
						matrix_data = packet->getOriMatrix(i);
						printf("%6.3f\t%6.3f\t%6.3f\n", matrix_data.m_data[0][0],matrix_data.m_data[0][1], matrix_data.m_data[0][2]);
						printf("%6.3f\t%6.3f\t%6.3f\n", matrix_data.m_data[1][0],matrix_data.m_data[1][1], matrix_data.m_data[1][2]);
						printf("%6.3f\t%6.3f\t%6.3f\n", matrix_data.m_data[2][0],matrix_data.m_data[2][1], matrix_data.m_data[2][2]);
						break;
					default:
						;
					}

					if ((mode & CMT_OUTPUTMODE_POSITION) != 0) {
						if (packet->containsPositionLLA()) {
							/* output position */
							printf("\n\n");
				
							printf("Contador %d:\n", contador_tempo);
							printf("Contador inicial %d:\n", inicial);

							CmtVector positionLLA = packet->getPositionLLA();
							if (res != XRV_OK) {
								printf("error %ud", res);
							}	                		
							for (int i = 0; i < 2; i++) {
								double deg = positionLLA.m_data[i];
								double min = (deg - (int)deg)*60;
								double sec = (min - (int)min)*60;
								printf("%3d\xa7%2d\'%Lf\"\t", (int)deg, (int)min, sec);
							if ( (contador_tempo >= inicial-10) && (contador_tempo <= inicial+10) )
								fprintf(GNSS_IMU,"%3d\º%2d\'%Lf\"\t", (int)deg, (int)min, sec);
		
							}
							printf(" %Lf\n", positionLLA.m_data[2]);

							if ( (contador_tempo >= inicial-10) && (contador_tempo <= inicial+10) )
							{
								fprintf(GNSS_IMU,"%Lf\n", positionLLA.m_data[2]);

								stringstream ss1, ss2;
//								ss1 << "c:/Temp/img_" << coleta << "esq" << ".jpg";
//								imwrite(ss1.str(), frame1);
								ss2 << "c:/Temp/img_" << coleta << "dir" << ".jpg";
								imwrite(ss2.str(), frame2);
                                
								printf("Coletou %d informacoes e salvou as imagens!\n",coleta);
								cout << contr << endl;
								cout<< hue << endl;
								inicial = contador_tempo+10;
								coleta++;
								//MySIFT();
							}							

						} else {
							printf("Posicao do GNSS nao esta disponivel!\n");
						}
					}
				}
			}	
		}				

		if (_kbhit())
			userQuit = 1;
		contador_tempo++;
	}
    //cvReleaseCapture(&capture1); //Release capture.
    cvReleaseCapture(&capture2); //Release capture.

	//cvDestroyWindow("Camera Esquerda"); //Destroy Window
	cvDestroyWindow("Camera Direita"); //Destroy Window

	fclose(GNSS_IMU);
	delete packet;

	clrscr();
	cmt3.closePort();

 
	return 0;
}

//////////////////////////////////////////////////////////////////////////
// doHardwareScan
//
// Checks available COM ports and scans for MotionTrackers
void doHardwareScan()
{
	XsensResultValue res;
	List<CmtPortInfo> portInfo;
	unsigned long portCount = 0;

	printf("Procurando dispositivo conectado...");
	xsens::cmtScanPorts(portInfo);
	portCount = portInfo.length();
	printf("Pronto!\n");

	if (portCount == 0) {
		printf("No MotionTrackers found\n\n");
		exit(0);
	}

	for(int i = 0; i < (int)portCount; i++) {	
		printf("Usando porta COM %d com %d MHz\n\n",
			(long) portInfo[i].m_portNr, portInfo[i].m_baudrate);	
	}

	printf("Abrindo portas...");
	//open the port which the device is connected to and connect at the device's baudrate.
	for(int p = 0; p < (int)portCount; p++){
		res = cmt3.openPort(portInfo[p].m_portNr, portInfo[p].m_baudrate);
		EXIT_ON_ERROR(res,"cmtOpenPort");  
	}
	printf("Pronto!\n\n");

	//get the Mt sensor count.
	printf("Retrieving MotionTracker count (excluding attached Xbus Master(s))\n");
	mtCount = cmt3.getMtCount();
	mtCount = mtCount;
	printf("MotionTracker count: %i\n\n",mtCount);

	// retrieve the device IDs 
	printf("Retrieving MotionTrackers device ID(s)\n");
	for(unsigned int j = 0; j < mtCount; j++ ){
		res = cmt3.getDeviceId((unsigned char)(j+1), deviceIds[j]);
		EXIT_ON_ERROR(res,"getDeviceId");
		printf("Device ID at busId %i: %08x\n",j+1,(long) deviceIds[j]);
	}	
}

//////////////////////////////////////////////////////////////////////////
// getUserInputs
//
// Request user for output data
void getUserInputs()
{
	do{
		printf("Seleciona modo de operacao desejado:\n");
		printf("1 - Dados calibrados\n");
		printf("2 - Dados de orientacao e posicao (apenas para sensores MTi-G)\n");
		printf("3 - Dados calibrados e de orientacao\n");
		printf("4 - Temperatura e dados calibrados\n");
		printf("5 - Temperatura e dados de orientacao\n");
		printf("6 - Temperatura, dados calibrados e de orientacao\n");
		printf("Entre com sua escolha: ");
		scanf_s("%d", &mode);
		// flush stdin
		while (getchar() != '\n') continue;

		if (mode < 1 || mode > 6) {
			printf("\n\nPor favor, entre com um numero valido!\n");
		}
	}while(mode < 1 || mode > 6);
	clrscr();

	switch(mode)
	{
	case 1:
		mode = CMT_OUTPUTMODE_CALIB;
		break;
	case 2:
		mode = CMT_OUTPUTMODE_ORIENT | CMT_OUTPUTMODE_POSITION;
		break;
	case 3:
		mode = CMT_OUTPUTMODE_CALIB | CMT_OUTPUTMODE_ORIENT;
		break;
	case 4:
		mode = CMT_OUTPUTMODE_TEMP | CMT_OUTPUTMODE_CALIB;
		break;
	case 5:
		mode = CMT_OUTPUTMODE_TEMP | CMT_OUTPUTMODE_ORIENT;
		break;
	case 6:
		mode = CMT_OUTPUTMODE_TEMP | CMT_OUTPUTMODE_CALIB | CMT_OUTPUTMODE_ORIENT;
		break;
	}

	if ((mode & CMT_OUTPUTMODE_ORIENT) != 0) {
		do{
			printf("Seleciona o formato de saida dos dados de orientacao\n");
			printf("1 - Quaternions\n");
			printf("2 - Angulos de Euler\n");
			printf("3 - Matricial\n");
			printf("Entre com sua escolha: ");
			scanf_s("%d", &settings);
			// flush stdin
			while (getchar() != '\n') continue;

			if (settings < 1  || settings > 3) {
				printf("\n\nPor favor, entre com um numero valido!\n");
			}
		}while(settings < 1 || settings > 3);

		// Update outputSettings to match data specs of SetOutputSettings
		switch(settings) {
		case 1:
			settings = CMT_OUTPUTSETTINGS_ORIENTMODE_QUATERNION;
			break;
		case 2:
			settings = CMT_OUTPUTSETTINGS_ORIENTMODE_EULER;
			break;
		case 3:
			settings = CMT_OUTPUTSETTINGS_ORIENTMODE_MATRIX;
			break;
		}
	}
	else{
		settings = 0;
	}
	settings |= CMT_OUTPUTSETTINGS_TIMESTAMP_SAMPLECNT;
}

//////////////////////////////////////////////////////////////////////////
// doMTSettings
//
// Set user settings in MTi/MTx
// Assumes initialized global MTComm class
void doMtSettings(void) 
{
	XsensResultValue res;

	// set sensor to config sate
	res = cmt3.gotoConfig();
	EXIT_ON_ERROR(res,"gotoConfig");

	unsigned short sampleFreq;
	sampleFreq = cmt3.getSampleFrequency();

	// set the device output mode for the device(s)
	printf("Configurando seu modo selecionado");
	CmtDeviceMode deviceMode(mode, settings, sampleFreq);
	for(unsigned int i = 0; i < mtCount; i++){
		CmtDeviceMode deviceMode(mode, settings, sampleFreq);
		if ((deviceIds[i] & 0xFFF00000) != 0x00500000) {
			// not an MTi-G, remove all GPS related stuff
			deviceMode.m_outputMode &= 0xFF0F;
		}

		res = cmt3.setDeviceMode(deviceMode,true, deviceIds[i]);
		EXIT_ON_ERROR(res,"setDeviceMode");
	}

	// start receiving data
	res = cmt3.gotoMeasurement();
	EXIT_ON_ERROR(res,"gotoMeasurement");
}

//////////////////////////////////////////////////////////////////////////
// writeHeaders
//
// Write appropriate headers to screen
void writeHeaders()
{
	for (unsigned int i = 0; i < mtCount; i++) {	
		gotoxy(0, 2 + i * screenSensorOffset);
		printf("MotionTracker %d\n", i + 1);

		if ((mode & CMT_OUTPUTMODE_TEMP) != 0) {
			temperatureOffset = 3;
			gotoxy(0,3 + i * screenSensorOffset);
			printf("Temperatura");
			gotoxy(7,4 + i * screenSensorOffset);
			printf("Graus Celcius");
			gotoxy(0,6 + i * screenSensorOffset);
		}

		if ((mode & CMT_OUTPUTMODE_CALIB) != 0) {
			gotoxy(0,3 + temperatureOffset + i * screenSensorOffset);
			printf("Dados calibrados do sensor");
			gotoxy(0,4 + temperatureOffset + i * screenSensorOffset);
			printf(" Acc X\t Acc Y\t Acc Z");
			gotoxy(23, 5 + temperatureOffset + i * screenSensorOffset);
			printf("(m/s^2)");
			gotoxy(0,6 + temperatureOffset + i * screenSensorOffset);
			printf(" Gyr X\t Gyr Y\t Gyr Z");
			gotoxy(23, 7 + temperatureOffset + i * screenSensorOffset);
			printf("(rad/s)");
			gotoxy(0,8 + temperatureOffset + i * screenSensorOffset);
			printf(" Mag X\t Mag Y\t Mag Z");
			gotoxy(23, 9 + temperatureOffset + i * screenSensorOffset);
			printf("(a.u.)");
			gotoxy(0,11 + temperatureOffset + i * screenSensorOffset);
		}

		if ((mode & CMT_OUTPUTMODE_ORIENT) != 0) {
			printf("Dados de Orientacao\n");
			switch(settings & CMT_OUTPUTSETTINGS_ORIENTMODE_MASK) {
			case CMT_OUTPUTSETTINGS_ORIENTMODE_QUATERNION:
				printf("    q0\t    q1\t    q2\t    q3\n");
				break;
			case CMT_OUTPUTSETTINGS_ORIENTMODE_EULER:
				printf("  Roll\t Pitch\t   Yaw\n");
				printf("                       graus\n");
				break;
			case CMT_OUTPUTSETTINGS_ORIENTMODE_MATRIX:
				printf(" Matriz\n");
				break;
			default:
				;
			}			
		}

		if ((mode & CMT_OUTPUTMODE_POSITION) != 0) {
			printf("\nLongitude\tLatitude\tAltitude\n");
		}
	}
}

//////////////////////////////////////////////////////////////////////////
// calcScreenOffset
//
// Calculates offset for screen data with multiple sensors.
void calcScreenOffset()
{
	// 1 line for "Sensor ..."
	screenSensorOffset += 1;
	if ((mode & CMT_OUTPUTMODE_TEMP) != 0)
		screenSensorOffset += 3;
	if ((mode & CMT_OUTPUTMODE_CALIB) != 0)
		screenSensorOffset += 8;
	if ((mode & CMT_OUTPUTMODE_ORIENT) != 0) {
		switch(settings & CMT_OUTPUTSETTINGS_ORIENTMODE_MASK) {
		case CMT_OUTPUTSETTINGS_ORIENTMODE_QUATERNION:
			screenSensorOffset += 4;
			break;
		case CMT_OUTPUTSETTINGS_ORIENTMODE_EULER:
			screenSensorOffset += 4;
			break;
		case CMT_OUTPUTSETTINGS_ORIENTMODE_MATRIX:
			screenSensorOffset += 6;
			break;
		default:
			;
		}
	}
	if ((mode & CMT_OUTPUTMODE_POSITION) != 0) {
		screenSensorOffset += 4;
	}
}

//////////////////////////////////////////////////////////////////////////
// clrscr
//
// Clear console screen
void clrscr() 
{
#ifdef WIN32
	CONSOLE_SCREEN_BUFFER_INFO csbi;
	HANDLE hStdOut = GetStdHandle(STD_OUTPUT_HANDLE);
	COORD coord = {0, 0};
	DWORD count;

	GetConsoleScreenBufferInfo(hStdOut, &csbi);
	FillConsoleOutputCharacter(hStdOut, ' ', csbi.dwSize.X * csbi.dwSize.Y, coord, &count);
	SetConsoleCursorPosition(hStdOut, coord);
#else
	int i;

	for (i = 0; i < 100; i++)
		// Insert new lines to create a blank screen
		putchar('\n');
	gotoxy(0,0);
#endif
}

//////////////////////////////////////////////////////////////////////////
// gotoxy
//
// Sets the cursor position at the specified console position
//
// Input
//	 x	: New horizontal cursor position
//   y	: New vertical cursor position
void gotoxy(int x, int y)
{
#ifdef WIN32
	COORD coord;
	coord.X = x;
	coord.Y = y;
	SetConsoleCursorPosition(GetStdHandle(STD_OUTPUT_HANDLE), coord);
#else
	char essq[100];		// String variable to hold the escape sequence
	char xstr[100];		// Strings to hold the x and y coordinates
	char ystr[100];		// Escape sequences must be built with characters

	/*
	** Convert the screen coordinates to strings
	*/
	sprintf(xstr, "%d", x);
	sprintf(ystr, "%d", y);

	/*
	** Build the escape sequence (vertical move)
	*/
	essq[0] = '\0';
	strcat(essq, "\033[");
	strcat(essq, ystr);

	/*
	** Described in man terminfo as vpa=\E[%p1%dd
	** Vertical position absolute
	*/
	strcat(essq, "d");

	/*
	** Horizontal move
	** Horizontal position absolute
	*/
	strcat(essq, "\033[");
	strcat(essq, xstr);
	// Described in man terminfo as hpa=\E[%p1%dG
	strcat(essq, "G");

	/*
	** Execute the escape sequence
	** This will move the cursor to x, y
	*/
	printf("%s", essq);
#endif
}

//////////////////////////////////////////////////////////////////////////
// exitFunc
//
// Closes cmt nicely
void exitFunc(void)
{
	// Close any open COM ports
	cmt3.closePort();
	
	// get rid of keystrokes before we post our message
	while (_kbhit()) _getch();

	// wait for a keypress
	if (!userQuit)
	{
		printf("Pressiona qualquer tecla para sair\n");
		_getch();
	}
}