From f6695a86749d8ff174ee218068f9d33093f8b1ba Mon Sep 17 00:00:00 2001 From: faragher Date: Mon, 21 Oct 2024 17:55:58 -0500 Subject: [PATCH] Raw GPS.h Upload --- GPS.h | 493 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 493 insertions(+) create mode 100644 GPS.h diff --git a/GPS.h b/GPS.h new file mode 100644 index 0000000..084f1fb --- /dev/null +++ b/GPS.h @@ -0,0 +1,493 @@ +// Copyright (C) 2024, Michael Faragher +// Extended from the RNode Firmware. Copyright (C) 2023, Mark Qvist +// +// This program is free software: you can redistribute it and/or modify +// it under the terms of the GNU General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// (at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. +// +// You should have received a copy of the GNU General Public License +// along with this program. If not, see . + +//#include + + +char GPS_Buffer[100]; +byte GPS_Index; +char GPSGGA[100] = " "; +char GPSGLL[100] = " "; +char GPSVTG[100] = " "; +char GPSRMC[100] = " "; +char GPSTime[6] = {'9','0','0','0','0','0'}; +bool isGPS_Automated = false; +bool isGPS_Waiting_Time = false; +bool isGPS_Waiting_Location = false; +bool isGPS_Waiting_NMEA = false; +char GPS_Lat[8] = {'N','O',' ',' ','F','I','X','X'}; +char GPS_Lon[9] = {'N','O',' ',' ',' ','F','I','X','X'}; + +#define GPS_CMD_TIME 0x00 +#define GPS_CMD_LOCATION 0x01 +#define GPS_CMD_RATE 0x02 +#define GPS_CMD_SENTENCE 0x03 +#define GPS_CMD_ENABLE_HEARTBEAT 0x10 +#define GPS_CMD_DISABLE_HEARTBEAT 0x11 +#define GPS_CMD_ENABLE_AUTOMATIC 0x12 +#define GPS_CMD_DISABLE_AUTOMATIC 0x13 +#define GPS_CMD_HOT 0x20 +#define GPS_CMD_COLD 0x21 + + +byte NMEA_checksum(char message[], byte length){ + byte buffer = 0; + byte working; + for(byte i = 0; i < length; i++){ + working = message[int(i)];// Apparently not & 127; //Lower seven bits + buffer = buffer ^ working; //XORed + } + return buffer; +} + +void GPS_poling(byte Target, byte Rate){ //Divisor, 0 = off, 1-?, 0-9 in this implementation + //Targets: + // 0 GGA + // 1 GLL + // 2 GSA + // 3 GSV + // 4 RMC + // 5 VTG + char cRate[10] = {'0','1','2','3','4','5','6','7','8','9'}; + //char cTarget[6] = {'0','1','2','3','4','5'}; + char cTargetA[6] = {'G','G','G','G','R','V'}; + char cTargetB[6] = {'G','L','S','S','M','T'}; + char cTargetC[6] = {'A','L','A','V','C','G'}; + char message[24] = "PUBX,40,GGA,0,0,0,0,0,0"; + if(Rate>9){Rate=9;} + if(Rate<0){Rate=0;} + //itoa(Rate, cRate,10); + message[14]=cRate[Rate]; + message[8] = cTargetA[int(Target)]; + message[9] = cTargetB[int(Target)]; + message[10] = cTargetC[int(Target)]; + byte checksum = NMEA_checksum(message, 19); + //char hexlookup[16] = {'0','1','2','3','4','5','6','7','8','9','A','B','C','D','E','F'}; + //char checksumA = hexlookup[int(checksum ^ 16)]; + //char checksumB = hexlookup[int((checksum>>4)^16)]; + + Serial2.write('$'); + for(int i = 0; i < 19; i++){ + Serial2.write(message[i]); + } + + Serial2.write('*'); + Serial2.print(checksum,HEX); + Serial2.write(0x0d); + Serial2.write(0x0a); + +//// Dump outgoing commands to serial console +// Serial.write('$'); +// for(int i = 0; i < 24; i++){ +// Serial.write(message[i]); +// } +// +// Serial.write('*'); +// Serial.print(checksum,HEX); +// Serial.write(0x0d); +// Serial.write(0x0a); + +} + +void GPS_request(byte Target){ + //Targets: + // 0 GGA + // 1 GLL + // 2 GSA + // 3 GSV + // 4 RMC + // 5 VTG + if(Target>5){return;} + char cTargetA[6] = {'G','G','G','G','R','V'}; + char cTargetB[6] = {'G','L','S','S','M','T'}; + char cTargetC[6] = {'A','L','A','V','C','G'}; + char message[10] = "EIGPQ,GGA"; + message[6] = cTargetA[int(Target)]; + message[7] = cTargetB[int(Target)]; + message[8] = cTargetC[int(Target)]; + byte checksum = NMEA_checksum(message, 9); + + Serial2.write('$'); + for(int i = 0; i < 9; i++){ + Serial2.write(message[i]); + } + + Serial2.write('*'); + Serial2.print(checksum,HEX); + Serial2.write(0x0d); + Serial2.write(0x0a); + +} + +void GPS_Query_Time(){ + if(GPSTime[0]!='9'){ + serial_write(FEND); + serial_write(CMD_GPS_CMD); + serial_write(GPS_CMD_TIME); + for(int i = 0; i < 6; i++){ + serial_write(GPSTime[i]); + } + serial_write(FEND); + //debug + serial_write(char(13)); + serial_write(char(10)); + //eng debug + } + else{//if time not buffered + isGPS_Waiting_Time = true; + } +} + +void GPS_Query_Location(){ + if(GPS_Lon[0]=='0'||GPS_Lon[0]=='1'){ + serial_write(' '); + serial_write(GPS_Lat[0]); + serial_write(GPS_Lat[1]); + serial_write(' '); + serial_write(GPS_Lat[2]); + serial_write(GPS_Lat[3]); + serial_write('.'); + serial_write(GPS_Lat[4]); + serial_write(GPS_Lat[5]); + serial_write(GPS_Lat[6]); + serial_write('\''); + serial_write(' '); + serial_write(GPS_Lat[7]); + serial_write(char(13)); + serial_write(char(10)); + + serial_write(GPS_Lon[0]); + serial_write(GPS_Lon[1]); + serial_write(GPS_Lon[2]); + serial_write(' '); + serial_write(GPS_Lon[3]); + serial_write(GPS_Lon[4]); + serial_write('.'); + serial_write(GPS_Lon[5]); + serial_write(GPS_Lon[6]); + serial_write(GPS_Lon[7]); + serial_write('\''); + serial_write(' '); + serial_write(GPS_Lon[8]); + serial_write(char(13)); + serial_write(char(10)); + } + else{ + isGPS_Waiting_Location = true; + } +} + + +void GPS_Parse_Time(){ + if(GPSGGA[8]==','){return;} + for(int i = 0; i < 6; i++){ + GPSTime[i] = GPSGGA[8+i]; + } + //debug + //Serial.print("Pos 8: "); + //Serial.println(GPSGGA[8]); + //Serial.println(GPSTime); +} + +void GPS_Parse_Location(){ + if(GPSGGA[18]!=','){ + if(GPSGGA[31]=='0'||GPSGGA[31]=='1'){ + GPS_Lat[0] = GPSGGA[18]; + GPS_Lat[1] = GPSGGA[19]; + GPS_Lat[2] = GPSGGA[20]; + GPS_Lat[3] = GPSGGA[21]; + GPS_Lat[4] = GPSGGA[23]; + GPS_Lat[5] = GPSGGA[24]; + GPS_Lat[6] = GPSGGA[25]; + GPS_Lat[7] = GPSGGA[29]; + + GPS_Lon[0] = GPSGGA[31]; + GPS_Lon[1] = GPSGGA[32]; + GPS_Lon[2] = GPSGGA[33]; + GPS_Lon[3] = GPSGGA[34]; + GPS_Lon[4] = GPSGGA[35]; + GPS_Lon[5] = GPSGGA[37]; + GPS_Lon[6] = GPSGGA[38]; + GPS_Lon[7] = GPSGGA[39]; + GPS_Lon[8] = GPSGGA[43]; + } + } + + //debug + +// Serial.print("Locations: "); +// Serial.print(GPSGGA[18]); //4 +// Serial.print(GPSGGA[19]); //3 +// Serial.print(GPSGGA[20]); //2 +// Serial.print(GPSGGA[21]); //5 +// Serial.print(GPSGGA[22]); //. +// Serial.print(GPSGGA[23]); //2 +// Serial.print(GPSGGA[24]); //7 +// Serial.print(GPSGGA[25]); //0 +// Serial.print(GPSGGA[26]); //0 +// Serial.print(GPSGGA[27]); //1 +// Serial.println(GPSGGA[29]); //N +// +// Serial.print(GPSGGA[31]); //0 +// Serial.print(GPSGGA[32]); //8 +// Serial.print(GPSGGA[33]); //8 +// Serial.print(GPSGGA[34]); //1 +// Serial.print(GPSGGA[35]); //2 +// Serial.print(GPSGGA[36]); //. +// Serial.print(GPSGGA[37]); //2 +// Serial.print(GPSGGA[38]); //5 +// Serial.print(GPSGGA[39]); //0 +// Serial.print(GPSGGA[40]); //2 +// Serial.print(GPSGGA[41]); //2 +// Serial.println(GPSGGA[43]); //W + + + +} + +void GPS_init(){ + Serial2.begin(GPS_BAUD_RATE, SERIAL_8N1, PIN_GPS_RX, PIN_GPS_TX); + //Serial.println("Serial2 is up"); + GPS_poling(0,0); + GPS_poling(1,0); + GPS_poling(2,0); + GPS_poling(3,0); + GPS_poling(4,0); + GPS_poling(5,0); + + //debug +// isGPS_Automated = true; +// GPS_poling(0,1); +// GPS_request(0); +} + +void GPS_sweep(){ + char coi[1]; + Serial2.readBytes(coi,1); + if(coi[0] == char(13)){ + + if(GPS_Buffer[4]=='G'&&GPS_Buffer[5]=='G'&&GPS_Buffer[6]=='A'){ + memset(GPSGGA,0xFF,100); + strncpy(GPSGGA,GPS_Buffer,GPS_Index); + GPS_Parse_Time(); + GPS_Parse_Location(); + if(isGPS_Waiting_Time){ + isGPS_Waiting_Time = false; + GPS_Query_Time(); + } + if(isGPS_Waiting_Location){ + isGPS_Waiting_Location = false; + GPS_Query_Location(); + } + } + if(GPS_Buffer[4]=='G'&&GPS_Buffer[5]=='L'&&GPS_Buffer[6]=='L'){ + memset(GPSGLL,0xFF,100); + strncpy(GPSGLL,GPS_Buffer,GPS_Index); + } + if(GPS_Buffer[4]=='V'&&GPS_Buffer[5]=='T'&&GPS_Buffer[6]=='G'){ + memset(GPSVTG,0xFF,100); + strncpy(GPSVTG,GPS_Buffer,GPS_Index); + } + if(GPS_Buffer[4]=='R'&&GPS_Buffer[5]=='M'&&GPS_Buffer[6]=='C'){ + memset(GPSRMC,0xFF,100); + strncpy(GPSRMC,GPS_Buffer,GPS_Index); + } + GPS_Index = 0; + memset(GPS_Buffer,0xFF,100); + } + else { + GPS_Buffer[GPS_Index] = coi[0]; + GPS_Index++; + } + + if(GPS_Index > 98){ + //Serial.println("ERROR"); + //Serial.println("Buffer Overflow: GPS"); + GPS_Index=0; + memset(GPS_Buffer,0xFF,100); + } + +} + + + + + + + +void GPS_SetAutomatic(bool state) +{ + isGPS_Automated = state; +} + +// Updates all useful buffers at full rate +void GPS_Go_Hot(){ + GPS_poling(0,1); + GPS_poling(1,1); + GPS_poling(2,0); + GPS_poling(3,0); + GPS_poling(4,1); + GPS_poling(5,1); +} + +// Shuts down automated buffering +void GPS_Go_Cold(){ + GPS_poling(0,0); + GPS_poling(1,0); + GPS_poling(2,0); + GPS_poling(3,0); + GPS_poling(4,0); + GPS_poling(5,0); +} + +void GPS_Enable_Heartbeat(bool state){ + if(state){ + GPS_poling(0,1); + GPS_SetAutomatic(true); + } + else + { + GPS_poling(0,0); + GPS_SetAutomatic(false); + } + +} + +void GPS_send(){ + if(GPSGGA[1]=='$'){ + byte i = 0; + char coi; + serial_write(FEND); + serial_write(CMD_GPS_CMD); + serial_write(GPS_CMD_SENTENCE); + while(coi != 0xFF){ + coi = GPSGGA[i]; + if(coi != 0xFF){serial_write(coi);} + i++; + if(i>99){break;} + } + serial_write(FEND); +// Serial.println(""); +// GPSGGA[0]=0; + memset(GPSGGA,0xFF,100); + } + + if(GPSGLL[1]=='$'){ + byte i = 0; + char coi; + serial_write(FEND); + serial_write(CMD_GPS_CMD); + serial_write(GPS_CMD_SENTENCE); + while(coi != 0xFF){ + coi = GPSGLL[i]; + if(coi != 0xFF){serial_write(coi);} + i++; + if(i>99){break;} + } + serial_write(FEND); +// GPSGLL[0]=0; + memset(GPSGLL,0xFF,100); + } + + if(GPSVTG[1]=='$'){ + byte i = 0; + char coi; + serial_write(FEND); + serial_write(CMD_GPS_CMD); + serial_write(GPS_CMD_SENTENCE); + while(coi != 0xFF){ + coi = GPSVTG[i]; + if(coi != 0xFF){serial_write(coi);} + i++; + if(i>99){break;} + } + serial_write(FEND); +// GPSVTG[0]=0; + memset(GPSVTG,0xFF,100); + } + + if(GPSRMC[1]=='$'){ + byte i = 0; + char coi; + serial_write(FEND); + serial_write(CMD_GPS_CMD); + serial_write(GPS_CMD_SENTENCE); + while(coi != 0xFF){ + coi = GPSRMC[i]; + if(coi != 0xFF){serial_write(coi);} + i++; + if(i>99){break;} + } + serial_write(FEND); +// GPSRMC[0]=0; + memset(GPSRMC,0xFF,100); + } +} + +void kiss_GPS_send(){ + GPS_send(); +} + +void kiss_GPS_location(){ + +} + +void kiss_GPS_time(){ + +} + +void GPS_Query_NMEA(){ + GPS_send(); +} + + +void GPS_process(){ + GPS_sweep(); + if(isGPS_Automated || isGPS_Waiting_NMEA){ + GPS_send(); + } +} + +void GPS_handler(byte CMD){ + if(CMD==GPS_CMD_TIME){ + GPS_Query_Time(); + } + if(CMD== GPS_CMD_LOCATION){ + GPS_Query_Location(); + } + if(CMD==GPS_CMD_SENTENCE){ + GPS_Query_NMEA(); + } + if(CMD==GPS_CMD_ENABLE_HEARTBEAT){ + GPS_Enable_Heartbeat(true); + } + if(CMD==GPS_CMD_DISABLE_HEARTBEAT){ + GPS_Enable_Heartbeat(false); + } + if(CMD==GPS_CMD_HOT){ + GPS_Go_Hot(); + } + if(CMD==GPS_CMD_COLD){ + GPS_Go_Cold(); + } + if(CMD==GPS_CMD_ENABLE_AUTOMATIC){ + GPS_SetAutomatic(true); + } + if(CMD==GPS_CMD_DISABLE_AUTOMATIC){ + GPS_SetAutomatic(false); + } + +} \ No newline at end of file