diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp index 5bed55df26..bf6c28a101 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.cpp @@ -198,6 +198,16 @@ void SensorGpsSim::Run() sensor_gps.timestamp = hrt_absolute_time(); _sensor_gps_pub.publish(sensor_gps); + + const float gps1_offx = _param_gps1_offx.get(); + const float gps1_offy = _param_gps1_offy.get(); + + if (fabsf(gps1_offx) > 0.f || fabsf(gps1_offy) > 0.f) { + sensor_gps.latitude_deg = latitude + (double)gps1_offx / CONSTANTS_RADIUS_OF_EARTH * (180.0 / M_PI); + sensor_gps.longitude_deg = longitude + (double)gps1_offy / CONSTANTS_RADIUS_OF_EARTH * (180.0 / M_PI) / cos(latitude * M_PI / 180.0); + sensor_gps.timestamp = hrt_absolute_time(); + _sensor_gps_pub2.publish(sensor_gps); + } } perf_end(_loop_perf); diff --git a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp index a02360917a..0a55f5a9ed 100644 --- a/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp +++ b/src/modules/simulation/sensor_gps_sim/SensorGpsSim.hpp @@ -81,6 +81,7 @@ private: uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position_groundtruth)}; uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; + uORB::PublicationMulti _sensor_gps_pub2{ORB_ID(sensor_gps)}; perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; @@ -101,6 +102,8 @@ private: static constexpr float _vel_markov_time{0.54f}; DEFINE_PARAMETERS( - (ParamInt) _sim_gps_used + (ParamInt) _sim_gps_used, + (ParamFloat) _param_gps1_offx, + (ParamFloat) _param_gps1_offy ) };