void dgnss_init(u8 num_sats, sdiff_t *sdiffs, double receiver_ecef[3])
{
DEBUG_ENTRY();
sdiff_t corrected_sdiffs[num_sats];
init_sats_management(&sats_management, num_sats, sdiffs, corrected_sdiffs);
create_ambiguity_test(&ambiguity_test);
if (num_sats <= 1) {
DEBUG_EXIT();
return;
}
double dd_measurements[2*(num_sats-1)];
make_measurements(num_sats-1, corrected_sdiffs, dd_measurements);
set_nkf(
&nkf,
dgnss_settings.amb_drift_var,
dgnss_settings.phase_var_kf, dgnss_settings.code_var_kf,
dgnss_settings.amb_init_var,
num_sats, corrected_sdiffs, dd_measurements, receiver_ecef
);
DEBUG_EXIT();
}
void dgnss_init(u8 num_sats, sdiff_t *sdiffs, double reciever_ecef[3], double dt)
{
sdiff_t corrected_sdiffs[num_sats];
init_sats_management(&sats_management, num_sats, sdiffs, corrected_sdiffs);
double dd_measurements[2*(num_sats-1)];
make_measurements(num_sats-1, corrected_sdiffs, dd_measurements);
get_kf(
&kf,
dgnss_settings.phase_var_kf, dgnss_settings.code_var_kf,
dgnss_settings.pos_trans_var, dgnss_settings.vel_trans_var,
dgnss_settings.int_trans_var,
dgnss_settings.pos_init_var, dgnss_settings.vel_init_var,
dgnss_settings.amb_init_var,
num_sats, corrected_sdiffs, dd_measurements, reciever_ecef,
dt
);
set_nkf(
&nkf,
dgnss_settings.phase_var_kf, dgnss_settings.code_var_kf,
dgnss_settings.amb_init_var,
num_sats, corrected_sdiffs, dd_measurements, reciever_ecef
);
create_ambiguity_test(&ambiguity_test);
}
/** Updates sats to the new measurements' sat set
*/
s8 rebase_sats_management(sats_management_t *sats_management,
const u8 num_sdiffs, const sdiff_t *sdiffs, sdiff_t *sdiffs_with_ref_first)
{
DEBUG_ENTRY();
s8 return_code;
u8 ref_prn;
if (sats_management->num_sats <= 1) {
// Need to init first.
init_sats_management(sats_management, num_sdiffs, sdiffs, 0);
}
// Check if old reference is in sdiffs
if (bsearch(&(sats_management->prns[0]), sdiffs, num_sdiffs, sizeof(sdiff_t), &sdiff_search_prn)) {
ref_prn = sats_management->prns[0];
return_code = OLD_REF;
}
else {
sdiff_t intersection_sats[num_sdiffs];
u8 num_intersection = intersect_sats(sats_management->num_sats-1, num_sdiffs,
&(sats_management->prns[1]), sdiffs, intersection_sats);
if (num_intersection < INTERSECTION_SATS_THRESHOLD_SIZE) {
DEBUG_EXIT();
return NEW_REF_START_OVER;
}
else {
if (DEBUG) {
printf("sdiff prns= {");
for (u8 yo_mama=0; yo_mama< num_sdiffs; yo_mama++) {
printf("%u, ", sdiffs[yo_mama].prn);
}
printf("}\n");
printf("sats_man_prns= {");
for (u8 so_fetch=0; so_fetch < sats_management->num_sats; so_fetch++) {
printf("%u, ", sats_management->prns[so_fetch]);
}
printf("}\n");
printf("num intersect_sats= %u\nintersection= {", num_intersection);
for (u8 bork=0; bork<num_intersection; bork++) {
printf("%u, ", intersection_sats[bork].prn);
}
printf("}\n");
}
ref_prn = choose_reference_sat(num_intersection, intersection_sats);
return_code = NEW_REF;
}
}
set_reference_sat(ref_prn, sats_management,
num_sdiffs, sdiffs, sdiffs_with_ref_first);
DEBUG_EXIT();
return return_code;
}
