Beispiel #1
0
bool GPS_NMEA_newFrame(char c) {
    uint8_t frameOK = 0;
    static uint8_t param = 0, offset = 0, parity = 0;
    static char string[15];
    static uint8_t checksum_param, frame = 0;
  
    if (c == '$') {
      param = 0; offset = 0; parity = 0;
    } else if (c == ',' || c == '*') {
      string[offset] = 0;
      if (param == 0) { //frame identification
        frame = 0;
        if (string[0] == 'G' && string[1] == 'N' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A') frame = FRAME_GGA;
        if (string[0] == 'G' && string[1] == 'N' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C') frame = FRAME_RMC;
      } else if (frame == FRAME_GGA) {
        if (param == 2) {
          GPS_Info.GPS_coord[LAT] = GPS_coord_to_degrees(string);
        }
        else if (param == 3 && string[0] == 'S') 
          GPS_Info.GPS_coord[LAT] = -GPS_Info.GPS_coord[LAT];
        else if (param == 4)                     
          {GPS_Info.GPS_coord[LON] = GPS_coord_to_degrees(string);}
        else if (param == 5 && string[0] == 'W') 
          GPS_Info.GPS_coord[LON] = -GPS_Info.GPS_coord[LON];
        else if (param == 6) {
          fgps.GPS_FIX = (string[0]  > '0');
          GPS_Info.GPS_Fixed = fgps.GPS_FIX;
        }
        else if (param == 7) {
          GPS_Info.GPS_numSat = grab_fields(string,0);
        }
        else if (param == 9) {
          GPS_Info.GPS_altitude = grab_fields(string,0);
        }  // altitude in meters added by Mis
      } else if (frame == FRAME_RMC) {
          if (param == 7) {
            GPS_Info.GPS_speed = ((uint32_t)grab_fields(string,1)*5144L)/1000L;}  //gps speed in cm/s will be used for navigation
          else if (param == 8) {
            GPS_Info.GPS_ground_course = grab_fields(string,1);
          }                 //ground course deg*10 
      }
      param++; offset = 0;
      if (c == '*') checksum_param=1;
      else parity ^= c;
    } else if (c == '\r' || c == '\n') {
      if (checksum_param) { //parity checksum
        uint8_t checksum = hex_c(string[0]);
        checksum <<= 4;
        checksum += hex_c(string[1]);
        if (checksum == parity) frameOK = 1;
      }
      checksum_param=0;
    } else {
       if (offset < 15) string[offset++] = c;
       if (!checksum_param) parity ^= c;
    }
    if (frame) GPS_Info.GPS_Present = 1;
    return frameOK && (frame==FRAME_GGA);
}
Beispiel #2
0
static bool GPS_NMEA_newFrame(char c)
{
	uint8_t frameOK = 0;
	static uint8_t param = 0, offset = 0, parity = 0;
	static char string[15];
	static uint8_t checksum_param, frame = 0;

	if (c == '$')
	{
		param = 0;
		offset = 0;
		parity = 0;
	}
	else if (c == ',' || c == '*')
	{
		string[offset] = 0;
		if (param == 0)
		{ //frame identification
			frame = 0;
			if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G'
					&& string[3] == 'G' && string[4] == 'A')
				frame = FRAME_GGA;
			if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R'
					&& string[3] == 'M' && string[4] == 'C')
				frame = FRAME_RMC;
		}
		else if (frame == FRAME_GGA)
		{
			if (param == 2)
			{
				gps.coord[LAT] = GPS_coord_to_degrees(string);
			}
			else if (param == 3 && string[0] == 'S')
				gps.coord[LAT] = -gps.coord[LAT];
			else if (param == 4)
			{
				gps.coord[LON] = GPS_coord_to_degrees(string);
			}
			else if (param == 5 && string[0] == 'W')
				gps.coord[LON] = -gps.coord[LON];
			else if (param == 6)
			{
				if (string[0] > '0')
					flagSet(FLAG_GPS_FIX);
				else
					flagClear(FLAG_GPS_FIX);
			}
			else if (param == 7)
			{
				gps.numSat = grab_fields(string, 0);
			}
			else if (param == 9)
			{
				gps.altitude = grab_fields(string, 0); // altitude in meters added by Mis
			}
		}
		else if (frame == FRAME_RMC)
		{
			if (param == 7)
			{
				gps.speed = (grab_fields(string, 1) * 5144L) / 1000L; // speed in cm/s added by Mis
			}
			else if (param == 8)
			{
				gps.ground_course = grab_fields(string, 1); // ground course deg * 10
			}
		}
		param++;
		offset = 0;
		if (c == '*')
			checksum_param = 1;
		else
			parity ^= c;
	}
	else if (c == '\r' || c == '\n')
	{
		if (checksum_param)
		{ // parity checksum
			uint8_t checksum = hex_c(string[0]);
			checksum <<= 4;
			checksum += hex_c(string[1]);
			if (checksum == parity)
				frameOK = 1;
		}
		checksum_param = 0;
	}
	else
	{
		if (offset < 15)
			string[offset++] = c;
		if (!checksum_param)
			parity ^= c;
	}
	return frameOK && (frame == FRAME_GGA);
}
Beispiel #3
0
static bool gpsNewFrameNMEA(char c)
{
    static gpsDataNmea_t gps_Msg;

    uint8_t frameOK = 0;
    static uint8_t param = 0, offset = 0, parity = 0;
    static char string[15];
    static uint8_t checksum_param, gps_frame = NO_FRAME;
    static uint8_t svMessageNum = 0;
    uint8_t svSatNum = 0, svPacketIdx = 0, svSatParam = 0;

    switch (c) {
        case '$':
            param = 0;
            offset = 0;
            parity = 0;
            break;
        case ',':
        case '*':
            string[offset] = 0;
            if (param == 0) {       //frame identification
                gps_frame = NO_FRAME;
                if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A')
                    gps_frame = FRAME_GGA;
                if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C')
                    gps_frame = FRAME_RMC;
                if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'S' && string[4] == 'V')
                    gps_frame = FRAME_GSV;
            }

            switch (gps_frame) {
                case FRAME_GGA:        //************* GPGGA FRAME parsing
                    switch(param) {
            //          case 1:             // Time information
            //              break;
                        case 2:
                            gps_Msg.latitude = GPS_coord_to_degrees(string);
                            break;
                        case 3:
                            if (string[0] == 'S')
                                gps_Msg.latitude *= -1;
                            break;
                        case 4:
                            gps_Msg.longitude = GPS_coord_to_degrees(string);
                            break;
                        case 5:
                            if (string[0] == 'W')
                                gps_Msg.longitude *= -1;
                            break;
                        case 6:
                            if (string[0] > '0') {
                                ENABLE_STATE(GPS_FIX);
                            } else {
                                DISABLE_STATE(GPS_FIX);
                            }
                            break;
                        case 7:
                            gps_Msg.numSat = grab_fields(string, 0);
                            break;
                        case 9:
                            gps_Msg.altitude = grab_fields(string, 0);     // altitude in meters added by Mis
                            break;
                    }
                    break;
                case FRAME_RMC:        //************* GPRMC FRAME parsing
                    switch(param) {
                        case 7:
                            gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L);    // speed in cm/s added by Mis
                            break;
                        case 8:
                            gps_Msg.ground_course = (grab_fields(string, 1));      // ground course deg * 10
                            break;
                    }
                    break;
                case FRAME_GSV:
                    switch(param) {
                      /*case 1:
                            // Total number of messages of this type in this cycle
                            break; */
                        case 2:
                            // Message number
                            svMessageNum = grab_fields(string, 0);
                            break;
                        case 3:
                            // Total number of SVs visible
                            GPS_numCh = grab_fields(string, 0);
                            break;
                    }
                    if(param < 4)
                        break;

                    svPacketIdx = (param - 4) / 4 + 1; // satellite number in packet, 1-4
                    svSatNum    = svPacketIdx + (4 * (svMessageNum - 1)); // global satellite number
                    svSatParam  = param - 3 - (4 * (svPacketIdx - 1)); // parameter number for satellite

                    if(svSatNum > GPS_SV_MAXSATS)
                        break;

                    switch(svSatParam) {
                        case 1:
                            // SV PRN number
                            GPS_svinfo_chn[svSatNum - 1]  = svSatNum;
                            GPS_svinfo_svid[svSatNum - 1] = grab_fields(string, 0);
                            break;
                      /*case 2:
                            // Elevation, in degrees, 90 maximum
                            break;
                        case 3:
                            // Azimuth, degrees from True North, 000 through 359
                            break; */
                        case 4:
                            // SNR, 00 through 99 dB (null when not tracking)
                            GPS_svinfo_cno[svSatNum - 1] = grab_fields(string, 0);
                            GPS_svinfo_quality[svSatNum - 1] = 0; // only used by ublox
                            break;
                    }

                    GPS_svInfoReceivedCount++;

                    break;
            }

            param++;
            offset = 0;
            if (c == '*')
                checksum_param = 1;
            else
                parity ^= c;
            break;
        case '\r':
        case '\n':
            if (checksum_param) {   //parity checksum
                shiftPacketLog();
                uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
                if (checksum == parity) {
                    *gpsPacketLogChar = LOG_IGNORED;
                    GPS_packetCount++;
                    switch (gps_frame) {
                    case FRAME_GGA:
                      *gpsPacketLogChar = LOG_NMEA_GGA;
                      frameOK = 1;
                      if (STATE(GPS_FIX)) {
                            GPS_coord[LAT] = gps_Msg.latitude;
                            GPS_coord[LON] = gps_Msg.longitude;
                            GPS_numSat = gps_Msg.numSat;
                            GPS_altitude = gps_Msg.altitude;
                        }
                        break;
                    case FRAME_RMC:
                        *gpsPacketLogChar = LOG_NMEA_RMC;
                        GPS_speed = gps_Msg.speed;
                        GPS_ground_course = gps_Msg.ground_course;
                        break;
                    } // end switch
                } else {
                    *gpsPacketLogChar = LOG_ERROR;
                }
            }
            checksum_param = 0;
            break;
        default:
            if (offset < 15)
                string[offset++] = c;
            if (!checksum_param)
                parity ^= c;
    }
    return frameOK;
}
Beispiel #4
0
static int gpsNewFrameNMEA(char c)
{
    uint8_t frameOK = 0;
    static uint8_t param = 0, offset = 0, parity = 0;
    static char string[15];
    static uint8_t checksum_param, frame = 0;

    if (c == '$') {
        param = 0;
        offset = 0;
        parity = 0;
    } else if (c == ',' || c == '*') {
        string[offset] = 0;
        if (param == 0) {       // frame identification
            frame = 0;
            if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A')
                frame = FRAME_GGA;
            if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C')
                frame = FRAME_RMC;
        } else if (frame == FRAME_GGA) {
            if (param == 2) {
                gpsData.lat = GPS_coord_to_degrees(string);
            } else if (param == 3 && string[0] == 'S')
                gpsData.lat = -gpsData.lat;
            else if (param == 4) {
                gpsData.lon = GPS_coord_to_degrees(string);
            } else if (param == 5 && string[0] == 'W')
                gpsData.lon = -gpsData.lon;
            else if (param == 6) {
                gpsData.fix = string[0] > '0';
            } else if (param == 7) {
                gpsData.numSatellites = grab_fields(string, 0);
            } else if (param == 9) {
                gpsData.height = stringToFloat(string);
            }
        } else if (frame == FRAME_RMC) {
            if (param == 7) {
                gpsData.speed = stringToFloat(string) * 51.44444f;      // speed in cm/s
            }
        }
        param++;
        offset = 0;
        if (c == '*')
            checksum_param = 1;
        else
            parity ^= c;
    } else if (c == '\r' || c == '\n') {
        if (checksum_param) {   // parity checksum
            uint8_t checksum = hex_c(string[0]);
            checksum <<= 4;
            checksum += hex_c(string[1]);
            if (checksum == parity)
                frameOK = 1;
        }
        checksum_param = 0;
    } else {
        if (offset < 15)
            string[offset++] = c;
        if (!checksum_param)
            parity ^= c;
    }
    if (frame)
        gpsData.present = 1;
    return frameOK && (frame == FRAME_GGA);
}
Beispiel #5
0
static bool GPS_newFrame(char c)
{
    uint8_t frameOK = 0;
    static uint8_t param = 0, offset = 0, parity = 0;
    static char string[15];
    static uint8_t checksum_param, frame = 0;

    if (c == '$') {
        param = 0;
        offset = 0;
        parity = 0;
    } else if (c == ',' || c == '*') {
        string[offset] = 0;
        if (param == 0) {       //frame identification
            frame = 0;
            if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A')
                frame = FRAME_GGA;
            if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C')
                frame = FRAME_RMC;
        } else if (frame == FRAME_GGA) {
            if (param == 2) {
                GPS_latitude = GPS_coord_to_degrees(string);
            } else if (param == 3 && string[0] == 'S')
                GPS_latitude = -GPS_latitude;
            else if (param == 4) {
                GPS_longitude = GPS_coord_to_degrees(string);
            } else if (param == 5 && string[0] == 'W')
                GPS_longitude = -GPS_longitude;
            else if (param == 6) {
                GPS_fix = string[0] > '0';
            } else if (param == 7) {
                GPS_numSat = grab_fields(string, 0);
            } else if (param == 9) {
                GPS_altitude = grab_fields(string, 0);
            }                   // altitude in meters added by Mis
        } else if (frame == FRAME_RMC) {
            if (param == 7) {
                GPS_speed = ((uint32_t) grab_fields(string, 1) * 514444L) / 100000L;
            }                   // speed in cm/s added by Mis
            if (param == 8) {
                GPS_heading = grab_fields(string, 0);
            }                   // heading
        }
        param++;
        offset = 0;
        if (c == '*')
            checksum_param = 1;
        else
            parity ^= c;
    } else if (c == '\r' || c == '\n') {
        if (checksum_param) {   // parity checksum
            uint8_t checksum = hex_c(string[0]);
            checksum <<= 4;
            checksum += hex_c(string[1]);
            if (checksum == parity)
                frameOK = 1;
        }
        checksum_param = 0;
    } else {
        if (offset < 15)
            string[offset++] = c;
        if (!checksum_param)
            parity ^= c;
    }
    return frameOK && (frame == FRAME_GGA);
}
Beispiel #6
0
static bool gpsNewFrameNMEA(char c)
{
    static gpsDataNmea_t gps_Msg;

    uint8_t frameOK = 0;
    static uint8_t param = 0, offset = 0, parity = 0;
    static char string[NMEA_BUFFER_SIZE];
    static uint8_t checksum_param, gps_frame = NO_FRAME;

    switch (c) {
        case '$':
            param = 0;
            offset = 0;
            parity = 0;
            break;
        case ',':
        case '*':
            string[offset] = 0;
            if (param == 0) {       //frame identification
                gps_frame = NO_FRAME;
                if (strcmp(string, "GPGGA") == 0 || strcmp(string, "GNGGA") == 0) {
                    gps_frame = FRAME_GGA;
                }
                else if (strcmp(string, "GPRMC") == 0 || strcmp(string, "GNRMC") == 0) {
                    gps_frame = FRAME_RMC;
                }
            }

            switch (gps_frame) {
                case FRAME_GGA:        //************* GPGGA FRAME parsing
                    switch(param) {
            //          case 1:             // Time information
            //              break;
                        case 2:
                            gps_Msg.latitude = GPS_coord_to_degrees(string);
                            break;
                        case 3:
                            if (string[0] == 'S')
                                gps_Msg.latitude *= -1;
                            break;
                        case 4:
                            gps_Msg.longitude = GPS_coord_to_degrees(string);
                            break;
                        case 5:
                            if (string[0] == 'W')
                                gps_Msg.longitude *= -1;
                            break;
                        case 6:
                            if (string[0] > '0') {
                                gps_Msg.fix = true;
                            } else {
                                gps_Msg.fix = false;
                            }
                            break;
                        case 7:
                            gps_Msg.numSat = grab_fields(string, 0);
                            break;
                        case 8:
                            gps_Msg.hdop = grab_fields(string, 1) * 10;          // hdop
                            break;
                        case 9:
                            gps_Msg.altitude = grab_fields(string, 1) * 10;     // altitude in cm
                            break;
                    }
                    break;
                case FRAME_RMC:        //************* GPRMC FRAME parsing
                    switch(param) {
                        case 7:
                            gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L);    // speed in cm/s added by Mis
                            break;
                        case 8:
                            gps_Msg.ground_course = (grab_fields(string, 1));      // ground course deg * 10
                            break;
                    }
                    break;
            }

            param++;
            offset = 0;
            if (c == '*')
                checksum_param = 1;
            else
                parity ^= c;
            break;
        case '\r':
        case '\n':
            if (checksum_param) {   //parity checksum
                uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
                if (checksum == parity) {
                    gpsStats.packetCount++;
                    switch (gps_frame) {
                    case FRAME_GGA:
                        frameOK = 1;
                        gpsSol.numSat = gps_Msg.numSat;
                        if (gps_Msg.fix) {
                            gpsSol.fixType = GPS_FIX_3D;    // NMEA doesn't report fix type, assume 3D

                            gpsSol.llh.lat = gps_Msg.latitude;
                            gpsSol.llh.lon = gps_Msg.longitude;
                            gpsSol.llh.alt = gps_Msg.altitude;

                            // EPH/EPV are unreliable for NMEA as they are not real accuracy
                            gpsSol.hdop = gpsConstrainHDOP(gps_Msg.hdop);
                            gpsSol.eph = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER);
                            gpsSol.epv = gpsConstrainEPE(gps_Msg.hdop * GPS_HDOP_TO_EPH_MULTIPLIER);
                            gpsSol.flags.validEPE = 0;
                        }
                        else {
                            gpsSol.fixType = GPS_NO_FIX;
                        }

                        // NMEA does not report VELNED
                        gpsSol.flags.validVelNE = 0;
                        gpsSol.flags.validVelD = 0;
                        break;
                    case FRAME_RMC:
                        gpsSol.groundSpeed = gps_Msg.speed;
                        gpsSol.groundCourse = gps_Msg.ground_course;
                        break;
                    } // end switch
                }
                else {
                    gpsStats.errors++;
                }
            }
            checksum_param = 0;
            break;
        default:
            if (offset < (NMEA_BUFFER_SIZE-1)) {    // leave 1 byte to trailing zero
                string[offset++] = c;

                // only checksum if character is recorded and used (will cause checksum failure on dropped characters)
                if (!checksum_param)
                    parity ^= c;
            }
    }
    return frameOK;
}
Beispiel #7
0
static bool gpsNewFrameNMEA(char c)
{
    typedef struct gpsdata_s {
        int32_t latitude;
        int32_t longitude;
        uint8_t numSat;
        uint16_t altitude;
        uint16_t speed;
        uint16_t ground_course;
    } gpsdata_t;

    static gpsdata_t gps_Msg;

    uint8_t frameOK = 0;
    static uint8_t param = 0, offset = 0, parity = 0;
    static char string[15];
    static uint8_t checksum_param, gps_frame = NO_FRAME;

    switch (c) {
    case '$':
        param = 0;
        offset = 0;
        parity = 0;
        break;
    case ',':
    case '*':
        string[offset] = 0;
        if (param == 0) {       //frame identification
            gps_frame = NO_FRAME;
            if (string[0] == 'G' && string[1] == 'P' && string[2] == 'G' && string[3] == 'G' && string[4] == 'A')
                gps_frame = FRAME_GGA;
            if (string[0] == 'G' && string[1] == 'P' && string[2] == 'R' && string[3] == 'M' && string[4] == 'C')
                gps_frame = FRAME_RMC;
        }

        switch (gps_frame) {
        case FRAME_GGA:        //************* GPGGA FRAME parsing
            switch (param) {
//          case 1:             // Time information
//              break;
            case 2:
                gps_Msg.latitude = GPS_coord_to_degrees(string);
                break;
            case 3:
                if (string[0] == 'S')
                    gps_Msg.latitude *= -1;
                break;
            case 4:
                gps_Msg.longitude = GPS_coord_to_degrees(string);
                break;
            case 5:
                if (string[0] == 'W')
                    gps_Msg.longitude *= -1;
                break;
            case 6:
                if (string[0] > '0') {
                    ENABLE_STATE(GPS_FIX);
                } else {
                    DISABLE_STATE(GPS_FIX);
                }
                break;
            case 7:
                gps_Msg.numSat = grab_fields(string, 0);
                break;
            case 9:
                gps_Msg.altitude = grab_fields(string, 0);     // altitude in meters added by Mis
                break;
            }
            break;
        case FRAME_RMC:        //************* GPRMC FRAME parsing
            switch (param) {
            case 7:
                gps_Msg.speed = ((grab_fields(string, 1) * 5144L) / 1000L);    // speed in cm/s added by Mis
                break;
            case 8:
                gps_Msg.ground_course = (grab_fields(string, 1));      // ground course deg * 10
                break;
            }
            break;
        }

        param++;
        offset = 0;
        if (c == '*')
            checksum_param = 1;
        else
            parity ^= c;
        break;
    case '\r':
    case '\n':
        if (checksum_param) {   //parity checksum
            uint8_t checksum = 16 * ((string[0] >= 'A') ? string[0] - 'A' + 10 : string[0] - '0') + ((string[1] >= 'A') ? string[1] - 'A' + 10 : string[1] - '0');
            if (checksum == parity) {
                switch (gps_frame) {
                case FRAME_GGA:
                  frameOK = 1;
                  if (STATE(GPS_FIX)) {
                        GPS_coord[LAT] = gps_Msg.latitude;
                        GPS_coord[LON] = gps_Msg.longitude;
                        GPS_numSat = gps_Msg.numSat;
                        GPS_altitude = gps_Msg.altitude;
                    }
                    break;
                case FRAME_RMC:
                    GPS_speed = gps_Msg.speed;
                    GPS_ground_course = gps_Msg.ground_course;
                    break;
                } // end switch
            }
        }
        checksum_param = 0;
        break;
    default:
        if (offset < 15)
            string[offset++] = c;
        if (!checksum_param)
            parity ^= c;
    }
    return frameOK;
}