/** Send tracking state SBP message.
* Send information on each tracking channel to host.
*/
void tracking_send_state()
{
tracking_channel_state_t states[nap_track_n_channels];
if (simulation_enabled_for(SIMULATION_MODE_TRACKING)) {
u8 num_sats = simulation_current_num_sats();
for (u8 i=0; i < num_sats; i++) {
states[i] = simulation_current_tracking_state(i);
}
if (num_sats < nap_track_n_channels) {
for (u8 i = num_sats; i < nap_track_n_channels; i++) {
states[i].state = 0;
states[i].sid.code = 0;
states[i].sid.sat = 0;
states[i].cn0 = -1;
}
}
} else {
for (u8 i=0; i<nap_track_n_channels; i++) {
tracker_channel_t *tracker_channel = tracker_channel_get(i);
const tracker_common_data_t *common_data = &tracker_channel->common_data;
bool running;
gnss_signal_t sid;
float cn0;
tracker_channel_lock(tracker_channel);
{
running =
(tracker_channel_state_get(tracker_channel) == STATE_ENABLED);
sid = tracker_channel->info.sid;
cn0 = common_data->cn0;
}
tracker_channel_unlock(tracker_channel);
if (!running) {
states[i].state = 0;
states[i].sid.code = 0;
states[i].sid.sat = 0;
states[i].cn0 = -1;
} else {
states[i].state = 1;
states[i].sid = sid_to_sbp(sid);
states[i].cn0 = cn0;
}
}
}
sbp_send_msg(SBP_MSG_TRACKING_STATE, sizeof(states), (u8*)states);
}
/** Send results of an acquisition to the host.
*
* \param sid SID of the acquisition
* \param snr Signal to noise ratio of best point from acquisition.
* \param cp Code phase of best point.
* \param cf Carrier frequency of best point.
*/
static void acq_result_send(gnss_signal_t sid, float snr, float cp, float cf)
{
msg_acq_result_t acq_result_msg;
acq_result_msg.sid = sid_to_sbp(sid);
acq_result_msg.snr = snr;
acq_result_msg.cp = cp;
acq_result_msg.cf = cf;
sbp_send_msg(SBP_MSG_ACQ_RESULT,
sizeof(msg_acq_result_t),
(u8 *)&acq_result_msg);
}
/** 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. */
gnss_signal_t sid = {
.constellation = simulation_almanacs[i].sid.constellation,
.band = simulation_almanacs[i].sid.band,
.sat = simulation_almanacs[i].sid.sat + SIM_PRN_OFFSET
};
sim_state.tracking_channel[num_sats_selected].state = TRACKING_RUNNING;
sim_state.tracking_channel[num_sats_selected].sid = sid_to_sbp(sid);
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;
}
