unit GpsFunc;

interface

uses SysUtils,Math;

const
  ER = 6378137;           // 地球半徑(長軸)
  FLAT = 0.0033528106648; // 扁率

// 函數集合
// 經緯度坐標到屏幕坐標的轉換
// 經緯度單位換算
// 從NEMA語句中分離值

type
  TSatInfo = Record
    PRN: integer;         // 衛星號碼
    EL,AZ: integer;       // 仰角、方位
    SN: integer;          // 信號噪音比
  end;

  TWayPoint = Record
    Lat,Lon: Double;
    LatHemi,LonHemi: Char;
    Alt: Single;
  end;
  
  TGPSInfo = Record
    Fix: String[10];       // 定位方式
    Lat,Lon: Double;      // 經緯度
    LatHemi,LonHemi: Char;// N、S、E、W
    UTC: String[6];
    Alt : Single;         // 海拔
    Speed: Single;        // 速度
    Course: Single;       // 方向
    SatInfo: Array [1..12] of TSatInfo;
  end;

function Do_GPSString(Str: string; var gps: TGpsInfo):boolean;
function Dists(sLon,sLat,tLon,tLat: Double; var El: double):Double;    // 計算大圓距離和方位角

implementation

//$PGRME,45.3,M,,M,45.3,M*00
//Estimated Error Information (PGRME)
//The GARMIN Proprietary sentence $PGRME reports estimated position error
//information.
//$PGRME,<1>,M,<2>,M,<3>,M*hh<CR><LF>
//<1> Estimated horizontal position error (HPE), 0.0 to 999.9 meters
//<2> Estimated vertical position error (VPE), 0.0 to 999.9 meters
//<3> Estimated position error (EPE), 0.0 to 999.9 meters

// 處理GPS發來的所有數據
//$GPGSV,3,1,09,05,18,043,43,11,04,290,44,14,59,034,43,15,01,184,*70
//$GPGSV,3,2,09,16,13,200,,18,22,153,,23,44,160,,25,54,288,51*7E
//$GPGSV,3,3,09,30,46,061,30,,,,,,,,,,,,*45
//$PGRME,45.3,M,,M,45.3,M*00
//$GPRMC,131558,A,2818.7145,N,10944.4235,E,000.0,000.0,1n0503,002.0,W*61
//$GPGGA,131558,2818.7145,N,10944.4235,E,1,04,4.6,238.2,M,-25.4,M,,*66
//$GPGSA,A,2,05,11,14,,,,,25,,,,,4.6,4.6,*1A
function Do_GPSString(Str: string; var gps: TGpsInfo):boolean;
  function GetNextItem(start: integer; len: integer; var tmp: string):integer;
  var
    i: integer;
  begin
    tmp := '';
    result := 0;
    i := start + 1;
    while (Str[i] <> '$') and (Str[i] <> ',') and (Str[i] <> '*') and (i<Len) do begin
      tmp := tmp + Str[i];
      inc(i);
      inc(result);
    end;
  end;

  function CountChar(start: integer; len: integer):integer;
  var
    i: integer;
  begin
    i:=start;
    Result := 0;
    while (Str[i] <> Chr($0d)) and (Str[i] <> Chr($0a)) and (Str[i] <> '$') and (i<Len) do begin
      if Str[i] = ',' then inc(Result);
      inc(i);
    end;
  end;
var
  CurPos,strLen: integer;
  tmpStr: string;
  tmpFloat: Double;
  tmpi,i: integer;
begin
  CurPos := 0;
  strLen := Length(Str);
  while CurPos<strLen do begin
    while (Str[CurPos] <> '$') and (CurPos<strLen) do inc(CurPos);
    CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
    if tmpStr = 'GPGGA' then begin
      if CountChar(CurPos,strLen) = 14 then begin
        //$GPGGA,131558,2818.7145,N,10944.4235,E,1,04,4.6,238.2,M,-25.4,M,,*66
        //$GPGGA,<1>,<2>,<3>,<4>,<5>,<6>,<7>,<8>,<9>,M,<10>,M,<11>,<12>*hh<CR><LF>
        //<1> UTC時間, hhmmss
        //<2> 緯度, ddmm.mmmm (輸出前導零)
        //<3> 南北, N or S
        //<4> 經度, dddmm.mmmm (輸出前導零)
        //<5> 東西, E or W
        //<6> GPS質量評估, 0 = 不能定位, 1 = 非差分定位
        //, 2 = 差分定位(DGPS), 6 = 估計
        //<7> 使用中的衛星數量, 00 to 12 (輸出前導零)
        //<8> Horizontal dilution of precision, 0.5 to 99.9
        //<9> 海拔高度, -9999.9 to 99999.9 米
        //<10> Geoidal height, -999.9 to 9999.9 meters
        //<11> 差分GPS(RTCM SC-104)數據壽命, number of seconds since last valid
        //RTCM transmission (非差分時為空)
        //<12> 差分GPS參考站ID, 0000 to 1023 (輸出前導零,非差分時為空)
        CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
        gps.UTC := tmpStr;
        CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
        tmpFloat := StrToFloatDef(tmpStr,0) / 100;
        gps.Lat := Int(tmpFloat) + (Frac(tmpFloat) * 5) / 3;
        CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
        gps.LatHemi := tmpStr[1];
        if gps.LatHemi = 'S' then gps.Lat := -1 * gps.Lat;
        CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
        tmpFloat := StrToFloatDef(tmpStr,0) / 100;
        gps.Lon := Int(tmpFloat) + (Frac(tmpFloat) * 5) / 3;
        CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
        gps.LonHemi := tmpStr[1];
        if gps.LonHemi = 'W' then gps.Lon := -1 * gps.Lon;
        CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
        CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
        CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
        CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
        gps.Alt := StrToFloatDef(tmpStr,0);
      end;
    end else if tmpStr = 'GPGSA' then begin
      if CountChar(CurPos,strLen) = 17 then begin
        //衛星狀態表
        //$GPGSA,A,2,05,11,14,,,,,25,,,,,4.6,4.6,*1A
        //$GPGSA,<1>,<2>,<3>,<3>,<3>,<3>,<3>,<3>,<3>,<3>,<3>,<3>,<3>,<3>,<4>,<5>,<6>
        //*hh<CR><LF>
        //<1> Mode, M = manual, A = automatic
        //<2> 定位狀態, 1 = not available, 2 = 2D, 3 = 3D
        //<3> 衛星號碼，最多12個
        //<4> Position dilution of precision, 0.5 to 99.9
        //<5> Horizontal dilution of precision, 0.5 to 99.9
        //<6> Vertical dilution of precision, 0.5 to 99.9
        CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
        CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
        case StrToIntDef(tmpStr,0) of
          2: gps.Fix := '2D Fix';
          3: gps.Fix := '3D Fix';
        else
          gps.Fix := 'No Fix';
        end;
      end;
    end else if tmpStr = 'GPRMC' then begin
      if CountChar(CurPos,strLen) = 11 then begin
        //Recommended Minimum Specific GPS/TRANSIT Data (RMC)
        //$GPRMC,<1>,<2>,<3>,<4>,<5>,<6>,<7>,<8>,<9>,<10>,<11>,<12>*hh<CR><LF>
        //$GPRMC,131558,A,2818.7145,N,10944.4235,E,000.0,000.0,1n0503,002.0,W*61
        //<1> UTC time of position fix, hhmmss format
        //<2> Status, A = Valid position, V = NAV receiver warning
        //<3> Latitude, ddmm.mmmm format (leading zeros will be transmitted)
        //<4> Latitude hemisphere, N or S
        //<5> Longitude, dddmm.mmmm format (leading zeros will be transmitted)
        //<6> Longitude hemisphere, E or W
        //<7> Speed over ground, 000.0 to 999.9 knots (leading zeros will be transmitted)
        //<8> Course over ground, 000.0 to 359.9 degrees, true (leading zeros will be
        //transmitted)
        //<9> UTC date of position fix, ddmmyy format
        //<10> Magnetic variation, 000.0 to 180.0 degrees (leading zeros will be transmitted)
        //<11> Magnetic variation direction, E or W (westerly variation adds to true course)
        //<12> Mode indicator (only output if NMEA 2.30 active), A = Autonomous, D =
        //Differential, E = Estimated, N = Data not valid
        CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
        CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
        CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
        CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
        CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
        CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
        CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
        gps.Speed := StrToFloatDef(tmpStr,0);
        CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
        gps.Course := StrToFloatDef(tmpStr,0);
      end;
    end else if tmpStr = 'GPGSV' then begin
      if CountChar(CurPos,strLen) = 19 then begin
        //$GPGSV,3,1,09,05,18,043,43,11,04,290,44,14,59,034,43,15,01,184,*70
        //$GPGSV,3,2,09,16,13,200,,18,22,153,,23,44,160,,25,54,288,51*7E
        //$GPGSV,3,3,09,30,46,061,30,,,,,,,,,,,,*45
        //$GPGSV,<1>,<2>,<3>,<4>,<5>,<6>,<7>,...<4>,<5>,<6>,<7>*hh<CR><LF>
        //<1> Total number of GSV sentences to be transmitted
        //<2> Number of current GSV sentence
        //<3> Total number of satellites in view, 00 to 12 (leading zeros will be transmitted)
        //<4> Satellite PRN number, 01 to 32 (leading zeros will be transmitted)
        //<5> 衛星仰角, 00-90度
        //<6> 衛星方位角, 000-359度
        //<7> 衛星信號噪音比 00-99db
        //NOTE: Items <4>,<5>,<6> and <7> repeat for each satellite in view to a maximum of
        //four (4) satellites per sentence. Additional satellites in view information must be
        //sent in subsequent sentences. These fields will be null if unused.
        CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
        CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
        tmpi := StrToIntDef(tmpStr,1);
        CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
        for i:=1 to 4 do begin
          CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
          gps.SatInfo[(tmpi-1)*4 + i].PRN := StrToIntDef(tmpStr,0);
          CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
          gps.SatInfo[(tmpi-1)*4 + i].EL := StrToIntDef(tmpStr,0);
          CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
          gps.SatInfo[(tmpi-1)*4 + i].AZ := StrToIntDef(tmpStr,0);
          CurPos:= CurPos + 1 + GetNextItem(CurPos,strLen,tmpStr);
          gps.SatInfo[(tmpi-1)*4 + i].SN := StrToIntDef(tmpStr,0);
        end;
      end;
    end;
    inc(CurPos);
  end;
  Result := True;
end;

// sLon,sLat:原點經緯度 tLon,tLat: 目標點經緯度
// 經緯度單位為 ddd.dddddddd 度表示，東經、北緯為正
// El: 方位角
// 返回值： 距離，單位米
function Dists(sLon,sLat,tLon,tLat: Double; var El: double):Double;    // 計算大圓距離和方位角
var
  dLon: double;               // 經度差
  gcd,btr: double;
  tmp: double;
begin
  sLon := DegToRad(sLon);
  tLon := DegToRad(tLon);
  if sLon < 0 then sLon := sLon + 2 * Pi;
  if tLon < 0 then tLon := tLon + 2 * Pi;
  dLon := sLon - tLon;
  if abs(dLon) > Pi then dLon := dLon - 2*Pi*Sign(dLon);

  gcd := ArcCos(sin(sLat)*sin(tLat) + cos(sLat)*cos(tLat)*cos(dLon));
  if gcd < 0.000000000001 then gcd := 0.000000000001;

  if cos(sLat) > 0.000000000001 then begin
    tmp := cos(sLat) * sin(gcd);
    if abs(tmp) > 0.0000001 then
      btr := ArcCos((sin(tLat) - sin(sLat)*cos(gcd)) / tmp);
    if dLon>0 then btr := 2 * Pi - btr;
  end else if sLat >=0 then btr := 0 else btr := Pi;

  //if cos(tLat) > 0.0000001 then begin
  //  brt := ArcCos((sin(sLat) - sin(tLat) * cos(gcd)) / (cos(tLat)*sin(gcd)));
  //  if dLon < 0 then brt := 2 * Pi - brt;
  //end else if tLat >=0 then brt := 0 else brt := Pi;

  btr := btr * 180 / Pi;
  //brt := brt * 180 / Pi;
  gcd := gcd * ER;

  El := btr;
  Result := gcd;
end;

end.