diff --git a/sw/simulator/nps/nps_sensor_gps.c b/sw/simulator/nps/nps_sensor_gps.c index 5014ec08e8..76f2bd592b 100644 --- a/sw/simulator/nps/nps_sensor_gps.c +++ b/sw/simulator/nps/nps_sensor_gps.c @@ -73,6 +73,17 @@ void nps_sensor_gps_run_step(struct NpsSensorGps* gps, double time) { UpdateSensorLatency(time, &cur_pos_reading, &gps->pos_history, gps->pos_latency, &gps->ecef_pos); + /* + * simulate lla pos + */ + /* convert current ecef reading to lla */ + struct LlaCoor_d cur_lla_reading; + lla_of_ecef_d(&cur_lla_reading, (EcefCoor_d*) &cur_pos_reading); + + /* store that for later and retrieve a previously stored data */ + UpdateSensorLatency(time, &cur_lla_reading, &gps->lla_history, gps->pos_latency, &gps->lla_pos); + + gps->next_update += NPS_GPS_DT; gps->data_available = TRUE; diff --git a/sw/simulator/nps/nps_sensor_gps.h b/sw/simulator/nps/nps_sensor_gps.h index 16b6656baf..8956cc3388 100644 --- a/sw/simulator/nps/nps_sensor_gps.h +++ b/sw/simulator/nps/nps_sensor_gps.h @@ -13,6 +13,7 @@ struct NpsSensorGps { struct EcefCoor_d ecef_pos; struct EcefCoor_d ecef_vel; + struct LlaCoor_d lla_pos; struct DoubleVect3 pos_noise_std_dev; struct DoubleVect3 speed_noise_std_dev; struct DoubleVect3 pos_bias_initial; @@ -21,6 +22,7 @@ struct NpsSensorGps { double pos_latency; double speed_latency; GSList* pos_history; + GSList* lla_history; GSList* speed_history; double next_update; bool_t data_available;