// presumes that the first alm entry is the reference sat
void assign_de_mtx_from_alms(u8 num_sats, almanac_t *alms, gps_time_t timestamp, double ref_ecef[3], double *DE)
{
memset(DE, 0, (num_sats - 1) * 3 * sizeof(double));
double e0[3];
double v0[3];
calc_sat_state_almanac(&alms[0], timestamp.tow, timestamp.wn, e0, v0);
// printf("\nprn=%u\n", alms[i].prn);
// VEC_PRINTF(e, 3);
double x0 = e0[0] - ref_ecef[0];
double y0 = e0[1] - ref_ecef[1];
double z0 = e0[2] - ref_ecef[2];
double norm0 = sqrt(x0*x0 + y0*y0 + z0*z0);
e0[0] = x0 / norm0;
e0[1] = y0 / norm0;
e0[2] = z0 / norm0;
// VEC_PRINTF(ref_ecef, 3);
for (u8 i=1; i<num_sats; i++) {
double e[3];
double v[3];
// calc_sat_state_almanac(&alms[i], timestamp.tow, -1, e, v); //TODO change -1 back to timestamp.wn
calc_sat_state_almanac(&alms[i], timestamp.tow, timestamp.wn, e, v);
// printf("\nprn=%u\n", alms[i].prn);
// VEC_PRINTF(e, 3);
double x = e[0] - ref_ecef[0];
double y = e[1] - ref_ecef[1];
double z = e[2] - ref_ecef[2];
double norm = sqrt(x*x + y*y + z*z);
DE[3*(i-1)] = x / norm - e0[0];
DE[3*(i-1) + 1] = y / norm - e0[1];
DE[3*(i-1) + 2] = z / norm - e0[2];
}
}
/** Calculate the azimuth and elevation of a satellite from a reference
* position given the satellite almanac.
*
* \param alm Pointer to an almanac structure for the satellite of interest.
* \param t GPS time of week at which to calculate the az/el.
* \param week GPS week number modulo 1024 or pass -1 to assume within one
* half-week of the almanac time of applicability.
* \param ref ECEF coordinates of the reference point from which the azimuth
* and elevation is to be determined, passed as [X, Y, Z], all in
* meters.
* \param az Pointer to where to store the calculated azimuth output.
* \param el Pointer to where to store the calculated elevation output.
*/
void calc_sat_az_el_almanac(almanac_t* alm, double t, s16 week,
double ref[3], double* az, double* el)
{
double sat_pos[3];
calc_sat_state_almanac(alm, t, week, sat_pos, 0);
wgsecef2azel(sat_pos, ref, az, el);
}
/** Simulates real observations for the current position and the satellite almanac and week
* given in simulator_data.
*
* NOTES:
*
* - This simulates the pseudorange as the true distance to the satellite + noise.
* - This simulates the carrier phase as the true distance in wavelengths + bais + noise.
* - The bias is an integer multiple of 10 for easy debugging.
* - The satellite SNR/CN0 is proportional to the elevation of the satellite.
*
* USES:
* - Pipe observations into internals for testing
* - For integration testing with other devices that has to carry the radio signal.
*
* \param elapsed Number of seconds elapsed since last simulation step.
*/
void simulation_step_tracking_and_observations(double elapsed)
{
(void)elapsed;
u8 week = -1;
double t = sim_state.noisy_solution.time.tow;
/* First we calculate all the current sat positions, velocities */
for (u8 i=0; i<simulation_num_almanacs; i++) {
calc_sat_state_almanac(&simulation_almanacs[i], t, week,
simulation_sats_pos[i], simulation_sats_vel[i]);
}
/* Calculate the first sim_settings.num_sats amount of visible sats */
u8 num_sats_selected = 0;
double az, el;
for (u8 i=0; i<simulation_num_almanacs; i++) {
calc_sat_az_el_almanac(&simulation_almanacs[i], t, week,
sim_state.pos, &az, &el);
if (el > 0 &&
num_sats_selected < sim_settings.num_sats &&
num_sats_selected < MAX_CHANNELS) {
/* Generate a code measurement which is just the pseudorange: */
double points_to_sat[3];
double base_points_to_sat[3];
vector_subtract(3, simulation_sats_pos[i], sim_state.pos, points_to_sat);
vector_subtract(3, simulation_sats_pos[i], sim_settings.base_ecef, base_points_to_sat);
double distance_to_sat = vector_norm(3, points_to_sat);
double base_distance_to_sat = vector_norm(3, base_points_to_sat);
/* Fill out the observation details into the NAV_MEAS structure for this satellite, */
/* We simulate the pseudorange as a noisy range measurement, and */
/* the carrier phase as a noisy range in wavelengths + an integer offset. */
populate_nav_meas(&sim_state.nav_meas[num_sats_selected],
distance_to_sat, el, i);
populate_nav_meas(&sim_state.base_nav_meas[num_sats_selected],
base_distance_to_sat, el, i);
/* As for tracking, we just set each sat consecutively in each channel. */
/* This will cause weird jumps when a satellite rises or sets. */
sim_state.tracking_channel[num_sats_selected].state = TRACKING_RUNNING;
sim_state.tracking_channel[num_sats_selected].prn = simulation_almanacs[i].prn + SIM_PRN_OFFSET;
sim_state.tracking_channel[num_sats_selected].cn0 = sim_state.nav_meas[num_sats_selected].snr;
num_sats_selected++;
}
}
sim_state.noisy_solution.n_used = num_sats_selected;
}
/** Calculate the Doppler shift of a satellite as observed at a reference
* position given the satellite almanac.
*
* \param alm Pointer to an almanac structure for the satellite of interest.
* \param t GPS time of week at which to calculate the Doppler shift.
* \param week GPS week number modulo 1024 or pass -1 to assume within one
* half-week of the almanac time of applicability.
* \param ref ECEF coordinates of the reference point from which the azimuth
* and elevation is to be determined, passed as [X, Y, Z], all in
* meters.
* \return The Doppler shift in Hz.
*/
double calc_sat_doppler_almanac(almanac_t* alm, double t, s16 week,
double ref[3])
{
double sat_pos[3];
double sat_vel[3];
double vec_ref_sat[3];
calc_sat_state_almanac(alm, t, week, sat_pos, sat_vel);
/* Find the vector from the reference position to the satellite. */
vector_subtract(3, sat_pos, ref, vec_ref_sat);
/* Find the satellite velocity projected on the line of sight vector from the
* reference position to the satellite. */
double radial_velocity = vector_dot(3, vec_ref_sat, sat_vel) / \
vector_norm(3, vec_ref_sat);
/* Return the Doppler shift. */
return GPS_L1_HZ * radial_velocity / NAV_C;
}
void assign_e_mtx_from_alms(u8 num_sats, almanac_t *alms, gps_time_t timestamp, double ref_ecef[3], double *E)
{
memset(E, 0, num_sats * 3 * sizeof(double));
// VEC_PRINTF(ref_ecef, 3);
for (u8 i=0; i<num_sats; i++) {
double e[3];
double v[3];
// calc_sat_state_almanac(&alms[i], timestamp.tow, -1, e, v); //TODO change -1 back to timestamp.wn
calc_sat_state_almanac(&alms[i], timestamp.tow, timestamp.wn, e, v);
// printf("\nprn=%u\n", alms[i].prn);
// VEC_PRINTF(e, 3);
double x = e[0] - ref_ecef[0];
double y = e[1] - ref_ecef[1];
double z = e[2] - ref_ecef[2];
double norm = sqrt(x*x + y*y + z*z);
E[3*i] = x / norm;
E[3*i + 1] = y / norm;
E[3*i + 2] = z / norm;
}
}
/** Convert a list of almanacs to a list of single differences.
* This only fills the position, velocity and prn.
*
* It's only useful for using functions that need a sdiff_t when you have almanac_t.
*/
void almanacs_to_single_diffs(u8 n, almanac_t *alms, gps_time_t timestamp, sdiff_t *sdiffs)
{
for (u8 i=0; i<n; i++) {
double p[3];
double v[3];
calc_sat_state_almanac(&alms[i], timestamp.tow, timestamp.wn, p, v);
memcpy(sdiffs[i].sat_pos, &p[0], 3 * sizeof(double));
memcpy(sdiffs[i].sat_vel, &v[0], 3 * sizeof(double));
sdiffs[i].prn = alms[i].prn;
if (i==0) {
sdiffs[i].snr = 1;
// printf("ref_prn=%d\n", sdiffs[i].prn);
}
else {
sdiffs[i].snr = 0;
}
// if (sdiffs[i].prn == 16) {
// sdiffs[i].snr = 2;
// }
}
}