windows mobile gps 串口通信 NMEA命令和数据处理(2) (c#)（转）

下面是 GPS命名空间里面 的 NMEAProtocol 的这个类的代码

 

using System;
using System.Collections.Specialized;
using System.Text;
using System.Text.RegularExpressions;

using System.IO;

namespace GPS
{
 public class NMEAProtocol
 {
  private const int NP_MAX_CMD_LEN =   8;  // maximum command length (NMEA address)
  private const int NP_MAX_DATA_LEN =   256; // maximum data length
  private const int NP_MAX_CHAN =       36;     // maximum number of channels
  private const int NP_WAYPOINT_ID_LEN =  32;     // waypoint max string len

  private NMEAState nState;             // Current state protocol parser is in
  private byte checksum;     // Calculated NMEA sentence checksum
  private byte receivedChecksum;   // Received NMEA sentence checksum (if exists)
  private int index;      // Index used for command and data
  private byte[] command;     // NMEA command
  private byte[] data;     // NMEA data

  // number of NMEA commands received (processed or not processed)
  public int CommandCount;

  public GPGGAData GPGGA;
  public GPGSAData GPGSA = new GPGSAData();
  public GPGSVData GPGSV = new GPGSVData();
  public GPRMBData GPRMB;
  public GPRMCData GPRMC;
  public GPZDAData GPZDA;

        public NMEAProtocol()
  {
   command = new byte[NP_MAX_CMD_LEN];
   data = new byte[NP_MAX_DATA_LEN];
  }

  public void ParseBuffer(byte[] buffer)
  {
   foreach(byte b in buffer)
   {
    ProcessNMEA(b);
   }
   return;
  }

  public void ProcessNMEA(byte btData)
  {
   switch(nState)
   {
     ///////////////////////////////////////////////////////////////////////
     // Search for start of message '$'
    case NMEAState.NP_STATE_SOM :
     if(btData == '$')
     {
      checksum = 0;   // reset checksum
      index = 0;    // reset index
      nState = NMEAState.NP_STATE_CMD;
     }
     break;

     ///////////////////////////////////////////////////////////////////////
     // Retrieve command (NMEA Address)
    case NMEAState.NP_STATE_CMD :
     if(btData != ',' && btData != '*')
     {
      command[index++] = btData;
      checksum ^= btData;

      // Check for command overflow
      if(index >= NP_MAX_CMD_LEN)
      {
       nState = NMEAState.NP_STATE_SOM;
      }
     }
     else
     {
      command[index] = (byte)'\0'; // terminate command
      checksum ^= btData;
      index = 0;
      nState = NMEAState.NP_STATE_DATA;  // goto get data state
     }
     break;

     ///////////////////////////////////////////////////////////////////////
     // Store data and check for end of sentence or checksum flag
    case NMEAState.NP_STATE_DATA:
     if(btData == '*') // checksum flag?
     {
      data[index] = (byte)'\0';
      nState = NMEAState.NP_STATE_CHECKSUM_1;
     }
     else // no checksum flag, store data
     {
      //
      // Check for end of sentence with no checksum
      //
      if(btData == '\r')
      {
       data[index] = (byte) '\0';
       ProcessCommand(EncodeToString( command), data);
       nState = NMEAState.NP_STATE_SOM;
       return;
      }

      //
      // Store data and calculate checksum
      //
      checksum ^= btData;
      data[index] = btData;
      if(++index >= NP_MAX_DATA_LEN) // Check for buffer overflow
      {
       nState = NMEAState.NP_STATE_SOM;
      }
     }
     break;

     ///////////////////////////////////////////////////////////////////////
    case NMEAState.NP_STATE_CHECKSUM_1:
     if( (btData - '0') <= 9)
     {
      receivedChecksum = (byte)((btData - '0') << 4);
     }
     else
     {
      receivedChecksum = (byte)((btData - 'A' + 10) << 4);
     }

     nState = NMEAState.NP_STATE_CHECKSUM_2;

     break;

     ///////////////////////////////////////////////////////////////////////
    case NMEAState.NP_STATE_CHECKSUM_2:
     if( (btData - '0') <= 9)
     {
      receivedChecksum |= (byte)((btData - (byte)'0'));
     }
     else
     {
      receivedChecksum |= (byte)((btData - (byte)'A' + 10));
     }

     if(checksum == receivedChecksum)
     {
      ProcessCommand(EncodeToString(command), data);
     }

     nState = NMEAState.NP_STATE_SOM;
     break;

     ///////////////////////////////////////////////////////////////////////
    default :
     nState = NMEAState.NP_STATE_SOM;
     break;
   }
  }

  #region INMEAProtocol Members

        public void ProcessGPRMB(GPRMBData gprmc)
        {
            // TODO:  Add NMEAProtocol.ProcessGPRMB implementation
        }

        public void ProcessGPZDA(GPZDAData gpzda)
  {
   // TODO:  Add NMEAProtocol.ProcessGPZDA implementation
  }

  public void ProcessGPRMC(string data)
  {
            string[] fields = Regex.Split(data, ",");

            //Time: Hour, Minute, Second
            //Time is Zulu
            GPRMC.Hour = Convert.ToInt32(fields[0].Substring(0, 2));
            GPRMC.Minute = Convert.ToInt32(fields[0].Substring(2, 2));
            GPRMC.Second = Convert.ToInt32(fields[0].Substring(4, 2));

            GPRMC.Day = Convert.ToInt32(fields[8].Substring(0, 2));
            GPRMC.Month = Convert.ToInt32(fields[8].Substring(2, 2));
            GPRMC.Year = Convert.ToInt32(fields[8].Substring(4, 2));

            GPRMC.DataValid = Convert.ToChar(fields[1]);

            //Latitude
            GPRMC.Latitude = Convert.ToDouble(fields[2]) / 100;
            if (fields[3] == "S")
                GPRMC.LatitudeHemisphere = Cardinal.South;
            else
                GPRMC.LatitudeHemisphere = Cardinal.North;

            //Longitude
            GPRMC.Longitude = Convert.ToDouble(fields[4]) / 100;
            if (fields[5] == "E")
                GPRMC.LatitudeHemisphere = Cardinal.East;
            else
                GPRMC.LatitudeHemisphere = Cardinal.West;

            GPRMC.GroundSpeed = Convert.ToDouble(fields[6]);

            //TODO: MagVar and Course
            GPRMC.Count++;
        }

  public void ProcessGPGSV(string data)
  {

   //parses the GPGSV stream to extract satellite information

   string[] fields = Regex.Split(data,",");
   
   uint totalNumberOfMessages = Convert.ToUInt32(fields[0]);

   //make sure the data is OK. valid range is 1..8 channels
   if ((totalNumberOfMessages > 8) || (totalNumberOfMessages <= 0))
    return;

   GPGSV.TotalNumberOfMessages = totalNumberOfMessages;

            //message number
   int nMessageNumber = Convert.ToInt32(fields[1]);
   
   //make sure it is 0..9...is there an inconsistency? 8/9?
   if ((nMessageNumber > 9) || (nMessageNumber < 0))
    return;

   //sats in view
   GPGSV.SatellitesInView  = Convert.ToInt32(fields[2]);

            //for(int iSat = 0; iSat < GPGSV.SatellitesInView; iSat++)
            for (int iSat = 0; iSat < 4; iSat++)
            {
    Satellite sat = new Satellite();

    sat.Id = Convert.ToInt32(fields[3+iSat*4]);
    sat.Elevation = Convert.ToInt32(fields[4+iSat*4]);
    sat.Azimuth = Convert.ToInt32(fields[5+iSat*4]);
    sat.SignalQuality = Convert.ToInt32(fields[6+iSat*4]);
    sat.Used = IsSatelliteUsed(sat.Id);
    
    GPGSV.Satellites.Add(sat);
   }
   GPGSV.Count ++;
  }

  private bool IsSatelliteUsed(int satelliteId)
  {
   if(null == GPGSA.SatsInSolution) return false;

   foreach(int satId in GPGSA.SatsInSolution)
   {
    if (satId == satelliteId) return true;
   }
   return false;
  } 

  

  public void ProcessGPGSA(string data)
  {
            try
            {
                string[] fields = Regex.Split(data, ",");

                GPGSA.Mode = Convert.ToChar(fields[0]);
                GPGSA.mFixMode = Convert.ToByte(fields[1]);
                GPGSA.SatsInSolution = new int[12];
                for (int i = 2; i <= 13; i++)
                {
                       int satId = -1;
                       if (int.TryParse(fields[i], out satId))
                       {
                           if (satId != -1)
                           {
                               GPGSA.SatsInSolution[i - 2] = satId;
                           }
                       }
                }
                GPGSA.PDOP = Convert.ToDouble(fields[14]);
                GPGSA.HDOP = Convert.ToDouble(fields[15]);
                GPGSA.VDOP = Convert.ToDouble(fields[16]);

                GPGSA.Count++;
            }
            catch (Exception e)
            {
                System.Diagnostics.Trace.WriteLine("Chaos in ProcessGPGSA! " + e.ToString());
            }
  }

  public void ProcessGPGGA(string data)
  {
   try
   {
    string[] fields = Regex.Split(data,",");

    //Time: Hour, Minute, Second
    //Time is Zulu
    GPGGA.Hour = Convert.ToInt32( fields[0].Substring(0,2));
    GPGGA.Minute = Convert.ToInt32(fields[0].Substring(2,2));
    GPGGA.Second = Convert.ToInt32(fields[0].Substring(4,2));

                //Latitude
    GPGGA.Latitude = Convert.ToDouble(fields[1])/100;
    if(fields[2]=="S")
     GPGGA.LatitudeHemisphere = Cardinal.South;
    else
     GPGGA.LatitudeHemisphere = Cardinal.North;

    //Longitude
    GPGGA.Longitude = Convert.ToDouble(fields[3])/100;
    if(fields[4]=="E")
     GPGGA.LatitudeHemisphere = Cardinal.East;
    else
     GPGGA.LatitudeHemisphere = Cardinal.West;

    //GPS Signal Quality
    GPGGA.GPSQuality = (GPSQuality)Convert.ToUInt32( fields[5] );

    //Satellites
    GPGGA.NumberOfSatellitesInUse = Convert.ToInt32(fields[6]);

    //HDOP
    GPGGA.HDOP = Convert.ToDouble( fields[7] );

    //Altitude
    GPGGA.Altitude = Convert.ToDouble(fields[8]);

    //increase message count
    GPGGA.Count ++;
   }
   catch(Exception e)
   {
                System.Diagnostics.Trace.WriteLine("Chaos in ProcessGPGGA! " + e.ToString());
            }
  }

  public bool IsSatUsedInSolution(int SatelliteID)
  {
            return IsSatelliteUsed(SatelliteID);
  }

  public void Reset()
  {
   // TODO:  Add NMEAProtocol.Reset implementation
  }

  private string EncodeToString(byte[] buffer)
  {
   string s =  System.Text.ASCIIEncoding.GetEncoding(1252).GetString(buffer);

            // TRIM OFF NULLS!
            //s = s.TrimEnd((char)'\0');
            //return s;
           
            //TRIM at the FIRST NULL
            string[] strings = s.Split('\0');
            return strings[0];
  }

  public void ProcessCommand(string sCmd, byte[] bData)
  {
   string data = EncodeToString(bData);
   
   switch(sCmd)
   {
    case "GPGGA":
     ProcessGPGGA(data);
     break;

    case "GPGSA":
     ProcessGPGSA(data);
     break;

    case "GPGSV":
     ProcessGPGSV(data);
     break;

                case "GPRMC":
                    ProcessGPRMC(data);
                    break;
               
                case "GPRMB":
     //ProcessGPRMB(pData);
     break;
    
    case "GPZDA":
     //ProcessGPZDA(pData);
     break;

    default:
     break;
   }
   CommandCount = CommandCount + 1;
  }
  #endregion
 }
}