gps/soc/software/gps_system/src/gps/gps_ephemeris_cache.c

#include "gps/gps_ephemeris_cache.h"

#include <math.h>
#include <stddef.h>
#include <string.h>

bool gps_ephemeris_cache_same_issue(
	const gps_ephemeris_t* first,
	const gps_ephemeris_t* second)
{
	return (first->SVID == second->SVID) &&
	       (first->iode == second->iode) &&
	       (first->iodc == second->iodc) &&
	       (first->WN == second->WN) &&
	       (first->toe.sec == second->toe.sec) &&
	       (first->toc.sec == second->toc.sec);
}

void gps_ephemeris_cache_init(
	gps_ephemeris_cache_t* cache,
	uint64_t max_age_ticks)
{
	if(cache == NULL)
	{
		return;
	}
	memset(cache, 0, sizeof(*cache));
	cache->max_age_ticks = max_age_ticks;
}

bool gps_ephemeris_cache_store(
	gps_ephemeris_cache_t* cache,
	unsigned int prn,
	const gps_ephemeris_t* ephemeris,
	uint64_t now)
{
	if((cache == NULL) || (ephemeris == NULL) ||
	   (prn >= GPS_PRN_COUNT) ||
	   !ephemeris->ephemeris_valid ||
	   (ephemeris->SVID != prn))
	{
		return false;
	}
	gps_ephemeris_cache_entry_t* entry = &cache->entry[prn];
	bool same_issue = entry->valid &&
		gps_ephemeris_cache_same_issue(&entry->ephemeris, ephemeris);
	entry->ephemeris = *ephemeris;
	/* Repeated reception of the same IOD is useful confirmation but must not
	 * make one broadcast issue immortal in RAM.  Only a genuinely new issue
	 * starts a fresh cache-validity interval. */
	if(!same_issue)
	{
		entry->committed_tick = now;
		entry->age_bias_ticks = 0u;
	}
	entry->valid = true;
	return true;
}

bool gps_ephemeris_cache_import(
	gps_ephemeris_cache_t* cache,
	unsigned int prn,
	const gps_ephemeris_t* ephemeris,
	uint64_t now,
	uint64_t age_ticks)
{
	if((cache == NULL) || (ephemeris == NULL) ||
	   (prn >= GPS_PRN_COUNT) || !ephemeris->ephemeris_valid ||
	   (ephemeris->SVID != prn) || (age_ticks > cache->max_age_ticks))
	{
		return false;
	}
	gps_ephemeris_cache_entry_t* entry = &cache->entry[prn];
	entry->ephemeris = *ephemeris;
	/* RF/sample phase is reset at power-on and is never imported. */
	entry->ephemeris.abs_sample = 0.0;
	entry->committed_tick = now;
	entry->age_bias_ticks = age_ticks;
	memset(&entry->time_anchor, 0, sizeof(entry->time_anchor));
	entry->valid = true;
	return true;
}

bool gps_ephemeris_cache_restore(
	gps_ephemeris_cache_t* cache,
	unsigned int prn,
	uint64_t now,
	gps_ephemeris_t* ephemeris_out)
{
	if((cache == NULL) || (ephemeris_out == NULL) ||
	   (prn >= GPS_PRN_COUNT))
	{
		return false;
	}
	gps_ephemeris_cache_entry_t* entry = &cache->entry[prn];
	if(!entry->valid || (now < entry->committed_tick) ||
	   ((now - entry->committed_tick) >
	    (cache->max_age_ticks -
	     (entry->age_bias_ticks <= cache->max_age_ticks ?
	      entry->age_bias_ticks : cache->max_age_ticks))))
	{
		entry->valid = false;
		return false;
	}
	*ephemeris_out = entry->ephemeris;
	return true;
}

bool gps_ephemeris_cache_store_time_anchor(
	gps_ephemeris_cache_t* cache,
	unsigned int prn,
	const gps_cached_time_anchor_t* anchor,
	uint64_t now)
{
	if((cache == NULL) || (anchor == NULL) ||
	   (prn >= GPS_PRN_COUNT) || !cache->entry[prn].valid ||
	   !anchor->valid || !anchor->week_valid ||
	   (anchor->tow.week < 0) || !isfinite(anchor->tow.sec) ||
	   (anchor->tow.sec < 0.0) ||
	   (anchor->tow.sec >= 604800.0) ||
	   !isfinite(anchor->corrected_sample))
	{
		return false;
	}

	gps_cached_time_anchor_t stored = *anchor;
	stored.committed_tick = now;
	cache->entry[prn].time_anchor = stored;
	return true;
}

bool gps_ephemeris_cache_restore_time_anchor(
	gps_ephemeris_cache_t* cache,
	unsigned int prn,
	uint64_t now,
	uint64_t max_age_ticks,
	gps_cached_time_anchor_t* anchor_out)
{
	if((cache == NULL) || (anchor_out == NULL) ||
	   (prn >= GPS_PRN_COUNT) || (max_age_ticks == 0u))
	{
		return false;
	}

	gps_ephemeris_cache_entry_t* entry = &cache->entry[prn];
	if(!entry->valid || !entry->time_anchor.valid ||
	   !entry->time_anchor.week_valid ||
	   ((now - entry->time_anchor.committed_tick) > max_age_ticks))
	{
		entry->time_anchor.valid = false;
		return false;
	}

	*anchor_out = entry->time_anchor;
	return true;
}

void gps_ephemeris_cache_invalidate(
	gps_ephemeris_cache_t* cache,
	unsigned int prn)
{
	if((cache == NULL) || (prn >= GPS_PRN_COUNT))
	{
		return;
	}
	memset(&cache->entry[prn], 0, sizeof(cache->entry[prn]));
}