mc_gpx.cpp

GPS Source code with C++. It is usefull for introducing gps

/*
  Name:         mc_gpx.cpp
  Copyright:    Keins
  Author:       Thomas Wiens
  Date: 19.02.07 18:04
  Description:  Es werden die gespeicherten GPS-Punkte aus dem Controller ausgelesen
                und als Trackpunkte im GPX-Format abgespeichert.
*/
/* Verwenden der seriellen Schnittstelle
   MSDN:
   http://msdn.microsoft.com/library/default.asp?url=/library/en-us/dnfiles/html/msdn_serial.asp
 
   Klasse zur Verwendung der seriellen Schnittstelle
   http://www.winapi.net/index.php?inhalt=t3
*/
#include "mc_gpx.h"
#define DEBUG

using namespace std;

ofstream Ausgabe;
CSerial com;

const char GpxHeader[] =  "<?xml version=\"1.0\"?>\n"
                    "<gpx version=\"1.0\"\n"
                    " creator=\"GPS-Logger by T.Wiens\"\n"
                    " xmlns:xsi=\"http://www.w3.org/2001/XMLSchema-instance\"\n"
                    " xmlns=\"http://www.topografix.com/GPX/1/0\"\n"
                    " xsi:schemaLocation=\"http://www.topografix.com/GPX/1/0"
                    " http://www.topografix.com/GPX/1/0/gpx.xsd\">\n";
                    
// *****************************************************************************
// Funktion:
//                       get_data_from_logger
// 躡ergabeparameter:
//                       int com_port
//                                   Nummer des zu 鰂fnenden seriellen Ports
//                       char trackname
//                                   Bezeichnung der Tracks (max. 50 Zeichen)
//                       char track_description            
//                                   Beschreibung des Tracks (max. 50 Zeichen)
//                       char waypoint_names
//                                   Bezeichnung der Wegpunkte (falls vorhanden, werden durchnummeriert)
//  R點kgabewert:
//                       0 -> kein Fehler
//                       >0 -> Fehlercode siehe Headerfile                                                                                                                                                            
// ***************************************************************************** 
int get_data_from_logger(int com_port, char filename[50],char trackname[50], char track_description[50], char waypoint_names[50]) {
    tGPXtrackPoint gpx;
    const char trkpt_request[] = "$TREQ*\r\n";
    const char waypt_request[] = "$WREQ*\r\n";
    char buf[61], temp [20];
    int datasets;
        
    // *** Schnittstelle 鰂fnen **
    if(!com.Open(com_port, 4800, 8, NOPARITY, ONESTOPBIT)) {
        cout << "Fehler: Schnittstelle konnte nicht geoeffnet werden\n";
        return GPX_ERR_SERIAL_OPEN;
    }
    // *** Datei 鰂fnen ***
    Ausgabe.open(filename, ios_base::out);
    if (!Ausgabe.good()){
        cout << "Fehler: Datei " << filename << " konnte nicht zum Schreiben geoeffnet werden." << endl;
        return GPS_ERR_FILE_OPEN;
    }
    // GPX-Header schreiben
    Ausgabe << GpxHeader;
    Ausgabe << "<trk>\n";
    Ausgabe << "<name>" << trackname << "</name>\n";
    Ausgabe << "<desc>" << track_description << "</desc>\n";
    Ausgabe << "<trkseg>\n";
    
    // Anfragen wie viele Datens鋞ze aufgezeichnet wurden
    com.SendData(trkpt_request, sizeof(trkpt_request));
    // Antwort holen
    // Antwort in Form $TCNT,12*<CR><NL>       
    if(com.ReadData(buf, 20)) {
        extract_data(buf, "$TCNT,", "*\r\n");
        datasets = atoi(buf);
        cout << "Es sind "  << datasets << " Datensaetze vorhanden" << endl;
    }
    else {
        cout << "Fehler: Controller auf Anfrage der Anzahl Datensaetze falsch geantwortet" << endl;
        com.Close();
        return GPS_ERR_CONTR_COM;
    }    
    
    // Trackpoint Datens鋞ze abholen
    for (int i = 0; i < datasets; i++) {
        if (!get_gps_trkPoint(i, buf)) { 
            get_gpx_data(buf, &gpx);  
            add_gpx_trkPoint_to_file(&gpx);  
        }        
        else {
            com.Close();   
            return GPS_ERR_CONTR_COM;
        }
        
    } 
    // Ende des Track-Segmentes
    Ausgabe << "</trkseg>\n";
    Ausgabe << "</trk>\n";
    
    // Anfragen wie viele Waypoints aufgezeichnet wurden
    com.SendData(waypt_request, sizeof(waypt_request));
    if(com.ReadData(buf, 20)) {
        extract_data(buf, "$WCNT,", "*\r\n");
        datasets = atoi(buf);
        cout << "Es sind "  << datasets << " Wegpunkte vorhanden" << endl;
    }
    else {
        cout << "Fehler: Controller auf Anfrage der Anzahl Datensaetze falsch geantwortet" << endl;
        com.Close();
        return GPS_ERR_CONTR_COM;
    }  
    if (datasets > 0) {
        // Waypoint Datens鋞ze abholen
        for (int i = 0; i < datasets; i++) {   
            if (!get_gps_trkPoint(get_waypoint(i), buf)) { 
                get_gpx_data(buf, &gpx);  
                sprintf(temp, "%s %d", waypoint_names, i + 1);
                add_gpx_wayPoint_to_file(&gpx, temp);  
            }
            else {
                com.Close();                           
                return GPS_ERR_CONTR_COM;
            }
        }       
    }
    // Ende des GPX-Segmentes
    Ausgabe << "</gpx>\n";
    Ausgabe.close();
    com.Close();    
    return 0;
}

// *****************************************************************************
// Trackpoint abholen
// ***************************************************************************** 
int get_gps_trkPoint(int trkpt_number, char *line) {
    char buf[61], str[20];
    int ret_chars;
    // Anfragestring basteln
    strcpy(str, "$TSET,");
    sprintf(buf, "%i", trkpt_number);
    strcat(str, buf);
    strcat(str, "*\r\n");   
    // Anfrage senden
    com.SendData(str, strlen(str));
    ret_chars = com.ReadData(buf, TRKPT_DATASET_LENGTH);        
    // Antwort auswerten
    if (ret_chars) {
        extract_data(buf, "$", "*\r\n");
        if (strlen(buf) == TRKPT_DATASET_LENGTH - 4) {                             
            #ifdef DEBUG
            cout << "Datensatz Nr. " << trkpt_number + 1 << ": " << buf << endl;   
            #endif    
            strcpy(line, buf);              
        }
        else {
            #ifdef DEBUG 
            cout << "Fehlerhafte Datensatzlaenge!" << endl;
            #endif
            return GPS_ERR_CONTR_COM;
        }                     
    }
    else {
        #ifdef DEBUG 
        cout << "Fehler: Controller hat auf Datensatzanfrage nicht oder falsch geantwortet" << endl;
        #endif
        return GPS_ERR_CONTR_COM;    
    }
    return 0; 
}

// ***************************************************************************** 
// Wapoint abholen
// ***************************************************************************** 
int get_waypoint(int point_number) {
    char buf[61], str[20];  
    // Anfragestring basteln
    strcpy(str, "$WSET,");
    sprintf(buf, "%i", point_number);
    strcat(str, buf);
    strcat(str, "*\r\n");   
    // Anfrage senden
    com.SendData(str, strlen(str));     
    // Antwort auswerten
    if (com.ReadData(buf, WAYPT_DATASET_LENGTH)) {
        extract_data(buf, "$", "*\r\n");
        if (strlen(buf) == WAYPT_DATASET_LENGTH - 4) {                             
            #ifdef DEBUG
            cout << "Datensatz Nr. " << point_number + 1 << ": " << buf << endl;   
            #endif
            buf[5] = '\0';    
            return (atoi(buf));                   
        }
        else {
            #ifdef DEBUG 
            cout << "Fehlerhafte Datensatzlaenge!" << endl;
            #endif
            return GPS_ERR_CONTR_COM;
        }                     
    }
    else {
        #ifdef DEBUG 
        cout << "Fehler: Controller hat auf Datensatzanfrage nicht oder falsch geantwortet" << endl;
        #endif
        return GPS_ERR_CONTR_COM;    
    }     
}

// *****************************************************************************       
// Es wird ein Zeiger auf einen String 黚ergeben. Bei Auftreten von
// begin und end werden diese Zeichenketten entfernt.
// ***************************************************************************** 
int extract_data(char *ext_string, char begin[10], char end[10]) {
    char *ptr_anf;
    char *ptr_end;
    char buf[100];
    char temp[100];    
    strncpy(buf, ext_string, 99);    
    ptr_anf = strstr(buf, begin);
    ptr_end = strstr(buf, end);       
    if (ptr_anf && ptr_end) {
        *ptr_end = '\0';
        strcpy(temp, ptr_anf + strlen(begin)); 
        strcpy(ext_string, temp);
        return 0;
    }
    else {
        return 1;       // Zeichenketten nicht gefunden
    }
}

// *****************************************************************************  
// Umwandeln der Daten in das GPX Format
// *****************************************************************************  
void get_gpx_data(char *line, tGPXtrackPoint *gpx) {
    double grad, minutes, dezimal_lat, dezimal_lon;
    char temp[20];
    int altitude, i;   
    
    // Zeitstempel bauen
    // 0000160806 0000194137 0052153447 0008031685 0079NE
    // 0123456789 0123456789 0123456789 0123456789 012345    
    strcpy(gpx->datetime, "20");                                                 // Im 20.Jahrhundert
    gpx->datetime[2] = line[8];
    gpx->datetime[3] = line[9];
    gpx->datetime[4] = '-';
    gpx->datetime[5] = line[6];
    gpx->datetime[6] = line[7];
    gpx->datetime[7] = '-';
    gpx->datetime[8] = line[4];
    gpx->datetime[9] = line[5];
    gpx->datetime[10] = 'T';
    gpx->datetime[11] = line[14];
    gpx->datetime[12] = line[15];
    gpx->datetime[13] = ':';
    gpx->datetime[14] = line[16];
    gpx->datetime[15] = line[17];
    gpx->datetime[16] = ':';
    gpx->datetime[17] = line[18];
    gpx->datetime[18] = line[19];
    gpx->datetime[19] = 'Z';
    gpx->datetime[20] = '\0';
    #ifdef DEBUG
    cout << "datetime: " << gpx->datetime << endl;   
    #endif
    // *** Gradzahlen kopieren ***    
    // Breitengrad 0052153407
    for (i = 0; i < 4; i++) {
        temp[i] = line[i + 20];
    }
    temp[4] = '\0';
    grad = atof(temp); 
    #ifdef DEBUG   
    cout << "Breitenrad als Text: " << temp << endl;
    #endif
    temp[0] = line[24];
    temp[1] = line[25];
    temp[2] = '.';
    for (i = 0; i < 5; i++) {
        temp[i + 3] = line[i + 26];
    }   
    temp[7] = '\0';
    #ifdef DEBUG   
    cout << "Minuten als Text: " << temp << endl;
    #endif
    minutes = atof(temp);    
    dezimal_lat = grad + minutes / 60;   
    // S黡licher Breitengrad ist negativ
    if (line[44] == 'S')
        dezimal_lat = -dezimal_lat;
    #ifdef DEBUG
    cout << "Grad: " << grad << " Minuten: " << minutes << " Breitengrad dezimal: " << dezimal_lat << endl;
    #endif
    sprintf(gpx->latitude, "%.6f", dezimal_lat);
    #ifdef DEBUG
    cout << "Text: " << gpx->latitude << endl;
    #endif
    // L鋘gengrad
    for (i = 0; i < 4; i++) {
        temp[i] = line[i + 30];
    }
    temp[4] = '\0';
    grad = atof(temp);    
    temp[0] = line[34];
    temp[1] = line[35];
    temp[2] = '.';
    for (i = 0; i < 4; i++) {
        temp[i + 3] = line[i + 36];
    }   
    temp[7] = '\0';
    minutes = atof(temp);    
    dezimal_lon = grad + minutes / 60;   
    // Westlicher L鋘gengrad ist negativ
    if (line[45] == 'W')
        dezimal_lat = -dezimal_lon;
    #ifdef DEBUG
    cout << "Grad: " << grad << " Minuten: " << minutes << " Laengengrad dezimal: " << dezimal_lon << endl;
    #endif
    sprintf(gpx->longitude, "%.6f", dezimal_lon);
    #ifdef DEBUG
    cout << "Text: " << gpx->longitude << endl;    
    #endif    
    // H鰄enangabe
    temp[0] = line[40];
    temp[1] = line[41];
    temp[2] = line[42];
    temp[3] = line[43];
    temp[4] = '\0';
    altitude = atoi(temp);
    #ifdef DEBUG
    cout << "Hoehe: " << altitude << endl;
    #endif
    sprintf(gpx->altitude, "%d", altitude);
    #ifdef DEBUG
    cout << "Text: " << gpx->altitude << endl;    
    #endif  
}

void add_gpx_trkPoint_to_file(tGPXtrackPoint *gpx) {
    Ausgabe << "<trkpt lat=\"" << gpx->latitude << "\" ";
    Ausgabe << "lon=\"" << gpx->longitude << "\">\n";
    Ausgabe << "<ele>" << gpx->altitude << "</ele>\n";
    Ausgabe << "<time>" << gpx->datetime << "</time>\n";
    Ausgabe << "</trkpt>\n";
}

void add_gpx_wayPoint_to_file(tGPXtrackPoint *gpx, char *pointname) {
    Ausgabe << "<wpt lat=\"" << gpx->latitude << "\" ";
    Ausgabe << "lon=\"" << gpx->longitude << "\">\n";
    Ausgabe << "<name>" << pointname << "</name>\n";
    Ausgabe << "<type>Waypoint</type>\n";
    Ausgabe << "</wpt>\n";
}