From 613564fffae8dd48e6630c95ac0177a4e969358e Mon Sep 17 00:00:00 2001
From: Beniamino Pozzan <beniamino.pozzan@phd.unipd.it>
Date: Mon, 18 Dec 2023 22:08:30 +0100
Subject: [PATCH] [rc.simulator] fix PX4_HOME_ALT and cleanup

Signed-off-by: Beniamino Pozzan <beniamino.pozzan@gmail.com>
---
 .../init.d-posix/px4-rc.simulator             | 73 ++++++++-----------
 1 file changed, 32 insertions(+), 41 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
index 595aa974f6..d8ea0f3b95 100644
--- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
+++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
@@ -37,6 +37,9 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"
 	fi
 
 elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
+	# Use Gazebo
+
+	echo "INFO  [init] Gazebo simulator"
 
 	# set local coordinate frame reference
 	if [ -n "${PX4_HOME_LAT}" ]; then
@@ -48,7 +51,7 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
 	fi
 
 	if [ -n "${PX4_HOME_ALT}" ]; then
-		param set SIM_GZ_HOME_ALT ${PX4_HOME_LON}
+		param set SIM_GZ_HOME_ALT ${PX4_HOME_ALT}
 	fi
 
 	# Only start up Gazebo if PX4_GZ_STANDALONE is not set.
@@ -90,6 +93,7 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
 			fi
 
 		else
+			# Gazebo is already running, do not start the simulator, nor the GUI
 			echo "INFO  [init] gazebo already running world: ${gz_world}"
 			PX4_GZ_WORLD=${gz_world}
 		fi
@@ -102,62 +106,31 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
 		# model specified, gz_bridge will spawn model
 
 		if [ -n "${PX4_GZ_MODEL_POSE}" ]; then
+			# model pose provided: [x, y, z, roll, pitch, yaw]
+
 			# Clean potential input line formatting.
 			model_pose="$( echo "${PX4_GZ_MODEL_POSE}" | sed -e 's/^[ \t]*//; s/[ \t]*$//; s/,/ /g; s/  / /g; s/ /,/g' )"
 			echo "INFO  [init] PX4_GZ_MODEL_POSE set, spawning at: ${model_pose}"
 
 		else
+			# model pose not provided, origin will be used
+
 			echo "WARN  [init] PX4_GZ_MODEL_POSE not set, spawning at origin."
 			model_pose="0,0,0,0,0,0"
 		fi
 
 		# start gz bridge with pose arg.
-		if gz_bridge start -p "${model_pose}" -m "${PX4_SIM_MODEL#*gz_}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then
-			if param compare -s SENS_EN_BAROSIM 1
-			then
-				sensor_baro_sim start
-			fi
-			if param compare -s SENS_EN_GPSSIM 1
-			then
-				sensor_gps_sim start
-			fi
-			if param compare -s SENS_EN_MAGSIM 1
-			then
-				sensor_mag_sim start
-			fi
-			if param compare -s SENS_EN_ARSPDSIM 1
-			then
-				sensor_airspeed_sim start
-			fi
-
-		else
-			echo "ERROR [init] gz_bridge failed to start"
+		if ! gz_bridge start -p "${model_pose}" -m "${PX4_SIM_MODEL#*gz_}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then
+			echo "ERROR [init] gz_bridge failed to start and spawn model"
 			exit 1
 		fi
 
 	elif [ -n "${PX4_GZ_MODEL_NAME}" ]; then
 		# model name specificed, gz_bridge will attach to existing model
 
-		if gz_bridge start -n "${PX4_GZ_MODEL_NAME}" -w "${PX4_GZ_WORLD}"; then
-			if param compare -s SENS_EN_BAROSIM 1
-			then
-				sensor_baro_sim start
-			fi
-			if param compare -s SENS_EN_GPSSIM 1
-			then
-				sensor_gps_sim start
-			fi
-			if param compare -s SENS_EN_MAGSIM 1
-			then
-				sensor_mag_sim start
-			fi
-			if param compare -s SENS_EN_ARSPDSIM 1
-			then
-				sensor_airspeed_sim start
-			fi
-
-		else
-			echo "ERROR [init] gz_bridge failed to start"
+		echo "INFO  [init] PX4_GZ_MODEL_NAME set, PX4 will attach to existing model"
+		if ! gz_bridge start -n "${PX4_GZ_MODEL_NAME}" -w "${PX4_GZ_WORLD}"; then
+			echo "ERROR [init] gz_bridge failed to start and attach to existing model"
 			exit 1
 		fi
 
@@ -166,6 +139,24 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
 		exit 1
 	fi
 
+	# Start the sensor simulator modules
+	if param compare -s SENS_EN_BAROSIM 1
+	then
+		sensor_baro_sim start
+	fi
+	if param compare -s SENS_EN_GPSSIM 1
+	then
+		sensor_gps_sim start
+	fi
+	if param compare -s SENS_EN_MAGSIM 1
+	then
+		sensor_mag_sim start
+	fi
+	if param compare -s SENS_EN_ARSPDSIM 1
+	then
+		sensor_airspeed_sim start
+	fi
+
 elif [ "$PX4_SIM_MODEL" = "jmavsim_iris" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "10017" ]; then
 
 	echo "INFO  [init] jMAVSim simulator"