diff --git a/.gitmodules b/.gitmodules index 136aebb441..42df16a98a 100644 --- a/.gitmodules +++ b/.gitmodules @@ -6,12 +6,12 @@ path = src/drivers/uavcan/libuavcan url = https://github.com/dronecan/libuavcan.git branch = main -[submodule "Tools/jMAVSim"] - path = Tools/jMAVSim +[submodule "Tools/simulation/jmavsim/jMAVSim"] + path = Tools/simulation/jmavsim/jMAVSim url = https://github.com/PX4/jMAVSim.git branch = master -[submodule "Tools/sitl_gazebo"] - path = Tools/sitl_gazebo +[submodule "Tools/simulation/gazebo/sitl_gazebo"] + path = Tools/simulation/gazebo/sitl_gazebo url = https://github.com/PX4/PX4-SITL_gazebo.git branch = master [submodule "src/drivers/gps/devices"] @@ -31,10 +31,10 @@ url = https://github.com/PX4/NuttX-apps.git branch = px4_firmware_nuttx-10.1.0+ [submodule "Tools/flightgear_bridge"] - path = Tools/flightgear_bridge + path = Tools/simulation/flightgear/flightgear_bridge url = https://github.com/PX4/PX4-FlightGear-Bridge.git -[submodule "Tools/jsbsim_bridge"] - path = Tools/jsbsim_bridge +[submodule "Tools/simulation/jsbsim/jsbsim_bridge"] + path = Tools/simulation/jsbsim/jsbsim_bridge url = https://github.com/PX4/px4-jsbsim-bridge.git [submodule "src/drivers/cyphal/libcanard"] path = src/drivers/cyphal/libcanard diff --git a/.vscode/cmake-variants.yaml b/.vscode/cmake-variants.yaml index 3cd8f3f8e7..06c4c14c92 100644 --- a/.vscode/cmake-variants.yaml +++ b/.vscode/cmake-variants.yaml @@ -2,7 +2,7 @@ CONFIG: default: px4_sitl_default choices: px4_sitl_default: - short: px4_sitl + short: px4_sitl_default buildType: RelWithDebInfo settings: CONFIG: px4_sitl_default diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 415f3ce06a..3fcaac5334 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -54,7 +54,7 @@ "type": "shell", "command": "ant create_run_jar copy_res", "options": { - "cwd": "${workspaceFolder}/Tools/jMAVSim" + "cwd": "${workspaceFolder}/Tools/simulation/jmavsim/jMAVSim" }, "problemMatcher": [], "presentation":{ @@ -72,7 +72,7 @@ "dependsOn": "jmavsim build", "command": "java -Djava.ext.dirs= -jar jmavsim_run.jar -r 250 -lockstep -tcp localhost:4560 -qgc", "options": { - "cwd": "${workspaceFolder}/Tools/jMAVSim/out/production", + "cwd": "${workspaceFolder}/Tools/simulation/jmavsim/jMAVSim/out/production", "env": { "PX4_SIM_SPEED_FACTOR": "1" } @@ -86,6 +86,62 @@ "showReuseMessage": false, "clear": false }, + "problemMatcher": [] + }, + { + "label": "jmavsim kill", + "type": "shell", + "command": "kill $(ps aux | grep jmavsim | grep -v 'grep' | awk '{print $2}')", + "presentation": { + "echo": false, + "reveal": "never", + "focus": false, + "panel": "shared", + "showReuseMessage": false, + "clear": false + }, + "problemMatcher": [], + "dependsOn":["px4_sitl_cleanup"] + }, + { + "label": "gazebo build", + "type": "shell", + "command": "make px4_sitl_default sitl_gazebo", + "options": { + "cwd": "${workspaceFolder}" + }, + "problemMatcher": [], + "presentation":{ + "echo": true, + "reveal": "always", + "focus": false, + "panel": "shared", + "showReuseMessage": false, + "clear": false, + } + }, + { + "label": "gazebo start", + "type": "shell", + "dependsOn": "gazebo build", + "options": { + "cwd": "${workspaceFolder}", + "env": { + "GAZEBO_PLUGIN_PATH": "${workspaceFolder}/build/px4_sitl_default/build_gazebo", + "GAZEBO_MODEL_PATH": "${workspaceFolder}/Tools/simulation/gazebo/sitl_gazebo/models", + "PX4_SIM_SPEED_FACTOR": "1" + } + }, + "command": "gzserver --verbose ${workspaceFolder}/Tools/simulation/gazebo/sitl_gazebo/worlds/${input:gazeboWorld}.world", + "isBackground": true, + "presentation": { + "echo": true, + "reveal": "never", + "focus": false, + "panel": "shared", + "showReuseMessage": false, + "clear": false + }, "problemMatcher": [ { "pattern": [ @@ -105,31 +161,56 @@ ] }, { - "label": "jmavsim kill", + "label": "gazebo", "type": "shell", - "command": "kill $(ps aux | grep jmavsim | grep -v 'grep' | awk '{print $2}')", + "dependsOn": "gazebo start", + "options": { + "cwd": "${workspaceFolder}", + "env": { + "GAZEBO_PLUGIN_PATH": "${workspaceFolder}/build/px4_sitl_default/build_gazebo", + "GAZEBO_MODEL_PATH": "${workspaceFolder}/Tools/simulation/gazebo/sitl_gazebo/models", + "PX4_SIM_SPEED_FACTOR": "1" + } + }, + "command": "gz model --verbose --spawn-file=${workspaceFolder}/Tools/simulation/gazebo/sitl_gazebo/models/${input:gazeboModel}/${input:gazeboModel}.sdf --model-name=${input:gazeboModel} -x 1.01 -y 0.98 -z 0.83", + "isBackground": false, "presentation": { - "echo": false, + "echo": true, "reveal": "never", "focus": false, "panel": "shared", "showReuseMessage": false, "clear": false }, - "problemMatcher": [] + "problemMatcher": [ + { + "pattern": [ + { + "regexp": ".", + "file": 1, + "location": 2, + "message": 3 + } + ], + "background": { + "activeOnStart": true, + "beginsPattern": ".", + "endsPattern": ".", + } + } + ] }, - { - "label": "gazebo", + "label": "ign gazebo", "type": "shell", "options": { "cwd": "${workspaceFolder}", "env": { - "IGN_GAZEBO_RESOURCE_PATH": "${workspaceFolder}/Tools/simulation/gazebo/models", + "IGN_GAZEBO_RESOURCE_PATH": "${workspaceFolder}/Tools/simulation/ignition/models", "PX4_SIM_SPEED_FACTOR": "1" } }, - "command": "ign gazebo -v 4 -r ${workspaceFolder}/Tools/simulation/gazebo/worlds/${input:gazeboWorld}.sdf", + "command": "ign gazebo -v 4 -r ${workspaceFolder}/Tools/simulation/ignition/worlds/${input:ignWorld}.sdf", "isBackground": true, "presentation": { "echo": true, @@ -159,10 +240,26 @@ } ] }, - { "label": "gazebo kill", "type": "shell", + "command": "pkill -9 -f gzserver || true", + "presentation": { + "echo": true, + "reveal": "never", + "revealProblems": "onProblem", + "focus": false, + "panel": "dedicated", + "showReuseMessage": false, + "clear": false, + "close": true + }, + "problemMatcher": [], + "dependsOn":["px4_sitl_cleanup"] + }, + { + "label": "ign gazebo kill", + "type": "shell", "command": "pkill -9 -f ign || true", "presentation": { "echo": true, @@ -247,12 +344,46 @@ } ], "inputs": [ + { + "type": "pickString", + "id": "ignWorld", + "description": "Ignition world", + "options": [ + "default" + ], + "default": "default" + }, + { + "type": "pickString", + "id": "gazeboModel", + "description": "gazebo model", + "options": [ + "iris", + "typhoon_h480", + "plane", + "plane_catapult", + "plane_lidar", + "standard_vtol", + "tailsitter", + "tiltrotor", + "r1_rover", + "boat" + ], + "default": "iris" + }, { "type": "pickString", "id": "gazeboWorld", "description": "gazebo world", "options": [ - "empty" + "baylands", + "empty", + "ksql_airport", + "mcmillan_airfield", + "sonoma_raceway", + "warehouse", + "windy", + "yosemite" ], "default": "empty" } diff --git a/Jenkinsfile b/Jenkinsfile index 3538c3d130..1dfbe33093 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -26,15 +26,15 @@ pipeline { // echo $0; // mkdir -p catkin_ws/src; // cd catkin_ws; - // git -C ${WORKSPACE}/catkin_ws/src/Firmware submodule update --init --recursive --force Tools/sitl_gazebo - // git clone --recursive ${WORKSPACE}/catkin_ws/src/Firmware/Tools/sitl_gazebo src/mavlink_sitl_gazebo; + // git -C ${WORKSPACE}/catkin_ws/src/Firmware submodule update --init --recursive --force Tools/simulation/gazebo/sitl_gazebo + // git clone --recursive ${WORKSPACE}/catkin_ws/src/Firmware/Tools/simulation/gazebo/sitl_gazebo src/mavlink_sitl_gazebo; // git -C ${WORKSPACE}/catkin_ws/src/Firmware fetch --tags; // source /opt/ros/melodic/setup.bash; // export PYTHONPATH=/opt/ros/$ROS_DISTRO/lib/python2.7/dist-packages:/usr/lib/python2.7/dist-packages:/usr/local/lib/python2.7/dist-packages; // catkin init; // catkin build -j$(nproc) -l$(nproc); // ''' - // // test if the binary was correctly installed and runs using 'mavros_posix_silt.launch' + // // test if the binary was correctly installed and runs using 'mavros_posix_sitl.launch' // sh '''#!/bin/bash -l // echo $0; // source catkin_ws/devel/setup.bash; @@ -68,7 +68,7 @@ pipeline { unset ROS_DISTRO; mkdir -p colcon_ws/src; cd colcon_ws; - git -C ${WORKSPACE}/colcon_ws/src/Firmware submodule update --init --recursive --force Tools/sitl_gazebo; + git -C ${WORKSPACE}/colcon_ws/src/Firmware submodule update --init --recursive --force Tools/simulation/gazebo/sitl_gazebo; git -C ${WORKSPACE}/colcon_ws/src/Firmware fetch --tags; source /opt/ros/foxy/setup.sh; colcon build --event-handlers console_direct+ --symlink-install; diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10040_quadx b/ROMFS/px4fmu_common/init.d-posix/airframes/10040_quadx index a7bce6916f..8780575f6b 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10040_quadx +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10040_quadx @@ -9,6 +9,8 @@ . ${R}etc/init.d/rc.mc_defaults +PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=quadx} # disable some checks to allow to fly: # - with usb diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_airplane b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_airplane index 5f20770c1f..0b7b254e05 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10041_airplane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10041_airplane @@ -8,6 +8,8 @@ . ${R}etc/init.d/rc.fw_defaults +PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=airplane} # disable some checks to allow to fly: # - with usb diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert index dc8629c8da..c87491c413 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/10042_xvert @@ -8,6 +8,9 @@ . ${R}etc/init.d/rc.vtol_defaults +PX4_SIMULATOR=${PX4_SIMULATOR:=sihsim} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=xvert} + param set-default VT_ELEV_MC_LOCK 0 param set-default VT_TYPE 0 param set-default VT_FW_DIFTHR_EN 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_x3 b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_x3 index dd22a63375..92ab54c1b9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_x3 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_x3 @@ -7,6 +7,10 @@ . ${R}etc/init.d/rc.mc_defaults +PX4_SIMULATOR=${PX4_SIMULATOR:=ignition} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=x3} +PX4_SIM_WORLD=${PX4_SIM_WORLD:=default} + param set-default SYS_CTRL_ALLOC 1 param set-default CA_AIRFRAME 0 @@ -28,19 +32,19 @@ param set-default CA_ROTOR3_PX -0.13 param set-default CA_ROTOR3_PY 0.20 param set-default CA_ROTOR3_KM -0.05 -param set-default SIM_GZ_FUNC1 101 -param set-default SIM_GZ_FUNC2 102 -param set-default SIM_GZ_FUNC3 103 -param set-default SIM_GZ_FUNC4 104 +param set-default SIM_IGN_FUNC1 101 +param set-default SIM_IGN_FUNC2 102 +param set-default SIM_IGN_FUNC3 103 +param set-default SIM_IGN_FUNC4 104 -param set-default SIM_GZ_MIN1 150 -param set-default SIM_GZ_MIN2 150 -param set-default SIM_GZ_MIN3 150 -param set-default SIM_GZ_MIN4 150 +param set-default SIM_IGN_MIN1 150 +param set-default SIM_IGN_MIN2 150 +param set-default SIM_IGN_MIN3 150 +param set-default SIM_IGN_MIN4 150 -param set-default SIM_GZ_MAX1 1000 -param set-default SIM_GZ_MAX2 1000 -param set-default SIM_GZ_MAX3 1000 -param set-default SIM_GZ_MAX4 1000 +param set-default SIM_IGN_MAX1 1000 +param set-default SIM_IGN_MAX2 1000 +param set-default SIM_IGN_MAX3 1000 +param set-default SIM_IGN_MAX4 1000 param set-default MPC_THR_HOVER 0.60 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/6001_x4 b/ROMFS/px4fmu_common/init.d-posix/airframes/6001_x4 index 44099ef47b..a1add4c464 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/6001_x4 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/6001_x4 @@ -7,6 +7,10 @@ . ${R}etc/init.d/rc.mc_defaults +PX4_SIMULATOR=${PX4_SIMULATOR:=ignition} +PX4_SIM_MODEL=${PX4_SIM_MODEL:=x4} +PX4_SIM_WORLD=${PX4_SIM_WORLD:=default} + param set-default SYS_CTRL_ALLOC 1 param set-default CA_AIRFRAME 0 @@ -36,26 +40,26 @@ param set-default CA_ROTOR5_PX 0.25 param set-default CA_ROTOR5_PY 0.15 param set-default CA_ROTOR5_KM -0.05 -param set-default SIM_GZ_FUNC1 101 -param set-default SIM_GZ_FUNC2 102 -param set-default SIM_GZ_FUNC3 103 -param set-default SIM_GZ_FUNC4 104 -param set-default SIM_GZ_FUNC5 105 -param set-default SIM_GZ_FUNC6 106 +param set-default SIM_IGN_FUNC1 101 +param set-default SIM_IGN_FUNC2 102 +param set-default SIM_IGN_FUNC3 103 +param set-default SIM_IGN_FUNC4 104 +param set-default SIM_IGN_FUNC5 105 +param set-default SIM_IGN_FUNC6 106 -param set-default SIM_GZ_MIN1 150 -param set-default SIM_GZ_MIN2 150 -param set-default SIM_GZ_MIN3 150 -param set-default SIM_GZ_MIN4 150 -param set-default SIM_GZ_MIN5 150 -param set-default SIM_GZ_MIN6 150 +param set-default SIM_IGN_MIN1 150 +param set-default SIM_IGN_MIN2 150 +param set-default SIM_IGN_MIN3 150 +param set-default SIM_IGN_MIN4 150 +param set-default SIM_IGN_MIN5 150 +param set-default SIM_IGN_MIN6 150 -param set-default SIM_GZ_MAX1 1000 -param set-default SIM_GZ_MAX2 1000 -param set-default SIM_GZ_MAX3 1000 -param set-default SIM_GZ_MAX4 1000 -param set-default SIM_GZ_MAX5 1000 -param set-default SIM_GZ_MAX6 1000 +param set-default SIM_IGN_MAX1 1000 +param set-default SIM_IGN_MAX2 1000 +param set-default SIM_IGN_MAX3 1000 +param set-default SIM_IGN_MAX4 1000 +param set-default SIM_IGN_MAX5 1000 +param set-default SIM_IGN_MAX6 1000 param set-default MC_PITCHRATE_D 0.0016 diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink b/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink index 0867fa5c64..07cc78b4eb 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlink @@ -32,7 +32,7 @@ mavlink start -x -u $udp_onboard_payload_port_local -r 4000 -f -m onboard -o $ud mavlink start -x -u $udp_onboard_gimbal_port_local -r 400000 -m gimbal -o $udp_onboard_gimbal_port_remote # To display for SIH sitl -if [ "$SIM_MODE" = "sihsim" ]; then +if [ "$PX4_SIMULATOR" = "sihsim" ]; then udp_sihsim_port_local=$((19450+px4_instance)) udp_sihsim_port_remote=$((19410+px4_instance)) mavlink start -x -u $udp_sihsim_port_local -r 400000 -m custom -o $udp_sihsim_port_remote diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index 547196b6a5..fba31e6bf8 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -1,25 +1,50 @@ #!/bin/sh # shellcheck disable=SC2154 -PX4_SIM_WORLD="${PX4_SIM_WORLD:="empty"}" # default to empty world? +# Simulator IMU data provided at 250 Hz +param set-default IMU_INTEG_RATE 250 -echo "PX4_SIM_MODEL: ${PX4_SIM_MODEL}" -echo "PX4_SIM_WORLD: ${PX4_SIM_WORLD}" +if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" ]; then -# TODO: verify if world exists -ign service --info --service /world/${PX4_SIM_WORLD}/create -echo $? + if ! simulator_sih start; then + echo "ERROR [init] simulator_sih failed to start" + exit 1 + fi -ign service -s /world/${PX4_SIM_WORLD}/create --reqtype ignition.msgs.EntityFactory --reptype ignition.msgs.Boolean --timeout 1000 --req "sdf_filename: \"${PX4_SIM_MODEL}/model.sdf\"" +elif [ "$PX4_SIMULATOR" = "ignition" ]; then -if ignition_simulator start -m "${PX4_SIM_MODEL}" -w "${PX4_SIM_WORLD}" -then + # source generated gazebo_env.sh for IGN_GAZEBO_RESOURCE_PATH + if [ -f gazebo_env.sh ]; then + . ./gazebo_env.sh + elif [ -f ../gazebo_env.sh ]; then + . ../gazebo_env.sh + fi - sensor_baro_sim start - sensor_gps_sim start - sensor_mag_sim start + if ! ign service --info --service /world/${PX4_SIM_WORLD}/create | grep "ignition.msgs.EntityFactory"; then + # starting ign gazebo with ${PX4_SIM_WORLD} world + echo "INFO [init] starting ign gazebo" + + if [ -z $HEADLESS ]; then + ign gazebo --verbose=1 -r "${PX4_IGN_GAZEBO_WORLDS}/${PX4_SIM_WORLD}.sdf" & + else + # starting ign gazebo headless + ign gazebo --verbose=1 -r -s "${PX4_IGN_GAZEBO_WORLDS}/${PX4_SIM_WORLD}.sdf" & + fi + else + echo "INFO [init] ign gazebo already running" + fi + + if simulator_ignition_bridge start -m "${PX4_SIM_MODEL}" -w "${PX4_SIM_WORLD}"; then + sensor_baro_sim start + sensor_gps_sim start + sensor_mag_sim start + else + echo "ERROR [init] ign gazebo failed to start" + exit 1 + fi else + # otherwise start simulator (mavlink) module simulator_tcp_port=$((4560+px4_instance)) # Check if PX4_SIM_HOSTNAME environment variable is empty @@ -29,15 +54,15 @@ else if [ -z "${PX4_SIM_HOST_ADDR}" ]; then echo "PX4 SIM HOST: localhost" - simulator start -c $simulator_tcp_port + simulator_mavlink start -c $simulator_tcp_port else echo "PX4 SIM HOST: $PX4_SIM_HOST_ADDR" - simulator start -t $PX4_SIM_HOST_ADDR $simulator_tcp_port + simulator_mavlink start -t $PX4_SIM_HOST_ADDR $simulator_tcp_port fi else echo "PX4 SIM HOST: $PX4_SIM_HOSTNAME" - simulator start -h $PX4_SIM_HOSTNAME $simulator_tcp_port + simulator_mavlink start -h $PX4_SIM_HOSTNAME $simulator_tcp_port fi fi diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index 20a68ecc11..40f5eb5af9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -38,10 +38,24 @@ set LOGGER_BUF 1000 set RUN_MINIMAL_SHELL no -# Use the variable set by sitl_run.sh to choose the model settings. -if [ "$PX4_SIM_MODEL" = "shell" ]; then +set SYS_AUTOSTART=0 + +if [ "$PX4_SIM_MODEL" = "shell" ] +then set RUN_MINIMAL_SHELL yes -else + +elif [ -n "$PX4_SYS_AUTOSTART" ] +then + echo "env SYS_AUTOSTART: ${PX4_SYS_AUTOSTART}" + SYS_AUTOSTART=${PX4_SYS_AUTOSTART} + +elif [ "$PX4_SIM_MODEL" = "none" ] || [ -z $PX4_SIM_MODEL ] +then + # no airframe selected + SYS_AUTOSTART=0 + +elif [ -n "$PX4_SIM_MODEL" ] +then # Find the matching Autostart ID (file name has the form: [0-9]+_${PX4_SIM_MODEL}) # TODO: unify with rc.autostart generation # shellcheck disable=SC2012 @@ -50,12 +64,13 @@ else echo "ERROR [init] Unknown model $PX4_SIM_MODEL (not found by name on ${R}etc/init.d-posix/airframes)" exit 1 else + SYS_AUTOSTART=$REQUESTED_AUTOSTART echo "INFO [init] found model autostart file as SYS_AUTOSTART=$REQUESTED_AUTOSTART" fi fi # Load parameters -set PARAM_FILE eeprom/parameters_"$REQUESTED_AUTOSTART" +set PARAM_FILE parameters.bson param select $PARAM_FILE if [ -f $PARAM_FILE ] @@ -66,26 +81,16 @@ then else echo "[param] FAILED loading $PARAM_FILE" fi -else - echo "[param] parameter file not found, creating $PARAM_FILE" fi # exit early when the minimal shell is requested [ $RUN_MINIMAL_SHELL = yes ] && exit 0 +if param compare SYS_AUTOSTART $SYS_AUTOSTART +then + set AUTOCNF no -# Use environment variable PX4_ESTIMATOR to choose estimator. -if [ "$PX4_ESTIMATOR" = "q" ]; then - param set SYS_MC_EST_GROUP 3 -elif [ "$PX4_ESTIMATOR" = "ekf2" ]; then - param set SYS_MC_EST_GROUP 2 -elif [ "$PX4_ESTIMATOR" = "lpe" ]; then - param set SYS_MC_EST_GROUP 1 -elif [ "$PX4_ESTIMATOR" = "inav" ]; then - param set SYS_MC_EST_GROUP 0 -fi - -if param compare SYS_AUTOSTART $REQUESTED_AUTOSTART +elif [ "$SYS_AUTOSTART" -eq 0 ] then set AUTOCNF no else @@ -106,7 +111,7 @@ param set MAV_SYS_ID $((px4_instance+1)) if [ $AUTOCNF = yes ] then - param set SYS_AUTOSTART $REQUESTED_AUTOSTART + param set SYS_AUTOSTART $SYS_AUTOSTART param set CAL_ACC0_ID 1310988 # 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION param set CAL_GYRO0_ID 1310988 # 1310988: DRV_IMU_DEVTYPE_SIM, BUS: 1, ADDR: 1, TYPE: SIMULATION @@ -190,27 +195,26 @@ do ;; esac done -if [ ! -e "$autostart_file" ]; then + +if [ -e "$autostart_file" ] +then + . "$autostart_file" + +elif [ ! -e "$autostart_file" ] && [ "$SYS_AUTOSTART" -ne "0" ] +then echo "Error: no autostart file found ($autostart_file)" exit 1 fi -. "$autostart_file" - -# Simulator IMU data provided at 250 Hz -param set IMU_INTEG_RATE 250 - #user defined params for instances can be in PATH . px4-rc.params dataman start -# start sih in sih_sim mode, otherwise simulator module -if [ "$SIM_MODE" = "sihsim" ]; then - sih start + # only start the simulator if not in replay mode, as both control the lockstep time -elif ! replay tryapplyparams +if ! replay tryapplyparams then - . px4-rc.simulator + . px4-rc.simulator fi load_mon start diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 90a27cb3c6..d500f307c8 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -368,7 +368,7 @@ else # start the simulator in hardware if needed if param compare SYS_HITL 2 then - sih start + simulator_sih start fi else diff --git a/Tools/fetch_file.py b/Tools/fetch_file.py old mode 100644 new mode 100755 diff --git a/Tools/fix_headers.sh b/Tools/fix_headers.sh deleted file mode 100755 index e54d8d5fb5..0000000000 --- a/Tools/fix_headers.sh +++ /dev/null @@ -1,901 +0,0 @@ -#! /bin/bash - -# Written by Carlo Wood, September/October 2016. - -function fatal -{ - echo "$0: ERROR: $*" - exit 1 -} - -# Make sure we're not having a broken gawk. -AWK_VERSION=$(awk -V | head -n 1) -if [[ $AWK_VERSION =~ ^GNU\ Awk\ 4\.[0-9]+\.[0-9]+ ]]; then - AWK_VERSION=$(echo $AWK_VERSION | sed -e 's/GNU Awk \(4\.[0-9]*\.[0-9]*\).*/\1/') - if [[ $AWK_VERSION =~ ^4\.0*([2-9]+|1\.0*[2-9]+) ]]; then - fatal "Your version of awk ($AWK_VERSION) is broken. Please use version 4.1.1 or lower." - fi -fi -echo "AWK_VERSION=$AWK_VERSION" - -# Find out what the base directory is. -BASEDIR="$(dirname $(dirname $(readlink -en "$0")))" -echo "BASEDIR=\"$BASEDIR\"" -striplen=$((${#BASEDIR} + 2)) - -# BASEDIR may not contain a space, that's just too hard to get to work. -expr index "$BASEDIR" " " >/dev/null && fatal "it is not supported that BASEDIR contains a space." - -# Make sure that worked. -test -f $BASEDIR/cmake/posix/px4_impl_posix.cmake || fatal "Failed to determine BASEDIR: '\$BASEDIR/cmake/posix/px4_impl_posix.cmake' is not a regular file." - -# Parse command line parameters. -debug=0 # Set to non-zero to enable debugging. -force=0 # Set to 1 to force running of script even when there are uncommitted changes. -merge=0 # Set to 1 when merging a branch that didn't run this script into master that did already run this script. -while [[ $# -gt 0 ]] -do - case $1 in - --debug) - debug=1 - ;; - --force) - force=1 - ;; - --merge) - force=1 - merge=1 - fatal "--merge is not implemented yet." - ;; - -*) - fatal "Unknown option $1" - ;; - --|*) - break - ;; - esac - shift -done -non_option_arguments=$# -if [ $non_option_arguments -eq 0 -a $debug -ne 0 ]; then - fatal "--debug screws up the source files with debug output! You must provide a single filename to run on." -fi - -# Better not run this script with changes that still need to be committed. -cd "$BASEDIR" || fatal "Could not change directory to \"$BASEDIR\"" -if ! git diff-index --quiet HEAD --; then - if [ $non_option_arguments -ne 0 -o $force -eq 1 ]; then - if [ $force -eq 1 ]; then - echo "Uncommitted changes, but running anyway because --force is used." - else - echo -n "WARNING: You have uncommitted changes (use --force to remove this warning). Run anyway? [y/N] " - read answer - if [ "x$answer" != "xy" -a "x$answer" != "xY" ]; then exit 0; fi - fi - else - fatal "Your working directory has uncommitted changes (see 'git status')! Bailing out." - fi -fi - -# Find a reasonable tmp directory. -# First make a list of all build directories by looking for a CMakeCache.txt in them. Sort them so the most recent one is first. -CMAKECACHE_FILES=$(find "$BASEDIR" -mindepth 2 -maxdepth 2 -type f -name CMakeCache.txt -wholename "$BASEDIR/build/*/CMakeCache.txt" | xargs /bin/ls -td) -# Make a list of all candidate tmp directories. -TMPDIRS= -for f in $CMAKECACHE_FILES; do - if [ -d $(dirname $f)/tmp ]; then - TMPDIRS+=" $(dirname $f)/tmp" - fi -done -# Put BASEDIR first in case there are no build directories because /tmp is probably on a different file system. -TMPDIRS+=" $BASEDIR /tmp ." -# Pick the first one that is actually writable. -for tmp in $TMPDIRS; do - TMPDIR="$tmp" - if [ -w "$TMPDIR" ]; then - break; - fi -done -test -n "$TMPDIR" || fatal "Can not find a writable tmp directory." -echo "TMPDIR=\"$TMPDIR\"" - -# Make a list of all source and header files that we need to fix. -# List of directories that we don't want to touch. -EXCLUDE_FOLDERS=".git Tools" -EXCLUDE_PATTERNS="examples matlab/scripts tests test unit_test *_test *_tests test_* apps/test_* UnitTests" -# A regular expression for the exclude patterns. -EXCLUDE_PATTERNS_RE="($(echo $EXCLUDE_PATTERNS | sed -e 's/\*/[^\/]*/g;s/ /|/g'))" -# Extensions of files that we do want to change (leaving out .y and .py for now). -C_EXTENSIONS=".c .c_in .c_shipped" -H_EXTENSIONS=".h .h.in .h_template" -CXX_EXTENSIONS=".cc .cpp .cpp.in .cxx .cpp_template" -HXX_EXTENSIONS=".hh .hpp .hxx" -# The regular expression that we consider to be an #include. -INCLUDE_RE='^[[:space:]]*#[[:space:]]*include[[:space:]]*[<"]' -# Regular expression for empty lines. -EMPTY_LINE_RE='^[[:space:]]*$' -# Regular expression for one-line comments. -COMMENT_LINE_RE='^[[:space:]]*(\/\/.*|\/\*([^*]|\*+[^\/*])*\*+\/[[:space:]]*)$' -# Regular expression for a #define (on one line). -DEFINE_RE='^[[:space:]]*#[[:space:]]*define[[:space:]].*[^\\]$' -# Regular expression for an #if[[n]def]. -IF_RE='^[[:space:]]*#[[:space:]]*if(n?def)?[[:space:]]' -# Regular expression for an #endif. -ENDIF_RE='^[[:space:]]*#[[:space:]]*endif($|[^[:alnum:]])' -# Regular expression for header file extension. -HEADER_RE="($(echo $H_EXTENSIONS $HXX_EXTENSIONS | sed -e 's/\./\\./g;s/ /|/g'))" -# Regular expression for C++ source and header files. -CXXSRC_RE="($(echo $CXX_EXTENSIONS $HXX_EXTENSIONS | sed -e 's/\./\\./g;s/ /|/g'))" -# List of standard C header files. Note that cfcntl, cshed and cunistd are NOT standard header files, even though they are in NuttX/nuttx/include/cxx. -REAL_STDC_HEADERS_RE='(cassert|ccomplex|cctype|cerrno|cfenv|cfloat|cinttypes|ciso646|climits|clocale|cmath|csetjmp|csignal|cstdalign|cstdarg|cstdbool|cstddef|cstdint|cstdio|cstdlib|cstring|ctgmath|ctime|cuchar|cwchar|cwctype)' -STDC_HEADERS=$(find "$BASEDIR/NuttX/nuttx/include/cxx" -mindepth 1 -maxdepth 1 -type f | xargs basename -a | grep -E "$REAL_STDC_HEADERS_RE" | xargs echo) -# Regular expression of standard C header files, but with the leading 'c' stripped. -STDC_HEADERS_RE="($(echo $STDC_HEADERS | sed -e 's/^c//;s/ c/|/g'))" -# Actual list of standard C header files. -# List of standard C++ header files. -REAL_STDCXX_HEADERS_RE='(algorithm|any|array|atomic|bitset|cassert|ccomplex|cctype|cerrno|cfenv|cfloat|chrono|cinttypes|ciso646|climits|clocale|cmath|codecvt|complex|condition_variable|csetjmp|csignal|cstdalign|cstdarg|cstdbool|cstddef|cstdint|cstdio|cstdlib|cstring|ctgmath|ctime|cuchar|cwchar|cwctype|deque|exception|execution|filesystem|forward_list|fstream|functional|future|initializer_list|iomanip|ios|iosfwd|iostream|istream|iterator|limits|list|locale|map|memory|memory_resource|mutex|new|numeric|optional|ostream|queue|random|ratio|regex|scoped_allocator|set|shared_mutex|sstream|stack|stdexcept|streambuf|string|string_view|strstream|system_error|thread|tuple|typeindex|typeinfo|type_traits|unordered_map|unordered_set|utility|valarray|variant|vector)' -STDCXX_HEADERS=$(find "$BASEDIR/NuttX/misc/uClibc++/include/uClibc++" -mindepth 1 -maxdepth 1 -type f | xargs basename -a | grep -E "$REAL_STDCXX_HEADERS_RE" | grep -E -v "$REAL_STDC_HEADERS_RE" | xargs echo) -# Regular expression of C++ header files. -STDCXX_HEADERS_RE="($(echo $STDCXX_HEADERS | sed -e 's/ /|/g'))" -# Regular expression for #pragma once. -PRAGMA_ONCE_RE='^#pragma once' -# Regular expression to recognize the start of a C-comment block. -COMMENT_BEGIN_RE='(^|[^\/])\/\*([^*]|\*+($|[^\/*]))*$' -# Regular expression to recognize the end of a C-comment block. -COMMENT_END_RE='\*\/' -# Regular expression to match C++ unsafe headers. We currently don't have any C++ unsafe headers, do we? -# v2.0/standard/mavlink.h is not unsafe, but this way the script will leave it alone and not -# move it above function declarations that need to be declared before including it. -UNSAFE_HEADERS_RE='(v2\.0\/standard\/mavlink\.h)' -#UNSAFE_HEADERS_RE='(stm32\.h|arch\/board\/board\.h)' - -# Find all submodules. -test -f $BASEDIR/.gitmodules || fatal "No such file: $BASEDIR/.gitmodules" -SUBMODULES=$(grep -A 1 '^\[submodule' $BASEDIR/.gitmodules | grep '^[[:space:]]*path = ' | sed -r -e 's/^[[:space:]]*path = //' | xargs echo) -echo "SUBMODULES=\"$SUBMODULES\"" -SUBMODULES_RE="($(echo $SUBMODULES | sed -e 's/ /|/g'))" - -# Disable path name expansion (otherwise the find patterns will be expanded against the files in the current working directory). -set -f - -EXCLUDE_ARGS= -for excl in $EXCLUDE_FOLDERS; do - if [ -z "$EXCLUDE_ARGS" ]; then - EXCLUDE_ARGS="-wholename $BASEDIR/$excl/*" - else - EXCLUDE_ARGS+=" -o -wholename $BASEDIR/$excl/*" - fi -done -for excl in $EXCLUDE_PATTERNS; do - EXCLUDE_ARGS+=" -o -wholename */$excl/*" -done -INCLUDE_H_ARGS= -for ext in $H_EXTENSIONS $HXX_EXTENSIONS; do - if [ -z "$INCLUDE_H_ARGS" ]; then - INCLUDE_H_ARGS="-name *$ext" - else - INCLUDE_H_ARGS+=" -o -name *$ext" - fi -done -INCLUDE_C_ARGS= -for ext in $C_EXTENSIONS $CXX_EXTENSIONS; do - if [ -z "$INCLUDE_C_ARGS" ]; then - INCLUDE_C_ARGS="-name *$ext" - else - INCLUDE_C_ARGS+=" -o -name *$ext" - fi -done -# Also exclude all submodules -- because we don't maintain those (are we?). -for subm in $SUBMODULES; do - if [ -z "$SUBMODULES_ARGS" ]; then - SUBMODULES_ARGS="-wholename $BASEDIR/$subm/*" - else - SUBMODULES_ARGS+=" -o -wholename $BASEDIR/$subm/*" - fi -done - -echo -n "Finding all source files with #include's (excluding submodules and build directory)... " -find $BASEDIR -mindepth 2 -type f ! \( -wholename $BASEDIR/build/* -o $EXCLUDE_ARGS -o $SUBMODULES_ARGS \) \( $INCLUDE_C_ARGS -o $INCLUDE_H_ARGS \) > $TMPDIR/fix_headers_sources -cat "$TMPDIR/fix_headers_sources" | xargs grep -l "$INCLUDE_RE" > $TMPDIR/fix_headers_sources_with_includes -echo "done" -number_of_files=$(sed -n '$=' "$TMPDIR/fix_headers_sources_with_includes") -count=0 - -echo -n "Finding all submodule header files (excluding stdc++ headers)... " -find $BASEDIR -type f ! \( $EXCLUDE_ARGS \) \( $SUBMODULES_ARGS \) \( $INCLUDE_H_ARGS \) > $TMPDIR/fix_headers_SUBMODULE_HEADERS -echo "done" - -echo -n "Finding all header files (excluding stdc++ headers)... " -find $BASEDIR -type f ! \( $EXCLUDE_ARGS \) -wholename $BASEDIR/build/* \( $INCLUDE_H_ARGS \) > $TMPDIR/fix_headers_HEADERS -grep -E "$HEADER_RE" $TMPDIR/fix_headers_sources >> $TMPDIR/fix_headers_HEADERS -cat $TMPDIR/fix_headers_SUBMODULE_HEADERS >> $TMPDIR/fix_headers_HEADERS -echo "done" - -echo -n "Finding all include paths... " -for f in `cat $TMPDIR/fix_headers_sources_with_includes`; do grep -E "$INCLUDE_RE" $f | sed -r -e "s%$INCLUDE_RE%%"';s/[">].*//'; done | sort -u | grep -E -v "(/|^)$EXCLUDE_PATTERNS_RE/" > $TMPDIR/fix_headers_include_paths -echo "done" - -function include_path() -{ - # If the include path starts with a '.', then it is a local header. - if [[ $1 =~ ^\. ]]; then return 1; fi - # If the include path starts with 'platforms/' then it is a local header; - # added this exception here because not everyone has all build_ directories for all targets installed. - if [[ $1 =~ platforms/ ]]; then return 1; fi - # apps.h is generated from apps.h.in. - if [ $1 = "apps.h" ]; then return 1; fi - # Treat the following headers from src/platforms/*/include as system header because they replace what is found in nuttx (for posix and qurt). - if [ $1 = "arch/board/board.h" -o $1 = "crc32.h" -o $1 = "i2c.h" -o $1 = "queue.h" -o $1 = "poll.h" -o $1 = "sys/ioctl.h" ]; then return 2; fi - # Escape the path for reg.exp. matching. - PATH_RE=$(echo $1 | sed -e 's/\([+.]\)/\\\1/') - issubmodule=0; - islocal=0; - foo=0 - for includedir in $(grep "/$PATH_RE\$" $TMPDIR/fix_headers_HEADERS | cut -c $striplen-); do - # If the include directory is NuttX header that was copied to the build directory, then it's still a system file. - if [[ $includedir/ =~ ^build/.*/NuttX/ ]]; then - issubmodule=1 - # If the include directory is a submodule, then treat it as a system file. - elif [[ $includedir/ =~ ^$SUBMODULES_RE/ ]]; then - issubmodule=1; - else - islocal=1 - fi - done - if [ $islocal -eq 0 ]; then - if [ $issubmodule -eq 0 ]; then - # If an include path can't be found then usually it will be a real system header, - # however, there are a few (ros related?) files that start with px4... In that - # case just leave the quotes alone ("px4muorb.h" and several ). - if [[ $1 =~ ^px4 ]]; then return 0; fi - # While if the include path starts with uORB/topics or topics, and it isn't found, - # then likely we just don't have a build directory. These should be local though. - # Same for the generated files mixer_multirotor.generated.h and build_git_version.h. - if [[ $1 =~ ((/|^)topics/|mixer_multirotor\.generated\.h|build_git_version\.h) ]]; then return 1; fi - fi - return 2; - fi # Submodule or system header. - if [ $issubmodule -eq 0 ]; then return 1; fi # Local. - # Files that are both local and submodule are simply left alone. - # These are (at this moment): "battery.h" "common.h" "Matrix.hpp" "mavlink.h" "protocol.h" "pwm.h" "spi.h" "Vector.hpp". - return 0; -} - -# Run the include_path function for each of the files in $TMPDIR/fix_headers_include_paths -echo -n "Determining which headers need to be included with double quotes... " -echo -n > $TMPDIR/fix_headers_quotes -for arg in $(cat "$TMPDIR/fix_headers_include_paths"); do - include_path $arg - localsystem=$? - if [ $localsystem -eq 1 ]; then - echo "$arg \"$arg\"" >> $TMPDIR/fix_headers_quotes - elif [ $localsystem -eq 2 ]; then - echo "$arg <$arg>" >> $TMPDIR/fix_headers_quotes - fi -done -echo "done" - -# Truncate the error log. -echo -n > $TMPDIR/fix_headers_ERROR.log - -function print_error -{ - echo - echo -n " "; - echo "*** $1" | tee -a "$TMPDIR/fix_headers_ERROR.log" - return 1 -} - -if [ $debug -ne 0 ]; then - # Debug Line. - DL='if (cdbl != NR) { printf "\n%u. \"%s\"", NR, $0; cdbl = NR }' - # Debug Begin. - DB='if (cdbl != NR) { printf "\n%u. \"%s\" ---> ", NR, $0; cdbl = NR } else printf "; "; printf' - # Debug End. - DE='' -else - DL='#' - DB='#' - DE='' -fi -# Error Prefix. -EP='###' - -# The main function that is called for each source file. -function fixup_header -{ - count=$((count + 1)) - echo -n "[$((100 * count / number_of_files))%] Fixing headers of $1... " - - # Is this a header? - echo "$1" | sed -e 's/\.in$/;in/;s/.*\././;s/;in$/.in/' | grep -v -E "$HEADER_RE\$" >/dev/null; is_header=$? - if [ $debug -ne 0 ]; then echo "is_header = \"$is_header\""; fi - - # Is this C++ source? - echo "$1" | sed -e 's/.*\././' | grep -v -E $CXXSRC_RE >/dev/null; is_cxxsrc=$? - if [ $debug -ne 0 ]; then echo "is_cxxsrc = \"$is_cxxsrc\""; fi - dont_make_cxxsrc=1 - if [ $is_cxxsrc -eq 0 -a $is_header -ne 0 ]; then - grep -m 1 -q -E "^[[:space:]]*(#[[:space:]]*include[[:space:]]*<$STDCXX_HEADERS_RE>|(template|namespace|class)(\$|[^[:alnum:]_]))" "$1" - dont_make_cxxsrc=$? - fi - if [ $dont_make_cxxsrc -eq 0 ]; then - is_cxxsrc=1 - fi - - # Current directory. - curdir=$(dirname "$BASEDIR/$1") - - # Parse the file. - # - # Returns an array of line[]'s. The first line is either the first #include, when it is outside - # any #if*...#endif constructs not counting the header guard if the file is a header. For example: - # - # // Anything here except #include lines. - # #include // <-- first line. - # - # Or, the first #if* that contains the first #include line. For example: - # // Anything here except #include lines. - # #ifndef FOO_H // header guard. - # #define FOO_H - # int global = 1; // Anything unknown. - # #ifdef SOMETHING // <-- first line. - # #if maybe_more - # // anything except #include lines. - # #else - # // anything except #include lines. - # #include - # - # Subsequent line[]'s mark the beginning of a new block, where we have the following blocks: - # type[] Description - type_include=0 # An #include, outside #if*...#endif constructs except a possible header guard. - type_ifincludeendif=1 # #if*...#endif constructs with #include's. - type_ifendif=2 # #if*...#endif constructs without #include's. - type_decls=3 # __BEGIN_DECLS ... __END_DECLS block. - type_macro=4 # Contiguous #define block. - type_comment=5 #(Multi-line) comments. - type_emptyline=6 # Empty lines. - type_pragmaonce=7 # #pragma once (must be outside any #if*...#endif constructs). - type_end=8 # The first line of the remainder of the file. - # - # However, any block NOT containing one or more #include's (all types > 1) will - # cause subsequent blocks that do not contain #include's to be ignored, - # with as result that those blocks will be treated as contiguous code blocks. - # A comment that is followed by a type that is not to be ignored as such - # is given the type that follows (which itself is then ignored). - # Empty lines are ignored unless they appear directly in front of a type - # that is not to be ignored according to the above rules, where 'previous - # types' then are the types before the empty line. - # - # For example: - # - # > #include # type_include - # > #include # type_include - # > # type_emptyline - # > #include # type_include - # ^ #ifdef FOO # type_ifendif - # | #define BAR 1 - # | #endif # (detected here) - # | <-- ignored because: - # v #define BAZ(x) x // Do baz <-- ignored (prev and this type in {type_ifendif, type_macro}) - # > # type_emptyline - # ^ // This include is important: # type_comment, but then replaced by type_ifincludeendif - # | <-- ignored because: - # | // more here. <-- ignored same type - # | #ifdef BAR <-- "ignored": type_ifincludeendif, but put 3 lines higher. - # | #include - # v #endif # (detected here) - # > # type_emptyline - # > #include # type_include - # - # This script stops parsing at the first not recognized line outside #if*...#endif constructs - # unless no first line was found yet. It does not attempt overly hard to decode rare constructs, - # most notably anything with a leading C comment is not recognized and will thus lead to an abort. - # For example the following lines are not recognized: - # - # /* Some comment */ #include - # /* Some comment */ // Another comment. - # - # Lines that have a trailing comment are recognized (by ignoring the comment). - - result=$(awk "\ - function add(l, t) { - # First add always succeeds. - if (ptr > 0) { - # An empty line is always added, at first, unless the immediate preceding type is an empty line. - if (t == $type_emptyline && type[ptr - 1] == $type_emptyline) { - return; $DB \"ignored because line %d is also empty.\", line[ptr - 1] $DE - } - # A comment is always, added at first, unless the preceding non-empty line type is a comment. - # Same for #include's. - if (t == $type_comment && last_none_type == t) { - $DB \"ignoring because same type as last_none_type (%s)\", type_name[t] $DE - # Gobble up preceding empty lines. - if (type[ptr - 1] == $type_emptyline) { - --ptr; $DB \"ptr = %d; Removed type_emptyline @ line %d\", ptr, line[ptr] $DE - } - return; - - } - # {ifendif, macro}'s are collapsed too. - if ((t == $type_ifendif || t == $type_macro) && (last_nonws_type == $type_ifendif || last_nonws_type == $type_macro)) { - # Gobble up preceding comments and empty lines. - while (ptr > 0 && (type[ptr - 1] == $type_emptyline || type[ptr - 1] == $type_comment)) { - --ptr; $DB \"ptr = %d; Removed %s @ line %d\", ptr, type_name[type[ptr]], line[ptr] $DE - } - # ptr > 0 here because the first add is never for an empty line or comment. - last_none_type = type[ptr - 1]; $DB \"last_none_type = %s\", type_name[last_none_type] $DE - return; - } - # type_include and type_pragmaonce and higher are always added. - } - if (t == $type_end) { - # Remove drag. - while(ptr > 0 && line[ptr - 1] >= l) --ptr; - } - # If this type is not an empty line and it was preceded by a comment, then melt it together with that comment. - else if (t != $type_emptyline && last_none_type == $type_comment) { - # In this case t cannot be type_comment. - # Gobble up preceding empty lines. - if (type[ptr - 1] == $type_emptyline) { - --ptr; $DB \"ptr = %d; Removed type_emptyline @ line %d\", ptr, line[ptr] $DE - } - # And replace the comment type. - --ptr; $DB \"ptr = %d; replacing the %s @ line %d\", ptr, type_name[type[ptr]], line[ptr] $DE - l = line[ptr]; - - } - line[ptr] = l; $DB \"ptr = %d; %s @ line %d\", ptr, type_name[t], l $DE; - type[ptr++] = t; - if (t != $type_emptyline) { - last_none_type = t; $DB \"last_none_type = %s\", type_name[last_none_type] $DE - if (t != $type_comment) - last_nonws_type = t; - } - } - - BEGIN { - debug = $debug; # 0: no debug output; non-zero: print debug output. - header = $is_header; # 0: do not look for a header guard; 1: treat first #ifndef as header guard. - in_if = 0; # The number of nested levels inside #if, #ifdef or #ifndef ... #endif constructs. - in_if_base = 0; # 0: no header guard was found (or #pragma once); 1: an #ifndef header guard was found. - in_decl = 0; # 0: not inside a __BEGIN_DECLS ... __END_DECLS block; 1: inside such a block. - found_guard = 0; # 0: no header guard was found; 1: a header guard was found (including #pragma once). - base_if = 0; # The current base-level #if that we are scanning. - drag = 0; # The number of lines since the last certainly relevant line (a base-level #include or a base-level #endif containing one of more #includes). - skipped = 0; # 0: No #include was encountered in the current (base-level) #if block; 1: one or more #include's encountered in the current base-level #if block. - in_comment = 0; # 0: not in a multi-line C comment; 1: in a multi-line C comment. - cdbl = 0; # Current debug line. - error = 0; # 0: no error occured; 1: an error occured. - ptr = 0; # Current pointer into line[] and type[]. - found_comment_end = 0; # The last line (possibly the current line) that is/was a multi-line C comment termination. - last_none_type = -1; # The last non-emptyline type that was added. - last_nonws_type = -1; # The last non-whitespace type that was added. - # For debug purposes: - type_name[$type_ifendif] = \"type_ifendif\"; - type_name[$type_ifincludeendif] = \"type_ifincludeendif\"; - type_name[$type_decls] = \"type_decls\"; - type_name[$type_comment] = \"type_comment\"; - type_name[$type_emptyline] = \"type_emptyline\"; - type_name[$type_include] = \"type_include\"; - type_name[$type_macro] = \"type_macro\"; - type_name[$type_pragmaonce] = \"type_pragmaonce\"; - type_name[$type_end] = \"type_end\"; - } - - END { - last_line = NR - drag; - add(last_line + 1, $type_end); $DB \"\n\" $DE; - # Print output. - if (error || ptr == 0 || last_line < line[0]) { - print \"error=1\"; - exit - } - printf \"lines=\\\"\"; - for (i = 0; i < ptr - 1; ++i) - printf \"%d \", line[i]; - printf \"%d\\\"; \", line[ptr - 1]; - printf \"types=\\\"\"; - for (i = 0; i < ptr - 1; ++i) - printf \"%d \", type[i]; - printf \"%d\\\"; \", type[ptr - 1]; - print \"error=0; first_line=\" line[0] \"; last_line=\" last_line - } - - #====================================================================================================== - # Handle multi-line C comments. - - /$COMMENT_END_RE/ { - if (in_comment) { - in_comment = 0; $DB \"comment end\" $DE; - found_comment_end = NR; - sub(/^([^*]|\*+[^*\/])*\*+\//, \"\") # Remove the tail of the comment. - } - # FALL-THROUGH - } - - { - if (in_comment) { - ++drag; $DB \"in comment; drag = %d\", drag $DE; - next - } - found_comment_begin = 0; - # FALL-THROUGH - } - - /$COMMENT_BEGIN_RE/ { - in_comment = 1; $DB \"comment begin\" $DE; - found_comment_begin = 1; - sub(/\/\*([^*]|\*+($|[^*\/]))*$/, \"\") # Remove the head of the comment so that we'll recognize this as an empty line if it is. - # FALL-THROUGH - } - - #====================================================================================================== - # Detect and handle header guard. - - /$PRAGMA_ONCE_RE/ { - ++drag; $DB \"drag = %d\", drag $DE; - if (header && found_guard == 0 && in_if == 0) { - found_guard = NR; $DB \"found_guard = %d\", found_guard $DE; - if (ptr > 0) - add(NR, $type_pragmaonce) - next - } - print \"\\n$EP $1:\" NR \": unexpected #pragma once\"; - error = 1; - exit - } - - /^#ifndef / { - if (ptr == 0 && header && found_guard == 0 && in_if == 0) { - found_guard = NR; $DB \"found_guard = %d\", found_guard $DE; - in_if = 1; $DB \"in_if = %d\", in_if $DE; - in_if_base = 1; $DB \"in_if_base = %d\", in_if_base $DE; - next - } - # FALL-THROUGH - } - - #====================================================================================================== - # Detect and handle __BEGIN_DECLS ... __END_DECLS blocks. - - /^[[:space:]]*__BEGIN_DECLS/ { - ++drag; $DB \"drag = %d\", drag $DE; - if (in_decl == 0) { - in_decl = 1; $DB \"in_decl = 1\" $DE - add(NR, $type_decls); - next - } - print \"\\n$EP $1:\" NR \": Nested __BEGIN_DECLS!\"; - error = 1; - exit - } - - /^[[:space:]]*__END_DECLS/ { - ++drag; $DB \"drag = %d\", drag $DE; - if (in_decl == 1) { - in_decl = 0; $DB \"in_decl = 0\" $DE - if (skipped) { - drag = 0; - } else if (ptr == 1) { - ptr = 0; $DB \"erase DECLS block\" $DE - last_none_type = -1; - } - next - } - print \"\\n$EP $1:\" NR \": __END_DECLS without matching __BEGIN_DECLS!\"; - error = 1; - exit - } - - #====================================================================================================== - # Detect and handle #if ... #endif blocks. - - /$IF_RE/ { - ++drag; $DB \"drag = %d\", drag $DE; - if (in_if == in_if_base && in_decl == 0) { - skipped = 0; $DB \"skipped = 0\" $DE; - base_if = NR; $DB \"base_if = %d\", NR $DE; - } - ++in_if; $DB \"in_if = %d\", in_if $DE; - next - } - - /$ENDIF_RE/ { - --in_if; $DB \"in_if = %d\", in_if $DE; - if (in_if < 0) { - print \"\\n$EP $1:\" NR \": #endif without matching #if!\"; - error = 1; - exit - } - ++drag; - if (in_if == in_if_base && in_decl == 0) { - if (skipped) { - drag = 0; - add(base_if, $type_ifincludeendif); - } else if (ptr > 0) - add(base_if, $type_ifendif); - } $DB \"drag = %d\", drag $DE; - # Left header guard? - if (in_if < in_if_base) { $DB \"left header guard:\" $DE; - # assert(in_if == 0 && in_if_base == 1 && header && found_guard) - exit - } - next - } - - #====================================================================================================== - # Handle #include lines. - - /$INCLUDE_RE/ { - if (!/\"(\.\/)?mavlink_msg/) { - # If we're inside a __BEGIN_DECLS ... __END_DECLS block then only certain headers may be included. - hname = gensub(/^[[:space:]]*#[[:space:]]*include[[:space:]]*[<\"]([^>\"]*)[>\"].*/, \"\\\1\", \"1\"); - cpp_safe = !(hname ~ /$UNSAFE_HEADERS_RE/); - $DB \"hname = \\\"\" hname \"\\\"; cpp_safe = \" cpp_safe \"; in_decl = \" in_decl \"; is_cxxsrc = $is_cxxsrc\" $DE - if (in_decl && cpp_safe) { - print \"\\n$EP $1:\" NR \": including \" hname \" inside a __BEGIN_DECLS ... __END_DECLS block.\"; - error = 1; - exit - } else if (!in_decl && !cpp_safe && $is_cxxsrc) { - print \"\\n$EP $1:\" NR \": including \" hname \" outside a __BEGIN_DECLS ... __END_DECLS block!\"; - error = 1; - exit - } - if (in_if > in_if_base || in_decl) { - skipped = 1; $DB \"skipped = 1\" $DE; - } else { - drag = 0; $DB \"drag = 0\" $DE; - add(NR, $type_include); $DB \"first_line = %d\", NR $DE; - } - next - } - } - - #====================================================================================================== - # Ignore #define's, empty lines and lines with just comments. - - /$DEFINE_RE/ { - ++drag; $DB \"drag = %d\", drag $DE; - if (ptr > 0 && in_if == in_if_base && in_decl == 0) { - add(NR, $type_macro); - } - next - } - - /$EMPTY_LINE_RE/ { - ++drag; $DB \"drag = %d\", drag $DE; - if (ptr > 0 && in_if == in_if_base && in_decl == 0) { - if (found_comment_begin) - add(NR, $type_comment); - else if (found_comment_end != NR) - add(NR, $type_emptyline); - } - next - } - - /$COMMENT_LINE_RE/ { - ++drag; $DB \"drag = %d\", drag $DE; - if (ptr > 0 && in_if == in_if_base && in_decl == 0 && type[ptr - 1] != $type_comment) { - add(NR, $type_comment); - } - next - } - - #====================================================================================================== - # Handle everything else (unrecognized lines). - - { - ++drag; $DB \"unknown; drag = %d\", drag $DE; - if (ptr > 0 && in_if <= in_if_base && in_decl == 0) { - exit - } - } - - " "$BASEDIR/$1") - - # Decode the result. - vars=$(echo "$result" | tail -n 1) - error_msg=$(echo "$result" | grep "^$EP " | sed -e 's/^....//') - if [ $debug -ne 0 ]; then - len=$(echo "$result" | wc --lines) - echo "$result" | head -n $((len - 1)) | grep -v "^$EP " # Debug messages - echo "vars: $vars" - fi - # Evaluate the last line printed in END. - error=1; eval $vars - test -z "$error_msg" || print_error "$error_msg" || return - test $error -eq 0 -a $first_line -gt 0 || print_error "FAILED to find an #include in $1?!" || return - test $last_line -ge $first_line || print_error "FAILED to find a sensible last line in $1?!" || return - - # Calculate the number of lines starting from the current line. - # Use sed to count lines, because wc --lines doesn't report the last line when that doesn't end on a new-line, contrary to the fact that tail treats such lines as lines. - total_lines=$(sed -n '$=' "$BASEDIR/$1") - if [ $debug -ne 0 ]; then echo "total_lines = \"$total_lines\""; fi - - # Edit the first_line...last_line block. - # Header files are ordered as follows (lowest value first): - cat_winsock=0; # Winsock2.h - cat_posix_sys=1; # posix_sys.h or one of the px4_ headers that include it. - cat_px4=2; # Other px4_*.h - cat_local=3; # "*.h" - cat_cxx=4; # , ie - cat_c=5; # , ie - cat_system=6; # <*.h> - - head -n $last_line "$BASEDIR/$1" | tail -n $((last_line - first_line + 1)) | awk " - - function sort_by_type_line_header_type_hname(blk2, v2, blk1, v1) { - # Return true if blk1 comes before blk2. - # Move type_include before the rest. Keep the same line order for the rest. - return (type[blk2] != $type_include && (type[blk1] == $type_include || line[blk1] < line[blk2])) || - (type[blk2] == $type_include && type[blk1] == $type_include && - # If both are include's then put include with a lower header_type first; sort alphabetically for the same header type. - (header_type[blk1] < header_type[blk2] || (header_type[blk1] == header_type[blk2] && hname[blk1] < hname[blk2]))); - } - - BEGIN { - first_line = $first_line; - split(\"$lines\", line); - split(\"$types\", type); - i = 0; - do { - line[++i] -= first_line - 1; - } while(type[i] != $type_end) - for(b = 0; b < i; ++b) header_type[b] = 100; - blk = 1; - n = 0; - is_cxxsrc = $is_cxxsrc; - # px4_posix.h includes px4_defines.h includes px4_log.h includes posix_sys.h which must be the first header included. - sys_val[\"px4_posix.h\"] = 1; - sys_val[\"px4_defines.h\"] = 2; - sys_val[\"px4_log.h\"] = 3; - sys_val[\"posix_sys.h\"] = 4; - saw_sys_val = 5; # Didn't see any of the above; otherwise the lowest value of the header seen. - for(b = 0; b < i; ++b) saw_sys[b] = saw_sys_val; - } - - END { - l = asorti(txt, k, \"sort_by_type_line_header_type_hname\"); - for (b = 1; b <= l; ++b) { - if (type[k[b]] == $type_include && header_type[k[b]] == $cat_posix_sys && saw_sys[k[b]] > saw_sys_val) continue; - len = length(txt[k[b]]); - for (n = 0; n < len; ++n) print txt[k[b]][n]; - if (b < l && type[k[b]] == $type_include && type[k[b+1]] != $type_emptyline && - (type[k[b+1]] != $type_include || (header_type[k[b]] != header_type[k[b+1]] && header_type[k[b+1]] != $cat_px4))) { - printf \"\n\"; - } - } - } - - { - if (NR == line[blk + 1]) { - ++blk; - n = 0; - } - } - - /$INCLUDE_RE/ { - # Don't use double quotes around standard header names. - \$0 = gensub(/^([[:space:]]*#[[:space:]]*include[[:space:]]*)\\\"$STDC_HEADERS_RE\\.h\\\"/, \"\\\1<\\\2.h>\", \"1\"); - if (is_cxxsrc) { - \$0 = gensub(/^([[:space:]]*#[[:space:]]*include[[:space:]]*)\\\"$STDCXX_HEADERS_RE\\\"/, \"\\\1<\\\2>\", \"1\"); - # Change deprecated C header names to standard C++ header names in C++ source files. - \$0 = gensub(/^([[:space:]]*#[[:space:]]*include[[:space:]]*<)$STDC_HEADERS_RE\\.h>/, \"\\\1c\\\2>\", \"1\"); - } - # Don't include \"./foo.h\", that is implied, so just include \"foo.h\". - \$0 = gensub(/^([[:space:]]*#[[:space:]]*include[[:space:]]*\\\")\.\//, \"\\\1\", \"1\"); - # Extract the header filename. - hname[blk] = gensub(/^[[:space:]]*#[[:space:]]*include[[:space:]]*[<\"]([^>\"]*)[>\"].*/, \"\\\1\", \"1\"); - # If the header exists in the directory of the including file, then it is a local header. - command = sprintf(\"test -e %s/%s\", \"$curdir\", hname[blk]); - if (system(command) == 0) { - \$0 = gensub(/^([[:space:]]*#[[:space:]]*include[[:space:]]*)[\"<]([^\">]*)[\">]/, \"\\\1\\\"\\\2\\\"\", \"1\"); - } else { - # Do we know if this is a local file, or a submodule / system header? - # The grep reg.exp needs \\ (for backslah) and \1 for back reference, thus: \\\1. - # However we print the grep command using sprintf, so each backslash needs to be escaped once more: \\\\\\1. - # Finally, this is a bash string and we need to escape each backslash once more to pass it corrently to awk, hence we need twelve backslahes: - command = sprintf(\"grep '^%s ' '%s' 2>/dev/null\", gensub(/([.+])/, \"\\\\\\\\\\\\1\", \"g\", hname[blk]), \"$TMPDIR/fix_headers_quotes\"); - ret = command | getline result; - if (ret != 0) { - result = substr(result, index(result, \" \") + 1); - \$0 = gensub(/^([[:space:]]*#[[:space:]]*include[[:space:]]*)[\"<][^\">]*[\">]/, \"\\\1\" result, \"1\"); - } - } - # Categorise the header. - if (hname[blk] == \"Winsock2.h\") { - if (header_type[blk] > $cat_winsock) - header_type[blk] = $cat_winsock; - } - else if (hname[blk] in sys_val) { - if (header_type[blk] > $cat_posix_sys) - header_type[blk] = $cat_posix_sys; - # posix_sys.h is sometimes included within #ifdef __PX4_POSIX ... #endif. The other headers should not be conditional. - if ((hname[blk] == \"posix_sys.h\" || type[blk] == $type_include)) { - type[blk] = $type_include; # Treat #ifdef __PX4_POSIX #include \"posix_sys.h\" #endif as an include for sorting purposes. - saw_sys[blk] = sys_val[hname[blk]]; # There will be only one include (header name) for this block. - if (sys_val[hname[blk]] < saw_sys_val) - saw_sys_val = sys_val[hname[blk]]; - } - # Use double quotes for these headers. - \$0 = gensub(/<([[:alnum:]_\/.]*)>/, \"\\\"\\\1\\\"\", \"1\"); - } - else if (hname[blk] ~ /^(platforms\/px4_|px4_)/) { - if (header_type[blk] > $cat_px4) - header_type[blk] = $cat_px4; - # Use double quotes for these headers. - \$0 = gensub(/<([[:alnum:]_\/.]*)>/, \"\\\"\\\1\\\"\", \"1\"); - } - else if (\$0 ~ /^[[:space:]]*#[[:space:]]*include[[:space:]]*\"/) { - if (header_type[blk] > $cat_local) - header_type[blk] = $cat_local; - } - else if (hname[blk] ~ /^$STDCXX_HEADERS_RE\$/) { - if (header_type[blk] > $cat_cxx) - header_type[blk] = $cat_cxx; - } - else if (hname[blk] ~ /^c$STDC_HEADERS_RE\$/) { - if (header_type[blk] > $cat_c) - header_type[blk] = $cat_c; - } - else if (hname[blk] ~ /^$STDC_HEADERS_RE\.h\$/) { - if (header_type[blk] > $cat_system) - header_type[blk] = $cat_system; - } - } - - { - # Remove empty lines before #include's. - if (type[blk] == $type_include) { - for (i = 1; blk > i && type[blk - i] == $type_emptyline; ++i) - delete txt[blk - i] - } - txt[blk][n++] = \$0; - } - - " > $TMPDIR/fix_headers_current_block - - # Construct a new file in TMPDIR. - head -n $((first_line - 1)) "$BASEDIR/$1" > $TMPDIR/fix_headers_current_file - - # Append the editted block. - cat $TMPDIR/fix_headers_current_block >> $TMPDIR/fix_headers_current_file - - # Append the rest. - tail -n $((total_lines - last_line)) "$BASEDIR/$1" >> $TMPDIR/fix_headers_current_file - - # Compare original with result. - if cmp --quiet "$BASEDIR/$1" $TMPDIR/fix_headers_current_file; then - echo "No change" - else - echo "Fixed lines $first_line-$last_line" - mv $TMPDIR/fix_headers_current_file "$BASEDIR/$1" || fatal "Failed to move $TMPDIR/fix_headers_current_file to $BASEDIR/$1 !?!" - fi -} - -if [ $non_option_arguments -ne 0 ]; then - fixup_header $1 - exit -fi - -# Run the fixup function for each of the files in $TMPDIR/fix_headers_sources_with_includes. -# Strip BASEDIR because we don't know how long that is and it might too much for bash. -for arg in $(cat "$TMPDIR/fix_headers_sources_with_includes" | cut -c $striplen-); do - fixup_header $arg -done - -# Clean up. -if [ $debug -eq 0 -o $# -eq 0 ]; then - rm "$TMPDIR/fix_headers_sources" "$TMPDIR/fix_headers_sources_with_includes" "$TMPDIR/fix_headers_SUBMODULE_HEADERS" "$TMPDIR/fix_headers_HEADERS" \ - "$TMPDIR/fix_headers_include_paths" "$TMPDIR/fix_headers_quotes" "$TMPDIR/fix_headers_current_block" -fi - -# Print all error messages again at the end. -if [ -s "$TMPDIR/fix_headers_ERROR.log" ]; then - echo "$0 finished with errors:" - cat "$TMPDIR/fix_headers_ERROR.log" -else - echo "SUCCESS" - rm "$TMPDIR/fix_headers_ERROR.log" -fi diff --git a/Tools/geotag_images_ulog.py b/Tools/geotag_images_ulog.py old mode 100644 new mode 100755 diff --git a/Tools/gz_sim.sh b/Tools/gz_sim.sh deleted file mode 100755 index d9cc7a32c9..0000000000 --- a/Tools/gz_sim.sh +++ /dev/null @@ -1,200 +0,0 @@ -#!/bin/bash -#Author: Benjamin Perseghetti -#Email: bperseghetti@rudislabs.com -# This script unifies running gazebo simulation for HITL and SITL -# You can run multiple instances of the 'px4' binary, with the gazebo SITL simulation -# This script assumes px4 is already built, with 'make px4_sitl_default gazebo' -# You can also run HITL with -h flag -# Generate world and/or model files with editable json -j [m (model), w (world), mw (model and world), or wm (world and model)] - -# The simulator in SITL is expected to send to TCP port 4560+i for i in [0, N-1] -# For example gazebo can be run like this for multiple SITL: -# ./Tools/gz_sim.sh -n 10 -m iris -# Or gazebo can be run like this for HITL: -# ./Tools/gz_sim.sh -h 1 -m standard_vtol - -function cleanup() { - echo "running the cleanup" - pkill -x px4 - pkill gazebo - pkill gzclient - pkill gzserver -} - -trap "cleanup" INT SIGINT SIGTERM EXIT - -function spawn_model() { - MODEL=$1 - N=$2 #Instance Number - WORLD_FILE=$3 - MJ=$4 - SITL_MODEL_NAME="${MODEL}_${N}" - sitl_path=${SCRIPT_DIR}/sitl_gazebo - jinja_model_script=${sitl_path}/scripts/jinja_model_gen.py - base_model="--base_model ${MODEL}" - model_json="--json_gen ${MJ}" - - if [ $hitl == true ]; then - python3 ${src_path}/Tools/boot_now.py "/dev/ttyACM0" - hil_mode="--hil_mode 1" - model_name="--model_name ${MODEL}" - hitl_launch_command="${model_json} ${base_model} ${hil_mode} ${model_name}" - echo "Generating: ${jinja_model_script} ${hitl_launch_command}" - python3 ${jinja_model_script} ${hitl_launch_command} - sleep 1 - source ${src_path}/Tools/setup_gazebo.bash ${src_path} ${src_path}/build/${target} - sleep 2 - gazebo ${sitl_path}/worlds/temp_${WORLD_FILE}.world --verbose - - else - mavlink_tcp="--mavlink_tcp_port $((4560+${N}))" - mavlink_udp="--mavlink_udp_port $((14560+${N}))" - model_name="--model_name ${SITL_MODEL_NAME}" - output_path="--output_path /tmp" - working_dir="$build_path/instance_$n" - sitl_launch_command="${model_json} ${base_model} ${mavlink_tcp} ${mavlink_udp} ${model_name} ${output_path}" - [ ! -d "$working_dir" ] && mkdir -p "$working_dir" - pushd "$working_dir" &>/dev/null - echo "starting instance $N in $(pwd)" - ../bin/px4 -i $N -d "$build_path/etc" -w sitl_${SITL_MODEL_NAME} -s etc/init.d-posix/rcS >out.log 2>err.log & - python3 ${jinja_model_script} ${sitl_launch_command} - echo "Generating: ${jinja_model_script} ${sitl_launch_command}" - echo "Spawning ${SITL_MODEL_NAME}" - gz model --spawn-file=/tmp/${SITL_MODEL_NAME}.sdf --model-name=${SITL_MODEL_NAME} -x 0.0 -y $((3*${N})) -z 0.2 - popd &>/dev/null - fi - -} - -if [ "$1" == "--help" ]; then - echo "Usage: $0 [-n ] [-m ] [-h ] [-w ] [-s