Commit 6d8cc73b authored by Peter Barker's avatar Peter Barker
Browse files

Merge pull request #551 from dronekit/hgw_dk_sitl_example_follow_me

Update follow_me example to use dk-sitl by default
parents 7800c40c d436bf37
Showing with 67 additions and 45 deletions
+67 -45
......@@ -51,54 +51,58 @@ Once you've done that:
cd dronekit-python/examples/follow_me/
#. Start the example, passing the :ref:`connection string <get_started_connect_string>` you wish to use in the ``--connect`` parameter:
#. You can run the example against a simulator (DroneKit-SITL) by specifying the Python script without any arguments.
The example will download SITL binaries (if needed), start the simulator, and then connect to it:
.. code-block:: bash
python follow_me.py --connect 127.0.0.1:14550
python follow_me.py
.. note::
On the command prompt you should see (something like):
The examples uses the ``--connect`` parameter to pass the :ref:`connection string <get_started_connect_string>` into the script.
The command above is used to connect to :ref:`SITL <sitl_setup>` running on the local Linux machine via UDP port 14550.
.. code:: bash
Starting copter simulator (SITL)
SITL already Downloaded.
Connecting to vehicle on: tcp:127.0.0.1:5760
>>> APM:Copter V3.4-dev (e0810c2e)
>>> Frame: QUAD
Link timeout, no heartbeat in last 5 seconds
Basic pre-arm checks
Waiting for GPS...: None
...
Waiting for GPS...: None
Taking off!
Altitude: 0.019999999553
...
Altitude: 4.76000022888
Reached target altitude
Going to: Location:lat=50.616468333,lon=7.131903333,alt=30,is_relative=True
...
Going to: Location:lat=50.616468333,lon=7.131903333,alt=30,is_relative=True
Going to: Location:lat=50.616468333,lon=7.131903333,alt=30,is_relative=True
User has changed flight modes - aborting follow-me
Close vehicle object
Completed
.. note::
The terminal output above was created using simulated GPS data
(which is why the same target location is returned every time).
To stop follow-me you can change the vehicle mode or do Ctrl+C
(on a real flight you can just change the mode switch on your
RC transmitter).
On the terminal you should see (something like):
#. You can run the example against a specific connection (simulated or otherwise) by passing the :ref:`connection string <get_started_connect_string>` for your vehicle in the ``--connect`` parameter.
For example, to connect to SITL running on UDP port 14550 on your local computer:
.. code-block:: bash
Connecting to vehicle on: 127.0.0.1:14550
>>> APM:Copter V3.4-dev (e0810c2e)
>>> Frame: QUAD
Link timeout, no heartbeat in last 5 seconds
Basic pre-arm checks
Waiting for GPS...: None
...
Waiting for GPS...: None
Taking off!
Altitude: 0.019999999553
...
Altitude: 4.76000022888
Reached target altitude
Going to: Location:lat=50.616468333,lon=7.131903333,alt=30,is_relative=True
...
Going to: Location:lat=50.616468333,lon=7.131903333,alt=30,is_relative=True
Going to: Location:lat=50.616468333,lon=7.131903333,alt=30,is_relative=True
User has changed flight modes - aborting follow-me
Close vehicle object
Completed
To stop follow-me you can change the vehicle mode (on a real flight you can just change the mode switch on your RC transmitter).
.. note::
.. code-block:: bash
The terminal output above was created using simulated GPS data
(which is why the same target location is returned every time). It was exited by changing
the mode from GUIDED to STABILIZE using MAVProxy.
python follow_me.py --connect 127.0.0.1:14550
How does it work?
......
......@@ -18,15 +18,29 @@ import sys
#Set up option parsing to get connection string
import argparse
parser = argparse.ArgumentParser(description='Tracks GPS position of your computer (Linux only). Connects to SITL on local PC by default.')
parser.add_argument('--connect', default='127.0.0.1:14550',
help="vehicle connection target. Default '127.0.0.1:14550'")
parser = argparse.ArgumentParser(description='Tracks GPS position of your computer (Linux only).')
parser.add_argument('--connect',
help="vehicle connection target string. If not specified, SITL automatically started and used.")
args = parser.parse_args()
connection_string = args.connect
sitl = None
#Start SITL if no connection string specified
if not args.connect:
print "Starting copter simulator (SITL)"
from dronekit_sitl import SITL
sitl = SITL()
sitl.download('copter', '3.3', verbose=True)
sitl_args = ['-I0', '--model', 'quad', '--home=-35.363261,149.165230,584,353']
sitl.launch(sitl_args, await_ready=True, restart=True)
connection_string = 'tcp:127.0.0.1:5760'
# Connect to the Vehicle
print 'Connecting to vehicle on: %s' % args.connect
vehicle = connect(args.connect, wait_ready=True)
print 'Connecting to vehicle on: %s' % connection_string
vehicle = connect(connection_string, wait_ready=True)
......@@ -44,8 +58,8 @@ def arm_and_takeoff(aTargetAltitude):
print "Arming motors"
# Copter should arm in GUIDED mode
vehicle.mode = VehicleMode("GUIDED")
vehicle.armed = True
vehicle.mode = VehicleMode("GUIDED")
vehicle.armed = True
while not vehicle.armed:
print " Waiting for arming..."
......@@ -102,4 +116,8 @@ except socket.error:
print "Close vehicle object"
vehicle.close()
# Shut down simulator if it was started.
if sitl is not None:
sitl.stop()
print("Completed")
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment