/** Encode an RTCMv3 message type 1002 (Extended L1-Only GPS RTK Observables)
* Message type 1002 has length `64 + n_sat*74` bits. Returned message length
* is rounded up to the nearest whole byte.
*
* \param buff A pointer to the RTCM data message buffer.
* \param id Reference station ID (DF003).
* \param t GPS time 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 The message length in bytes.
*/
u16 rtcm3_encode_1002(u8 *buff, u16 id, gps_time_t t, u8 n_sat,
navigation_measurement_t *nm, u8 sync)
{
rtcm3_write_header(buff, 1002, id, t, sync, n_sat, 0, 0);
u16 bit = 64; /* Start at end of header. */
u32 pr;
s32 ppr;
u8 amb, lock, cnr;
for (u8 i=0; i<n_sat; i++) {
gen_obs_gps(&nm[i], &amb, &pr, &ppr, &lock, &cnr);
setbitu(buff, bit, 6, nm[i].sid.sat); bit += 6;
/* TODO: set GPS code indicator if we ever support P(Y) code measurements. */
setbitu(buff, bit, 1, 0); bit += 1;
setbitu(buff, bit, 24, pr); bit += 24;
setbits(buff, bit, 20, ppr); bit += 20;
setbitu(buff, bit, 7, lock); bit += 7;
setbitu(buff, bit, 8, amb); bit += 8;
setbitu(buff, bit, 8, cnr); bit += 8;
}
/* Round number of bits up to nearest whole byte. */
return (bit + 7) / 8;
}
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;
}
