/** Decode an RTCMv3 message type 1002 (Extended L1-Only GPS RTK Observables)
*
* \param buff A pointer to the RTCM data message buffer.
* \param id Reference station ID (DF003).
* \param tow GPS time of week of epoch (DF004).
* \param n_sat Number of GPS satellites included in the message (DF006).
* \param nm Struct containing the observation.
* \param sync Synchronous GNSS Flag (DF005).
* \return If valid then return 0.
* Returns a negative number if the message is invalid:
* - `-1` : Message type mismatch
* - `-2` : Message uses unsupported P(Y) code
*/
s8 rtcm3_decode_1002(u8 *buff, u16 *id, double *tow, u8 *n_sat,
navigation_measurement_t *nm, u8 *sync)
{
u16 type;
u8 div_free, smooth;
rtcm3_read_header(buff, &type, id, tow, sync, n_sat, &div_free, &smooth);
if (type != 1002)
/* Unexpected message type. */
return -1;
/* TODO: Fill in t->wn. */
if (!nm)
/* No nav meas pointer, probably just interested in
* n_sat so we are all done. */
return 0;
u16 bit = 64;
for (u8 i=0; i<*n_sat; i++) {
/* TODO: Handle SBAS prns properly, numbered differently in RTCM? */
nm[i].sid.sat = getbitu(buff, bit, 6); bit += 6;
u8 code = getbitu(buff, bit, 1); bit += 1;
/* TODO: When we start storing the signal/system etc. properly we can
* store the code flag in the nav meas struct. */
if (code == 1)
/* P(Y) code not currently supported. */
return -2;
u32 pr = getbitu(buff, bit,24); bit += 24;
s32 ppr = getbits(buff, bit,20); bit += 20;
u8 lock = getbitu(buff, bit, 7); bit += 7;
u8 amb = getbitu(buff, bit, 8); bit += 8;
u8 cnr = getbitu(buff, bit, 8); bit += 8;
nm[i].raw_pseudorange = 0.02*pr + PRUNIT_GPS*amb;
nm[i].carrier_phase = (nm[i].raw_pseudorange + 0.0005*ppr) / (CLIGHT / FREQ1);
nm[i].lock_time = from_lock_ind(lock);
nm[i].snr = pow(10.0, ((cnr / 4.0) - 40.0) / 10.0);
}
return 0;
}
END_TEST
START_TEST(test_rtcm3_read_write_header)
{
u8 buff[22];
gps_time_t t = {
.wn = 22,
.tow = 22.222
};
rtcm3_write_header(buff, 1234, 2269, t, 1, 22, 1, 6);
u16 type, id;
u8 sync, n_sat, div_free, smooth;
double tow;
rtcm3_read_header(buff, &type, &id, &tow, &sync, &n_sat, &div_free, &smooth);
fail_unless(type == 1234, "type decode error, decoded %d, expected 1234", type);
fail_unless(id == 2269, "id decode error, decoded %d, expected 2269", id);
fail_unless(fabs(tow - t.tow) < 1e3, "TOW decode error, decoded %f, expected %f", tow, t.tow);
fail_unless(sync == 1, "id decode error, decoded %d, expected 1", id);
fail_unless(n_sat == 22, "n_sat decode error, decoded %d, expected 22", n_sat);
fail_unless(div_free == 1, "div_free decode error, decoded %d, expected 1", div_free);
fail_unless(smooth == 6, "smooth decode error, decoded %d, expected 6", smooth);
}
END_TEST
START_TEST(test_rtcm3_encode_decode)
{
navigation_measurement_t nm_orig[22];
navigation_measurement_t nm[22];
seed_rng();
for (u8 i=0; i<22; i++) {
nm[i].raw_pseudorange = frand(19e6, 21e6);
nm[i].carrier_phase = frand(-5e5, 5e5);
nm[i].lock_time = frand(0, 1000);
nm[i].snr = frand(0, 20);
}
memcpy(nm_orig, nm, sizeof(nm));
gps_time_t t = {
.wn = 1234,
.tow = frand(0, 604800)
};
u8 buff[355];
rtcm3_encode_1002(buff, 1234, t, 22, nm, 0);
navigation_measurement_t nm_out[22];
double tow_out;
u8 sync, n_sat = 222;
u16 id;
s8 ret = rtcm3_decode_1002(buff, &id, &tow_out, &n_sat, 0, &sync);
fail_unless(ret >= 0, "rtcm3_decode_1002 returned an error (%d)", ret);
fail_unless(id == 1234, "decoded station id as %d, expected 1234", id);
fail_unless(n_sat == 22, "decoded n_sat as %d, expected 22", n_sat);
fail_unless(fabs(tow_out - t.tow) < 1e-3, "decoded TOW as %f, expected %f, error %f",
tow_out, t.tow, tow_out - t.tow);
ret = rtcm3_decode_1002(buff, &id, &tow_out, &n_sat, nm_out, &sync);
for (u8 i=0; i<22; i++) {
double pr_err = nm[i].raw_pseudorange - nm_out[i].raw_pseudorange;
fail_unless(fabs(pr_err) < 0.02, "[%d] pseudorange error > 0.04m - "
"decoded %f, expected %f, error %f", i, nm_out[i].raw_pseudorange, nm[i].raw_pseudorange, pr_err);
double carr_err = nm[i].carrier_phase - nm_out[i].carrier_phase;
fail_unless(fabs(carr_err) < 0.003, "carrier phase error (fractional part) > 0.003 cycles - "
"[%d] decoded %f, expected %f, error %f", i, nm_out[i].carrier_phase, nm[i].carrier_phase, carr_err);
double snr_err = nm[i].snr - nm_out[i].snr;
/* Calculate error bound on SNR given logarithmic error bound on CNR. */
double err_bound = nm[i].snr * (pow(10.0, 1.0 / 40.0) - 1);
fail_unless(fabs(snr_err) < err_bound, "SNR error > 0.003 - "
"[%d] decoded %f, expected %f, error %f, bound %f", i, nm_out[i].snr, nm[i].snr, snr_err, err_bound);
fail_unless((nm_out[i].lock_time == 0) && (nm[i].lock_time == 0),
"lock time should be zero when adjusting int. amb. - [%d] decoded %f",
i, nm_out[i].lock_time, nm[i].lock_time);
double cp_adj = nm[i].carrier_phase - nm_orig[i].carrier_phase;
fail_unless(fmod(cp_adj, 1.0) == 0,
"carrier phase adjusted by non integer amount %f -> %f (%f)",
nm_orig[i].carrier_phase, nm[i].carrier_phase, cp_adj);
}
/* Re-encode after adjustment, now there should be no further adjustment and
* the lock time should be correct. */
for (u8 i=0; i<22; i++)
nm[i].lock_time = frand(0, 1000);
rtcm3_encode_1002(buff, 1234, t, 22, nm, 0);
rtcm3_decode_1002(buff, &id, &tow_out, &n_sat, nm_out, &sync);
for (u8 i=0; i<22; i++) {
double cp_adj = nm_out[i].carrier_phase - nm[i].carrier_phase;
fail_unless(cp_adj < 0.003, "carrier phase re-adjusted %f -> %f (%f)",
nm[i].carrier_phase, nm_out[i].carrier_phase, cp_adj);
fail_unless(nm_out[i].lock_time <= nm[i].lock_time,
"lock time error, should always be less than input lock time - [%d] decoded %f, expected %f",
i, nm_out[i].lock_time, nm[i].lock_time);
}
}
END_TEST
Suite* rtcm3_suite(void)
{
Suite *s = suite_create("RTCMv3");
TCase *tc_core = tcase_create("Core");
tcase_add_test(tc_core, test_rtcm3_check_frame);
tcase_add_test(tc_core, test_rtcm3_write_frame);
tcase_add_test(tc_core, test_rtcm3_read_write_header);
tcase_add_test(tc_core, test_rtcm3_encode_decode);
suite_add_tcase(s, tc_core);
return s;
}
