From e85f803e27fc704598d8ec0def1a8723ceed973e Mon Sep 17 00:00:00 2001
From: Stephen Dade <stephen_dade@hotmail.com>
Date: Wed, 1 Sep 2021 09:01:01 +1000
Subject: [PATCH] SITL: Expand multi-drone example

---
 libraries/SITL/examples/follow-copter.sh   | 65 +++++++++++++++++-----
 libraries/SITL/examples/follow-mavproxy.sh |  3 +
 2 files changed, 53 insertions(+), 15 deletions(-)
 create mode 100755 libraries/SITL/examples/follow-mavproxy.sh

diff --git a/libraries/SITL/examples/follow-copter.sh b/libraries/SITL/examples/follow-copter.sh
index d0b156196d..8806700171 100755
--- a/libraries/SITL/examples/follow-copter.sh
+++ b/libraries/SITL/examples/follow-copter.sh
@@ -2,15 +2,24 @@
 
 # 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
-# mavproxy --master mcast:
+# 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
 
-# assume we start the script from the root directory
-ROOTDIR=$PWD
+# 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"
@@ -18,6 +27,15 @@ 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)"
@@ -43,33 +61,50 @@ BASE_DEFAULTS="$ROOTDIR/Tools/autotest/default_params/copter.parm"
 	./waf copter
 }
 
-# start up main copter in the current directory
-$COPTER --model quad --uartA udpclient:$GCS_IP --uartC mcast:$MCAST_IP_PORT --defaults $BASE_DEFAULTS &
-
 # Set number of extra copters to be simulated, change this for increasing the count
-NCOPTERS="1"
+NCOPTERS="4"
 
+# start up main (leader) copter in the subdir (copter1)
+echo "Starting copter 1"
+mkdir -p copter1
 
-# now start another copter to follow the first, using
+# 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
-    echo "Starting copter $i"
-    mkdir -p copter$i
-
     SYSID=$(expr $i + 1)
-    FOLL_SYSID=$(expr $SYSID - 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 -5
+FOLL_OFS_X $(echo "-5*$i" | bc -l)
 FOLL_OFS_TYPE 1
-FOLL_SYSID $FOLL_SYSID
+FOLL_SYSID 1
 FOLL_DIST_MAX 1000
+FOLL_YAW_BEHAVE 2
+FOLL_ALT_TYPE 1
+AUTO_OPTIONS 7
 EOF
     pushd copter$i
-    $COPTER --model quad --uartA tcp:0 --uartC mcast:$MCAST_IP_PORT --instance $i --defaults $BASE_DEFAULTS,follow.parm &
+    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
+
diff --git a/libraries/SITL/examples/follow-mavproxy.sh b/libraries/SITL/examples/follow-mavproxy.sh
new file mode 100755
index 0000000000..54cbc889e2
--- /dev/null
+++ b/libraries/SITL/examples/follow-mavproxy.sh
@@ -0,0 +1,3 @@
+#!/bin/bash
+
+mavproxy.py --master=mcast: --console --map --cmd='map set showgpspos 0; map set showgps2pos 0;'