diff --git a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp index 70241950fc..d50ecab632 100644 --- a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp +++ b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.cpp @@ -114,7 +114,7 @@ void SensorAgpSim::Run() _time_last_update = now; if (!(_param_sim_agp_fail.get() & static_cast(FailureMode::Stuck))) { - _measured_lla = LatLonAlt(gpos.lat, gpos.lon, gpos.alt_ellipsoid); + _measured_lla = LatLonAlt(gpos.lat, gpos.lon, gpos.alt); } if (_param_sim_agp_fail.get() & static_cast(FailureMode::Drift)) { @@ -131,17 +131,17 @@ void SensorAgpSim::Run() CONSTANTS_RADIUS_OF_EARTH); const double altitude = (double)(_measured_lla.altitude() + (generate_wgn() * 0.5f)); - vehicle_global_position_s sample{}; + aux_global_position_s sample{}; + sample.id = 123; + sample.source = aux_global_position_s::SOURCE_VISION; sample.timestamp_sample = gpos.timestamp_sample; sample.lat = latitude; sample.lon = longitude; sample.alt = altitude; - sample.lat_lon_valid = true; - sample.alt_ellipsoid = altitude; - sample.alt_valid = true; sample.eph = 20.f; sample.epv = 5.f; + sample.lat_lon_reset_counter = 0; sample.timestamp = hrt_absolute_time(); _aux_global_position_pub.publish(sample); diff --git a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp index cc9cae1330..8f92182bfd 100644 --- a/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp +++ b/src/modules/simulation/sensor_agp_sim/SensorAgpSim.hpp @@ -44,6 +44,7 @@ #include #include #include +#include using namespace time_literals; @@ -85,7 +86,7 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position_groundtruth)}; - uORB::PublicationMulti _aux_global_position_pub{ORB_ID(aux_global_position)}; + uORB::PublicationMulti _aux_global_position_pub{ORB_ID(aux_global_position)}; perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};