/** 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;
}
void init_sats_management(sats_management_t *sats_management,
const u8 num_sdiffs, const sdiff_t *sdiffs, sdiff_t *sdiffs_with_ref_first)
{
DEBUG_ENTRY();
if (num_sdiffs == 0) {
sats_management->num_sats = 0;
DEBUG_EXIT();
return;
}
u8 ref_prn = choose_reference_sat(num_sdiffs, sdiffs);
set_reference_sat_and_prns(ref_prn, sats_management,
num_sdiffs, sdiffs, sdiffs_with_ref_first);
DEBUG_EXIT();
}
/** Calculate least squares baseline solution from a set of single difference
* observations and carrier phase ambiguities.
*
* \param num_sdiffs Number of single difference observations
* \param sdiffs Set of single differenced observations, length
* `num_sdiffs`, sorted by PRN
* \param ref_ecef The reference position in ECEF frame, for computing the
* sat direction vectors
* \param num_ambs Number of carrier phase ambiguities
* \param single_ambs Array of (carrier phase ambiguity, prn) pairs.
* length `num_ambs`
* \param num_used Pointer to where to store number of satellites used in the
* baseline solution
* \param b The output baseline in meters
* \param disable_raim True disables raim check/repair
* \param raim_threshold Threshold for raim checks.
* \return 0 on success,
* -1 if there were insufficient observations to calculate the
* baseline (the solution was under-constrained),
* -2 if an error occurred
*/
s8 baseline_(u8 num_sdiffs, const sdiff_t *sdiffs, const double ref_ecef[3],
u8 num_ambs, const ambiguity_t *single_ambs,
u8 *num_used, double b[3],
bool disable_raim, double raim_threshold)
{
if (num_sdiffs < 4 || num_ambs < 4) {
/* For a position solution, we need at least 4 sats. */
return -1;
}
assert(sdiffs != NULL);
assert(ref_ecef != NULL);
assert(single_ambs != NULL);
assert(num_used != NULL);
assert(b != NULL);
assert(is_set(num_ambs, sizeof(ambiguity_t), single_ambs, cmp_amb));
assert(is_set(num_sdiffs, sizeof(sdiff_t), sdiffs, cmp_sdiff));
assert(num_sdiffs <= MAX_CHANNELS);
/* Could use min(num_ambs, num_sdiffs) */
ambiguity_t intersection_ambs[num_ambs];
sdiff_t intersection_sdiffs[num_ambs];
s32 intersection_size = intersection(
num_ambs, sizeof(ambiguity_t), single_ambs, intersection_ambs,
num_sdiffs, sizeof(sdiff_t), sdiffs, intersection_sdiffs,
cmp_amb_sdiff);
if (intersection_size < 4) {
/* For a position solution, we need at least 4 sats. */
return -1;
}
u8 num_dds = intersection_size - 1;
/* Choose ref sat based on SNR. */
gnss_signal_t ref_sid = choose_reference_sat(intersection_size, intersection_sdiffs);
/* Calculate double differenced measurements. */
sdiff_t sdiff_ref_first[intersection_size];
u32 sdiff_ref_index = remove_element(intersection_size, sizeof(sdiff_t),
intersection_sdiffs,
&(sdiff_ref_first[1]), /* New set */
&ref_sid, cmp_sdiff_sid);
memcpy(sdiff_ref_first, &intersection_sdiffs[sdiff_ref_index],
sizeof(sdiff_t));
double dd_meas[2 * num_dds];
for (u32 i = 0; i < num_dds; i++) {
dd_meas[i] =
sdiff_ref_first[i+1].carrier_phase - sdiff_ref_first[0].carrier_phase;
dd_meas[i + num_dds] =
sdiff_ref_first[i+1].pseudorange - sdiff_ref_first[0].pseudorange;
}
double DE[num_dds * 3];
assign_de_mtx(intersection_size, sdiff_ref_first, ref_ecef, DE);
/* Calculate double differenced ambiguities. */
double dd_ambs[num_dds];
diff_ambs(ref_sid, intersection_size, intersection_ambs, dd_ambs);
/* Compute least squares solution. */
*num_used = intersection_size;
return lesq_solve_raim(num_dds, dd_meas, dd_ambs, DE, b,
disable_raim, raim_threshold, 0, 0, 0);
}