📄 firstatl.cpp
字号:
// FirstATL.cpp : Implementation of CFirstATL
#include "stdafx.h"
#include "Component.h"
#include "FirstATL.h"
#include "Globe.h"
#include "Compass.h"
#include "GPS.H"
#include "HMDMOUSE.H"
#include "geo_utm.h"
#include "LatusController.h"
//#include <iostream.h>
#include <sstream>
#include <conio.h>
#include <stdio.h>
#include <windows.h>
const char *devbird = "BIRD";
const char *devgps = "GPS";
const char *devcom = "COMPASS";
const char *devlat = "LATUS";
char dev[MAXSIZE][MAXSIZE], com[MAXSIZE][MAXSIZE];
char name[MAXSIZE][MAXSIZE];
int baud[MAXSIZE], other[MAXSIZE];
//GPS
GPS gps;
GPGGADATA data = {0};
double east, north;
long zone;
float fLatitude, fLongitude;
//COMPASS
Compass compass;
//BIRD
CHmdMouse* hyzmouse = new CHmdMouse;
//LATUS
LatusController ls;
double tx, ty, tz;
double rx, ry, rz;
int num = 0;
//out file
FILE *fq1 = fopen("gpsout.txt", "w");
FILE *fq2 = fopen("compassout.txt", "w");
FILE *fq3 = fopen("birdout.txt", "w");
/////////////////////////////////////////////////////////////////////////////
// CFirstATL
STDMETHODIMP CFirstATL::Initial(BOOL *result)
{
int i,j;
FILE *fp = fopen("config.txt", "r");
if(fp == NULL)
{
printf("cannot open file\n");
return -1;
}
for(i = 0; i < 4; i++)
{
fscanf(fp, "%s", name[i]);
printf("%s\t\t", name[i]);
}
i = 0;
while(!feof(fp))
{
fscanf(fp, "%s", com[i]);
fscanf(fp, "%s", dev[i]);
fscanf(fp, "%d", &baud[i]);
fscanf(fp, "%d", &other[i++]);
}
fclose(fp);
printf("\n");
j = i;
for(i = 0; i < j; i++)
printf("%s\t\t%s\t\t%d\t\t%d\n", com[i], dev[i], baud[i], other[i]);// TODO: Add your implementation code here
// cout << "# press any key to open port...\n";
// _getch();
i = 0;
if(strcmp(dev[i], devgps) == 0)
{
if(baud[i] != GPS_DEFAULT_BAUDRATE)
{
MessageBox(NULL, _T("GPS's baudrate should be 4800"), _T("Baudrate Error"), MB_OK);
return -1;
}
gps.InitSensor(10, 10, 2);
if(!gps.OpenPort(com[i][3]-'0', GPS_DEFAULT_BAUDRATE))
{
cout << "# fail to open port.\n";
return -1;
}
gps.StartThread();
double fEast = 0.0, fNorth = 0.0;
int count;
if(other[i] == 1) //计算原点的UTM坐标
{
count = 3;
Sleep(1000);
for(i = 0; i < count; i++)
{
gps.GetCurrentData(data);
fLatitude = data.fLatitude / 100.0;
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);
fEast += east;
fNorth += north;
}
fEast /= (double)count;
fNorth /= (double)count;
}
else //用户输入原点的经纬度坐标,转换为UTM
{
fp = fopen("gpsin.txt", "r");
count == 0;
while(!feof(fp))
{
fscanf(fp, "%f %f", &fLatitude, &fLongitude);
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);
fEast += east;
fNorth += north;
count++;
}
fEast /= (double)count;
fNorth /= (double)count;
}
fprintf(fq1, "X = %lf Y = %lf\n", fEast, fNorth);
}
else if(strcmp(dev[i], devcom) == 0)
{
HPRDATA data1 = {0};
if(baud[i] != COMPASS_DEFAULT_BAUDRATE)
{
MessageBox(NULL, _T("COMPASS's baudrate should be 19200"), _T("Baudrate Error"), MB_OK);
return -1;
}
compass.InitSensor(10, 10, 2);
if(!compass.OpenPort(com[i][3]-'0', COMPASS_DEFAULT_BAUDRATE))
{
cout << "# fail to open port.\n";
return -1;
}
compass.StartThread();
/* char *outs1 = "#F33.4=0*51\r\n";
char *outs2 = "#I26C?*31\r\n";
char *outs3 = "#F2FE.2=1*67\r\n";
char *outs4 = "#F33.4=1*50\r\n";
compass.WriteToPort(outs1);
Sleep(1000);
printf("# Start compass calibration...\n");
compass.mode = 1;
int temp;
do
{
compass.WriteToPort(outs2);
Sleep(1000);
compass.GetCurrentData(data1);
temp = 0;
printf("# data:%c%c%c\n", data1.szHeadStatus, data1.szPitchStatus, data1.szRollStatus);
if(isdigit(data1.szRollStatus) && isdigit(data1.szPitchStatus) && isdigit(data1.szRollStatus))
temp = 100 * (data1.szHeadStatus - '0') + 10 * (data1.szPitchStatus - '0') + (data1.szRollStatus - '0');
else if(!isdigit(data1.szRollStatus))
temp = 10 * (data1.szHeadStatus - '0') + (data1.szPitchStatus - '0');
else if(!isdigit(data1.szPitchStatus))
temp = data1.szHeadStatus - '0';
printf("%d\n", temp);
}while(temp != 275);
printf("# Calibration Finish, press any key to continue...\n\n");
_getch();
compass.WriteToPort(outs3);
compass.WriteToPort(outs4);
compass.mode = 0;
*/
}
else if(strcmp(dev[i], devlat) == 0)
{
if (!ls.Init(2))
{
cout << "ls.Init fail.\n";
return S_FALSE;
}
}
else if(strcmp(dev[i], devbird) == 0)
{
hyzmouse->SetHmdMouse("COM1", 9600, 0, 0);
}
return S_OK;
}
STDMETHODIMP CFirstATL::SensorRead(float SensorPos[], float SensorOrient[], BOOL *result)
{
/////////////////// GPS CODE /////////////////////////
int i = 0;
if(strcmp(dev[i], devgps) == 0)
{
cout << "# port opened, press any key to display data...\n";
_getch();
gps.GetCurrentData(data);
fLatitude = data.fLatitude / 100.0;
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 %.5f\t\n", fLatitude);
printf("\tLongitude:\t %.5f\t\n", fLongitude);
printf("\tAltitude:\t %.5f\t\n", data.fAltitude);
printf("\tUTM EAST:\t %.2f\n", east);
printf("\tUTM NORTH:\t %.2f\n", north);
fprintf(fq1, "# GPS data:\n");
fprintf(fq1, "\tTime:\t%d : %d : %d\n", data.nHours, data.nMinutes, data.nSeconds);
fprintf(fq1, "\tLagitude:\t %.5f\t\n", fLatitude);
fprintf(fq1, "\tLongitude:\t %.5f\t\n", fLongitude);
fprintf(fq1, "\tAltitude:\t %.5f\t\n", data.fAltitude);
fprintf(fq1, "\tUTM EAST:\t %.2f\n", east);
fprintf(fq1, "\tUTM NORTH:\t %.2f\n", north);
SensorPos[0] = (float)east;
SensorPos[1] = (float)north;
SensorPos[2] = data.fAltitude;
Sleep(1000);
}
////////////////// COMPASS CODE //////////////////////////////////
else if(strcmp(dev[i], devcom) == 0)
{
cout << "# port opened, press any key to display data...\n";
_getch();
HPRDATA data = {0};
compass.GetCurrentData(data);
cout << "# Sensor 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";
fprintf(fq2, "# Sensor data:\n");
fprintf(fq2, "\tHeading: %f\t[%c]\n", data.fHeading, data.szHeadStatus);
fprintf(fq2, "\tPitch:\t %f\t[%c]\n", data.fPitch, data.szPitchStatus);
fprintf(fq2, "\tRoll:\t %f\t[%c]\n", data.fRoll, data.szRollStatus);
SensorOrient[0] = data.fHeading;
SensorOrient[1] = data.fPitch;
SensorOrient[2] = data.fRoll;
}
////////////////////////////// BIRD CODE////////////////////////////
else if(strcmp(dev[i], devbird) == 0)
{
float startx, startz;
cout << "# port opened, press any key to display data...\n";
_getch();
if(other[i])
hyzmouse->bird_hemisphere();
FILE *fp = fopen("birdin.txt", "r");
fscanf(fp, "%f %f", &startx, &startz);
float birdPos[3];
float birdOrient[3];
short button[6];
short birdaddr;
birdaddr=hyzmouse->Get3DmouseAddress();
hyzmouse->GetSingleBirdData(POSANGLE,0,birdPos,birdOrient,button,birdaddr);
birdPos[0] *= InchtoM;
birdPos[1] *= InchtoM;
birdPos[2] *= InchtoM;
SensorOrient[0] = birdOrient[0];
SensorOrient[1] = birdOrient[1];
SensorOrient[2] = birdOrient[2];
SensorPos[0] = birdPos[1] + startx;
SensorPos[1] = birdPos[2];
SensorPos[2] = birdPos[0] + startz;
cout << "# Bird data:\n";
cout << "\tX:\t\t " << birdPos[0] << "\t" << "\n";
cout << "\tY:\t\t " << birdPos[1] << "\t" << "\n";
cout << "\tZ:\t\t " << birdPos[2] << "\t" << "\n";
cout << "\tHeading:\t " << birdOrient[0] << "\t" << "\n";
cout << "\tPitch:\t\t " << birdOrient[1] << "\t" << "\n";
cout << "\tRoll:\t\t " << birdOrient[2] << "\t" << "\n";
fprintf(fq3, "# Bird data:\n");
fprintf(fq3, "\tX:\t\t %f\n", SensorPos[0]);
fprintf(fq3, "\tY:\t\t %f\n", SensorPos[1]);
fprintf(fq3, "\tZ:\t\t %f\n", SensorPos[2]);
fprintf(fq3, "\tHeading:\t %f\n", SensorOrient[0]);
fprintf(fq3, "\tPitch:\t\t %f\n", SensorOrient[1]);
fprintf(fq3, "\tRoll:\t\t %f\n", SensorOrient[2]);
//Sleep(1000);
}
else if(strcmp(dev[i], devlat) == 0)
{
if(num == 0)
Sleep(2000);
ls.GetFrame(tx, ty, tz, rz, ry, rx);
SensorPos[0] = tx;
SensorPos[1] = ty;
SensorPos[2] = tz;
SensorOrient[0] = rz;
SensorOrient[1] = ry;
SensorOrient[2] = rx;
num++;
}
return S_OK;
}
STDMETHODIMP CFirstATL::Close(BOOL *result)
{
int i = 0;
if(strcmp(dev[i], devgps) == 0)
{
gps.ClosePort();
fclose(fq1);
}// TODO: Add your implementation code here
else if(strcmp(dev[i], devcom) == 0)
{
compass.ClosePort();
fclose(fq2);
}
else if(strcmp(dev[i], devbird) == 0)
fclose(fq3);
else if(strcmp(dev[i], devlat) == 0)
ls.Clear();
return S_OK;
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -