follow-copter.sh 1.1 KB

123456789101112131415161718192021222324252627282930313233343536373839404142
  1. #!/bin/bash
  2. # assume we start the script from the root directory
  3. ROOTDIR=$PWD
  4. COPTER=$ROOTDIR/build/sitl/bin/arducopter
  5. GCS_IP=$1
  6. BASE_DEFAULTS="$ROOTDIR/Tools/autotest/default_params/copter.parm,$ROOTDIR/libraries/SITL/examples/Airsim/quadX.parm"
  7. [ -x "$COPTER" ] || {
  8. ./waf configure --board sitl
  9. ./waf copter
  10. }
  11. # start up main rover in the current directory
  12. $COPTER --model airsim-copter --uartA udpclient:$GCS_IP --uartC mcast: --defaults $BASE_DEFAULTS &
  13. # now start another copter to follow the first, using
  14. # a separate directory to keep the eeprom.bin and logs separate
  15. # for increasing the number of copters, change the number in seq
  16. for i in $(seq 1); do
  17. echo "Starting copter $i"
  18. mkdir -p copter$i
  19. SYSID=$(expr $i + 1)
  20. FOLL_SYSID=$(expr $SYSID - 1)
  21. # create default parameter file for the follower
  22. cat <<EOF > copter$i/follow.parm
  23. SYSID_THISMAV $SYSID
  24. FOLL_ENABLE 1
  25. FOLL_OFS_X -5
  26. FOLL_OFS_TYPE 1
  27. FOLL_SYSID $FOLL_SYSID
  28. FOLL_DIST_MAX 1000
  29. EOF
  30. pushd copter$i
  31. $COPTER --model airsim-copter --uartA tcp:0 --uartC mcast: --instance $i --defaults $BASE_DEFAULTS,follow.parm &
  32. popd
  33. done
  34. wait