diff --git a/src/lqsimple.py b/src/lqsimple.py
index 4f651ac9c178320e1c3253b080c437acf8f9c2e3..bd1d13ad3017b75a553140541cca4f556150804b 100755
--- a/src/lqsimple.py
+++ b/src/lqsimple.py
@@ -39,18 +39,18 @@ min_up = -5.0
 
 
 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)
+    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)
+      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)
+    if resp.status != 0: print("Failed to clear the queues"); sys.exit (1)
 
     resp = qc.request_flyq ();
     if resp.status != 0: sys.exit (1)
@@ -102,45 +102,45 @@ def take_off (alt, speed):
     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)
+    if resp.status != 0: print("Failed to clear the queues"); sys.exit (1)
 
     resp = qc.request_flyq ();
-    print resp
+    print(resp)
     if resp.status != 0: sys.exit (1)
     flyqtoken = resp.token
 
     resp = qc.request_climbq ();
-    print resp
+    print(resp)
     if resp.status != 0: sys.exit (1)
     climbqtoken = resp.token
    
     resp = qc.flyq_reset (flyqtoken)
-    print resp
+    print(resp)
     if resp.status != 0: sys.exit (1)
 
     resp = qc.climbq_reset (climbqtoken)
-    print resp
+    print(resp)
     if resp.status != 0: sys.exit (1)
 
     resp = qc.flyq_add_take_off (flyqtoken, alt, speed)
-    print resp
+    print(resp)
     if resp.status != 0: sys.exit (1)
 
     resp = qc.climbq_add_use_fly (climbqtoken)
-    print resp
+    print(resp)
     if resp.status != 0: sys.exit (1)
 
     resp = qc.flyq_add_barrier (flyqtoken, 1, 1, 1, 0, 0)
-    print resp
+    print(resp)
     if resp.status != 0: sys.exit (1)
 
     resp = qc.climbq_add_barrier (climbqtoken, 1, 1, 1, 0, 0)
-    print resp
+    print(resp)
     if resp.status != 0: sys.exit (1)
 
 
     resp = qc.queues_run (flyqtoken, climbqtoken, 0, 0);
-    print resp
+    print(resp)
     if resp.status != 0: sys.exit (1)
 
     qc.wait_for_flyq ()
@@ -150,47 +150,47 @@ def land (alt, speed):
     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)
+    if resp.status != 0: print("Failed to clear the queues"); sys.exit (1)
 
 
     resp = qc.request_flyq ();
-    print resp
+    print(resp)
     if resp.status != 0: sys.exit (1)
     flyqtoken = resp.token
 
     resp = qc.request_climbq ();
-    print resp
+    print(resp)
     if resp.status != 0: sys.exit (1)
     climbqtoken = resp.token
    
     resp = qc.flyq_reset (flyqtoken)
-    print resp
+    print(resp)
     if resp.status != 0: sys.exit (1)
 
     resp = qc.climbq_reset (climbqtoken)
-    print resp
+    print(resp)
     if resp.status != 0: sys.exit (1)
 
     resp = qc.flyq_add_land (flyqtoken, alt, speed)
-    print resp
+    print(resp)
     if resp.status != 0: sys.exit (1)
 
     resp = qc.climbq_add_use_fly (climbqtoken)
-    print resp
+    print(resp)
     if resp.status != 0: sys.exit (1)
 
 
     resp = qc.flyq_add_barrier (flyqtoken, 1, 1, 1, 0, 0)
-    print resp
+    print(resp)
     if resp.status != 0: sys.exit (1)
 
     resp = qc.climbq_add_barrier (climbqtoken, 1, 1, 1, 0, 0)
-    print resp
+    print(resp)
     if resp.status != 0: sys.exit (1)
 
 
     resp = qc.queues_run (flyqtoken, climbqtoken, 0, 0);
-    print resp
+    print(resp)
     if resp.status != 0: sys.exit (1)
 
     qc.wait_for_flyq ()
@@ -201,10 +201,10 @@ def fly_in_dir_ref ():
     ns = rospy.get_namespace ()
     qc = QCon(ns, 10000)
 
-    print "fly_in_dir_ref: "
+    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)
+    if resp.status != 0: print("Failed to clear the queues"); sys.exit (1)
 
     resp = qc.request_flyq ();
     if resp.status != 0: sys.exit (1)
@@ -240,10 +240,10 @@ def fly_external ():
     ns = rospy.get_namespace ()
     qc = QCon(ns, 10000)
 
-    print "fly_external: "
+    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)
+    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)
@@ -288,11 +288,11 @@ def ptu_external ():
     ns = rospy.get_namespace ()
     qc = QCon(ns, 10000)
 
-    print "ptu_external: {0}".format (ns)
+    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)
+    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)
@@ -313,9 +313,9 @@ def yaw (heading):
     ns = rospy.get_namespace ()
     qc = QCon(ns, 10000)
 
-    print "yaw: {0}".format (heading)
+    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)
+    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)
@@ -342,15 +342,15 @@ def yawrel (heading):
     ns = rospy.get_namespace ()
     qc = QCon(ns, 10000)
 
-    print "yawrel: heading: {0}".format (heading)
+    print("yawrel: heading: {0}".format (heading))
 
     resp = qc.get_current_state ()
-    print resp
+    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)
+    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)
@@ -380,9 +380,9 @@ def climb (alt):
     ns = rospy.get_namespace ()
     qc = QCon(ns, 10000)
 
-    print "climb: {0} {1}".format (ns, alt)
+    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)
+    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)
@@ -404,21 +404,21 @@ def climbrel (alt, speed):
     ns = rospy.get_namespace ()
     qc = QCon(ns, 10000)
 
-    print "climb: {0} {1}".format (ns, alt)
+    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)
+    if resp.status != 0: print("Failed to clear the climb queue"); sys.exit (1)
     
     resp = qc.get_current_state ()
-    print resp
+    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)
+    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)
 
@@ -426,25 +426,25 @@ 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)
+    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)
+    if resp.status != 0: print("Failed to clear the climb queue"); sys.exit (1)
 
     resp = qc.get_current_state ()
-    print resp
+    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
+    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)
+    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)
 
@@ -453,15 +453,15 @@ 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)
+    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)
+    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
+#    print(resp)
 
 #    state_north_m = resp.lat_m
 #    state_east_m = resp.lon_m
@@ -469,7 +469,7 @@ def fly_rel (forward, right, up, speed, yaw_flag):
 #    state_alt = resp.alt
 
     resp = qc.get_current_targets ()
-    print resp
+    print(resp)
 
 #float64 east_m
 #float64 north_m
@@ -488,12 +488,12 @@ def fly_rel (forward, right, up, speed, yaw_flag):
     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)
+    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)
+    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)
@@ -503,7 +503,7 @@ def fly_triangle (speed):
     qc = QCon(ns, 10000)
 
     resp = qc.get_current_state ()
-    print resp
+    print(resp)
 
     state_north_m = resp.lat_m
     state_east_m = resp.lon_m
@@ -511,7 +511,7 @@ def fly_triangle (speed):
 
     tokens = qc.fly_triangle_yaw_climb_pause (state_east_m, state_north_m, 
                                               state_alt, speed)
-    print tokens
+    print(tokens)
 
     if options.confirm:
         raw_input("---> READY. Press Enter to execute...")
@@ -570,7 +570,7 @@ if __name__ == "__main__":
     sim = QSim(ns)
 
     if options.limits:
-        print "IMPORTING: limits.py"
+        print("IMPORTING: limits.py")
         import limits
         max_altitude = limits.max_altitude
         min_altitude = limits.min_altitude
@@ -582,13 +582,13 @@ if __name__ == "__main__":
         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 "-------------------"
+    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("-------------------")
 
 
 
@@ -620,15 +620,15 @@ if __name__ == "__main__":
 #            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.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)
+            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)
+            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)
+            print("Up {0} is outside limits: <{1},{2}>.".format (options.z, min_up, max_up));sys.exit (1)
 
     if options.manual:
         sim.to_manual ()
@@ -638,7 +638,7 @@ if __name__ == "__main__":
 
 
     if options.logfile != "":
-        print "ENABLE LOGGING"
+        print("ENABLE LOGGING")
         sim.start_logging (options.logfile + ".lgdat")
 
     
@@ -649,10 +649,10 @@ if __name__ == "__main__":
         sim.to_auto ()
 
 ####### VICON
-#    print "Setting UAV state source: {0:1d} ".format(options.state)    
+#    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)
+#    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),
@@ -685,7 +685,7 @@ if __name__ == "__main__":
 
     if options.getref:
         rdata = sim.get_ref_data ()
-        print rdata
+        print(rdata)
 
 
 #