void compute_dops(const double const H[4][4],
const double const pos_ecef[3],
dops_t *dops)
{
double H_pos_diag[3];
double H_ned[3];
dops->gdop = dops->pdop = dops->tdop = dops->hdop = dops->vdop = 0;
/* PDOP is the norm of the position elements of tr(H) */
for (u8 i=0; i<3; i++) {
dops->pdop += H[i][i];
/* Also get the trace of H position states for use in HDOP/VDOP
* calculations.
*/
H_pos_diag[i] = H[i][i];
}
dops->pdop = sqrt(dops->pdop);
/* TDOP is like PDOP but for the time state. */
dops->tdop = sqrt(H[3][3]);
/* Calculate the GDOP -- ||tr(H)|| = sqrt(PDOP^2 + TDOP^2) */
dops->gdop = sqrt(dops->tdop*dops->tdop + dops->pdop*dops->pdop);
/* HDOP and VDOP are Horizontal and Vertical; we need to rotate the
* PDOP into NED frame and then take the separate components.
*/
wgsecef2ned(H_pos_diag, pos_ecef, H_ned);
dops->vdop = sqrt(H_ned[2]*H_ned[2]);
dops->hdop = sqrt(H_ned[0]*H_ned[0] + H_ned[1]*H_ned[1]);
}
/** Returns the vector \e to a point given in WGS84 Earth Centered, Earth Fixed
* (ECEF) Cartesian coordinates \e from a reference point, also given in WGS84
* ECEF coordinates, in the local North, East, Down (NED) frame of the
* reference point.
*
* \see wgsecef2ned.
*
* \param ecef Cartesian coordinates of the point, passed as [X, Y, Z],
* all in meters.
* \param ref_ecef Cartesian coordinates of the reference point, passed as
* [X, Y, Z], all in meters.
* \param ned The North, East, Down vector is written into this array as
* [N, E, D], all in meters.
*/
void wgsecef2ned_d(const double ecef[3], const double ref_ecef[3],
double ned[3]) {
double tempv[3];
vector_subtract(3, ecef, ref_ecef, tempv);
wgsecef2ned(tempv, ref_ecef, ned);
}
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;
}
/** Try to calculate a single point gps solution
*
* \param n_used number of measurments
* \param nav_meas array of measurements
* \param disable_raim passing True will omit raim check/repair functionality
* \param soln output solution struct
* \param dops output doppler information
* \return Non-negative values indicate a valid solution.
* - `2`: Solution converged but RAIM unavailable or disabled
* - `1`: Solution converged, failed RAIM but was successfully repaired
* - `0`: Solution converged and verified by RAIM
* - `-1`: PDOP is too high to yield a good solution.
* - `-2`: Altitude is unreasonable.
* - `-3`: Velocity is greater than or equal to 1000 kts.
* - `-4`: RAIM check failed and repair was unsuccessful
* - `-5`: RAIM check failed and repair was impossible (not enough measurements)
* - `-6`: pvt_iter didn't converge
* - `-7`: < 4 measurements
*/
s8 calc_PVT(const u8 n_used,
const navigation_measurement_t nav_meas[n_used],
bool disable_raim,
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];
if (n_used < 4) {
return -7;
}
soln->valid = 0;
soln->n_used = n_used; // Keep track of number of working channels
gnss_signal_t removed_sid;
s8 raim_flag = pvt_solve_raim(rx_state, n_used, nav_meas, disable_raim,
H, &removed_sid, 0);
if (raim_flag < 0) {
/* Didn't converge or least squares integrity check failed. */
return raim_flag - 3;
}
/* Initial solution failed, but repair was successful. */
if (raim_flag == 1) {
soln->n_used--;
}
/* 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];
/* 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 position elements of state if solution fails. */
rx_state[0] = 0;
rx_state[1] = 0;
rx_state[2] = 0;
return -ret;
}
soln->valid = 1;
return raim_flag;
}