static msg_t nav_msg_thread(void *arg)
{
(void)arg;
chRegSetThreadName("nav msg");
for (u8 i=0; i<32; i++) {
es[i].prn = i;
}
while (TRUE) {
chThdSleepMilliseconds(1000);
/* Check if there is a new nav msg subframe to process.
* TODO: move this into a function */
/* TODO: This should be trigged by a semaphore from the tracking loop, not
* just ran periodically. */
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) {
__asm__("CPSID i;");
s8 ret = process_subframe(&tracking_channel[i].nav_msg,
&es[tracking_channel[i].prn]);
__asm__("CPSIE i;");
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);
} else {
sbp_send_msg(MSG_EPHEMERIS, sizeof(ephemeris_t), (u8 *)&es[tracking_channel[i].prn]);
}
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);*/
}
}
}
}
return 0;
}
static msg_t nav_msg_thread(void *arg)
{
(void)arg;
chRegSetThreadName("nav msg");
memset(es, 0, sizeof(es));
for (u8 i=0; i<32; i++) {
es[i].prn = i;
}
while (TRUE) {
/* TODO: This should be trigged by a semaphore from the tracking loop, not
* just ran periodically. */
for (u8 i=0; i<nap_track_n_channels; i++) {
chThdSleepMilliseconds(100);
/* Check if there is a new nav msg subframe to process.
* TODO: move this into a function */
if (tracking_channel[i].state == TRACKING_RUNNING &&
tracking_channel[i].nav_msg.subframe_start_index) {
/* Save old ephemeris before potentially updating. */
memcpy(&es_old[tracking_channel[i].prn],
&es[tracking_channel[i].prn],
sizeof(ephemeris_t));
__asm__("CPSID i;");
s8 ret = process_subframe(&tracking_channel[i].nav_msg,
&es[tracking_channel[i].prn]);
__asm__("CPSIE i;");
if (ret < 0) {
log_info("PRN %02d ret %d\n", tracking_channel[i].prn+1, ret);
} else if (ret == 1) {
/* Decoded a new ephemeris. */
if (memcmp(&es[tracking_channel[i].prn],
&es_old[tracking_channel[i].prn],
sizeof(ephemeris_t))) {
log_info("New ephemeris for PRN %02d\n", tracking_channel[i].prn+1);
}
if (!es[tracking_channel[i].prn].healthy) {
log_info("PRN %02d unhealthy\n", tracking_channel[i].prn+1);
} else {
sbp_send_msg(SBP_MSG_EPHEMERIS,
sizeof(ephemeris_t),
(u8 *)&es[tracking_channel[i].prn]);
}
}
}
}
}
return 0;
}
static void decoder_gps_l1ca_process(const decoder_channel_info_t *channel_info,
decoder_data_t *decoder_data)
{
gps_l1ca_decoder_data_t *data = decoder_data;
/* Process incoming nav bits */
s8 soft_bit;
while (tracking_channel_nav_bit_get(channel_info->tracking_channel,
&soft_bit)) {
/* Update TOW */
bool bit_val = soft_bit >= 0;
s32 TOW_ms = nav_msg_update(&data->nav_msg, bit_val);
s8 bit_polarity = data->nav_msg.bit_polarity;
if ((TOW_ms >= 0) && (bit_polarity != BIT_POLARITY_UNKNOWN)) {
if (!tracking_channel_time_sync(channel_info->tracking_channel, TOW_ms,
bit_polarity)) {
log_warn_sid(channel_info->sid, "TOW set failed");
}
}
}
/* Check if there is a new nav msg subframe to process. */
if (!subframe_ready(&data->nav_msg))
return;
/* Decode ephemeris to temporary struct */
ephemeris_t e = {.sid = channel_info->sid};
s8 ret = process_subframe(&data->nav_msg, &e);;
if (ret <= 0)
return;
/* Decoded a new ephemeris. */
ephemeris_new(&e);
ephemeris_t *eph = ephemeris_get(channel_info->sid);
if (!eph->valid) {
log_info_sid(channel_info->sid, "ephemeris is invalid");
}
}
static msg_t nav_msg_thread(void *arg)
{
(void)arg;
chRegSetThreadName("nav msg");
while (TRUE) {
/* TODO: This should be trigged by a semaphore from the tracking loop, not
* just ran periodically. */
for (u8 i=0; i<nap_track_n_channels; i++) {
chThdSleepMilliseconds(100);
tracking_channel_t *ch = &tracking_channel[i];
ephemeris_t e = {.sid = ch->sid};
/* Check if there is a new nav msg subframe to process.
* TODO: move this into a function */
if ((ch->state != TRACKING_RUNNING) ||
(ch->nav_msg.subframe_start_index == 0))
continue;
/* Decode ephemeris to temporary struct */
__asm__("CPSID i;");
s8 ret = process_subframe(&ch->nav_msg, &e);
__asm__("CPSIE i;");
if (ret <= 0)
continue;
/* Decoded a new ephemeris. */
ephemeris_new(&e);
if (!es[ch->sid.sat].healthy) {
log_info("PRN %02d unhealthy", ch->sid.sat+1);
} else {
msg_ephemeris_t msg;
pack_ephemeris(&es[ch->sid.sat], &msg);
sbp_send_msg(SBP_MSG_EPHEMERIS, sizeof(msg_ephemeris_t), (u8 *)&msg);
}
}
}
return 0;
}
static void ephemeris_msg_callback(u16 sender_id, u8 len, u8 msg[], void* context)
{
(void)sender_id; (void)context;
if (len != sizeof(msg_ephemeris_t)) {
log_warn("Received bad ephemeris from peer");
return;
}
ephemeris_t e;
unpack_ephemeris((msg_ephemeris_t *)msg, &e);
if (e.sid.sat >= MAX_SATS) {
log_warn("Ignoring ephemeris for invalid sat");
return;
}
ephemeris_new(&e);
}
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);
);
}
