// 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; // } } }