HITL, SITL: use reference home position and global position groundtru…

…th fully from gazebo navsat data

ROMFS/px4fmu_common/init.d-posix/airframes/4401_gz_ssrc_holybro_x500

@@ -12,7 +12,6 @@ PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=holybro_x500}
 
 param set-default SIM_GZ_EN 1
-param set-default SIM_GZ_SHOME 1
 if [ "$PX4_RUN_GZSIM" = "0" ]; then
   param set-default SIM_GZ_RUN_GZSIM 0
 else

ROMFS/px4fmu_common/init.d-posix/airframes/4404_gz_ssrc_standard_vtol

@@ -12,7 +12,6 @@ PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=ssrc_standard_vtol}
 
 param set-default SIM_GZ_EN 1
-param set-default SIM_GZ_SHOME 1
 if [ "$PX4_RUN_GZSIM" = "0" ]; then
   param set-default SIM_GZ_RUN_GZSIM 0
 else

ROMFS/px4fmu_common/init.d-posix/airframes/4430_gz_ssrc_strivermini

@@ -24,7 +24,6 @@ PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=strivermini}
 
 param set-default SIM_GZ_EN 1
-param set-default SIM_GZ_SHOME 1
 if [ "$PX4_RUN_GZSIM" = "0" ]; then
   param set-default SIM_GZ_RUN_GZSIM 0
 else

ROMFS/px4fmu_common/init.d-posix/airframes/4440_gz_ssrc_skywalker_x8

@@ -13,7 +13,6 @@ PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=skywalker_x8}
 
 param set-default SIM_GZ_EN 1
-param set-default SIM_GZ_SHOME 1
 if [ "$PX4_RUN_GZSIM" = "0" ]; then
   param set-default SIM_GZ_RUN_GZSIM 0
 else

ROMFS/px4fmu_common/init.d-posix/airframes/50005_gz_ssrc_scout_mini

@@ -15,7 +15,6 @@ PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
 PX4_SIM_MODEL=${PX4_SIM_MODEL:=scout_mini}
 
 param set-default SIM_GZ_EN 1
-param set-default SIM_GZ_SHOME 1
 if [ "$PX4_RUN_GZSIM" = "0" ]; then
   param set-default SIM_GZ_RUN_GZSIM 0
 else

ROMFS/px4fmu_common/init.d/rc.hitl_testing

@@ -8,8 +8,6 @@ param set SYS_HITL 1
 param set UAVCAN_ENABLE 0
 param set-default SYS_FAILURE_EN 1
 
-param set MAV_HITL_SHOME 0
-
 param set-default SENS_EN_GPSSIM 1
 param set-default SENS_EN_BAROSIM 1
 param set-default SENS_EN_MAGSIM 1

src/modules/mavlink/mavlink_params.c

@@ -154,37 +154,4 @@ PARAM_DEFINE_INT32(MAV_HB_FORW_EN, 1);
  * @min 1
  * @max 250
  */
-PARAM_DEFINE_INT32(MAV_RADIO_TOUT, 5);
-
-/**
- * mavlink receiver HITL use simulated origin (home) position
- *
- * @boolean
- * @reboot_required true
- * @group MAVLink
- */
-PARAM_DEFINE_INT32(MAV_HITL_SHOME, 0);
-
-/**
- * mavlink receiver HITL simulated origin latitude
- *
- * @unit deg
- * @group MAVLink
- */
-PARAM_DEFINE_FLOAT(MAV_HITL_LAT, 47.397742f);
-
-/**
- * mavlink receiver HITL simulated origin longitude
- *
- * @unit deg
- * @group MAVLink
- */
-PARAM_DEFINE_FLOAT(MAV_HITL_LON, 8.545594);
-
-/**
- * mavlink receiver HITL simulated origin altitude
- *
- * @unit m
- * @group MAVLink
- */
-PARAM_DEFINE_FLOAT(MAV_HITL_ALT, 488.0);
+PARAM_DEFINE_INT32(MAV_RADIO_TOUT, 5);

src/modules/mavlink/mavlink_receiver.cpp

@@ -2346,10 +2346,19 @@ MavlinkReceiver::handle_message_hil_gps(mavlink_message_t *msg)
 	mavlink_msg_hil_gps_decode(msg, &hil_gps);
 
 	if (_mavlink->get_hil_enabled()) {
-		_hitl_sim_gps_time_usec = hil_gps.time_usec;
-		_hitl_sim_home_lat = hil_gps.lat / 1e7;
-		_hitl_sim_home_lon = hil_gps.lon / 1e7;
-		_hitl_sim_home_alt = hil_gps.alt / 1e3f;
+		if (!_hil_pos_ref.isInitialized()) {
+			_hil_pos_ref.initReference(hil_gps.lat / 1e7, hil_gps.lon / 1e7, hrt_absolute_time());
+			_hil_alt_ref = hil_gps.alt / 1e3f;
+
+		} else {
+			// Publish GPS groundtruth
+			vehicle_global_position_s hil_global_position_groundtruth{};
+			hil_global_position_groundtruth.timestamp_sample = hrt_absolute_time();
+			hil_global_position_groundtruth.lat = hil_gps.lat / 1e7;
+			hil_global_position_groundtruth.lon = hil_gps.lon / 1e7;
+			hil_global_position_groundtruth.alt = hil_gps.alt / 1e3f;
+			_gpos_groundtruth_pub.publish(hil_global_position_groundtruth);
+		}
 
 	} else {
 		sensor_gps_s gps{};
@@ -2688,19 +2697,6 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 	 * fields (mmE3) with east in LAT, north in LON, up in ALT
 	 *[TODO create a new mavlink msg maybe called HIL_POSE_INFO for this]
 	 */
-
-	if (!_hil_pos_ref.isInitialized()) {
-		if (!_param_hitl_use_sim_home.get() || _hitl_sim_gps_time_usec) {
-			_hil_pos_ref.initReference(_param_hitl_use_sim_home.get() ? _hitl_sim_home_lat : (double)_param_hitl_home_lat.get(),
-						   _param_hitl_use_sim_home.get() ? _hitl_sim_home_lon : (double)_param_hitl_home_lon.get(),
-						   hrt_absolute_time());
-
-		} else {
-			// no reference set yet
-			return;
-		}
-	}
-
 	vehicle_local_position_s hil_local_position_groundtruth{};
 
 	hil_local_position_groundtruth.timestamp_sample = timestamp_sample;
@@ -2731,29 +2727,11 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg)
 		_hil_pos_ref.getProjectionReferenceLat(); // Reference point latitude in degrees
 	hil_local_position_groundtruth.ref_lon =
 		_hil_pos_ref.getProjectionReferenceLon(); // Reference point longitude in degrees
-	hil_local_position_groundtruth.ref_alt = _param_hitl_use_sim_home.get() ? static_cast<float>(_hitl_sim_home_alt) :
-			_param_hitl_home_alt.get();
+	hil_local_position_groundtruth.ref_alt = _hil_alt_ref; // Reference altitude AMSL in meters
 	hil_local_position_groundtruth.ref_timestamp = _hil_pos_ref.getProjectionReferenceTimestamp();
 
 	hil_local_position_groundtruth.timestamp = hrt_absolute_time();
 	_lpos_groundtruth_pub.publish(hil_local_position_groundtruth);
-
-	if (_hil_pos_ref.isInitialized()) {
-		/* publish position groundtruth */
-		vehicle_global_position_s hil_global_position_groundtruth{};
-
-		hil_global_position_groundtruth.timestamp_sample = hrt_absolute_time();
-
-		_hil_pos_ref.reproject(hil_local_position_groundtruth.x,
-				       hil_local_position_groundtruth.y,
-				       hil_global_position_groundtruth.lat,
-				       hil_global_position_groundtruth.lon);
-
-		hil_global_position_groundtruth.alt = (_param_hitl_use_sim_home.get() ? static_cast<float>(_hitl_sim_home_alt) :
-						       _param_hitl_home_alt.get()) - static_cast<float>(position(2));
-		hil_global_position_groundtruth.timestamp = hrt_absolute_time();
-		_gpos_groundtruth_pub.publish(hil_global_position_groundtruth);
-	}
 }
 
 #if !defined(CONSTRAINED_FLASH)

src/modules/mavlink/mavlink_receiver.h

@@ -374,11 +374,7 @@ class MavlinkReceiver : public ModuleParams
 	matrix::Vector3d _hil_velocity_prev{};
 	matrix::Vector3f _hil_euler_prev{};
 	MapProjection _hil_pos_ref{};
-
-	uint64_t _hitl_sim_gps_time_usec{0};
-	double _hitl_sim_home_lat{0.0};
-	double _hitl_sim_home_lon{0.0};
-	double _hitl_sim_home_alt{0.0};
+	double _hil_alt_ref{};
 
 	// Allocated if needed.
 	TunePublisher *_tune_publisher{nullptr};
@@ -406,11 +402,7 @@ class MavlinkReceiver : public ModuleParams
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::BAT_CRIT_THR>) _param_bat_crit_thr,
 		(ParamFloat<px4::params::BAT_EMERGEN_THR>) _param_bat_emergen_thr,
-		(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr,
-		(ParamBool<px4::params::MAV_HITL_SHOME>) _param_hitl_use_sim_home,
-		(ParamFloat<px4::params::MAV_HITL_LAT>) _param_hitl_home_lat,
-		(ParamFloat<px4::params::MAV_HITL_LON>) _param_hitl_home_lon,
-		(ParamFloat<px4::params::MAV_HITL_ALT>) _param_hitl_home_alt
+		(ParamFloat<px4::params::BAT_LOW_THR>) _param_bat_low_thr
 	);
 
 	// Disallow copy construction and move assignment.

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -104,6 +104,12 @@ int GZBridge::init()
 				model_pose_v.push_back(0.0);
 			}
 
+			// If model position z is less equal than 0, move above floor to prevent floor glitching
+			if (model_pose_v[2] <= 0.0) {
+				PX4_INFO("Model position z is less or equal 0.0, moving upwards");
+				model_pose_v[2] = 0.5;
+			}
+
 			gz::msgs::Pose *p = req.mutable_pose();
 			gz::msgs::Vector3d *position = p->mutable_position();
 			position->set_x(model_pose_v[0]);
@@ -238,7 +244,7 @@ int GZBridge::task_spawn(int argc, char *argv[])
 	int ch;
 	const char *myoptarg = nullptr;
 
-	while ((ch = px4_getopt(argc, argv, "w:m:p:i:n:v:", &myoptind, &myoptarg)) != EOF) {
+	while ((ch = px4_getopt(argc, argv, "w:m:p:i:n:", &myoptind, &myoptarg)) != EOF) {
 		switch (ch) {
 		case 'w':
 			// world
@@ -553,19 +559,6 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose)
 			vehicle_angular_velocity_groundtruth.timestamp = hrt_absolute_time();
 			_angular_velocity_ground_truth_pub.publish(vehicle_angular_velocity_groundtruth);
 
-			if (!_pos_ref.isInitialized()) {
-				if (!_param_use_sim_home.get() || _gz_sim_gps_time_usec) {
-					_pos_ref.initReference(_param_use_sim_home.get() ? _gz_home_lat : (double)_param_sim_home_lat.get(),
-							       _param_use_sim_home.get() ? _gz_home_lon : (double)_param_sim_home_lon.get(),
-							       hrt_absolute_time());
-
-				} else {
-					// no reference set yet
-					pthread_mutex_unlock(&_node_mutex);
-					return;
-				}
-			}
-
 			vehicle_local_position_s local_position_groundtruth{};
 #if defined(ENABLE_LOCKSTEP_SCHEDULER)
 			local_position_groundtruth.timestamp_sample = time_us;
@@ -592,33 +585,27 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose)
 
 			local_position_groundtruth.heading = euler.psi();
 
-			local_position_groundtruth.ref_lat = _pos_ref.getProjectionReferenceLat(); // Reference point latitude in degrees
-			local_position_groundtruth.ref_lon = _pos_ref.getProjectionReferenceLon(); // Reference point longitude in degrees
-			local_position_groundtruth.ref_alt = _param_use_sim_home.get() ? static_cast<float>(_gz_home_alt) :
-							     _param_sim_home_alt.get();
-			local_position_groundtruth.ref_timestamp = _pos_ref.getProjectionReferenceTimestamp();
-
-			local_position_groundtruth.timestamp = hrt_absolute_time();
-			_lpos_ground_truth_pub.publish(local_position_groundtruth);
-
 			if (_pos_ref.isInitialized()) {
-				// publish position groundtruth
-				vehicle_global_position_s global_position_groundtruth{};
-#if defined(ENABLE_LOCKSTEP_SCHEDULER)
-				global_position_groundtruth.timestamp_sample = time_us;
-#else
-				global_position_groundtruth.timestamp_sample = hrt_absolute_time();
-#endif
 
-				_pos_ref.reproject(local_position_groundtruth.x, local_position_groundtruth.y,
-						   global_position_groundtruth.lat, global_position_groundtruth.lon);
+				local_position_groundtruth.ref_lat = _pos_ref.getProjectionReferenceLat(); // Reference point latitude in degrees
+				local_position_groundtruth.ref_lon = _pos_ref.getProjectionReferenceLon(); // Reference point longitude in degrees
+				local_position_groundtruth.ref_alt = _alt_ref;
+				local_position_groundtruth.ref_timestamp = _pos_ref.getProjectionReferenceTimestamp();
+				local_position_groundtruth.xy_global = true;
+				local_position_groundtruth.z_global = true;
 
-				global_position_groundtruth.alt = (_param_use_sim_home.get() ? static_cast<float>(_gz_home_alt) :
-								   _param_sim_home_alt.get()) - static_cast<float>(position(2));
-				global_position_groundtruth.timestamp = hrt_absolute_time();
-				_gpos_ground_truth_pub.publish(global_position_groundtruth);
+			} else {
+				local_position_groundtruth.ref_lat = static_cast<double>(NAN);
+				local_position_groundtruth.ref_lon = static_cast<double>(NAN);
+				local_position_groundtruth.ref_alt = NAN;
+				local_position_groundtruth.ref_timestamp = 0;
+				local_position_groundtruth.xy_global = false;
+				local_position_groundtruth.z_global = false;
 			}
 
+			local_position_groundtruth.timestamp = hrt_absolute_time();
+			_lpos_ground_truth_pub.publish(local_position_groundtruth);
+
 			pthread_mutex_unlock(&_node_mutex);
 			return;
 		}
@@ -627,23 +614,6 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose)
 	pthread_mutex_unlock(&_node_mutex);
 }
 
-void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat)
-{
-	if (hrt_absolute_time() == 0 || _pos_ref.isInitialized()) {
-		return;
-	}
-
-	pthread_mutex_lock(&_node_mutex);
-
-	// initialize gz gps position
-	_gz_sim_gps_time_usec = (nav_sat.header().stamp().sec() * 1000000) + (nav_sat.header().stamp().nsec() / 1000);
-	_gz_home_lat = nav_sat.latitude_deg();
-	_gz_home_lon = nav_sat.longitude_deg();
-	_gz_home_alt = nav_sat.altitude();
-
-	pthread_mutex_unlock(&_node_mutex);
-}
-
 void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &odometry)
 {
 	if (hrt_absolute_time() == 0) {
@@ -720,6 +690,42 @@ void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &odometry
 	pthread_mutex_unlock(&_node_mutex);
 }
 
+void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat)
+{
+	if (hrt_absolute_time() == 0) {
+		return;
+	}
+
+	pthread_mutex_lock(&_node_mutex);
+
+	const uint64_t time_us = (nav_sat.header().stamp().sec() * 1000000) + (nav_sat.header().stamp().nsec() / 1000);
+
+	if (time_us > _world_time_us.load()) {
+		updateClock(nav_sat.header().stamp().sec(), nav_sat.header().stamp().nsec());
+	}
+
+	// initialize gps position
+	if (!_pos_ref.isInitialized()) {
+		_pos_ref.initReference(nav_sat.latitude_deg(), nav_sat.longitude_deg(), hrt_absolute_time());
+		_alt_ref = nav_sat.altitude();
+
+	} else {
+		// publish GPS groundtruth
+		vehicle_global_position_s global_position_groundtruth{};
+#if defined(ENABLE_LOCKSTEP_SCHEDULER)
+		global_position_groundtruth.timestamp_sample = time_us;
+#else
+		global_position_groundtruth.timestamp_sample = hrt_absolute_time();
+#endif
+		global_position_groundtruth.lat = nav_sat.latitude_deg();
+		global_position_groundtruth.lon = nav_sat.longitude_deg();
+		global_position_groundtruth.alt = nav_sat.altitude();
+		_gpos_ground_truth_pub.publish(global_position_groundtruth);
+	}
+
+	pthread_mutex_unlock(&_node_mutex);
+}
+
 void GZBridge::updateCmdVel()
 {
 	bool do_update = false;
@@ -856,4 +862,4 @@ int GZBridge::print_usage(const char *reason)
 extern "C" __EXPORT int gz_bridge_main(int argc, char *argv[])
 {
 	return GZBridge::main(argc, argv);
-}
+}

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -147,6 +147,7 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
 	pthread_mutex_t _node_mutex;
 
 	MapProjection _pos_ref{};
+	double _alt_ref{}; // starting altitude reference
 
 	matrix::Vector3d _position_prev{};
 	matrix::Vector3d _velocity_prev{};
@@ -164,22 +165,7 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
 
 	float _temperature{288.15};  // 15 degrees
 
-	uint64_t _gz_sim_gps_time_usec{0};
-
-	double _gz_home_lat{0.0};
-
-	double _gz_home_lon{0.0};
-
-	double _gz_home_alt{0.0};
-
 	gz::transport::Node::Publisher _cmd_vel_pub;
 
 	gz::transport::Node _node;
-
-	DEFINE_PARAMETERS(
-		(ParamBool<px4::params::SIM_GZ_SHOME>) _param_use_sim_home,
-		(ParamFloat<px4::params::SIM_GZ_HOME_LAT>) _param_sim_home_lat,
-		(ParamFloat<px4::params::SIM_GZ_HOME_LON>) _param_sim_home_lon,
-		(ParamFloat<px4::params::SIM_GZ_HOME_ALT>) _param_sim_home_alt
-	)
-};
+};