diff --git a/include/libswiftnav/pvt.h b/include/libswiftnav/pvt.h index 09178d92..3097ed61 100644 --- a/include/libswiftnav/pvt.h +++ b/include/libswiftnav/pvt.h @@ -16,6 +16,8 @@ #include #include +#include +#include #define PVT_MAX_ITERATIONS 10 @@ -85,4 +87,10 @@ s8 calc_PVT(const u8 n_used, gnss_solution *soln, dops_t *dops); +void calc_iono_tropo(const u8 n_ready_tdcp, + navigation_measurement_t *nav_meas_tdcp, + const double *pos_ecef, + const double *pos_llh, + const ionosphere_t *iono_params); + #endif /* LIBSWIFTNAV_PVT_H */ diff --git a/src/pvt.c b/src/pvt.c index 3c56d979..2d2c34a9 100644 --- a/src/pvt.c +++ b/src/pvt.c @@ -598,3 +598,56 @@ s8 calc_PVT(const u8 n_used, return raim_flag; } + +/** Calculates Iono and Tropo correction + * \param n_ready_tdcp The number of measurements written to `m_tdcp` + * \param nav_meas_tdcp Pointer to measurements array + * \param pos_ecef position in WGS84 Earth Centered, Earth Fixed (ECEF) Cartesian + * \param pos_llh position in WGS84 geodetic coordinates + * \param iono_params pointer to ionosphere parameters received from L1CA stream */ +void calc_iono_tropo(const u8 n_ready_tdcp, + navigation_measurement_t *nav_meas_tdcp, + const double *pos_ecef, + const double *pos_llh, + const ionosphere_t *iono_params) +{ + for (u8 i = 0; i < n_ready_tdcp; i++) { + double az, el, cp_correction; + double tropo_correction = 0; + double iono_correction = 0; + /* calculate azimuth and elevation of SV */ + wgsecef2azel(nav_meas_tdcp[i].sat_pos, pos_ecef, &az, &el); + /* calc iono correction if available */ + if (iono_params) { + /* calculate iono correction */ + iono_correction = calc_ionosphere(&nav_meas_tdcp[i].tot, + pos_llh[0], + pos_llh[1], + az,el, + iono_params); + + /* correct pseudorange */ + nav_meas_tdcp[i].pseudorange -= iono_correction; + /* correct carrier phase based on iono correction */ + cp_correction = iono_correction * (GPS_L1_HZ + nav_meas_tdcp[i].doppler) + / GPS_C; + nav_meas_tdcp[i].carrier_phase -= cp_correction; + } + + /* calculate tropo correction. + * ellipsoidal height is used due to lack of a geoid model */ + tropo_correction = calc_troposphere(&nav_meas_tdcp[i].tot, + pos_llh[0], + pos_llh[2], + el); + /* correct pseudorange */ + nav_meas_tdcp[i].pseudorange -= tropo_correction; + /* correct carrier phase based on tropo correction */ + cp_correction = tropo_correction * (GPS_L1_HZ + nav_meas_tdcp[i].doppler) + / GPS_C; + /* sign here is opposite to normal due to + * Piksi's unusual sign convention on carrier phase */ + nav_meas_tdcp[i].carrier_phase += cp_correction; + log_debug("%u: I %10.5f, T %10.5f", i, iono_correction, tropo_correction); + } +} diff --git a/tests/check_pvt.c b/tests/check_pvt.c index ddea7095..ef157b9f 100644 --- a/tests/check_pvt.c +++ b/tests/check_pvt.c @@ -3,6 +3,8 @@ #include #include +#include +#include #include "check_utils.h" @@ -146,6 +148,52 @@ START_TEST(test_dops) } END_TEST +START_TEST(iono_tropo_usage_test) +{ + /*NOTE: this unit test is checked calc_iono_tropo function usage only. + * Find iono and tropo correction unit tests in check_ionosphere.c and + * check_troposphere.c accordingly*/ + u8 n_ready_tdcp = 1; + navigation_measurement_t nav_meas_tdcp = { + .tot = {.wn = 1875, .tow = 479820}, + .sid = {.sat = 1}, + .pseudorange = 22932174.156858064, + .sat_pos = {-9680013.5408340245, -15286326.354385279, 19429449.383770257}, + }; + double pos_ecef[3]; + double pos_llh[3] = {-35.3 * D2R, 149.1 * D2R, 0}; + + wgsllh2ecef(pos_llh, pos_ecef); + + double pr_tropo_correced = 22932178.51241; + double pr_iono_tropo_corrected = 22932162.13463; + + ionosphere_t i = {.a0 = 0.1583e-7, .a1 = -0.7451e-8, + .a2 = -0.5960e-7, .a3 = 0.1192e-6, + .b0 = 0.1290e6, .b1 = -0.2130e6, + .b2 = 0.6554e5, .b3 = 0.3277e6}; + + calc_iono_tropo(n_ready_tdcp, + &nav_meas_tdcp, + pos_ecef, + pos_llh, + &i); + + fail_unless(nav_meas_tdcp.pseudorange - pr_iono_tropo_corrected < 0.0001, + "Pseudorange Iono and Tropo corrected is out of range"); + + nav_meas_tdcp.pseudorange = 22932174.156858064; + + calc_iono_tropo(n_ready_tdcp, + &nav_meas_tdcp, + pos_ecef, + pos_llh, + (ionosphere_t*)NULL); + + fail_unless(nav_meas_tdcp.pseudorange - pr_tropo_correced < 0.0001, + "Pseudorange Tropo corrected is out of range"); +} +END_TEST Suite* pvt_test_suite(void) { @@ -156,6 +204,7 @@ Suite* pvt_test_suite(void) tcase_add_test(tc_core, test_pvt_failed_repair); tcase_add_test(tc_core, test_disable_pvt_raim); tcase_add_test(tc_core, test_dops); + tcase_add_test(tc_core, iono_tropo_usage_test); suite_add_tcase(s, tc_core); return s;