l

.gitattributes

@@ -2,6 +2,7 @@
 /CMakeLists.txt -text
 /package.xml -text
 /setup.py -text
+src/lqsimple.py -text
 src/pyutil/__init__.py -text
 src/pyutil/relayutil.py -text
 src/pyutil/tdwutil.py -text

src/lqsimple.py

+#!/usr/bin/env python
+
+#
+# Module with a simplified interface to the Link Quad
+#
+
+import math
+import sys
+import os
+import time
+
+import rospy
+from lrs_msgs.msg import *
+
+from quad.qsim import *
+from quad.qcon import *
+
+#
+# Put this in a file call limits.py and make sure it is in the path
+# This will override the default values below. All limit variables have to be defined 
+# in the file.
+#
+
+
+max_altitude = 140.5
+min_altitude = 0.1
+
+max_speed = 5.0
+
+max_forward = 20.0
+min_forward = -20.0
+
+max_right = 20.0
+min_right = -20.0
+
+max_up = 5.0
+min_up = -5.0
+
+#
+#
+#
+if os.path.isfile ("limits.py"):
+    print "READ FILE: limits.py"
+    import limits
+    max_altitude = limits.max_altitude
+    min_altitude = limits.min_altitude
+    max_speed = limits.max_speed
+    max_forward = limits.max_forward
+    min_forward = limits.min_forward
+    max_right = limits.max_right
+    min_right = limits.min_right
+    max_up = limits.max_up
+    min_up = limits.min_up
+    
+print "----- LIMITS ------"
+print "Altitude limits: <{0},{1}> ".format(min_altitude,max_altitude)
+print "Max speed: ", max_speed
+print "Forward limits: <{0},{1}> ".format(min_forward,max_forward)
+print "Right limits: <{0},{1}> ".format(min_right,max_right)
+print "Up limits: <{0},{1}> ".format(min_up,max_up)
+print "-------------------"
+
+
+def fly_straight_line (east_m, north_m, relalt, speed, yaw_flag):
+    print "fly_straight_line: east_m:{0} north_m:{1} alt_m:{2} spd:{3}".format (east_m, north_m, relalt, speed)
+
+    ns = rospy.get_namespace ()
+    qc = QCon(ns, 10000)
+
+    if options.limits: 
+      if relalt > max_altitude: print "Target too high... Exiting. "; sys.exit (1),
+      if relalt < min_altitude: print "Target too low... Exiting. "; sys.exit (1)
+
+
+    resp = qc.queues_clear_token (1, 1, yaw_flag, 0);
+    if resp.status != 0: print "Failed to clear the queues"; sys.exit (1)
+
+    resp = qc.request_flyq ();
+    if resp.status != 0: sys.exit (1)
+
+    flyqtoken = resp.token
+
+    yawqtoken = 0
+    if yaw_flag:
+        resp = qc.request_yawq ();
+        if resp.status != 0: sys.exit (1)
+
+        yawqtoken = resp.token
+
+    resp = qc.request_climbq ();
+    if resp.status != 0: sys.exit (1)
+
+    climbqtoken = resp.token
+   
+    resp = qc.flyq_reset (flyqtoken)
+    if resp.status != 0: sys.exit (1)
+
+    if yaw_flag:
+        resp = qc.yawq_reset (yawqtoken)
+        if resp.status != 0: sys.exit (1)
+
+    resp = qc.climbq_reset (climbqtoken)
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.add_flyto_3d_m (flyqtoken, north_m, east_m, relalt, speed, 0, 1, 1)
+    if resp.status != 0: sys.exit (1)
+
+    if yaw_flag:
+        resp = qc.yawq_add_use_fly (yawqtoken)
+        if resp.status != 0: sys.exit (1)
+
+    resp = qc.climbq_add_use_fly (climbqtoken)
+    if resp.status != 0: sys.exit (1)
+
+    if options.confirm:
+        raw_input("---> READY. Press Enter to execute...")
+
+    resp = qc.queues_run (flyqtoken, climbqtoken, yawqtoken*yaw_flag, 0)
+    if resp.status != 0: sys.exit (1)
+
+    qc.wait_for_flyq ()
+
+def take_off (alt, speed):
+    ns = rospy.get_namespace ()
+    qc = QCon(ns, 10000)
+
+    resp = qc.queues_clear_token (1, 1, 1, 0);
+    if resp.status != 0: print "Failed to clear the queues"; sys.exit (1)
+
+    resp = qc.request_flyq ();
+    print resp
+    if resp.status != 0: sys.exit (1)
+    flyqtoken = resp.token
+
+    resp = qc.request_climbq ();
+    print resp
+    if resp.status != 0: sys.exit (1)
+    climbqtoken = resp.token
+   
+    resp = qc.flyq_reset (flyqtoken)
+    print resp
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.climbq_reset (climbqtoken)
+    print resp
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.flyq_add_take_off (flyqtoken, alt, speed)
+    print resp
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.climbq_add_use_fly (climbqtoken)
+    print resp
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.flyq_add_barrier (flyqtoken, 1, 1, 1, 0, 0)
+    print resp
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.climbq_add_barrier (climbqtoken, 1, 1, 1, 0, 0)
+    print resp
+    if resp.status != 0: sys.exit (1)
+
+
+    resp = qc.queues_run (flyqtoken, climbqtoken, 0, 0);
+    print resp
+    if resp.status != 0: sys.exit (1)
+
+    qc.wait_for_flyq ()
+
+def land (alt, speed):
+    ns = rospy.get_namespace ()
+    qc = QCon(ns, 10000)
+
+    resp = qc.queues_clear_token (1, 1, 1, 0);
+    if resp.status != 0: print "Failed to clear the queues"; sys.exit (1)
+
+
+    resp = qc.request_flyq ();
+    print resp
+    if resp.status != 0: sys.exit (1)
+    flyqtoken = resp.token
+
+    resp = qc.request_climbq ();
+    print resp
+    if resp.status != 0: sys.exit (1)
+    climbqtoken = resp.token
+   
+    resp = qc.flyq_reset (flyqtoken)
+    print resp
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.climbq_reset (climbqtoken)
+    print resp
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.flyq_add_land (flyqtoken, alt, speed)
+    print resp
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.climbq_add_use_fly (climbqtoken)
+    print resp
+    if resp.status != 0: sys.exit (1)
+
+
+    resp = qc.flyq_add_barrier (flyqtoken, 1, 1, 1, 0, 0)
+    print resp
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.climbq_add_barrier (climbqtoken, 1, 1, 1, 0, 0)
+    print resp
+    if resp.status != 0: sys.exit (1)
+
+
+    resp = qc.queues_run (flyqtoken, climbqtoken, 0, 0);
+    print resp
+    if resp.status != 0: sys.exit (1)
+
+    qc.wait_for_flyq ()
+
+
+
+def fly_in_dir_ref ():
+    ns = rospy.get_namespace ()
+    qc = QCon(ns, 10000)
+
+    print "fly_in_dir_ref: "
+
+    resp = qc.queues_clear_token (1, 1, 1, 0);
+    if resp.status != 0: print "Failed to clear the queues"; sys.exit (1)
+
+    resp = qc.request_flyq ();
+    if resp.status != 0: sys.exit (1)
+
+    flyqtoken = resp.token
+
+    resp = qc.request_climbq ();
+    if resp.status != 0: sys.exit (1)
+
+    climbqtoken = resp.token
+   
+    resp = qc.flyq_reset (flyqtoken)
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.climbq_reset (climbqtoken)
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.add_fly_in_dir_ref (flyqtoken)
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.add_climb_in_dir_ref (climbqtoken)
+    if resp.status != 0: sys.exit (1)
+
+    if options.confirm:
+        raw_input("---> READY. Press Enter to execute...")
+
+    resp = qc.queues_run (flyqtoken, climbqtoken, 0, 0)
+    if resp.status != 0: sys.exit (1)
+
+#    qc.wait_for_flyq ()
+
+def fly_external ():
+    ns = rospy.get_namespace ()
+    qc = QCon(ns, 10000)
+
+    print "fly_external: "
+
+    resp = qc.queues_clear_token (1, 1, 1, 0);
+    if resp.status != 0: print "Failed to clear the queues: {}".format(resp.status); sys.exit (1)
+
+    resp = qc.request_flyq ();
+    if resp.status != 0: sys.exit (1)
+
+    flyqtoken = resp.token
+
+    resp = qc.request_climbq ();
+    if resp.status != 0: sys.exit (1)
+    climbqtoken = resp.token
+
+    resp = qc.request_yawq ();
+    if resp.status != 0: sys.exit (1)
+    yawqtoken = resp.token
+
+   
+    resp = qc.flyq_reset (flyqtoken)
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.climbq_reset (climbqtoken)
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.yawq_reset (yawqtoken)
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.add_fly_ext (flyqtoken)
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.add_climb_ext (climbqtoken)
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.add_yaw_ext (yawqtoken)
+    if resp.status != 0: sys.exit (1)
+
+#    if options.confirm:
+#        raw_input("---> READY. Press Enter to execute...")
+
+    resp = qc.queues_run (flyqtoken, climbqtoken, yawqtoken, 0)
+    if resp.status != 0: sys.exit (1)
+
+
+def ptu_external ():
+    ns = rospy.get_namespace ()
+    qc = QCon(ns, 10000)
+
+    print "ptu_external: {0}".format (ns)
+
+
+    resp = qc.queues_clear_token (0, 0, 0, 1);
+    if resp.status != 0: print "Failed to clear the ptu queue: {}".format(resp.status); sys.exit (1)
+
+    resp = qc.request_ptuq ();
+    if resp.status != 0: sys.exit (1)
+
+    ptuqtoken = resp.token
+
+    resp = qc.ptuq_reset (ptuqtoken)
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.add_ptu_ext (ptuqtoken)
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.queues_run (0, 0, 0, ptuqtoken)
+    if resp.status != 0: sys.exit (1)
+
+
+def yaw (heading):
+    ns = rospy.get_namespace ()
+    qc = QCon(ns, 10000)
+
+    print "yaw: {0}".format (heading)
+    resp = qc.queues_clear_token (0, 0, 1, 0)
+    if resp.status != 0: print "Failed to clear the yaw queue"; sys.exit (1)
+
+    resp = qc.request_yawq ()
+    if resp.status != 0: sys.exit (1)
+
+    yawqtoken = resp.token
+
+    resp = qc.yawq_reset (yawqtoken)
+    if resp.status != 0: sys.exit (1)
+
+    rate_only = 0
+    use_rate_sign = 0
+    rate = 5
+
+    resp = qc.add_yaw (yawqtoken, heading, rate, rate_only, use_rate_sign)
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.queues_run (0, 0, yawqtoken, 0)
+    if resp.status != 0: sys.exit (1)
+
+    qc.wait_for_yawq ()
+
+
+def yawrel (heading):
+    ns = rospy.get_namespace ()
+    qc = QCon(ns, 10000)
+
+    print "yawrel: heading: {0}".format (heading)
+
+    resp = qc.get_current_state ()
+    print resp
+    state_yaw = resp.yaw
+    heading = heading + state_yaw
+
+    resp = qc.queues_clear_token (0, 0, 1, 0)
+    if resp.status != 0: print "Failed to clear the yaw queue"; sys.exit (1)
+
+    resp = qc.request_yawq ()
+    if resp.status != 0: sys.exit (1)
+
+    yawqtoken = resp.token
+
+    resp = qc.yawq_reset (yawqtoken)
+    if resp.status != 0: sys.exit (1)
+
+    rate_only = 0
+    use_rate_sign = 0
+    rate = 5
+
+    resp = qc.add_yaw (yawqtoken, heading, rate, rate_only. use_rate_sign)
+    if resp.status != 0: sys.exit (1)
+
+    if options.confirm:
+        raw_input("---> READY. Press Enter to execute...")
+
+    resp = qc.queues_run (0, 0, yawqtoken, 0)
+    if resp.status != 0: sys.exit (1)
+
+    qc.wait_for_yawq ()
+
+
+def climb (alt):
+    ns = rospy.get_namespace ()
+    qc = QCon(ns, 10000)
+
+    print "climb: {0} {1}".format (ns, alt)
+    resp = qc.queues_clear_token (0, 1, 0, 0)
+    if resp.status != 0: print "Failed to clear the climb queue"; sys.exit (1)
+    
+    resp = qc.request_climbq ()
+    if resp.status != 0: sys.exit (1)
+
+    climbqtoken = resp.token
+
+    resp = qc.climbq_reset (climbqtoken)
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.add_climb (climbqtoken, alt)
+    if resp.status != 0: sys.exit (1)
+
+    resp = qc.queues_run (0, climbqtoken, 0, 0)
+    if resp.status != 0: sys.exit (1)
+
+    qc.wait_for_climbq ()
+
+def climbrel (alt, speed):
+    ns = rospy.get_namespace ()
+    qc = QCon(ns, 10000)
+
+    print "climb: {0} {1}".format (ns, alt)
+
+    resp = qc.queues_clear_token (0, 1, 0, 0);
+    if resp.status != 0: print "Failed to clear the climb queue"; sys.exit (1)
+    
+    resp = qc.get_current_state ()
+    print resp
+
+    state_north_m = resp.lat_m
+    state_east_m = resp.lon_m
+    state_alt = resp.alt
+
+    alt_m = state_alt+alt
+    if alt_m > 140.5: print "Target too high... Exiting. "; sys.exit (1),
+    if alt_m < 0.5: print "Target too low... Exiting. "; sys.exit (1)
+
+    fly_straight_line (state_east_m, state_north_m, alt_m, speed, 0)
+
+def fly_xy_rel (forward, right, speed):
+    ns = rospy.get_namespace ()
+    qc = QCon(ns, 10000)
+
+    print "Flyto xy rel: unit:{0} forwrd:{1:.3f} right:{2:.3f} speed:{3:.3f}".format(ns, forward, right, speed)
+
+    resp = qc.queues_clear_token (1, 0, 0, 0);
+    if resp.status != 0: print "Failed to clear the climb queue"; sys.exit (1)
+
+    resp = qc.get_current_state ()
+    print resp
+
+    state_north_m = resp.lat_m
+    state_east_m = resp.lon_m
+    state_yaw_rad = resp.yaw/180.0 * math.pi
+    state_alt = resp.alt
+
+    print "-----> Current state: north: ", state_north_m, "east: ", state_east_m, "yaw: ", state_yaw_rad/math.pi*180.0
+
+    north_m = forward * math.cos(state_yaw_rad) - right * math.sin(state_yaw_rad)
+    east_m = forward * math.sin(state_yaw_rad) + right * math.cos(state_yaw_rad)
+            
+    print "-----> Relative current pos: north: {0:.3f} east: {1:.3f}".format(north_m,east_m)
+
+    fly_straight_line (state_east_m + east_m, state_north_m + north_m, state_alt, speed, 0)
+
+
+def fly_rel (forward, right, up, speed, yaw_flag):
+    ns = rospy.get_namespace ()
+    qc = QCon(ns, 10000)
+
+    print "Flyto relative: unit: {0} forward: {1:.3f} right: {2:.3f} up: {3:.3f} speed: {4:.3f} yaw to waypoint: {5}".format(ns, forward, right, up, speed,yaw_flag)
+    if forward==0.0 and right==0 and up==0: print "All parameters are 0.0. Exiting...", sys.exit(1)
+
+    resp = qc.queues_clear_token (1, 1, yaw_flag, 0);
+    if resp.status != 0: print "Failed to clear the queues"; sys.exit (1)
+
+# old way of doing this - current state instead of targets
+#    resp = qc.get_current_state ()
+#    print resp
+
+#    state_north_m = resp.lat_m
+#    state_east_m = resp.lon_m
+#    state_yaw_rad = resp.yaw/180.0 * math.pi
+#    state_alt = resp.alt
+
+    resp = qc.get_current_targets ()
+    print resp
+
+#float64 east_m
+#float64 north_m
+#float64 altitude
+#float64 vel_east
+#float64 vel_north
+#float64 vel_up
+#float64 acc_east
+#float64 acc_north
+#float64 acc_up
+#float64 heading
+
+
+    state_north_m = resp.north_m
+    state_east_m = resp.east_m
+    state_yaw_rad = resp.heading/180.0 * math.pi
+    state_alt = resp.altitude
+
+    print "-----> Current targets: north: {0}, east: {1}, alt: {2}, yaw: {3}".format (state_north_m, state_east_m, state_alt, state_yaw_rad/math.pi*180.0)
+
+    north_m = forward * math.cos(state_yaw_rad) - right * math.sin(state_yaw_rad)
+    east_m = forward * math.sin(state_yaw_rad) + right * math.cos(state_yaw_rad)
+            
+    print "-----> Relative current pos: north: {0:.3f} east: {1:.3f} up: {2}".format(north_m,east_m, up)
+
+    fly_straight_line (state_east_m + east_m, state_north_m + north_m, 
+                       state_alt + up, speed, yaw_flag)
+
+def fly_triangle (speed):
+    ns = rospy.get_namespace ()
+    qc = QCon(ns, 10000)
+
+    resp = qc.get_current_state ()
+    print resp
+
+    state_north_m = resp.lat_m
+    state_east_m = resp.lon_m
+    state_alt = resp.alt
+
+    tokens = qc.fly_triangle_yaw_climb_pause (state_east_m, state_north_m, 
+                                              state_alt, speed)
+    print tokens
+
+    if options.confirm:
+        raw_input("---> READY. Press Enter to execute...")
+
+    resp = qc.queues_run (tokens[0], tokens[1], tokens[2], 0)
+    if resp.status != 0: sys.exit (1)
+
+    qc.wait_for_flyq ()
+
+
+if __name__ == "__main__":
+    from optparse import OptionParser
+    parser = OptionParser()
+    parser.add_option ("", "--log", action="store", type="string", dest="logfile", default="")
+    parser.add_option ("-u", "--unit", action="store", type="int", dest="unit", default=0)
+    parser.add_option ("", "--looprate", action="store", type="int", dest="looprate", default=500)
+    parser.add_option ("", "--confirm", action="store_true", dest="confirm", help="If given some fly actions have a confirm in it")
+    parser.add_option ("", "--limits", action="store_true", dest="limits", help="Check some pre defined limits before flying")
+    parser.add_option ("", "--getref", action="store_true", dest="getref", help="Get the reference data")
+    parser.add_option ("", "--setref", action="store_true", dest="setref", help="Set the reference data")
+    parser.add_option ("", "--set", action="store_true", dest="set", help="Set values and exit")
+    parser.add_option ("", "--fly", action="store_true", dest="fly", help="Fly straight line")
+    parser.add_option ("", "--flytriangle", action="store_true", dest="flytriangle", help="Fly triangle with pauses")
+    parser.add_option ("", "--flyindirref", action="store_true", dest="flyindirref", help="Fly in direction according to ref data")
+    parser.add_option ("", "--flyxyrel", action="store_true", dest="flyxyrel", help="Fly straight line from current position to cp+(x,y), y is forward and x to the right of the LQ")
+    parser.add_option ("", "--flyrel", action="store_true", dest="flyrel", help="Fly straight line from current position to cp+(x,y,z), y is forward and x to the right of the LQ and z is up")
+    parser.add_option ("", "--land", action="store_true", dest="land", help="Land by flying to alt z with speed")
+    parser.add_option ("", "--takeoff", action="store_true", dest="takeoff", help="Take off by flying to alt z with speed")
+    parser.add_option ("", "--useyaw", action="store_true", dest="useyaw", help="Fly with yaw towards a waypoint. Otherwise, keep the current yaw", default=False)
+    parser.add_option ("", "--yaw", action="store_true", dest="yaw", help="Yaw to heading")
+    parser.add_option ("", "--yawrel", action="store_true", dest="yawrel", help="Yaw to current_heading+heading")
+    parser.add_option ("", "--climb", action="store_true", dest="climb", help="Climb to z")
+    parser.add_option ("", "--climbrel", action="store_true", dest="climbrel", help="Climb to current_alt+z")
+    parser.add_option ("", "--lqreal", action="store_true", dest="lqreal", help="Fly the real LQ")
+    parser.add_option ("", "--manual", action="store_true", dest="manual", help="Go to manual in the simulator")
+
+    parser.add_option ("-x", "--xpos", action="store", type="float", dest="x", default=8)
+    parser.add_option ("-y", "--ypos", action="store", type="float", dest="y", default=2)
+    parser.add_option ("-z", "--zpos", action="store", type="float", dest="z", default=2)
+    parser.add_option ("", "--forward", action="store", type="float", dest="y", help="The amount of forward to fly", default=0)
+    parser.add_option ("", "--right", action="store", type="float", dest="x", help="The amount of right to fly", default=0)
+    parser.add_option ("", "--up", action="store", type="float", dest="z", help="The amount of up to fly", default=0)
+    parser.add_option ("-s", "--speed", action="store", type="float", dest="speed", default=2)
+    parser.add_option ("", "--xyspeed", action="store", type="float", dest="xyspeed", default=0.2)
+    parser.add_option ("", "--zspeed", action="store", type="float", dest="zspeed", default=0.2)
+    parser.add_option ("", "--heading", action="store", type="float", dest="heading", default=45, help="In degree between 0-360 (negative angles currently do not work)")
+    parser.add_option ("", "--state", action="store", type="int", dest="state", default=0, help="0 (default) is sim/real outside, 1 is vicon room")
+    parser.add_option ("", "--joystick", action="store_true", dest="joystick", help="Set ext config and exit")
+    parser.add_option ("", "--safenav", action="store_true", dest="safenav", help="Set ext config and exit")
+    (options, args) = parser.parse_args()
+
+    rospy.init_node('lqsimple', anonymous=True)
+
+    ns = rospy.get_namespace ()
+    qc = QCon(ns, 10000)
+    sim = QSim(ns)
+
+    if options.joystick:
+        qc.set_ext_config_joystick()
+        sys.exit(0)
+
+    if options.safenav:
+        qc.set_ext_config_safenav()
+        sys.exit(0)
+
+    if options.set:
+        sim.set_loop_rate (options.looprate)
+        sys.exit(0)
+
+    if options.lqreal:
+        if options.takeoff:
+            options.z = 1.7
+            options.speed = 1.0
+#            options.speed = 1.5
+# outdoor gps
+#            options.z = 10
+#            options.speed = 2.0
+        if options.land:
+            options.z = -100
+            options.speed = 0.3
+# outdoor gps
+#            options.z = -100
+#            options.speed = 1.0
+
+    if options.limits:
+        if options.speed > max_speed: print "Speed {0} is too high (limit: {1}).".format (options.speed, max_speed);sys.exit (1)
+        if options.y > max_forward or options.y < min_forward: 
+            print "Forward {0} is outside limits: <{1},{2}>.".format (options.y, min_forward, max_forward);sys.exit (1)
+
+        if options.x > max_right or options.x < min_right: 
+            print "Right {0} is outside limits: <{1},{2}>.".format (options.x, min_right, max_right);sys.exit (1)
+
+        if options.z > max_up or options.z < min_up: 
+            print "Up {0} is outside limits: <{1},{2}>.".format (options.z, min_up, max_up);sys.exit (1)
+
+    if options.manual:
+        sim.to_manual ()
+        time.sleep (1)
+        sim.to_auto ()
+        sys.exit(0)
+
+
+    if options.logfile != "":
+        print "ENABLE LOGGING"
+        sim.start_logging (options.logfile + ".lgdat")
+
+    
+    # Make sure the motor is on in the simulator
+    if not options.lqreal:
+        sim.set_use_sticks (0),
+        sim.set_motor (1)
+        sim.to_auto ()
+
+####### VICON
+#    print "Setting UAV state source: {0:1d} ".format(options.state)    
+#    resp = qc.set_uav_states_config (0, options.state)
+#    print resp
+#    if resp.status != 0: print "Failed to set the uav states config"; sys.exit (1)
+
+    if not options.lqreal:
+        sim.set_use_sticks (0),
+        sim.to_auto () # go to auto
+
+
+    if options.fly:
+        fly_straight_line (options.x, options.y, options.z, options.speed, 1)
+
+    if options.land:
+        land (options.z, options.speed)
+
+    if options.takeoff:
+        take_off (options.z, options.speed)
+
+    if options.flytriangle:
+        fly_triangle (options.speed)
+    
+    if options.yaw:
+        yaw (options.heading)
+
+    if options.yawrel:
+        yawrel (options.heading)
+
+    if options.flyindirref:
+        fly_in_dir_ref ()
+
+    if options.setref:
+        sim.set_ref_data (options.x, options.y, options.z, options.xyspeed, options.zspeed)
+
+    if options.getref:
+        rdata = sim.get_ref_data ()
+        print rdata
+
+
+#
+# Maybe dangerous, use fly_straight_line instead
+#
+    if options.climb:
+        climb (options.z)
+
+    if options.climbrel:
+        climbrel (options.z, options.speed)
+
+    if options.flyxyrel:
+        fly_xy_rel (options.y, options.x, options.speed)
+
+    if options.flyrel:
+        fly_rel (options.y, options.x, options.z, options.speed, options.useyaw)
+
+    if options.logfile != "":
+        time.sleep (10)
+        sim.stop_logging ()