Merge branch 'PX4:main' into RG-DFV000SW

.gitmodules

@@ -76,3 +76,6 @@
 	path = src/lib/heatshrink/heatshrink
 	url = https://github.com/PX4/heatshrink.git
 	branch = px4
+[submodule "Tools/simulation/gz"]
+	path = Tools/simulation/gz
+	url = https://github.com/PX4/PX4-gazebo-models.git

CMakeLists.txt

@@ -148,6 +148,7 @@ define_property(GLOBAL PROPERTY PX4_MODULE_PATHS
                  BRIEF_DOCS "PX4 module paths"
                  FULL_DOCS "List of paths to all PX4 modules"
                  )
+
 define_property(GLOBAL PROPERTY PX4_SRC_FILES
                  BRIEF_DOCS "src files from all PX4 modules & libs"
                  FULL_DOCS "SRC files from px4_add_{module,library}"

ROMFS/px4fmu_common/init.d-posix/airframes/1061_gazebo-classic_r1_rover

@@ -4,31 +4,9 @@
 # @type Rover
 # @class Rover
 
-. ${R}etc/init.d/rc.rover_defaults
+. ${R}etc/init.d/rc.rover_differential_defaults
 
-param set-default GND_L1_DIST 5
-param set-default GND_SP_CTRL_MODE 1
-param set-default GND_SPEED_D 3
-param set-default GND_SPEED_I 0.001
-param set-default GND_SPEED_IMAX 0.125
-param set-default GND_SPEED_P 0.25
-param set-default GND_SPEED_THR_SC 1
-param set-default GND_SPEED_TRIM 4
-param set-default GND_THR_CRUISE 0.3
-param set-default GND_THR_MAX 0.5
-param set-default GND_THR_MIN 0
-
-param set-default NAV_ACC_RAD 0.5
-param set-default NAV_LOITER_RAD 2
-
-param set-default GND_MAX_ANG 0.6
-param set-default GND_WHEEL_BASE 2.0
-
-param set-default CA_AIRFRAME 6
-
-param set-default CA_R_REV 3
 param set-default PWM_MAIN_FUNC1 101
 param set-default PWM_MAIN_FUNC2 101
 param set-default PWM_MAIN_FUNC6 102
 param set-default PWM_MAIN_FUNC7 102
-

ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol

@@ -66,7 +66,6 @@ param set-default SIM_GZ_SV_FUNC2 202
 param set-default SIM_GZ_SV_FUNC3 203
 
 param set-default COM_RC_IN_MODE 1
-param set-default ASPD_PRIMARY 1
 
 param set-default CA_SV_CS_COUNT 3
 param set-default CA_SV_CS0_TYPE 1

ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover

@@ -0,0 +1,31 @@
+#!/bin/sh
+# @name Aion Robotics R1 Rover
+# @type Rover
+# @class Rover
+
+. ${R}etc/init.d/rc.rover_differential_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=r1_rover}
+
+param set-default SIM_GZ_EN 1 # Gazebo bridge
+
+# Simulated sensors
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 0
+param set-default SENS_EN_MAGSIM 1
+param set-default SENS_EN_ARSPDSIM 1
+
+# Actuator mapping
+param set-default SIM_GZ_WH_FUNC1 101 # right wheel
+param set-default SIM_GZ_WH_MIN1 0
+param set-default SIM_GZ_WH_MAX1 200
+param set-default SIM_GZ_WH_DIS1 100
+
+param set-default SIM_GZ_WH_FUNC2 102 # left wheel
+param set-default SIM_GZ_WH_MIN2 0
+param set-default SIM_GZ_WH_MAX2 200
+param set-default SIM_GZ_WH_DIS2 100
+
+param set-default SIM_GZ_WH_REV 1 # reverse right wheel

ROMFS/px4fmu_common/init.d-posix/airframes/4010_gz_x500_mono_cam

@@ -0,0 +1,10 @@
+#!/bin/sh
+#
+# @name Gazebo x500 mono cam
+#
+# @type Quadrotor
+#
+
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_mono_cam}
+
+. ${R}etc/init.d-posix/airframes/4001_gz_x500

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -80,6 +80,8 @@ px4_add_romfs_files(
 	4005_gz_x500_vision
 	4006_gz_px4vision
 	4008_gz_advanced_plane
+	4009_gz_r1_rover
+	4010_gz_x500_mono_cam
 
 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post

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,110 +51,86 @@ 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
 
-	# source generated gz_env.sh for GZ_SIM_RESOURCE_PATH
-	if [ -f ./gz_env.sh ]; then
-		. ./gz_env.sh
+	# Only start up Gazebo if PX4_GZ_STANDALONE is not set.
+	if [ -z "${PX4_GZ_STANDALONE}" ]; then
 
-	elif [ -f ../gz_env.sh ]; then
-		. ../gz_env.sh
-	fi
+		# "gz sim" only available in Garden and later
+		GZ_SIM_VERSIONS=$(gz sim --versions 2>&1)
+		if [ $? -eq 0 ] && [ "${GZ_SIM_VERSIONS}" != "" ]
+		then
+			# "gz sim" from Garden on
+			gz_command="gz"
+			gz_sub_command="sim"
+		else
+			echo "ERROR [init] Gazebo gz please install gz-garden"
+			exit 1
+		fi
 
-	# "gz sim" only avaiilable in Garden and later
-	GZ_SIM_VERSIONS=$(gz sim --versions 2>&1)
-	if [ $? -eq 0 ] && [ "${GZ_SIM_VERSIONS}" != "" ]
-	then
-		# "gz sim" from Garden on
-		gz_command="gz"
-		gz_sub_command="sim"
-	else
-		echo "ERROR [init] Gazebo gz please install gz-garden"
-		exit 1
-	fi
+		# look for running ${gz_command} gazebo world
+		gz_world=$( ${gz_command} topic -l | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
 
-	# look for running ${gz_command} gazebo world
-	gz_world=$( ${gz_command} topic -l | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
+		# shellcheck disable=SC2153
+		if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLD}" ]; then
 
-	# shellcheck disable=SC2153
-	if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLDS}" ] && [ -n "${PX4_GZ_WORLD}" ]; then
+			# source generated gz_env.sh for GZ_SIM_RESOURCE_PATH
+			if [ -f ./gz_env.sh ]; then
+				. ./gz_env.sh
 
-		echo "INFO  [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
+			elif [ -f ../gz_env.sh ]; then
+				. ../gz_env.sh
+			fi
 
-		${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
+			echo "INFO  [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
 
-		if [ -z "${HEADLESS}" ]; then
-			# HEADLESS not set, starting gui
-			${gz_command} ${gz_sub_command} -g &
-		fi
+			${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
 
+			if [ -z "${HEADLESS}" ]; then
+				# HEADLESS not set, starting gui
+				${gz_command} ${gz_sub_command} -g &
+			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
 	else
-		echo "INFO  [init] gazebo already running world: ${gz_world}"
-		PX4_GZ_WORLD=${gz_world}
+		echo "INFO  [init] Standalone PX4 launch, waiting for Gazebo"
 	fi
 
 	# start gz_bridge
 	if [ -n "${PX4_SIM_MODEL#*gz_}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ]; 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
 
@@ -160,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"

ROMFS/px4fmu_common/init.d-posix/rcS

@@ -153,7 +153,6 @@ fi
 
 param set-default BAT1_N_CELLS 4
 
-param set-default CBRK_AIRSPD_CHK 0
 param set-default CBRK_SUPPLY_CHK 894281
 
 # disable check, no CPU load reported on posix yet

ROMFS/px4fmu_common/init.d/CMakeLists.txt

@@ -51,6 +51,8 @@ px4_add_romfs_files(
 	rc.thermal_cal
 	rc.rover_apps
 	rc.rover_defaults
+	rc.rover_differential_apps
+	rc.rover_differential_defaults
 	rc.uuv_apps
 	rc.uuv_defaults
 	rc.vehicle_setup

ROMFS/px4fmu_common/init.d/airframes/13013_deltaquad

@@ -28,13 +28,13 @@ param set-default BAT1_CAPACITY 23000
 param set-default BAT1_N_CELLS 4
 param set-default BAT1_R_INTERNAL 0.0025
 
-param set-default CBRK_AIRSPD_CHK 162128
+param set-default SYS_HAS_NUM_ASPD 0
 param set-default CBRK_IO_SAFETY 22027
 
 param set-default EKF2_GPS_POS_X -0.12
 param set-default EKF2_IMU_POS_X -0.12
 
-param set-default FW_ARSP_MODE 1
+param set-default FW_USE_AIRSPD 0
 param set-default NPFG_PERIOD 25
 param set-default FW_PR_FF 0.7
 param set-default FW_PR_I 0.18

ROMFS/px4fmu_common/init.d/airframes/50003_aion_robotics_r1_rover

@@ -11,52 +11,15 @@
 # @board bitcraze_crazyflie exclude
 #
 
-. ${R}etc/init.d/rc.rover_defaults
-
-
+. ${R}etc/init.d/rc.rover_differential_defaults
 
 param set-default BAT1_N_CELLS 4
 
 param set-default EKF2_GBIAS_INIT 0.01
 param set-default EKF2_ANGERR_INIT 0.01
-param set-default EKF2_MAG_TYPE 1
-
-param set-default FW_AIRSPD_MIN 0
-param set-default FW_AIRSPD_TRIM 1
-param set-default FW_AIRSPD_MAX 3
-
-param set-default GND_SP_CTRL_MODE 1
-param set-default GND_L1_DIST 5
-param set-default GND_L1_PERIOD 3
-param set-default GND_THR_CRUISE 0.7
-param set-default GND_THR_MAX 0.5
-
-# Because this is differential drive, it can make a turn with radius 0.
-# This corresponds to a turn angle of pi radians.
-# If a special case is made for differential-drive, this will need to change.
-param set-default GND_MAX_ANG 3.142
-param set-default GND_WHEEL_BASE 0.3
-
-# TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase
-# to support negative throttle.
-param set-default GND_THR_MIN 0
-param set-default GND_SPEED_P 0.25
-param set-default GND_SPEED_I 3
-param set-default GND_SPEED_D 0.001
-param set-default GND_SPEED_IMAX 0.125
-param set-default GND_SPEED_THR_SC 1
-
-param set-default NAV_ACC_RAD 0.5
-
-# Differential drive acts like ackermann steering with a maximum turn angle of 180 degrees, or pi radians
-param set-default GND_MAX_ANG 3.1415
 
 # Set geometry & output configration
-param set-default CA_AIRFRAME 6
-param set-default CA_R_REV 3
-
-
 param set-default RBCLW_ADDRESS 128
 param set-default RBCLW_FUNC1 101
 param set-default RBCLW_FUNC2 102
-param set-default RBCLW_REV 1
+param set-default RBCLW_REV 1 # reverse right wheels

ROMFS/px4fmu_common/init.d/rc.fw_defaults

@@ -54,3 +54,5 @@ param set-default MIS_TKO_LAND_REQ 2
 # FW takeoff acceleration can easily exceed ublox GPS 2G default.
 #
 param set-default GPS_UBX_DYNMODEL 8
+
+param set-default SYS_HAS_NUM_ASPD 1 # by default require an airspeed sensor