diff --git a/.gitattributes b/.gitattributes
index 320b0ca457eb8425544a9b00c4de476ece44f402..2e145e4d78a17e2fcf3245830f300ec8c6fa012c 100644
--- a/.gitattributes
+++ b/.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
diff --git a/src/lqsimple.py b/src/lqsimple.py
new file mode 100755
index 0000000000000000000000000000000000000000..77bff48faafbb2bdde373746fa4f8095be9863b1
--- /dev/null
+++ b/src/lqsimple.py
@@ -0,0 +1,708 @@
+#!/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 ()