static void decode_thread(void *arg)
{
(void)arg;
chRegSetThreadName("decode");
while (TRUE) {
for (u32 i=0; i<NUM_DECODER_CHANNELS; i++) {
decoder_channel_t *d = &decoder_channels[i];
switch (decoder_channel_state_get(d)) {
case DECODER_CHANNEL_STATE_ENABLED: {
const decoder_interface_t *interface = decoder_interface_get(d->info.sid);
interface_function(d, interface->process);
}
break;
case DECODER_CHANNEL_STATE_DISABLE_REQUESTED: {
const decoder_interface_t *interface = decoder_interface_get(d->info.sid);
interface_function(d, interface->disable);
event(d, EVENT_DISABLE);
}
break;
default:
break;
}
}
chThdSleep(MS2ST(1));
}
}
/** Check the state of a tracker channel and generate events as required.
* \param tracker_channel Tracker channel to use.
* \param update_required True when correlations are pending for the
* tracking channel.
*/
static void tracker_channel_process(tracker_channel_t *tracker_channel,
bool update_required)
{
switch (tracker_channel_state_get(tracker_channel)) {
case STATE_ENABLED: {
if (update_required) {
tracker_channel_lock(tracker_channel);
{
interface_function(tracker_channel,
tracker_channel->interface->update);
}
tracker_channel_unlock(tracker_channel);
}
}
break;
case STATE_DISABLE_REQUESTED: {
nap_channel_disable(tracker_channel);
tracker_channel_lock(tracker_channel);
{
interface_function(tracker_channel,
tracker_channel->interface->disable);
tracker_channel->disable_time = chVTGetSystemTimeX();
event(tracker_channel, EVENT_DISABLE);
}
tracker_channel_unlock(tracker_channel);
}
break;
case STATE_DISABLE_WAIT: {
nap_channel_disable(tracker_channel);
if (chVTTimeElapsedSinceX(tracker_channel->disable_time) >=
MS2ST(CHANNEL_DISABLE_WAIT_TIME_ms)) {
event(tracker_channel, EVENT_DISABLE_WAIT_COMPLETE);
}
}
break;
case STATE_DISABLED: {
if (update_required) {
/* Tracking channel is not owned by the update thread, but the update
* register must be written to clear the interrupt flag. Set error
* flag to indicate that NAP is in an unknown state. */
nap_channel_disable(tracker_channel);
error_flags_add(tracker_channel, ERROR_FLAG_INTERRUPT_WHILE_DISABLED);
}
}
break;
default: {
assert(!"Invalid tracking channel state");
}
break;
}
}
/** Initialize a decoder channel to process telemetry for sid from the
* specified tracking channel.
*
* \param tracking_channel Tracking channel to use.
* \param sid Signal to be decoded.
*
* \return true if a decoder channel was initialized, false otherwise.
*/
bool decoder_channel_init(u8 tracking_channel, gnss_signal_t sid)
{
decoder_channel_t *d = decoder_channel_get(tracking_channel);
if (decoder_channel_state_get(d) != DECODER_CHANNEL_STATE_DISABLED)
return false;
const decoder_interface_t *interface = decoder_interface_get(sid);
decoder_t *decoder;
if (!available_decoder_get(interface, &decoder))
return false;
/* Set up channel */
d->info.tracking_channel = tracking_channel;
d->info.sid = sid;
d->decoder = decoder;
/* Empty the nav bit FIFO */
s8 soft_bit;
while (tracking_channel_nav_bit_get(d->info.tracking_channel, &soft_bit)) {
;
}
interface_function(d, interface->init);
event(d, EVENT_ENABLE);
return true;
}
/** Initialize a tracker channel to track the specified sid.
*
* \param id ID of the tracker channel to be initialized.
* \param sid Signal to be tracked.
* \param ref_sample_count NAP sample count at which code_phase was acquired.
* \param code_phase Code phase
* \param carrier_freq Carrier frequency Doppler (Hz).
* \param cn0_init Initial C/N0 estimate (dBHz).
* \param elevation Elevation (deg).
*
* \return true if the tracker channel was initialized, false otherwise.
*/
bool tracker_channel_init(tracker_channel_id_t id, gnss_signal_t sid,
u32 ref_sample_count, float code_phase,
float carrier_freq, float cn0_init, s8 elevation)
{
tracker_channel_t *tracker_channel = tracker_channel_get(id);
const tracker_interface_t *tracker_interface;
tracker_t *tracker;
if(!tracker_channel_runnable(tracker_channel, sid, &tracker,
&tracker_interface)) {
return false;
}
tracker_channel_lock(tracker_channel);
{
/* Set up channel */
tracker_channel->info.sid = sid;
tracker_channel->info.context = tracker_channel;
tracker_channel->info.nap_channel = id;
tracker_channel->interface = tracker_interface;
tracker_channel->tracker = tracker;
tracker_channel->elevation = elevation;
common_data_init(&tracker_channel->common_data, ref_sample_count,
carrier_freq, cn0_init);
internal_data_init(&tracker_channel->internal_data, sid);
interface_function(tracker_channel, tracker_interface->init);
/* Clear error flags before starting NAP tracking channel */
error_flags_clear(tracker_channel);
/* Change the channel state to ENABLED. */
event(tracker_channel, EVENT_ENABLE);
}
tracker_channel_unlock(tracker_channel);
nap_track_init(tracker_channel->info.nap_channel, sid, ref_sample_count,
carrier_freq, code_phase);
return true;
}
