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"