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;
}
/** Set measurement precise Doppler using time difference of carrier phase.
* \note The return array `m_tdcp` should have space to contain the number
* of measurements with common PRNs between `m_new` and `m_old`. Making the
* array at least `MIN(n_new, n_old)` long will ensure sufficient space.
*
* \param n_new Number of measurements in `m_new`
* \oaram m_new Array of new navigation measurements
* \param n_old Number of measurements in `m_old`
* \oaram m_new Array of old navigation measurements, sorted by PRN
* \param m_tdcp Array in which to store the output measurements
* \return The number of measurements written to `m_tdcp`
*/
u8 tdcp_doppler(u8 n_new, navigation_measurement_t *m_new,
u8 n_old, navigation_measurement_t *m_old,
navigation_measurement_t *m_corrected)
{
/* Sort m_new, m_old should already be sorted. */
qsort(m_new, n_new, sizeof(navigation_measurement_t), nav_meas_cmp);
u8 i, j, n = 0;
/* Loop over m_new and m_old and check if a PRN is present in both. */
for (i=0, j=0; i<n_new && j<n_old; i++, j++) {
if (m_new[i].prn < m_old[j].prn)
j--;
else if (m_new[i].prn > m_old[j].prn)
i--;
else {
/* Copy m_new to m_corrected. */
memcpy(&m_corrected[n], &m_new[i], sizeof(navigation_measurement_t));
/* Calculate the Doppler correction between raw and corrected. */
double dopp_corr = m_corrected[n].doppler - m_corrected[n].raw_doppler;
/* Calculate raw Doppler from time difference of carrier phase. */
/* TODO: check that using difference of TOTs here is a valid
* approximation. */
m_corrected[n].raw_doppler = (m_new[i].carrier_phase - m_old[j].carrier_phase)
/ gpsdifftime(m_new[i].tot, m_old[j].tot);
/* Re-apply the same correction to the raw Doppler to get the corrected Doppler. */
m_corrected[n].doppler = m_corrected[n].raw_doppler + dopp_corr;
n++;
}
}
return n;
}
u8 propagate(u8 n, double ref_ecef[3],
navigation_measurement_t *m_in_base, gps_time_t *t_base,
navigation_measurement_t *m_in_rover, gps_time_t *t_rover,
navigation_measurement_t *m_out_base)
{
double dt = gpsdifftime(*t_rover, *t_base);
(void)dt;
double dr_[n];
for (u8 i=0; i<n; i++) {
m_out_base[i].prn = m_in_base[i].prn;
m_out_base[i].snr = m_in_base[i].snr;
m_out_base[i].lock_time = m_in_base[i].lock_time;
/* Calculate delta range. */
double dr[3];
vector_subtract(3, m_in_rover[i].sat_pos, m_in_base[i].sat_pos, dr);
/* Subtract linear term (initial satellite velocity * dt),
* we are going to add back in the linear term derived from the Doppler
* instead. */
/*vector_add_sc(3, dr, m_in_base[i].sat_vel, -dt, dr);*/
/* Make unit vector to satellite, e. */
double e[3];
vector_subtract(3, m_in_rover[i].sat_pos, ref_ecef, e);
vector_normalize(3, e);
/* Project onto the line of sight vector. */
dr_[i] = vector_dot(3, dr, e);
/*printf("# ddr_ = %f\n", dr_[i]);*/
/* Add back in linear term now using Doppler. */
/*dr_[i] -= m_in_base[i].raw_doppler * dt * GPS_L1_LAMBDA;*/
/*printf("# dr_dopp = %f\n", -m_in_base[i].raw_doppler * dt * GPS_L1_LAMBDA);*/
/*printf("# raw dopp = %f\n", m_in_rover[i].raw_doppler);*/
/*printf("# my dopp = %f\n", -vector_dot(3, e, m_in_rover[i].sat_vel) / GPS_L1_LAMBDA);*/
/*printf("# ddopp = %f\n", m_in_rover[i].raw_doppler - vector_dot(3, e, m_in_rover[i].sat_vel) / GPS_L1_LAMBDA);*/
/*printf("[%f, %f],", m_in_base[i].raw_doppler, vector_dot(3, e, m_in_rover[i].sat_vel) / GPS_L1_LAMBDA);*/
/*printf("# dr_ = %f\n", dr_[i]);*/
m_out_base[i].raw_pseudorange = m_in_base[i].raw_pseudorange + dr_[i];
m_out_base[i].pseudorange = m_in_base[i].pseudorange;
m_out_base[i].carrier_phase = m_in_base[i].carrier_phase - dr_[i] / GPS_L1_LAMBDA;
m_out_base[i].raw_doppler = m_in_base[i].raw_doppler;
m_out_base[i].doppler = m_in_base[i].doppler;
/*m_in_base[i].carrier_phase -= dr_[i] / GPS_L1_LAMBDA;*/
}
return 0;
}
static void manage_calc_scores(void)
{
double az, el;
gps_time_t t;
if (time_quality != TIME_UNKNOWN)
t = get_current_time();
for (u8 prn=0; prn<32; prn++) {
if (!almanac[prn].valid ||
time_quality == TIME_UNKNOWN ||
position_quality == POSITION_UNKNOWN) {
/* No almanac or position/time information, give it the benefit of the
* doubt. */
acq_prn_param[prn].score = 0;
} else {
calc_sat_az_el_almanac(&almanac[prn], t.tow, t.wn-1024, position_solution.pos_ecef, &az, &el);
acq_prn_param[prn].score = (s8)(el/D2R);
gps_time_t toa;
toa.wn = almanac[prn].week + 1024;
toa.tow = almanac[prn].toa;
double dt_alm = fabs(gpsdifftime(t, toa));
double dt_pos = fabs(gpsdifftime(t, position_solution.time));
if (time_quality == TIME_GUESS ||
dt_pos > 1*24*3600 ||
dt_alm > 4*24*3600) {
/* Don't exclude other sats if our time is just a guess, our last
* position solution was ages ago or our almanac is old. */
if (acq_prn_param[prn].score < 0)
acq_prn_param[prn].score = 0;
}
}
}
}
void clock_est_update(clock_est_state_t *s, gps_time_t meas_gpst,
double meas_clock_period, double localt, double q,
double r_gpst, double r_clock_period)
{
double temp[2][2];
double phi_t_0[2][2] = {{1, localt}, {0, 1}};
double phi_t_0_tr[2][2];
matrix_transpose(2, 2, (const double *)phi_t_0, (double *)phi_t_0_tr);
double P_[2][2];
memcpy(P_, s->P, sizeof(P_));
P_[0][0] += q;
double y[2];
gps_time_t pred_gpst = s->t0_gps;
pred_gpst.tow += localt * s->clock_period;
pred_gpst = normalize_gps_time(pred_gpst);
y[0] = gpsdifftime(meas_gpst, pred_gpst);
y[1] = meas_clock_period - s->clock_period;
double S[2][2];
matrix_multiply(2, 2, 2, (const double *)phi_t_0, (const double *)P_, (double *)temp);
matrix_multiply(2, 2, 2, (const double *)temp, (const double *)phi_t_0_tr, (double *)S);
S[0][0] += r_gpst;
S[1][1] += r_clock_period;
double Sinv[2][2];
matrix_inverse(2, (const double *)S, (double *)Sinv);
double K[2][2];
matrix_multiply(2, 2, 2, (const double *)P_, (const double *)phi_t_0_tr, (double *)temp);
matrix_multiply(2, 2, 2, (const double *)temp, (const double *)Sinv, (double *)K);
double dx[2];
matrix_multiply(2, 2, 1, (const double *)K, (const double *)y, (double *)dx);
s->t0_gps.tow += dx[0];
s->t0_gps = normalize_gps_time(s->t0_gps);
s->clock_period += dx[1];
matrix_multiply(2, 2, 2, (const double *)K, (const double *)phi_t_0, (double *)temp);
temp[0][0] = 1 - temp[0][0];
temp[0][1] = -temp[0][1];
temp[1][1] = 1 - temp[1][1];
temp[1][0] = -temp[1][0];
matrix_multiply(2, 2, 2, (const double *)temp, (const double *)P_, (double *)s->P);
}
void calc_navigation_measurement_(u8 n_channels, channel_measurement_t* meas[], navigation_measurement_t* nav_meas[], double nav_time, ephemeris_t* ephemerides[])
{
double TOTs[n_channels];
double min_TOT = DBL_MAX;
for (u8 i=0; i<n_channels; i++) {
TOTs[i] = 1e-3 * meas[i]->time_of_week_ms;
TOTs[i] += meas[i]->code_phase_chips / 1.023e6;
TOTs[i] += (nav_time - meas[i]->receiver_time) * meas[i]->code_phase_rate / 1.023e6;
/** \todo Handle GPS time properly here, e.g. week rollover */
nav_meas[i]->tot.wn = ephemerides[i]->toe.wn;
nav_meas[i]->tot.tow = TOTs[i];
if (gpsdifftime(nav_meas[i]->tot, ephemerides[i]->toe) > 3*24*3600)
nav_meas[i]->tot.wn -= 1;
if (TOTs[i] < min_TOT)
min_TOT = TOTs[i];
nav_meas[i]->raw_doppler = meas[i]->carrier_freq;
nav_meas[i]->snr = meas[i]->snr;
nav_meas[i]->prn = meas[i]->prn;
nav_meas[i]->carrier_phase = meas[i]->carrier_phase;
nav_meas[i]->carrier_phase += (nav_time - meas[i]->receiver_time) * meas[i]->carrier_freq;
nav_meas[i]->lock_counter = meas[i]->lock_counter;
}
double clock_err, clock_rate_err;
for (u8 i=0; i<n_channels; i++) {
nav_meas[i]->raw_pseudorange = (min_TOT - TOTs[i])*GPS_C + GPS_NOMINAL_RANGE;
calc_sat_pos(nav_meas[i]->sat_pos, nav_meas[i]->sat_vel, &clock_err, &clock_rate_err, ephemerides[i], nav_meas[i]->tot);
nav_meas[i]->pseudorange = nav_meas[i]->raw_pseudorange \
+ clock_err*GPS_C;
nav_meas[i]->doppler = nav_meas[i]->raw_doppler + clock_rate_err*GPS_L1_HZ;
nav_meas[i]->tot.tow -= clock_err;
nav_meas[i]->tot = normalize_gps_time(nav_meas[i]->tot);
}
}
/** Retrieve a channel measurement for a tracker channel.
*
* \param id ID of the tracker channel to use.
* \param ref_tc Reference timing count.
* \param meas Pointer to output channel_measurement_t.
*/
void tracking_channel_measurement_get(tracker_channel_id_t id, u64 ref_tc,
channel_measurement_t *meas)
{
tracker_channel_t *tracker_channel = tracker_channel_get(id);
tracker_internal_data_t *internal_data =
&tracker_channel->internal_data;
const tracker_common_data_t *common_data = &tracker_channel->common_data;
/* Update our channel measurement. */
meas->sid = tracker_channel->info.sid;
meas->code_phase_chips = common_data->code_phase_early;
meas->code_phase_rate = common_data->code_phase_rate;
meas->carrier_phase = common_data->carrier_phase;
meas->carrier_freq = common_data->carrier_freq;
meas->time_of_week_ms = common_data->TOW_ms;
meas->rec_time_delta = (double)((s32)(common_data->sample_count - (u32)ref_tc))
/ SAMPLE_FREQ;
meas->snr = common_data->cn0;
if (internal_data->bit_polarity == BIT_POLARITY_INVERTED) {
meas->carrier_phase += 0.5;
}
meas->lock_counter = internal_data->lock_counter;
/* Adjust carrier phase initial integer offset to be approximately equal to
pseudorange. */
if ((time_quality == TIME_FINE)
&& (internal_data->carrier_phase_offset == 0.0)) {
gps_time_t tor = rx2gpstime(ref_tc + meas->rec_time_delta);
gps_time_t tot;
tot.tow = 1e-3 * meas->time_of_week_ms;
tot.tow += meas->code_phase_chips / GPS_CA_CHIPPING_RATE;
gps_time_match_weeks(&tot, &tor);
internal_data->carrier_phase_offset = round(GPS_L1_HZ
* gpsdifftime(&tor, &tot));
}
meas->carrier_phase -= internal_data->carrier_phase_offset;
}
/** Convert GPS time to receiver time.
*
* \note The GPS time may only be a guess or completely unknown. time_quality
* should be checked first to determine the quality of the GPS time
* estimate.
*
* \param t gps_time_t to convert.
* \return Timing count in units of RX_DT_NOMINAL.
*/
double gps2rxtime(gps_time_t t)
{
return gpsdifftime(t, clock_state.t0_gps) / clock_state.clock_period;
}
int calc_sat_pos(double pos[3], double vel[3],
double *clock_err, double *clock_rate_err,
const ephemeris_t *ephemeris,
gps_time_t tot)
{
/****************************************************************************
* Calculate satellite position, velocity and clock offset from ephemeris
* Taken from IS-GPS-200D, Section 20.3.3.3.3.1 and Table 20-IV
*
* Usage: unsigned int ProcessEphemeris(unsigned int week, double secs,
* unsigned int sv, nav_info_t *Eph)
* week: GPS week number
* sec: GPS seconds of the week of time of transmission
* sv: Satellite vehicle number
* Eph: Ephemeris data structure located within channel structure
****************************************************************************/
double tempd1 = 0, tempd2, tempd3;
double tdiff;
double a; // semi major axis
double ma, ma_dot; // mean anomoly and first derivative (mean motion)
double ea, ea_dot, ea_old; // eccentric anomoly, first deriv, iteration var
double einstein; // relativistic correction
double al, al_dot; // argument of lattitude and first derivative
double cal, cal_dot; // corrected argument of lattitude and first deriv
double r, r_dot; // radius and first derivative
double inc, inc_dot; // inclination and first derivative
double x, x_dot, y, y_dot; // position in orbital plan and first derivatives
double om, om_dot; // omega and first derivatives
// Satellite clock terms
// Seconds from clock data reference time (toc)
tdiff = gpsdifftime(tot, ephemeris->toc);
*clock_err = ephemeris->af0 + tdiff * (ephemeris->af1 + tdiff * ephemeris->af2) - ephemeris->tgd;
*clock_rate_err = ephemeris->af1 + 2.0 * tdiff * ephemeris->af2;
// Seconds from the time from ephemeris reference epoch (toe)
tdiff = gpsdifftime(tot, ephemeris->toe);
// If tdiff is too large our ephemeris isn't valid, maybe we want to wait until we get a
// new one? At least let's warn the user.
// TODO: this doesn't exclude ephemerides older than a week so could be made better.
if (abs(tdiff) > 4*3600)
printf(" WARNING: using ephemeris older (or newer!) than 4 hours.\n");
// Calculate position per IS-GPS-200D p 97 Table 20-IV
a = ephemeris->sqrta * ephemeris->sqrta; // [m] Semi-major axis
ma_dot = sqrt (NAV_GM / (a * a * a)) + ephemeris->dn; // [rad/sec] Corrected mean motion
ma = ephemeris->m0 + ma_dot * tdiff; // [rad] Corrected mean anomaly
// Iteratively solve for the Eccentric Anomaly (from Keith Alter and David Johnston)
ea = ma; // Starting value for E
double ecc = ephemeris->ecc;
u32 count = 0;
/* TODO: Implement convergence test using integer difference of doubles,
* http://www.cygnus-software.com/papers/comparingfloats/comparingfloats.htm */
do {
ea_old = ea;
tempd1 = 1.0 - ecc * cos (ea_old);
ea = ea + (ma - ea_old + ecc * sin (ea_old)) / tempd1;
count++;
if (count > 5)
break;
} while (fabs (ea - ea_old) > 1.0E-14);
ea_dot = ma_dot / tempd1;
// Relativistic correction term
einstein = -4.442807633E-10 * ecc * ephemeris->sqrta * sin (ea);
// Begin calc for True Anomaly and Argument of Latitude
tempd2 = sqrt (1.0 - ecc * ecc);
al = atan2 (tempd2 * sin (ea), cos (ea) - ecc) + ephemeris->w; // [rad] Argument of Latitude = True Anomaly + Argument of Perigee
al_dot = tempd2 * ea_dot / tempd1;
// Calculate corrected argument of latitude based on position
cal = al + ephemeris->cus * sin (2.0 * al) + ephemeris->cuc * cos (2.0 * al);
cal_dot =
al_dot * (1.0 +
2.0 * (ephemeris->cus * cos (2.0 * al) -
ephemeris->cuc * sin (2.0 * al)));
// Calculate corrected radius based on argument of latitude
r =
a * tempd1 + ephemeris->crc * cos (2.0 * al) +
ephemeris->crs * sin (2.0 * al);
r_dot =
a * ecc * sin (ea) * ea_dot +
2.0 * al_dot * (ephemeris->crs * cos (2.0 * al) -
ephemeris->crc * sin (2.0 * al));
// Calculate inclination based on argument of latitude
inc =
ephemeris->inc + ephemeris->inc_dot * tdiff +
ephemeris->cic * cos (2.0 * al) + ephemeris->cis * sin (2.0 * al);
inc_dot =
ephemeris->inc_dot + 2.0 * al_dot * (ephemeris->cis * cos (2.0 * al) -
ephemeris->cic * sin (2.0 * al));
// Calculate position and velocity in orbital plane
x = r * cos (cal);
y = r * sin (cal);
x_dot = r_dot * cos (cal) - y * cal_dot;
y_dot = r_dot * sin (cal) + x * cal_dot;
// Corrected longitude of ascenting node
om_dot = ephemeris->omegadot - NAV_OMEGAE_DOT;
om = ephemeris->omega0 + tdiff * om_dot - NAV_OMEGAE_DOT * ephemeris->toe.tow;
// Compute the satellite's position in Earth-Centered Earth-Fixed coordiates
pos[0] = x * cos (om) - y * cos (inc) * sin (om);
pos[1] = x * sin (om) + y * cos (inc) * cos (om);
pos[2] = y * sin (inc);
tempd3 = y_dot * cos (inc) - y * sin (inc) * inc_dot;
// Compute the satellite's velocity in Earth-Centered Earth-Fixed coordiates
vel[0] = -om_dot * pos[1] + x_dot * cos (om) - tempd3 * sin (om);
vel[1] = om_dot * pos[0] + x_dot * sin (om) + tempd3 * cos (om);
vel[2] = y * cos (inc) * inc_dot + y_dot * sin (inc);
*clock_err += einstein;
return 0;
}
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);
);
}
