Mercurial > ublox > ublox8 > experimental
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); -}