/** Update the #base_obss state given a new set of obss.
* First sorts by PRN and computes the TDCP Doppler for the observation set. If
* #base_pos_known is false then a single point position solution is also
* calculated. Next the `has_pos`, `pos_ecef` and `sat_dists` fields are filled
* in. Finally the #base_obs_received semaphore is flagged to indicate that new
* observations are available.
*
* \note This function is stateful as it must store the previous observation
* set for the TDCP Doppler.
*/
static void update_obss(obss_t *new_obss)
{
/* Ensure observations sorted by PRN. */
qsort(new_obss->nm, new_obss->n,
sizeof(navigation_measurement_t), nav_meas_cmp);
/* Lock mutex before modifying base_obss.
* NOTE: We didn't need to lock it before reading in THIS context as this
* is the only thread that writes to base_obss. */
chMtxLock(&base_obs_lock);
/* Create a set of navigation measurements to store the previous
* observations. */
static u8 n_old = 0;
static navigation_measurement_t nm_old[MAX_CHANNELS];
/* Fill in the navigation measurements in base_obss, using TDCP method to
* calculate the Doppler shift. */
base_obss.n = tdcp_doppler(new_obss->n, new_obss->nm,
n_old, nm_old, base_obss.nm);
/* Copy over the time. */
base_obss.t = new_obss->t;
/* Copy the current observations over to nm_old so we can difference
* against them next time around. */
memcpy(nm_old, new_obss->nm,
new_obss->n * sizeof(navigation_measurement_t));
n_old = new_obss->n;
/* Reset the `has_pos` flag. */
u8 has_pos_old = base_obss.has_pos;
base_obss.has_pos = 0;
/* Check if the base station has sent us its position explicitly via a
* BASE_POS SBP message (as indicated by #base_pos_known), and if so use
* that. No need to lock before reading here as base_pos_* is only written
* from this thread (SBP).
*/
if (base_pos_known) {
/* Copy the known base station position into `base_obss`. */
memcpy(base_obss.pos_ecef, base_pos_ecef, sizeof(base_pos_ecef));
/* Indicate that the position is valid. */
base_obss.has_pos = 1;
/* The base station wasn't sent to us explicitly but if we have >= 4
* satellites we can calculate it ourselves (approximately). */
} else if (base_obss.n >= 4) {
gnss_solution soln;
dops_t dops;
/* Calculate a position solution. */
/* disable_raim controlled by external setting (see solution.c). */
s32 ret = calc_PVT(base_obss.n, base_obss.nm, disable_raim, &soln, &dops);
if (ret >= 0 && soln.valid) {
/* The position solution calculation was sucessful. Unfortunately the
* single point position solution is very noisy so lets smooth it if we
* have the previous position available. */
if (has_pos_old) {
/* TODO Implement a real filter for base position (potentially in
observation space), so we can do away with this terrible excuse
for smoothing. */
base_obss.pos_ecef[0] = 0.99995 * base_obss.pos_ecef[0]
+ 0.00005 * soln.pos_ecef[0];
base_obss.pos_ecef[1] = 0.99995 * base_obss.pos_ecef[1]
+ 0.00005 * soln.pos_ecef[1];
base_obss.pos_ecef[2] = 0.99995 * base_obss.pos_ecef[2]
+ 0.00005 * soln.pos_ecef[2];
} else {
memcpy(base_obss.pos_ecef, soln.pos_ecef, 3 * sizeof(double));
}
base_obss.has_pos = 1;
} else {
/* TODO(dsk) check for repair failure */
/* There was an error calculating the position solution. */
log_warn("Error calculating base station position: (%s).", pvt_err_msg[-ret-1]);
}
}
/* If the base station position is known then calculate the satellite ranges.
* This calculation will be used later by the propagation functions. */
if (base_obss.has_pos) {
for (u8 i=0; i < base_obss.n; i++) {
double dx[3];
vector_subtract(3, base_obss.nm[i].sat_pos, base_obss.pos_ecef, dx);
base_obss.sat_dists[i] = vector_norm(3, dx);
}
}
/* Unlock base_obss mutex. */
chMtxUnlock();
/* Signal that a complete base observation has been received. */
chBSemSignal(&base_obs_received);
}
int main(void)
{
init();
led_toggle(LED_RED);
printf("\n\nFirmware info - git: " GIT_VERSION ", built: " __DATE__ " " __TIME__ "\n");
u8 nap_git_hash[20];
nap_conf_rd_git_hash(nap_git_hash);
printf("SwiftNAP git: ");
for (u8 i=0; i<20; i++)
printf("%02x", nap_git_hash[i]);
if (nap_conf_rd_git_unclean())
printf(" (unclean)");
printf("\n");
printf("SwiftNAP configured with %d tracking channels\n\n", nap_track_n_channels);
cw_setup();
manage_acq_setup();
tick_timer_setup();
timing_setup();
position_setup();
channel_measurement_t meas[nap_track_n_channels];
navigation_measurement_t nav_meas[nap_track_n_channels];
/* TODO: Think about thread safety when updating ephemerides. */
static ephemeris_t es[32];
static ephemeris_t es_old[32];
while(1)
{
for (u32 i = 0; i < 3000; i++)
__asm__("nop");
sbp_process_messages();
manage_track();
manage_acq();
/* Check if there is a new nav msg subframe to process.
* TODO: move this into a function */
memcpy(es_old, es, sizeof(es));
for (u8 i=0; i<nap_track_n_channels; i++)
if (tracking_channel[i].state == TRACKING_RUNNING && tracking_channel[i].nav_msg.subframe_start_index) {
s8 ret = process_subframe(&tracking_channel[i].nav_msg, &es[tracking_channel[i].prn]);
if (ret < 0)
printf("PRN %02d ret %d\n", tracking_channel[i].prn+1, ret);
if (ret == 1 && !es[tracking_channel[i].prn].healthy)
printf("PRN %02d unhealthy\n", tracking_channel[i].prn+1);
if (memcmp(&es[tracking_channel[i].prn], &es_old[tracking_channel[i].prn], sizeof(ephemeris_t))) {
printf("New ephemeris for PRN %02d\n", tracking_channel[i].prn+1);
/* TODO: This is a janky way to set the time... */
gps_time_t t;
t.wn = es[tracking_channel[i].prn].toe.wn;
t.tow = tracking_channel[i].TOW_ms / 1000.0;
if (gpsdifftime(t, es[tracking_channel[i].prn].toe) > 2*24*3600)
t.wn--;
else if (gpsdifftime(t, es[tracking_channel[i].prn].toe) < 2*24*3600)
t.wn++;
set_time(TIME_COARSE, t);
}
}
DO_EVERY_TICKS(TICK_FREQ/10,
u8 n_ready = 0;
for (u8 i=0; i<nap_track_n_channels; i++) {
if (es[tracking_channel[i].prn].valid == 1 && \
es[tracking_channel[i].prn].healthy == 1 && \
tracking_channel[i].state == TRACKING_RUNNING && \
tracking_channel[i].TOW_ms > 0) {
__asm__("CPSID i;");
tracking_update_measurement(i, &meas[n_ready]);
__asm__("CPSIE i;");
n_ready++;
}
}
if (n_ready >= 4) {
/* Got enough sats/ephemerides, do a solution. */
/* TODO: Instead of passing 32 LSBs of nap_timing_count do something
* more intelligent with the solution time.
*/
calc_navigation_measurement(n_ready, meas, nav_meas, (double)((u32)nap_timing_count())/SAMPLE_FREQ, es);
dops_t dops;
if (calc_PVT(n_ready, nav_meas, &position_solution, &dops) == 0) {
position_updated();
sbp_send_msg(MSG_SOLUTION, sizeof(gnss_solution), (u8 *) &position_solution);
nmea_gpgga(&position_solution, &dops);
DO_EVERY(10,
sbp_send_msg(MSG_DOPS, sizeof(dops_t), (u8 *) &dops);
nmea_gpgsv(n_ready, nav_meas, &position_solution);
);
}
