89 lines
2.7 KiB
C
89 lines
2.7 KiB
C
#include "gps_time_anchor.h"
|
|
|
|
#include <math.h>
|
|
#include <stddef.h>
|
|
|
|
static double gps_time_normalize_week(double tow_sec, int* week)
|
|
{
|
|
while(tow_sec >= GPS_TIME_WEEK_SECONDS)
|
|
{
|
|
tow_sec -= GPS_TIME_WEEK_SECONDS;
|
|
(*week)++;
|
|
}
|
|
while(tow_sec < 0.0)
|
|
{
|
|
tow_sec += GPS_TIME_WEEK_SECONDS;
|
|
(*week)--;
|
|
}
|
|
return tow_sec;
|
|
}
|
|
|
|
int gps_time_resolve_how_week(bool previous_valid,
|
|
int previous_week,
|
|
double previous_tow_sec,
|
|
int fallback_week,
|
|
double how_tow_sec)
|
|
{
|
|
if(!previous_valid || !isfinite(previous_tow_sec) || !isfinite(how_tow_sec))
|
|
{
|
|
return fallback_week;
|
|
}
|
|
|
|
int week = previous_week;
|
|
double delta_sec = how_tow_sec - previous_tow_sec;
|
|
if(delta_sec < -GPS_TIME_HALF_WEEK_SECONDS)
|
|
{
|
|
week++;
|
|
}
|
|
else if(delta_sec > GPS_TIME_HALF_WEEK_SECONDS)
|
|
{
|
|
week--;
|
|
}
|
|
return week;
|
|
}
|
|
|
|
bool gps_time_from_sample_anchor(int anchor_week,
|
|
double anchor_tow_sec,
|
|
uint32_t anchor_raw_sample,
|
|
double anchor_corrected_sample,
|
|
uint32_t event_raw_sample,
|
|
double event_corrected_sample,
|
|
double samples_per_second,
|
|
double max_age_sec,
|
|
int* event_week_out,
|
|
double* event_tow_sec_out,
|
|
double* elapsed_sec_out)
|
|
{
|
|
if((event_week_out == NULL) || (event_tow_sec_out == NULL) ||
|
|
!(samples_per_second > 0.0) || !(max_age_sec > 0.0) ||
|
|
!isfinite(anchor_tow_sec) || !isfinite(anchor_corrected_sample) ||
|
|
!isfinite(event_corrected_sample))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
/* The integer part is unwrapped modulo 2^32. Preserve the code-phase
|
|
* fractions from both simultaneous ABS_SAMPLE/REM_CODE_PHASE latches. */
|
|
int32_t raw_delta = (int32_t)(event_raw_sample - anchor_raw_sample);
|
|
double anchor_fraction = anchor_corrected_sample - (double)anchor_raw_sample;
|
|
double event_fraction = event_corrected_sample - (double)event_raw_sample;
|
|
double sample_delta = (double)raw_delta + event_fraction - anchor_fraction;
|
|
double elapsed_sec = sample_delta / samples_per_second;
|
|
if(!isfinite(elapsed_sec) || (elapsed_sec < -0.001) ||
|
|
(elapsed_sec > max_age_sec))
|
|
{
|
|
return false;
|
|
}
|
|
|
|
int event_week = anchor_week;
|
|
double event_tow_sec = gps_time_normalize_week(anchor_tow_sec + elapsed_sec,
|
|
&event_week);
|
|
*event_week_out = event_week;
|
|
*event_tow_sec_out = event_tow_sec;
|
|
if(elapsed_sec_out != NULL)
|
|
{
|
|
*elapsed_sec_out = elapsed_sec;
|
|
}
|
|
return true;
|
|
}
|