/******************************************************************************    File Name:    include/GPS.h    Description:  GPS data definition and its transformation                  transformation uses 2-axis ellipsoid approximation******************************************************************************//******************************************************************************    Author:       alfred.liushu@gmail.com	    Upadate:      2008/09/16  File founded    Copyright 2008-2009 Robot Lab., Dept.of Automation, Tsinghua University******************************************************************************/#ifndef __GPS_H__#define __GPS_H__#include <math.h>#include "../include/Setting.h"#include "../include/Coordinate.h"/******************************                   Data structures                           ***************************************************//*Raw data structure*/typedef struct{    DATATYPE        longitude;                                          /*Longitude in degrees*/    DATATYPE        latitude;                                           /*Latitude in degrees*/}GPS;/*Use COORD as formatted data structure*//******************************                   Synchronition parameters                  ***************************************************/const TIME GPSPeriod = 1000;                                            /*GPS period time, measured in milliseconds*//******************************                   Geographic constants                      ***************************************************/const DATATYPE DegToRad = Pi/180;                                       /*Transform angle measured by degrees to radians*/const DATATYPE RadToDeg = 180/Pi;                                       /*Transform angle measured by radians to degrees*/const DATATYPE EarthRadiusAvg = DATATYPE(6371004);                      /*Average radius of the earth*/const DATATYPE EarthRadiusEquator = DATATYPE(6378140);                  /*Equator radius of the earth*/const DATATYPE EarthRadiusPole = DATATYPE(6356755);                     /*Pole radius of the earth*//******************************                   Transformation function definitions       ***************************************************/inline void GPS2Coord(const GPS& gps, const GPS& gpsCenter,                      COORD& coord);                                    /*Transform GPS data gps to coordinate coord, refer to gpsCenter*/inline void Coord2GPS(const COORD& coord, const GPS& gpsCenter,                      GPS& gps);                                        /*Transform coordinate coord to GPS data gps, refer to gpsCenter*//******************************                   Transformation function implementations   ***************************************************/void GPS2Coord(const GPS& gps, const GPS& gpsCenter,               COORD& coord){    coord.east = (gps.longitude - gpsCenter.longitude) * DegToRad * EarthRadiusEquator * DATATYPE(cos(gps.latitude*DegToRad));    coord.north = (gps.latitude - gpsCenter.latitude) * DegToRad * EarthRadiusPole;}void Coord2GPS(const COORD& coord, const GPS& gpsCenter,               GPS& gps){    gps.latitude = coord.north/EarthRadiusPole*RadToDeg + gpsCenter.latitude;    gps.longitude = coord.east/EarthRadiusEquator*RadToDeg/DATATYPE(cos(gps.latitude*DegToRad)) + gpsCenter.longitude;}#endif /*__GPS_H__*/