You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
110 lines
3.0 KiB
110 lines
3.0 KiB
#!/bin/bash |
|
|
|
# Usage - From ardupilot root directory, run - libraries/SITL/examples/follow-copter.sh $GCS_IP |
|
# $GCS_IP is the IP address of the system running the GCs, by default is 127.0.0.1 |
|
# Use "follow-mavproxy.sh" to run MAVProxy with all vehicles |
|
# Or connect your GCS using multicast UDP |
|
# If you can't use multicast, you can connect via UDP on vehicle 1, which will relay telemetry |
|
# from the other vehicles |
|
|
|
# Kill all SITL binaries when exiting |
|
trap "killall -9 arducopter" SIGINT SIGTERM EXIT |
|
|
|
# Get the ArduPilot directory (ROOTDIR) |
|
SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" |
|
ROOTDIR="$(dirname "$(dirname "$(dirname $SCRIPT_DIR)")")" |
|
COPTER=$ROOTDIR/build/sitl/bin/arducopter |
|
|
|
# Drones will be located here |
|
HOMELAT=-35.280252 |
|
HOMELONG=149.005821 |
|
HOMEALT=597.3 |
|
|
|
# Set GCS_IP address |
|
if [ -z $1 ]; then |
|
GCS_IP="127.0.0.1" |
|
else |
|
GCS_IP=$1 |
|
fi |
|
|
|
# Check if SITL copter has been built |
|
if [ -f "$COPTER" ] |
|
then |
|
echo "Found SITL executable" |
|
else |
|
echo "SITL executable not found ($COPTER). Exiting" |
|
exit |
|
fi |
|
|
|
# Check if Platform is Native Linux, WSL or Cygwin |
|
# Needed for correct multicast addressing |
|
unameOut="$(uname -s)" |
|
|
|
if [ "$(expr substr $unameOut 1 5)" == "Linux" ]; then |
|
# Check for WSL |
|
if grep -q Microsoft /proc/version; then |
|
MCAST_IP_PORT="127.0.0.1:14550" |
|
|
|
# Native Linux |
|
else |
|
MCAST_IP_PORT="" # Use default IP, port |
|
fi |
|
|
|
elif [ "$(expr substr $unameOut 1 6)" == "CYGWIN" ]; then |
|
MCAST_IP_PORT="0.0.0.0:14550" |
|
fi |
|
|
|
BASE_DEFAULTS="$ROOTDIR/Tools/autotest/default_params/copter.parm" |
|
|
|
[ -x "$COPTER" ] || { |
|
./waf configure --board sitl |
|
./waf copter |
|
} |
|
|
|
# Set number of extra copters to be simulated, change this for increasing the count |
|
NCOPTERS="4" |
|
|
|
# start up main (leader) copter in the subdir (copter1) |
|
echo "Starting copter 1" |
|
mkdir -p copter1 |
|
|
|
# create default parameter file for the leader |
|
cat <<EOF > copter1/leader.parm |
|
SYSID_THISMAV 1 |
|
AUTO_OPTIONS 7 |
|
EOF |
|
|
|
pushd copter1 |
|
$COPTER --model quad --home=$HOMELAT,$HOMELONG,$HOMEALT,0 --uartA udpclient:$GCS_IP --uartC mcast:$MCAST_IP_PORT --defaults $BASE_DEFAULTS,leader.parm & |
|
popd |
|
|
|
# now start other copters to follow the first, using |
|
# a separate directory to keep the eeprom.bin and logs separate |
|
# each copter will have an offset starting location (5*SYSID,5*SYSID)m from leader copter |
|
# each copter will follow at SYSID*5m in X dir from leader |
|
for i in $(seq $NCOPTERS); do |
|
SYSID=$(expr $i + 1) |
|
|
|
echo "Starting copter $SYSID" |
|
mkdir -p copter$SYSID |
|
|
|
# create default parameter file for the follower |
|
cat <<EOF > copter$i/follow.parm |
|
SYSID_THISMAV $SYSID |
|
FOLL_ENABLE 1 |
|
FOLL_OFS_X $(echo "-5*$i" | bc -l) |
|
FOLL_OFS_TYPE 1 |
|
FOLL_SYSID 1 |
|
FOLL_DIST_MAX 1000 |
|
FOLL_YAW_BEHAVE 2 |
|
FOLL_ALT_TYPE 1 |
|
AUTO_OPTIONS 7 |
|
EOF |
|
pushd copter$i |
|
LAT=$(echo "$HOMELAT + 0.0005*$i" | bc -l) |
|
LONG=$(echo "$HOMELONG + 0.0005*$i" | bc -l) |
|
$COPTER --model quad --home=$LAT,$LONG,$HOMEALT,0 --uartA tcp:0 --uartC mcast:$MCAST_IP_PORT --instance $i --defaults $BASE_DEFAULTS,follow.parm & |
|
popd |
|
done |
|
wait |
|
|
|
|