/** Iterates pvt_solve until it converges or PVT_MAX_ITERATIONS is reached.
*
* \return
* - `0`: solution converged
* - `-1`: solution failed to converge
*
* Results stored in rx_state, omp, H
*/
static s8 pvt_iter(double rx_state[],
const u8 n_used,
const navigation_measurement_t *nav_meas[n_used],
double omp[n_used],
double H[4][4])
{
/* Reset state to zero */
for(u8 i=4; i<8; i++) {
rx_state[i] = 0;
}
u8 iters;
/* Newton-Raphson iteration. */
for (iters=0; iters<PVT_MAX_ITERATIONS; iters++) {
if (pvt_solve(rx_state, n_used, nav_meas, omp, H) > 0) {
break;
}
}
if (iters >= PVT_MAX_ITERATIONS) {
/* Reset state if solution fails */
rx_state[0] = 0;
rx_state[1] = 0;
rx_state[2] = 0;
return -1;
}
return 0;
}
s8 calc_PVT(const u8 n_used,
const navigation_measurement_t const nav_meas[n_used],
gnss_solution *soln,
dops_t *dops)
{
/* Initial state is the center of the Earth with zero velocity and zero
* clock error, if we have some a priori position estimate we could use
* that here to speed convergence a little on the first iteration.
*
* rx_state format:
* pos[3], clock error, vel[3], intermediate freq error
*/
static double rx_state[8];
double H[4][4];
soln->valid = 0;
soln->n_used = n_used; // Keep track of number of working channels
/* reset state to zero !? */
for(u8 i=4; i<8; i++) {
rx_state[i] = 0;
}
double update;
u8 iters;
/* Newton-Raphson iteration. */
for (iters=0; iters<PVT_MAX_ITERATIONS; iters++) {
if ((update = pvt_solve(rx_state, n_used, nav_meas, H)) > 0) {
break;
}
}
/* Compute various dilution of precision metrics. */
compute_dops((const double(*)[4])H, rx_state, dops);
soln->err_cov[6] = dops->gdop;
/* Populate error covariances according to layout in definition
* of gnss_solution struct.
*/
soln->err_cov[0] = H[0][0];
soln->err_cov[1] = H[0][1];
soln->err_cov[2] = H[0][2];
soln->err_cov[3] = H[1][1];
soln->err_cov[4] = H[1][2];
soln->err_cov[5] = H[2][2];
if (iters >= PVT_MAX_ITERATIONS) {
/* Reset state if solution fails */
rx_state[0] = 0;
rx_state[1] = 0;
rx_state[2] = 0;
return -4;
}
/* Save as x, y, z. */
for (u8 i=0; i<3; i++) {
soln->pos_ecef[i] = rx_state[i];
soln->vel_ecef[i] = rx_state[4+i];
}
wgsecef2ned(soln->vel_ecef, soln->pos_ecef, soln->vel_ned);
/* Convert to lat, lon, hgt. */
wgsecef2llh(rx_state, soln->pos_llh);
soln->clock_offset = rx_state[3] / GPS_C;
soln->clock_bias = rx_state[7] / GPS_C;
/* Time at receiver is TOT plus time of flight. Time of flight is eqaul to
* the pseudorange minus the clock bias. */
soln->time = nav_meas[0].tot;
soln->time.tow += nav_meas[0].pseudorange / GPS_C;
/* Subtract clock offset. */
soln->time.tow -= rx_state[3] / GPS_C;
soln->time = normalize_gps_time(soln->time);
u8 ret;
if ((ret = filter_solution(soln, dops))) {
memset(soln, 0, sizeof(*soln));
/* Reset state if solution fails */
rx_state[0] = 0;
rx_state[1] = 0;
rx_state[2] = 0;
return -ret;
}
soln->valid = 1;
return 0;
}
