#!/bin/bash # home location lat, lon, alt, heading LOCATION="CMAC" TRACKER_LOCATION="CMAC_PILOTSBOX" VEHICLE="" BUILD_TARGET="sitl" FRAME="" NUM_PROCS=1 SPEEDUP="1" # check the instance number to allow for multiple copies of the sim running at once INSTANCE=0 USE_VALGRIND=0 USE_GDB=0 USE_GDB_STOPPED=0 USE_MAVLINK_GIMBAL=0 CLEAN_BUILD=0 START_ANTENNA_TRACKER=0 WIPE_EEPROM=0 REVERSE_THROTTLE=0 NO_REBUILD=0 START_HIL=0 TRACKER_ARGS="" EXTERNAL_SIM=0 usage() { cat <&2 usage exit 1 esac done shift $((OPTIND-1)) # kill existing copy if this is the '0' instance only kill_tasks() { [ "$INSTANCE" -eq "0" ] && { killall -q JSBSim lt-JSBSim ArduPlane.elf ArduCopter.elf APMrover2.elf AntennaTracker.elf pkill -f runsim.py pkill -f sim_tracker.py pkill -f sim_wrapper.py } } if [ $START_HIL == 0 ]; then kill_tasks fi trap kill_tasks SIGINT # setup ports for this instance MAVLINK_PORT="tcp:127.0.0.1:"$((5760+10*$INSTANCE)) SIMIN_PORT="127.0.0.1:"$((5502+10*$INSTANCE)) SIMOUT_PORT="127.0.0.1:"$((5501+10*$INSTANCE)) FG_PORT="127.0.0.1:"$((5503+10*$INSTANCE)) set -x [ -z "$VEHICLE" ] && { VEHICLE=$(basename $PWD) } [ -z "$FRAME" -a "$VEHICLE" = "APMrover2" ] && { FRAME="rover" } EXTRA_PARM="" EXTRA_SIM="" [ "$SPEEDUP" != "1" ] && { EXTRA_SIM="$EXTRA_SIM --speedup=$SPEEDUP" } # modify build target based on copter frame type case $FRAME in +|quad) BUILD_TARGET="sitl" EXTRA_SIM="$EXTRA_SIM --frame=quad" ;; X) BUILD_TARGET="sitl" EXTRA_PARM="param set FRAME 1;" EXTRA_SIM="$EXTRA_SIM --frame=X" ;; octa) BUILD_TARGET="sitl-octa" EXTRA_SIM="$EXTRA_SIM --frame=octa" ;; octa-quad) BUILD_TARGET="sitl-octa-quad" EXTRA_SIM="$EXTRA_SIM --frame=octa-quad" ;; heli) BUILD_TARGET="sitl-heli" EXTRA_SIM="$EXTRA_SIM --frame=heli" ;; IrisRos) BUILD_TARGET="sitl" EXTRA_SIM="$EXTRA_SIM --frame=IrisRos" ;; CRRCSim-heli) BUILD_TARGET="sitl-heli" EXTRA_SIM="$EXTRA_SIM --frame=CRRCSim-heli" ;; CRRCSim) BUILD_TARGET="sitl" EXTRA_SIM="$EXTRA_SIM --frame=CRRCSim" ;; elevon*) EXTRA_PARM="param set ELEVON_OUTPUT 4;" EXTRA_SIM="$EXTRA_SIM --elevon" ;; vtail) EXTRA_PARM="param set VTAIL_OUTPUT 4;" EXTRA_SIM="$EXTRA_SIM --vtail" ;; rover|rover-skid) EXTRA_SIM="$EXTRA_SIM --frame=$FRAME" ;; obc) BUILD_TARGET="sitl-obc" ;; "") ;; *) echo "Unknown frame type $FRAME" usage exit 1 ;; esac if [ $USE_MAVLINK_GIMBAL == 1 ]; then echo "Using MAVLink gimbal" EXTRA_SIM="$EXTRA_SIM --gimbal" fi autotest=$(dirname $(readlink -e $0)) if [ $NO_REBUILD == 0 ]; then pushd $autotest/../../$VEHICLE || { echo "Failed to change to vehicle directory for $VEHICLE" usage exit 1 } if [ ! -f $autotest/../../config.mk ]; then echo Generating a default configuration make configure fi if [ $CLEAN_BUILD == 1 ]; then make clean fi make $BUILD_TARGET -j$NUM_PROCS || { make clean make $BUILD_TARGET -j$NUM_PROCS } popd fi # get the location information SIMHOME=$(cat $autotest/locations.txt | grep -i "^$LOCATION=" | cut -d= -f2) [ -z "$SIMHOME" ] && { echo "Unknown location $LOCATION" usage exit 1 } echo "Starting up at $LOCATION : $SIMHOME" TRACKER_HOME=$(cat $autotest/locations.txt | grep -i "^$TRACKER_LOCATION=" | cut -d= -f2) [ -z "$TRACKER_HOME" ] && { echo "Unknown tracker location $TRACKER_LOCATION" usage exit 1 } if [ $START_ANTENNA_TRACKER == 1 ]; then pushd $autotest/../../AntennaTracker if [ $CLEAN_BUILD == 1 ]; then make clean fi make sitl -j$NUM_PROCS || { make clean make sitl -j$NUM_PROCS } TRACKER_INSTANCE=1 TRACKIN_PORT="127.0.0.1:"$((5502+10*$TRACKER_INSTANCE)) TRACKOUT_PORT="127.0.0.1:"$((5501+10*$TRACKER_INSTANCE)) TRACKER_UARTA="tcp:127.0.0.1:"$((5760+10*$TRACKER_INSTANCE)) cmd="nice /tmp/AntennaTracker.build/AntennaTracker.elf -I1" $autotest/run_in_terminal_window.sh "AntennaTracker" $cmd || exit 1 $autotest/run_in_terminal_window.sh "pysim(Tracker)" nice $autotest/pysim/sim_tracker.py --home=$TRACKER_HOME --simin=$TRACKIN_PORT --simout=$TRACKOUT_PORT $TRACKER_ARGS || exit 1 popd fi cmd="/tmp/$VEHICLE.build/$VEHICLE.elf -S -I$INSTANCE" if [ $WIPE_EEPROM == 1 ]; then cmd="$cmd -w" fi case $VEHICLE in ArduPlane) [ "$REVERSE_THROTTLE" == 1 ] && { EXTRA_SIM="$EXTRA_SIM --revthr" } jsbsim_version=$(JSBSim --version) if [[ $jsbsim_version != *"ArduPilot"* ]] then cat <> $tfile } $autotest/run_in_terminal_window.sh "ardupilot (gdb)" gdb -x $tfile --args $cmd || exit 1 else $autotest/run_in_terminal_window.sh "ardupilot" $cmd || exit 1 fi fi trap kill_tasks SIGINT sleep 2 rm -f $tfile if [ $EXTERNAL_SIM == 0 ]; then $autotest/run_in_terminal_window.sh "Simulator" $RUNSIM || { echo "Failed to start simulator: $RUNSIM" exit 1 } sleep 2 else echo "Using external ROS simulator" RUNSIM="$autotest/ROS/runsim.py --simin=$SIMIN_PORT --simout=$SIMOUT_PORT --fgout=$FG_PORT $EXTRA_SIM" $autotest/run_in_terminal_window.sh "ROS Simulator" $RUNSIM || { echo "Failed to start simulator: $RUNSIM" exit 1 } sleep 2 fi # mavproxy.py --master tcp:127.0.0.1:5760 --sitl 127.0.0.1:5501 --out 127.0.0.1:14550 --out 127.0.0.1:14551 options="" if [ $START_HIL == 0 ]; then options="--master $MAVLINK_PORT --sitl $SIMOUT_PORT" fi # If running inside of a vagrant guest, then we probably want to forward our mavlink out to the containing host OS if [ $USER == "vagrant" ]; then options="$options --out 10.0.2.2:14550" fi options="$options --out 127.0.0.1:14550 --out 127.0.0.1:14551" extra_cmd1="" if [ $WIPE_EEPROM == 1 ]; then extra_cmd="param forceload $autotest/$PARMS; $EXTRA_PARM; param fetch" fi if [ $START_ANTENNA_TRACKER == 1 ]; then options="$options --load-module=tracker" extra_cmd="$extra_cmd module load map; tracker set port $TRACKER_UARTA; tracker start;" fi if [ $START_HIL == 1 ]; then options="$options --load-module=HIL" fi if [ $USE_MAVLINK_GIMBAL == 1 ]; then options="$options --load-module=gimbal" fi mavproxy.py $options --cmd="$extra_cmd" $* if [ $START_HIL == 0 ]; then kill_tasks fi