LLH座標系に加えECEF座標系を追加

diff --git a/Yubeshi.sln b/Yubeshi.sln
index bf2668a..08d8df2 100755 (executable)
--- a/Yubeshi.sln
+++ b/Yubeshi.sln
@@ -11,27 +11,17 @@ EndProject
 Global\r
        GlobalSection(SolutionConfigurationPlatforms) = preSolution\r
                Debug|Any CPU = Debug|Any CPU\r
-               Debug|x64 = Debug|x64\r
                Release|Any CPU = Release|Any CPU\r
-               Release|x64 = Release|x64\r
        EndGlobalSection\r
        GlobalSection(ProjectConfigurationPlatforms) = postSolution\r
                {EC4BB8E0-FD4B-4072-A17A-BA0EC0C750AB}.Debug|Any CPU.ActiveCfg = Debug|Any CPU\r
                {EC4BB8E0-FD4B-4072-A17A-BA0EC0C750AB}.Debug|Any CPU.Build.0 = Debug|Any CPU\r
-               {EC4BB8E0-FD4B-4072-A17A-BA0EC0C750AB}.Debug|x64.ActiveCfg = Debug|Any CPU\r
-               {EC4BB8E0-FD4B-4072-A17A-BA0EC0C750AB}.Debug|x64.Build.0 = Debug|Any CPU\r
                {EC4BB8E0-FD4B-4072-A17A-BA0EC0C750AB}.Release|Any CPU.ActiveCfg = Release|Any CPU\r
                {EC4BB8E0-FD4B-4072-A17A-BA0EC0C750AB}.Release|Any CPU.Build.0 = Release|Any CPU\r
-               {EC4BB8E0-FD4B-4072-A17A-BA0EC0C750AB}.Release|x64.ActiveCfg = Release|x64\r
-               {EC4BB8E0-FD4B-4072-A17A-BA0EC0C750AB}.Release|x64.Build.0 = Release|x64\r
                {1173BFCF-CA10-433B-A8FF-B9C1148A3700}.Debug|Any CPU.ActiveCfg = Debug|Any CPU\r
                {1173BFCF-CA10-433B-A8FF-B9C1148A3700}.Debug|Any CPU.Build.0 = Debug|Any CPU\r
-               {1173BFCF-CA10-433B-A8FF-B9C1148A3700}.Debug|x64.ActiveCfg = Debug|Any CPU\r
-               {1173BFCF-CA10-433B-A8FF-B9C1148A3700}.Debug|x64.Build.0 = Debug|Any CPU\r
                {1173BFCF-CA10-433B-A8FF-B9C1148A3700}.Release|Any CPU.ActiveCfg = Release|Any CPU\r
                {1173BFCF-CA10-433B-A8FF-B9C1148A3700}.Release|Any CPU.Build.0 = Release|Any CPU\r
-               {1173BFCF-CA10-433B-A8FF-B9C1148A3700}.Release|x64.ActiveCfg = Release|x64\r
-               {1173BFCF-CA10-433B-A8FF-B9C1148A3700}.Release|x64.Build.0 = Release|x64\r
        EndGlobalSection\r
        GlobalSection(SolutionProperties) = preSolution\r
                HideSolutionNode = FALSE\r

diff --git a/Yubeshi/Constants.cs b/Yubeshi/Constants.cs
new file mode 100755 (executable)
index 0000000..a16a7ec
--- /dev/null
+++ b/Yubeshi/Constants.cs
@@ -0,0 +1,25 @@
+/*\r
+ *     Yubeshi GPS Parser\r
+ *\r
+ *     This software is distributed under a zlib-style license.\r
+ *     See license.txt for more information.\r
+ */\r
+\r
+using System;\r
+using System.Collections.Generic;\r
+using System.Text;\r
+\r
+namespace Yubeshi\r
+{\r
+    public static class Constants\r
+    {\r
+        #region fields\r
+        // based on WGS84\r
+        public const double SemiMajorAxisA = 6378137.00000;\r
+        public const double SemiMajorAxisB = 6356752.31425;\r
+        public const double FirstEccentricitySquared = 6.69437999013e-3;\r
+        public const double SecondEccentricitySquared = 6.73949674226e-3;\r
+        public const double Pi = 3.1415926535898;\r
+        #endregion\r
+    }\r
+}\r

diff --git a/Yubeshi/Coordinate.cs b/Yubeshi/Coordinate.cs
deleted file mode 100755 (executable)
index 255268d..0000000
--- a/Yubeshi/Coordinate.cs
+++ /dev/null
@@ -1,57 +0,0 @@
-/*\r
- *     Yubeshi GPS Parser\r
- *\r
- *     This software is distributed under a zlib-style license.\r
- *     See license.txt for more information.\r
- */\r
-\r
-using System;\r
-using System.Collections.Generic;\r
-using System.Text;\r
-\r
-namespace Yubeshi\r
-{\r
-    public class Coordinate\r
-    {\r
-        public Coordinate(string latitude, string ns, \r
-                                    string longitude, string ew)\r
-        {\r
-            decimal latRaw = Decimal.Parse(latitude);\r
-            decimal lngRaw = Decimal.Parse(longitude);\r
-            decimal lat = Math.Floor(latRaw / 100m) + (latRaw % 100m) / 60m;\r
-            decimal lng = Math.Floor(lngRaw / 100m) + (lngRaw % 100m) / 60m;\r
-            Latitude  = (ns == "S") ? -lat : lat;\r
-            Longitude = (ns == "W") ? -lng : lng;\r
-            Altitude = 0;\r
-        }\r
-\r
-        #region properties\r
-\r
-        /// <summary>\r
-        ///    in Degree\r
-        /// </summary>\r
-        public decimal Latitude\r
-        {\r
-            get;\r
-            set;\r
-        }\r
-        /// <summary>\r
-        ///     in Degree\r
-        /// </summary>\r
-        public decimal Longitude\r
-        {\r
-            get;\r
-            set;\r
-        }\r
-\r
-        /// <summary>\r
-        ///     in metre\r
-        /// </summary>\r
-        public decimal Altitude\r
-        {\r
-            get;\r
-            set;\r
-        }\r
-        #endregion\r
-    }\r
-}\r

diff --git a/Yubeshi/EcefCoordinate.cs b/Yubeshi/EcefCoordinate.cs
new file mode 100755 (executable)
index 0000000..5cc1879
--- /dev/null
+++ b/Yubeshi/EcefCoordinate.cs
@@ -0,0 +1,94 @@
+/*\r
+ *     Yubeshi GPS Parser\r
+ *\r
+ *     This software is distributed under a zlib-style license.\r
+ *     See license.txt for more information.\r
+ */\r
+\r
+using System;\r
+using System.Collections.Generic;\r
+using System.Text;\r
+\r
+namespace Yubeshi\r
+{\r
+    public class EcefCoordinate\r
+    {\r
+        #region fields\r
+        private const double a = Constants.SemiMajorAxisA;\r
+        private const double b = Constants.SemiMajorAxisB;\r
+        private const double e1sq = Constants.FirstEccentricitySquared;\r
+        private const double e2sq = Constants.SecondEccentricitySquared;\r
+        private const double pi = Constants.Pi;\r
+        #endregion\r
+\r
+        #region constructors\r
+\r
+        public EcefCoordinate(double x, double y, double z)\r
+            : this(x, y, z, Double.NaN)\r
+        {\r
+        }\r
+\r
+        public EcefCoordinate(double x, double y, double z, double accuracy)\r
+        {\r
+            X = x;\r
+            Y = y;\r
+            Z = z;\r
+            Accuracy = accuracy;\r
+        }\r
+\r
+        /// <summary>\r
+        ///     \r
+        /// </summary>\r
+        /// <param name="x">ECEF X coordinate in cm</param>\r
+        /// <param name="y">ECEF Y coordinate in cm</param>\r
+        /// <param name="z">ECEF Z coordinate in cm</param>\r
+        /// <param name="accuracy">3D Position Accuracy Estimate in cm</param>\r
+        public EcefCoordinate(int x, int y, int z, uint accuracy)\r
+            : this(x * 0.01, y * 0.01, z * 0.01, accuracy * 0.01)\r
+        {\r
+        }\r
+        #endregion\r
+\r
+        #region properties\r
+        public double X\r
+        {\r
+            get;\r
+            set;\r
+        }\r
+\r
+        public double Y\r
+        {\r
+            get;\r
+            set;\r
+        }\r
+\r
+        public double Z\r
+        {\r
+            get;\r
+            set;\r
+        }\r
+\r
+        public double Accuracy\r
+        {\r
+            get;\r
+            set;\r
+        }\r
+        #endregion\r
+\r
+        #region public methods\r
+        public GeodeticCoordinate ToGeodeticCoordinate()\r
+        {\r
+            // approximation\r
+            double lambda = Math.Atan2(Y, X);\r
+            double p = Math.Sqrt(X * X + Y * Y);\r
+            double theta = Math.Atan2(Z * a, p * b);\r
+            double sin = Math.Sin(theta);\r
+            double cos = Math.Cos(theta);\r
+\r
+            double phi = Math.Atan2(Z + e2sq * b * sin * sin * sin,\r
+                                            p - e1sq * a * cos * cos * cos);\r
+            return new GeodeticCoordinate(phi * 180 / pi, lambda * 180 / pi);\r
+        }\r
+        #endregion\r
+    }\r
+}\r

diff --git a/Yubeshi/EcefVelocity.cs b/Yubeshi/EcefVelocity.cs
new file mode 100755 (executable)
index 0000000..49cbb00
--- /dev/null
+++ b/Yubeshi/EcefVelocity.cs
@@ -0,0 +1,52 @@
+using System;\r
+using System.Collections.Generic;\r
+using System.Text;\r
+\r
+namespace Yubeshi\r
+{\r
+    public class EcefVelocity\r
+    {\r
+        #region constructors\r
+        /// <summary>\r
+        ///     \r
+        /// </summary>\r
+        /// <param name="x">ECEF X velocity in cm/s</param>\r
+        /// <param name="y">ECEF Y velocity in cm/s</param>\r
+        /// <param name="z">ECEF Z velocity in cm/s</param>\r
+        /// <param name="accuracy">Speed Accuracy Estimate in cm/s</param>\r
+        public EcefVelocity(int x, int y, int z, uint accuracy)\r
+        {\r
+            X = x * 0.01;\r
+            Y = y * 0.01;\r
+            Z = z * 0.01;\r
+            Accuracy = accuracy * 0.01;\r
+        }\r
+        #endregion\r
+\r
+        #region properties\r
+        public double X\r
+        {\r
+            get;\r
+            set;\r
+        }\r
+\r
+        public double Y\r
+        {\r
+            get;\r
+            set;\r
+        }\r
+\r
+        public double Z\r
+        {\r
+            get;\r
+            set;\r
+        }\r
+\r
+        public double Accuracy\r
+        {\r
+            get;\r
+            set;\r
+        }\r
+        #endregion\r
+    }\r
+}\r

diff --git a/Yubeshi/GeodeticCoordinate.cs b/Yubeshi/GeodeticCoordinate.cs
new file mode 100755 (executable)
index 0000000..a97912b
--- /dev/null
+++ b/Yubeshi/GeodeticCoordinate.cs
@@ -0,0 +1,100 @@
+/*\r
+ *     Yubeshi GPS Parser\r
+ *\r
+ *     This software is distributed under a zlib-style license.\r
+ *     See license.txt for more information.\r
+ */\r
+\r
+using System;\r
+using System.Collections.Generic;\r
+using System.Text;\r
+\r
+namespace Yubeshi\r
+{\r
+    public class GeodeticCoordinate\r
+    {\r
+        #region fields\r
+        private const double a = Constants.SemiMajorAxisA;\r
+        private const double b = Constants.SemiMajorAxisB;\r
+        private const double e1sq = Constants.FirstEccentricitySquared;\r
+        private const double pi = Constants.Pi;\r
+\r
+        #endregion\r
+        #region constructors\r
+\r
+        public GeodeticCoordinate(double latitude, double longitude)\r
+            : this(latitude, longitude, Double.NaN)\r
+        {\r
+        }\r
+\r
+        public GeodeticCoordinate(double latitude, double longitude, double altitude)\r
+        {\r
+            Latitude = latitude;\r
+            Longitude = longitude;\r
+            Altitude = altitude;\r
+        }\r
+\r
+        public GeodeticCoordinate(string latitude, string ns,\r
+                                    string longitude, string ew)\r
+        {\r
+            decimal latRaw = Decimal.Parse(latitude);\r
+            decimal lngRaw = Decimal.Parse(longitude);\r
+            decimal lat = Math.Floor(latRaw / 100m) + (latRaw % 100m) / 60m;\r
+            decimal lng = Math.Floor(lngRaw / 100m) + (lngRaw % 100m) / 60m;\r
+            Latitude = (double)((ns == "S") ? -lat : lat);\r
+            Longitude = (double)((ns == "W") ? -lng : lng);\r
+            Altitude = Double.NaN;\r
+        }\r
+        #endregion\r
+\r
+        #region properties\r
+\r
+        /// <summary>\r
+        ///    in Degree\r
+        /// </summary>\r
+        public double Latitude\r
+        {\r
+            get;\r
+            set;\r
+        }\r
+        /// <summary>\r
+        ///     in Degree\r
+        /// </summary>\r
+        public double Longitude\r
+        {\r
+            get;\r
+            set;\r
+        }\r
+\r
+        /// <summary>\r
+        ///     in metre\r
+        /// </summary>\r
+        public double Altitude\r
+        {\r
+            get;\r
+            set;\r
+        }\r
+        #endregion\r
+\r
+        #region public method\r
+        public EcefCoordinate ToEcefCoordinate()\r
+        {\r
+            double h = Double.IsNaN(Altitude) ? 0 : Altitude;\r
+            double sinPhi = Math.Sin(Latitude * pi / 180.0);\r
+            double cosPhi = Math.Cos(Latitude * pi / 180.0);\r
+            double khi = Math.Sqrt(1.0 - e1sq * sinPhi * sinPhi);\r
+            double n = a / khi;\r
+            double r = (n + h) * cosPhi;\r
+            double x = r * Math.Cos(Longitude * pi / 180);\r
+            double y = r * Math.Sin(Longitude * pi / 180);\r
+            double z = (n * (1 - e1sq) + h) * sinPhi;\r
+            return new EcefCoordinate(x, y, z);\r
+        }\r
+\r
+\r
+        #endregion\r
+\r
+        #region private methods\r
+        #endregion\r
+    }\r
+}\r

diff --git a/Yubeshi/Nmea/GpDtm.cs b/Yubeshi/Nmea/GpDtm.cs
index 95f06d3..f3a09ef 100755 (executable)
--- a/Yubeshi/Nmea/GpDtm.cs
+++ b/Yubeshi/Nmea/GpDtm.cs
@@ -68,7 +68,7 @@ namespace Yubeshi.Nmea
             private set;\r
         }\r
 \r
-        public Coordinate Offset\r
+        public GeodeticCoordinate Offset\r
         {\r
             get;\r
             private set;\r

diff --git a/Yubeshi/Nmea/GpGga.cs b/Yubeshi/Nmea/GpGga.cs
index 8505bbc..587f9f3 100755 (executable)
--- a/Yubeshi/Nmea/GpGga.cs
+++ b/Yubeshi/Nmea/GpGga.cs
@@ -57,7 +57,7 @@ namespace Yubeshi.Nmea
             Array.Copy(sentence, Raw, elements.PacketLength);\r
             string[] v = elements.Values;\r
             TimeOfFix = ParseTime(v[0]);\r
-            Position = new Coordinate(v[1], v[2], v[3], v[4]);\r
+            Position = new GeodeticCoordinate(v[1], v[2], v[3], v[4]);\r
             FixQuality = (FixQualityClass)Int32.Parse(v[5]);\r
             TrackedSatellites = Int32.Parse(v[6]);\r
             Dilution = Decimal.Parse(v[7]);\r
@@ -77,7 +77,7 @@ namespace Yubeshi.Nmea
             private set;\r
         }\r
         \r
-        public Coordinate Position\r
+        public GeodeticCoordinate Position\r
         {\r
             get;\r
             private set;\r

diff --git a/YubeshiTest/Nmea/PacketTest.cs b/YubeshiTest/Nmea/PacketTest.cs
index c5591bb..3fa93bf 100755 (executable)
--- a/YubeshiTest/Nmea/PacketTest.cs
+++ b/YubeshiTest/Nmea/PacketTest.cs
@@ -32,8 +32,8 @@ namespace YubeshiTest.Nmea
             Assert.AreEqual(true, GpGga.TryParse(P.GpGga, out packet));\r
             GpGga p = packet as GpGga;\r
             Assert.AreEqual(new TimeSpan(0, 9, 27, 25, 0), p.TimeOfFix);\r
-            Assert.AreEqual(2837.11399m / 60m, p.Position.Latitude);\r
-            Assert.AreEqual(513.9159m / 60m, p.Position.Longitude);\r
+            Assert.AreEqual(2837.11399 / 60, p.Position.Latitude);\r
+            Assert.AreEqual(513.9159 / 60, p.Position.Longitude);\r
         }\r
     }\r
 }\r

diff --git a/YubeshiTest/Nmea/SamplePackets.cs b/YubeshiTest/Nmea/SamplePackets.cs
index 2dc058e..4452539 100755 (executable)
--- a/YubeshiTest/Nmea/SamplePackets.cs
+++ b/YubeshiTest/Nmea/SamplePackets.cs
@@ -30,6 +30,8 @@ namespace YubeshiTest.Nmea
             Encoding ascii = Encoding.ASCII;\r
 \r
             Dummy = ascii.GetBytes("$GPXXX,YYY,ZZZ,*FF\r\n");\r
+\r
+            // from u-blox document\r
             GpDtm = ascii.GetBytes("$GPDTM,W84,,0.000000,N,0.000000,E,0.0,W84*6F\r\n");\r
             GpGga = ascii.GetBytes("$GPGGA,092725.00,4717.11399,N,00833.91590,E,1,8,1.01,499.6,M,48.0,M,,0*5B\r\n");\r
             GpGll = ascii.GetBytes("$GPGLL,4717.11364,N,00833.91565,E,092321.00,A,A*60\r\n");\r