changeset 52:2554cd13eddd

gnss-galileo: move ephemeris code into a separate file Signed-off-by: Josef 'Jeff' Sipek <jeffpc@josefsipek.net>
author Josef 'Jeff' Sipek <jeffpc@josefsipek.net>
date Thu, 16 Jan 2020 17:56:46 -0500
parents b668806db3fc
children c39111c37631
files CMakeLists.txt gnss-galileo-eph.c gnss-galileo.c
diffstat 3 files changed, 91 insertions(+), 64 deletions(-) [+]
line wrap: on
line diff
--- a/CMakeLists.txt	Thu Jan 16 16:48:46 2020 -0500
+++ b/CMakeLists.txt	Thu Jan 16 17:56:46 2020 -0500
@@ -56,6 +56,7 @@
 
 add_library(gnss
 	gnss-galileo.c
+	gnss-galileo-eph.c
 )
 
 target_link_libraries(gnss
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/gnss-galileo-eph.c	Thu Jan 16 17:56:46 2020 -0500
@@ -0,0 +1,90 @@
+/*
+ * Copyright (c) 2020 Josef 'Jeff' Sipek <jeffpc@josefsipek.net>
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <math.h>
+
+#include "gnss-galileo.h"
+
+#include <jeffpc/types.h>
+#include <jeffpc/error.h>
+
+void galileo_print_eph(const struct galileo_ephemeris *eph)
+{
+	printf("E%02u: iod %u "
+	       "m0 %e delta_n %e e %e sqrt_a %e omega0 %e i0 %e "
+	       "omega %e omegadot %e idot %e cuc %e cus %e "
+	       "crc %e crs %e cic %e cis %e t0 %u (%u raw)\n",
+	       eph->sv, eph->iod, eph->m0, eph->delta_n, eph->e,
+	       eph->sqrt_a, eph->omega0, eph->i0, eph->omega, eph->omegadot,
+	       eph->idot, eph->cuc, eph->cus, eph->crc, eph->crs, eph->cic,
+	       eph->cis, eph->t0.gst, eph->t0.raw);
+}
+
+void galileo_eph_ecef(const struct galileo_ephemeris *eph,
+		      const uint32_t gst, struct ecef *ecef)
+{
+	int approx_iter;
+
+	/* Based on Galileo ICD 5.1.1 */
+	const double mu = 3.986004418e14; /* m3/s2 */
+	const double omegaE = 7.2921151467e-5; /* rad/s */
+
+	double A = eph->sqrt_a * eph->sqrt_a;
+	double n0 = sqrt(mu / (A * A * A));
+	double tk = gst - eph->t0.gst;
+	double n = n0 + eph->delta_n;
+	double M = eph->m0 + n * tk;
+	double E = M;
+
+	for (approx_iter = 0; approx_iter < 1000; approx_iter++) {
+		double prevE = E;
+		E = M + eph->e * sin(prevE);
+
+		if (fabs(prevE - E) < 0.00000001)
+			break;
+	}
+
+	/*
+	 * True anomaly from eccentric anomaly - according to the ICD.
+	 *
+	 * There are other ways to calculate the true anomaly, but let's
+	 * stick with what's in the ICD.  For more information and the other
+	 * algorithms, see https://en.wikipedia.org/wiki/True_anomaly.
+	 */
+	double nu = atan2(sqrt(1 - eph->e * eph->e) * sin(E) / (1 - eph->e * cos(E)),
+			  (cos(E) - eph->e) / (1 - eph->e * cos(E)));
+
+	double Phi = nu + eph->omega;
+	double deltau = eph->cus * sin(2 * Phi) + eph->cuc * cos(2 * Phi);
+	double deltar = eph->crs * sin(2 * Phi) + eph->crc * cos(2 * Phi);
+	double deltai = eph->cis * sin(2 * Phi) + eph->cic * cos(2 * Phi);
+	double u = Phi + deltau;
+	double r = A * (1 - eph->e * cos(E)) + deltar;
+	double i = eph->i0 + deltai + eph->idot * tk;
+	double xprime = r * cos(u);
+	double yprime = r * sin(u);
+	double Omega = eph->omega0 + (eph->omegadot - omegaE) * tk - omegaE * eph->t0.raw;
+
+	ecef->x = xprime * cos(Omega) - yprime * cos(i) * sin(Omega);
+	ecef->y = xprime * sin(Omega) + yprime * cos(i) * cos(Omega);
+	ecef->z = yprime * sin(i);
+}
--- a/gnss-galileo.c	Thu Jan 16 16:48:46 2020 -0500
+++ b/gnss-galileo.c	Thu Jan 16 17:56:46 2020 -0500
@@ -20,8 +20,6 @@
  * SOFTWARE.
  */
 
-#include <math.h>
-
 #include "gnss-galileo.h"
 
 #include <jeffpc/types.h>
@@ -263,65 +261,3 @@
 		return true;
 	}
 }
-
-void galileo_print_eph(const struct galileo_ephemeris *eph)
-{
-	printf("E%02u: iod %u "
-	       "m0 %e delta_n %e e %e sqrt_a %e omega0 %e i0 %e "
-	       "omega %e omegadot %e idot %e cuc %e cus %e "
-	       "crc %e crs %e cic %e cis %e t0 %u (%u raw)\n",
-	       eph->sv, eph->iod, eph->m0, eph->delta_n, eph->e,
-	       eph->sqrt_a, eph->omega0, eph->i0, eph->omega, eph->omegadot,
-	       eph->idot, eph->cuc, eph->cus, eph->crc, eph->crs, eph->cic,
-	       eph->cis, eph->t0.gst, eph->t0.raw);
-}
-
-void galileo_eph_ecef(const struct galileo_ephemeris *eph,
-		      const uint32_t gst, struct ecef *ecef)
-{
-	int approx_iter;
-
-	/* Based on Galileo ICD 5.1.1 */
-	const double mu = 3.986004418e14; /* m3/s2 */
-	const double omegaE = 7.2921151467e-5; /* rad/s */
-
-	double A = eph->sqrt_a * eph->sqrt_a;
-	double n0 = sqrt(mu / (A * A * A));
-	double tk = gst - eph->t0.gst;
-	double n = n0 + eph->delta_n;
-	double M = eph->m0 + n * tk;
-	double E = M;
-
-	for (approx_iter = 0; approx_iter < 1000; approx_iter++) {
-		double prevE = E;
-		E = M + eph->e * sin(prevE);
-
-		if (fabs(prevE - E) < 0.00000001)
-			break;
-	}
-
-	/*
-	 * True anomaly from eccentric anomaly - according to the ICD.
-	 *
-	 * There are other ways to calculate the true anomaly, but let's
-	 * stick with what's in the ICD.  For more information and the other
-	 * algorithms, see https://en.wikipedia.org/wiki/True_anomaly.
-	 */
-	double nu = atan2(sqrt(1 - eph->e * eph->e) * sin(E) / (1 - eph->e * cos(E)),
-			  (cos(E) - eph->e) / (1 - eph->e * cos(E)));
-
-	double Phi = nu + eph->omega;
-	double deltau = eph->cus * sin(2 * Phi) + eph->cuc * cos(2 * Phi);
-	double deltar = eph->crs * sin(2 * Phi) + eph->crc * cos(2 * Phi);
-	double deltai = eph->cis * sin(2 * Phi) + eph->cic * cos(2 * Phi);
-	double u = Phi + deltau;
-	double r = A * (1 - eph->e * cos(E)) + deltar;
-	double i = eph->i0 + deltai + eph->idot * tk;
-	double xprime = r * cos(u);
-	double yprime = r * sin(u);
-	double Omega = eph->omega0 + (eph->omegadot - omegaE) * tk - omegaE * eph->t0.raw;
-
-	ecef->x = xprime * cos(Omega) - yprime * cos(i) * sin(Omega);
-	ecef->y = xprime * sin(Omega) + yprime * cos(i) * cos(Omega);
-	ecef->z = yprime * sin(i);
-}