Mercurial > ublox > ublox8 > experimental
view gnss-galileo-eph.c @ 64:8e85919405df
gnss-galileo: add a valid bit to each ephemeris
This is easier than trying to guess whether the ephemeris contains any
meaningful values.
Signed-off-by: Josef 'Jeff' Sipek <jeffpc@josefsipek.net>
author | Josef 'Jeff' Sipek <jeffpc@josefsipek.net> |
---|---|
date | Wed, 22 Jan 2020 10:10:26 -0500 |
parents | 2554cd13eddd |
children | 0bfd3bbea386 |
line wrap: on
line source
/* * 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: %svalid 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->valid ? "" : "NOT ", 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; ASSERT(eph->valid); /* 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); }