main.cpp

GPS的串口读取数据过程,并且进行了坐标转换,转化为通用的坐标系

// SensorReader.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"
#include "Globe.h"
#include "Compass.h"
#include "GPS.H"
#include "HMDMOUSE.H"
#include "geo_utm.h"
//#include "OpenGVS_Geo.h"
#include <sstream>
#include <conio.h>
#include <stdio.h>
#include <string.h>


const char *devbird = "BIRD";
const char *devgps = "GPS";
const char *devcom = "COMPASS";

int _tmain(int argc, _TCHAR* argv[])
{
	FILE *fp;
	fp = fopen("1.txt", "r");
	int i, j;
	char  dev[MAXSIZE][MAXSIZE], baud[MAXSIZE][MAXSIZE];
	char  com[MAXSIZE][MAXSIZE];

	if(fp == NULL)
	{
		printf("cannot open file\n");
		return -1;
	}



    i = 0;
	while(!feof(fp))
	{
		fscanf(fp, "%s", com[i]);
		fscanf(fp, "%s", dev[i]);
		fscanf(fp, "%s", baud[i++]);
	}
	j = i;
    printf("# User configuration files \n");
	for(i = 0; i < j; i++)
		printf("COM%s %s %s\n", com[i], dev[i], baud[i]); 



   
	HANDLE hStdout;
	hStdout = GetStdHandle(STD_OUTPUT_HANDLE);
	if (hStdout == INVALID_HANDLE_VALUE)
	{
		MessageBox(NULL,   _T("GetStdHandle"),   _T("Console   Error"),   MB_OK);
		return -1;
	}

	cout << "# press any key to open port...\n";
	_getch();


    i = 0;
	///////////////////////        GPS     CODE     /////////////////////////////////////
	while(i < j)
	{
		if(strcmp(dev[i], devgps) == 0)
	{
		if(atoi(baud[i]) != GPS_DEFAULT_BAUDRATE)
		{
			printf("# GPS's  baudrate should  be 4800\n");
			return -1;
		}
	 GPS  gps;
	 gps.InitSensor(10, 10, 2);
	if(!gps.OpenPort(atoi(com[i]), GPS_DEFAULT_BAUDRATE))
	{
		cout << "# fail to open port.\n";
		return -1;
	}
	cout << "# port opened, press any key to display data...\n";
	_getch();



	CONSOLE_SCREEN_BUFFER_INFO info = {0};
	GPGGADATA data = {0};
	int count = 10;

	while(true)
	{
		if (!GetConsoleScreenBufferInfo(hStdout, &info))
		{
			MessageBox(NULL,   _T("GetConsoleScreenBufferInfo"),   _T("Console   Error"),   MB_OK);
			return -1;
		}

	

		int i = 0;
		double east, north;
		long zone;
		while(i++ < count)
		{

			Sleep(1000);
			gps.GetCurrentData(data);

			double fLatitude = data.fLatitude / 100.0;
			double fLongitude = data.fLongitude / 100.0;
			if(fLongitude >= 0.0 && fLongitude < 180.0)
				zone = (long)(31.0 + fLongitude / 6.0);
			else if(fLongitude >= 180.0 && fLongitude < 360.0)
				zone = (long)(fLongitude / 6.0 - 29.0);			
			geo_to_utm(fLatitude, fLongitude, &zone, &east, &north);	

			cout << "# GPS data:\n";
			cout << "\tTime:\t " << data.nHours << ": " << data.nMinutes << " " << data.nSeconds << "\n";
			/*printf("\tLagitude:\t %.3f\t\n", data.fLatitude);
			printf("\tLongitude:\t %.3f\t\n", data.fLongitude);
			printf("\tAltitude:\t %.3f\t\n", data.fAltitude);*/
			cout << "\tLagitude:\t " << data.fLatitude << "\t" << "\n";
			cout << "\tLongitude:\t " << data.fLongitude << "\t" << "\n";
			cout << "\tAltitude:\t " << data.fAltitude << "\t" << "\n";
			cout << "\tUTM  EAST:\t " << east<< "\t" << "\n";
			cout << "\tUTM  NORTH:\t " << north << "\t" << "\n";
		}

		cout << "# press any key to continue display, or press [q] to quit...\n";
		if (_getch() == 'q' || _getch() == 'Q')
		{
			break;
		}
	}

	gps.ClosePort();
	
	//return 0;
}


///////////////////////          GPS     CODE   //////////////////////////////////////






//////////////////////          COMPASS   CODE  ////////////////////////////////////

	if(strcmp(dev[i], devcom) == 0)
	{
		if(atoi(baud[i]) != COMPASS_DEFAULT_BAUDRATE)
		{
			printf("# COMPASS's  baudrate should  be 19200\n");
			return -1;
		}



	Compass compass;
	compass.InitSensor(10, 10, 2);
	if (!compass.OpenPort(atoi(com[i]), COMPASS_DEFAULT_BAUDRATE))
	{
		cout << "# fail to open port.\n";
		return -1;
	}

	cout << "# port opened, press any key to display data...\n";
	_getch();

	CONSOLE_SCREEN_BUFFER_INFO info = {0};
	//COORD coord = {0};
	HPRDATA data = {0};
	int count = 10;

	while(true)
	{
		if (!GetConsoleScreenBufferInfo(hStdout, &info))
		{
			MessageBox(NULL,   _T("GetConsoleScreenBufferInfo"),   _T("Console   Error"),   MB_OK);
			return -1;
		}

		//coord = info.dwCursorPosition;
		//coord.Y++;
		//coord.X = 0;

		int i = 0;
		while(++i < count)
		{
			compass.GetCurrentData(data);

			//SetConsoleCursorPosition(hStdout,   coord);

			cout << "# Compass data:\n";
			cout << "\tHeading: " << data.fHeading << "\t[" << data.szHeadStatus << "]\n"; //航向
			cout << "\tPitch:\t " << data.fPitch << "\t[" << data.szPitchStatus << "]\n";  //俯仰
			cout << "\tRoll:\t " << data.fRoll << "\t[" << data.szRollStatus << "]\n";     //横
		}

		cout << "# press any key to continue display, or press [q] to quit...\n";
		if (_getch() == 'q' || _getch() == 'Q')
		{
			break;
		}
	}

	compass.ClosePort();
	
	//return 0;
}
/////////////////////////////////////////////////////////////////////////////////
	if(strcmp(dev[i], devbird) == 0 )
	{
        
		CHmdMouse* hyzmouse  = new CHmdMouse(com[i], atoi(baud[i]),0,0);
		short button[6];
		BIRDDATA data = {0};

        short birdaddr=hyzmouse->Get3DmouseAddress();

        hyzmouse->GetSingleBirdData(POSANGLE,0,data.birdPos,data.birdOrient,button,birdaddr);
		cout << "# Bird  data:\n";
		cout << "\tX:\t" << data.birdPos[0] << "\t" << "\n";
		cout << "\tY:\t" << data.birdPos[1] << "\t" << "\n";
		cout << "\tZ:\t" << data.birdPos[2] << "\t" << "\n";
		cout << "\talph:\t" << data.birdOrient[0] << "\t" << "\n";
		cout << "\tbeta:\t" << data.birdOrient[1] << "\t" << "\n";
		cout << "\tgama:\t" << data.birdOrient[2] << "\t" << "\n";


       /*glTranslatef(birdPos[0],birdPos[1],birdPos[2]);
       glRotatef(birdOrient[0], 1.0f, 0.0f, 0.0f);
       glRotatef(birdOrient[1], 0.0f, 1.0f, 0.0f);
       glRotatef(birdOrient[2], 0.0f, 0.0f, 1.0f);*/


	}
	i++;
	}
	}