diff --git a/ROMFS/px4fmu_common/init.d-posix/1019_iris_dual_gps b/ROMFS/px4fmu_common/init.d-posix/1019_iris_dual_gps new file mode 100644 index 0000000000..f763d6e834 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/1019_iris_dual_gps @@ -0,0 +1,14 @@ +#!/bin/sh +# +# @name 3DR Iris Quadrotor SITL (Dual GPS) +# +# @type Quadrotor Wide +# + +sh /etc/init.d-posix/10016_iris + +if [ $AUTOCNF = yes ] +then + # EKF2: Multi GPS blending + param set EKF2_GPS_MASK 7 # Uses speed, hpos and vpos accuracy +fi diff --git a/Tools/sitl_gazebo b/Tools/sitl_gazebo index e2bf962b5e..97106007eb 160000 --- a/Tools/sitl_gazebo +++ b/Tools/sitl_gazebo @@ -1 +1 @@ -Subproject commit e2bf962b5e46b89093696338794a201c7fc990ba +Subproject commit 97106007eb5c934b902a5329afb55b45e94d5063 diff --git a/platforms/posix/cmake/sitl_target.cmake b/platforms/posix/cmake/sitl_target.cmake index 78de84497f..e68a9359d8 100644 --- a/platforms/posix/cmake/sitl_target.cmake +++ b/platforms/posix/cmake/sitl_target.cmake @@ -76,7 +76,7 @@ ExternalProject_Add(flightgear_bridge set(viewers none jmavsim gazebo) set(debuggers none ide gdb lldb ddd valgrind callgrind) set(models none shell - if750a iris iris_opt_flow iris_opt_flow_mockup iris_vision iris_rplidar iris_irlock iris_obs_avoid iris_rtps px4vision solo typhoon_h480 + if750a iris iris_dual_gps iris_opt_flow iris_opt_flow_mockup iris_vision iris_rplidar iris_irlock iris_obs_avoid iris_rtps px4vision solo typhoon_h480 plane plane_cam plane_catapult plane_lidar standard_vtol tailsitter tiltrotor rover r1_rover boat diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index f4ac51a75c..ab0cce5ba0 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -248,7 +248,8 @@ private: uORB::Publication _input_rc_pub{ORB_ID(input_rc)}; // HIL GPS - uORB::Publication _vehicle_gps_position_pub{ORB_ID(vehicle_gps_position)}; + uORB::PublicationMulti *_vehicle_gps_position_pubs[ORB_MULTI_MAX_INSTANCES] {}; + uint8_t _gps_ids[ORB_MULTI_MAX_INSTANCES] {}; std::default_random_engine _gen{}; // uORB subscription handlers diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index ca86d467e2..6c2cec53aa 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -309,7 +309,21 @@ void Simulator::handle_message_hil_gps(const mavlink_message_t *msg) gps.satellites_used = hil_gps.satellites_visible; gps.s_variance_m_s = 0.25f; - _vehicle_gps_position_pub.publish(gps); + // New publishers will be created based on the HIL_GPS ID's being different or not + for (size_t i = 0; i < sizeof(_gps_ids) / sizeof(_gps_ids[0]); i++) { + if (_vehicle_gps_position_pubs[i] && _gps_ids[i] == hil_gps.id) { + _vehicle_gps_position_pubs[i]->publish(gps); + break; + + } + + if (_vehicle_gps_position_pubs[i] == nullptr) { + _vehicle_gps_position_pubs[i] = new uORB::PublicationMulti {ORB_ID(vehicle_gps_position)}; + _gps_ids[i] = hil_gps.id; + _vehicle_gps_position_pubs[i]->publish(gps); + break; + } + } } }