static bool sids_match(const gnss_signal_t *old_non_ref_sids, u16 num_non_ref_sdiffs,
const sdiff_t *non_ref_sdiffs)
{
if (sats_management.num_sats-1 != num_non_ref_sdiffs) {
/* lengths don't match */
return false;
}
for (u8 i=0; i<num_non_ref_sdiffs; i++) {
/* iterate through the non-reference_sats, checking they match. */
if (!sid_is_equal(old_non_ref_sids[i], non_ref_sdiffs[i].sid)) {
return false;
}
}
return true;
}
/** Set the elevation angle for a tracker channel by sid.
*
* \param sid Signal identifier for which the elevation should be set.
* \param elevation Elevation angle (deg).
*/
bool tracking_channel_evelation_degrees_set(gnss_signal_t sid, s8 elevation)
{
bool result = false;
for (u32 i=0; i < NUM_TRACKER_CHANNELS; i++) {
tracker_channel_t *tracker_channel = tracker_channel_get(i);
/* Check SID before locking. */
if (!sid_is_equal(tracker_channel->info.sid, sid)) {
continue;
}
/* Lock and update if SID matches. */
tracker_channel_lock(tracker_channel);
{
if (sid_is_equal(tracker_channel->info.sid, sid)) {
tracker_channel->elevation = elevation;
result = true;
}
}
tracker_channel_unlock(tracker_channel);
break;
}
return result;
}
static u8 get_de_and_phase(sats_management_t *sats_man,
u8 num_sdiffs, sdiff_t *sdiffs,
double ref_ecef[3],
double *de, double *phase)
{
gnss_signal_t ref_sid = sats_man->sids[0];
u8 num_sats = sats_man->num_sats;
double e0[3];
double phi0 = 0;
/* TODO: Detect if ref_sid is not in sids and return error? */
u8 i;
for (i=0; i<num_sdiffs; i++) {
if (sid_is_equal(sdiffs[i].sid, ref_sid)) {
e0[0] = sdiffs[i].sat_pos[0] - ref_ecef[0];
e0[1] = sdiffs[i].sat_pos[1] - ref_ecef[1];
e0[2] = sdiffs[i].sat_pos[2] - ref_ecef[2];
vector_normalize(3, e0);
phi0 = sdiffs[i].carrier_phase;
break;
}
}
i=1;
u8 j = 0;
while (i < num_sats) {
if (sid_compare(sdiffs[j].sid, sats_man->sids[i]) < 0) {
j++;
}
else if (sid_compare(sdiffs[j].sid, sats_man->sids[i]) > 0) {
/* This should never happen. */
log_warn("sdiffs should be a super set of sats_man sids");
i++;
}
else { /* else they match */
double e[3];
e[0] = sdiffs[j].sat_pos[0] - ref_ecef[0];
e[1] = sdiffs[j].sat_pos[1] - ref_ecef[1];
e[2] = sdiffs[j].sat_pos[2] - ref_ecef[2];
vector_normalize(3, e);
de[(i-1)*3 ] = e[0] - e0[0];
de[(i-1)*3 + 1] = e[1] - e0[1];
de[(i-1)*3 + 2] = e[2] - e0[2];
phase[i-1] = sdiffs[j].carrier_phase - phi0;
i++;
j++;
}
}
return num_sats;
}
