pps_ws/src/d_fall_pps/crazyradio/examples/flowsequenceSync.py → dfall_ws/src/dfall_pkg/crazyradio/examples/flowsequenceSync.py

@@ -34,9 +34,10 @@ import logging
 import time
 
 import cflib.crtp
+from cflib.crazyflie import Crazyflie
 from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
 
-URI = 'radio://0/80/250k'
+URI = 'radio://0/80/250K'
 
 # Only output errors from the logging framework
 logging.basicConfig(level=logging.ERROR)
@@ -46,7 +47,7 @@ if __name__ == '__main__':
     # Initialize the low-level drivers (don't list the debug drivers)
     cflib.crtp.init_drivers(enable_debug_driver=False)
 
-    with SyncCrazyflie(URI) as scf:
+    with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf:
         cf = scf.cf
 
         cf.param.set_value('kalman.resetEstimation', '1')

dfall_ws/src/dfall_pkg/crazyradio/examples/lighthouse/lighthouse_openvr_grab.py

+#!/usr/bin/env python3
+# Demo that makes one Crazyflie take off 30cm above the first controller found
+# Using the controller trigger it is then possible to 'grab' the Crazyflie
+# and to make it move.
+import sys
+import time
+
+import openvr
+
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.log import LogConfig
+from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
+from cflib.crazyflie.syncLogger import SyncLogger
+
+# URI to the Crazyflie to connect to
+uri = 'radio://0/80/2M'
+
+print('Opening')
+vr = openvr.init(openvr.VRApplication_Other)
+print('Opened')
+
+# Find first controller or tracker
+controllerId = None
+poses = vr.getDeviceToAbsoluteTrackingPose(openvr.TrackingUniverseStanding, 0,
+                                           openvr.k_unMaxTrackedDeviceCount)
+for i in range(openvr.k_unMaxTrackedDeviceCount):
+    if poses[i].bPoseIsValid:
+        device_class = vr.getTrackedDeviceClass(i)
+        if device_class == openvr.TrackedDeviceClass_Controller or \
+           device_class == openvr.TrackedDeviceClass_GenericTracker:
+            controllerId = i
+            break
+
+if controllerId is None:
+    print('Cannot find controller or tracker, exiting')
+    sys.exit(1)
+
+
+def wait_for_position_estimator(scf):
+    print('Waiting for estimator to find position...')
+
+    log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
+    log_config.add_variable('kalman.varPX', 'float')
+    log_config.add_variable('kalman.varPY', 'float')
+    log_config.add_variable('kalman.varPZ', 'float')
+
+    var_y_history = [1000] * 10
+    var_x_history = [1000] * 10
+    var_z_history = [1000] * 10
+
+    threshold = 0.001
+
+    with SyncLogger(scf, log_config) as logger:
+        for log_entry in logger:
+            data = log_entry[1]
+
+            var_x_history.append(data['kalman.varPX'])
+            var_x_history.pop(0)
+            var_y_history.append(data['kalman.varPY'])
+            var_y_history.pop(0)
+            var_z_history.append(data['kalman.varPZ'])
+            var_z_history.pop(0)
+
+            min_x = min(var_x_history)
+            max_x = max(var_x_history)
+            min_y = min(var_y_history)
+            max_y = max(var_y_history)
+            min_z = min(var_z_history)
+            max_z = max(var_z_history)
+
+            # print("{} {} {}".
+            #       format(max_x - min_x, max_y - min_y, max_z - min_z))
+
+            if (max_x - min_x) < threshold and (
+                    max_y - min_y) < threshold and (
+                    max_z - min_z) < threshold:
+                break
+
+
+def reset_estimator(scf):
+    cf = scf.cf
+    cf.param.set_value('kalman.resetEstimation', '1')
+    time.sleep(0.1)
+    cf.param.set_value('kalman.resetEstimation', '0')
+
+    wait_for_position_estimator(cf)
+
+
+def position_callback(timestamp, data, logconf):
+    x = data['kalman.stateX']
+    y = data['kalman.stateY']
+    z = data['kalman.stateZ']
+    print('pos: ({}, {}, {})'.format(x, y, z))
+
+
+def start_position_printing(scf):
+    log_conf = LogConfig(name='Position', period_in_ms=500)
+    log_conf.add_variable('kalman.stateX', 'float')
+    log_conf.add_variable('kalman.stateY', 'float')
+    log_conf.add_variable('kalman.stateZ', 'float')
+
+    scf.cf.log.add_config(log_conf)
+    log_conf.data_received_cb.add_callback(position_callback)
+    log_conf.start()
+
+
+def vector_substract(v0, v1):
+    return [v0[0] - v1[0], v0[1] - v1[1], v0[2] - v1[2]]
+
+
+def vector_add(v0, v1):
+    return [v0[0] + v1[0], v0[1] + v1[1], v0[2] + v1[2]]
+
+
+def run_sequence(scf):
+    cf = scf.cf
+
+    poses = vr.getDeviceToAbsoluteTrackingPose(
+        openvr.TrackingUniverseStanding, 0, openvr.k_unMaxTrackedDeviceCount)
+    controller_pose = poses[controllerId]
+    pose = controller_pose.mDeviceToAbsoluteTracking
+    setpoint = [-1*pose[2][3], -1*pose[0][3], pose[1][3] + 0.3]
+
+    grabbed = False
+    grab_controller_start = [0, 0, 0]
+    grab_setpoint_start = [0, 0, 0]
+
+    while True:
+        poses = vr.getDeviceToAbsoluteTrackingPose(
+            openvr.TrackingUniverseStanding, 0,
+            openvr.k_unMaxTrackedDeviceCount)
+        controller_state = vr.getControllerState(controllerId)[1]
+
+        trigger = ((controller_state.ulButtonPressed & 0x200000000) != 0)
+
+        controller_pose = poses[controllerId]
+        pose = controller_pose.mDeviceToAbsoluteTracking
+
+        if not grabbed and trigger:
+            print('Grab started')
+            grab_controller_start = [-1*pose[2][3], -1*pose[0][3], pose[1][3]]
+            grab_setpoint_start = setpoint
+
+        if grabbed and not trigger:
+            print('Grab ended')
+
+        grabbed = trigger
+
+        if trigger:
+            curr = [-1*pose[2][3], -1*pose[0][3], pose[1][3]]
+            setpoint = vector_add(grab_setpoint_start,
+                                  vector_substract(curr,
+                                                   grab_controller_start))
+
+        cf.commander.send_position_setpoint(setpoint[0],
+                                            setpoint[1],
+                                            setpoint[2],
+                                            0)
+        time.sleep(0.02)
+
+    cf.commander.send_setpoint
+    (0, 0, 0, 0)
+    # Make sure that the last packet leaves before the link is closed
+    # since the message queue is not flushed before closing
+    time.sleep(0.1)
+
+
+if __name__ == '__main__':
+    cflib.crtp.init_drivers(enable_debug_driver=False)
+
+    with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
+        reset_estimator(scf)
+        run_sequence(scf)
+
+    openvr.shutdown()

dfall_ws/src/dfall_pkg/crazyradio/examples/lighthouse/lighthouse_openvr_grab_color.py

+#!/usr/bin/env python3
+# Demo that makes one Crazyflie take off 30cm above the first controller found
+# Using the controller trigger it is then possible to 'grab' the Crazyflie
+# and to make it move.
+# If the Crazyflie has a ledring attached, the touchpad of the controller can
+# be used to change the color of the led-ring LEDs
+import colorsys
+import math
+import sys
+import time
+
+import openvr
+
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.log import LogConfig
+from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
+from cflib.crazyflie.syncLogger import SyncLogger
+
+# URI to the Crazyflie to connect to
+uri = 'radio://0/80/2M'
+
+print('Openning')
+vr = openvr.init(openvr.VRApplication_Other)
+print('Oppened')
+
+# Find first controller or tracker
+controllerId = None
+poses = vr.getDeviceToAbsoluteTrackingPose(openvr.TrackingUniverseStanding, 0,
+                                           openvr.k_unMaxTrackedDeviceCount)
+for i in range(openvr.k_unMaxTrackedDeviceCount):
+    if poses[i].bPoseIsValid:
+        device_class = vr.getTrackedDeviceClass(i)
+        if device_class == openvr.TrackedDeviceClass_Controller or\
+           device_class == openvr.TrackedDeviceClass_GenericTracker:
+            controllerId = i
+            break
+
+if controllerId is None:
+    print('Cannot find controller or tracker, exiting')
+    sys.exit(1)
+
+
+def wait_for_position_estimator(scf):
+    print('Waiting for estimator to find position...')
+
+    log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
+    log_config.add_variable('kalman.varPX', 'float')
+    log_config.add_variable('kalman.varPY', 'float')
+    log_config.add_variable('kalman.varPZ', 'float')
+
+    var_y_history = [1000] * 10
+    var_x_history = [1000] * 10
+    var_z_history = [1000] * 10
+
+    threshold = 0.001
+
+    with SyncLogger(scf, log_config) as logger:
+        for log_entry in logger:
+            data = log_entry[1]
+
+            var_x_history.append(data['kalman.varPX'])
+            var_x_history.pop(0)
+            var_y_history.append(data['kalman.varPY'])
+            var_y_history.pop(0)
+            var_z_history.append(data['kalman.varPZ'])
+            var_z_history.pop(0)
+
+            min_x = min(var_x_history)
+            max_x = max(var_x_history)
+            min_y = min(var_y_history)
+            max_y = max(var_y_history)
+            min_z = min(var_z_history)
+            max_z = max(var_z_history)
+
+            # print('{} {} {}'.
+            #       format(max_x - min_x, max_y - min_y, max_z - min_z))
+
+            if (max_x - min_x) < threshold and (
+                    max_y - min_y) < threshold and (
+                    max_z - min_z) < threshold:
+                break
+
+
+def reset_estimator(scf):
+    cf = scf.cf
+    cf.param.set_value('kalman.resetEstimation', '1')
+    time.sleep(0.1)
+    cf.param.set_value('kalman.resetEstimation', '0')
+
+    wait_for_position_estimator(cf)
+
+
+def position_callback(timestamp, data, logconf):
+    x = data['kalman.stateX']
+    y = data['kalman.stateY']
+    z = data['kalman.stateZ']
+    print('pos: ({}, {}, {})'.format(x, y, z))
+
+
+def start_position_printing(scf):
+    log_conf = LogConfig(name='Position', period_in_ms=500)
+    log_conf.add_variable('kalman.stateX', 'float')
+    log_conf.add_variable('kalman.stateY', 'float')
+    log_conf.add_variable('kalman.stateZ', 'float')
+
+    scf.cf.log.add_config(log_conf)
+    log_conf.data_received_cb.add_callback(position_callback)
+    log_conf.start()
+
+
+def vector_substract(v0, v1):
+    return [v0[0] - v1[0], v0[1] - v1[1], v0[2] - v1[2]]
+
+
+def vector_add(v0, v1):
+    return [v0[0] + v1[0], v0[1] + v1[1], v0[2] + v1[2]]
+
+
+def run_sequence(scf):
+    cf = scf.cf
+
+    cf.param.set_value('ring.effect', '7')
+
+    poses = vr.getDeviceToAbsoluteTrackingPose(
+        openvr.TrackingUniverseStanding, 0, openvr.k_unMaxTrackedDeviceCount)
+
+    controller_pose = poses[controllerId]
+    pose = controller_pose.mDeviceToAbsoluteTracking
+    # setpoint = [-1*pose[2][3], -1*pose[0][3], pose[1][3] + 0.3]
+    setpoint = [0, 0, 0.5]
+
+    grabbed = False
+    grab_controller_start = [0, 0, 0]
+    grab_setpoint_start = [0, 0, 0]
+
+    while True:
+        poses = vr.getDeviceToAbsoluteTrackingPose(
+            openvr.TrackingUniverseStanding, 0,
+            openvr.k_unMaxTrackedDeviceCount)
+        controller_state = vr.getControllerState(controllerId)[1]
+
+        trigger = ((controller_state.ulButtonPressed & 0x200000000) != 0)
+        trackpad_touched = ((controller_state.ulButtonTouched & 0x100000000) != 0)  # noqa
+
+        trackpad = (0, 0)
+        for i in range(openvr.k_unControllerStateAxisCount):
+            axis_type = vr.getInt32TrackedDeviceProperty(
+                controllerId, openvr.Prop_Axis0Type_Int32 + i)[0]
+            if axis_type == openvr.k_eControllerAxis_TrackPad:
+                trackpad = (controller_state.rAxis[i].x,
+                            controller_state.rAxis[i].y)
+
+        controller_pose = poses[controllerId]
+        pose = controller_pose.mDeviceToAbsoluteTracking
+
+        if trackpad_touched:
+            angle = math.atan2(trackpad[1], trackpad[0])
+            hue = (angle + math.pi) / (2*math.pi)
+            rgb = colorsys.hsv_to_rgb(hue, 1, 0.3)
+
+            print(rgb)
+
+            cf.param.set_value('ring.solidRed',
+                               '{}'.format(int(rgb[0] * 255)))
+            cf.param.set_value('ring.solidGreen',
+                               '{}'.format(int(rgb[1] * 255)))
+            cf.param.set_value('ring.solidBlue',
+                               '{}'.format(int(rgb[2] * 255)))
+
+        if not grabbed and trigger:
+            print('Grab started')
+            grab_controller_start = [-1*pose[2][3], -1*pose[0][3], pose[1][3]]
+            grab_setpoint_start = setpoint
+
+        if grabbed and not trigger:
+            print('Grab ended')
+
+        grabbed = trigger
+
+        if trigger:
+            curr = [-1*pose[2][3], -1*pose[0][3], pose[1][3]]
+            setpoint = vector_add(grab_setpoint_start,
+                                  vector_substract(curr, grab_controller_start)
+                                  )
+
+        cf.commander.send_position_setpoint(setpoint[0],
+                                            setpoint[1],
+                                            setpoint[2],
+                                            0)
+        time.sleep(0.02)
+
+    cf.commander.send_setpoint
+    (0, 0, 0, 0)
+    # Make sure that the last packet leaves before the link is closed
+    # since the message queue is not flushed before closing
+    time.sleep(0.1)
+
+
+if __name__ == '__main__':
+    cflib.crtp.init_drivers(enable_debug_driver=False)
+
+    with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
+        reset_estimator(scf)
+        # start_position_printing(scf)
+        run_sequence(scf)
+
+    openvr.shutdown()

dfall_ws/src/dfall_pkg/crazyradio/examples/lighthouse/lighthouse_openvr_multigrab.py

+#!/usr/bin/env python3
+# Demo that makes two Crazyflie take off 30cm above the first controller found
+# Using the controller trigger it is then possible to 'grab' the closest
+# Crazyflie and to make it move.
+import math
+import sys
+import time
+
+import openvr
+
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.log import LogConfig
+from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
+from cflib.crazyflie.syncLogger import SyncLogger
+
+# URI to the Crazyflie to connect to
+uri0 = 'radio://0/80/2M'
+uri1 = 'radio://0/80/2M/E7E7E7E701'
+
+print('Opening')
+vr = openvr.init(openvr.VRApplication_Other)
+print('Opened')
+
+# Find first controller or tracker
+controllerId = None
+poses = vr.getDeviceToAbsoluteTrackingPose(openvr.TrackingUniverseStanding, 0,
+                                           openvr.k_unMaxTrackedDeviceCount)
+for i in range(openvr.k_unMaxTrackedDeviceCount):
+    if poses[i].bPoseIsValid:
+        device_class = vr.getTrackedDeviceClass(i)
+        if device_class == openvr.TrackedDeviceClass_Controller or \
+           device_class == openvr.TrackedDeviceClass_GenericTracker:
+            controllerId = i
+            break
+
+if controllerId is None:
+    print('Cannot find controller or tracker, exiting')
+    sys.exit(1)
+
+
+def wait_for_position_estimator(scf):
+    print('Waiting for estimator to find position...')
+
+    log_config = LogConfig(name='Kalman Variance', period_in_ms=500)
+    log_config.add_variable('kalman.varPX', 'float')
+    log_config.add_variable('kalman.varPY', 'float')
+    log_config.add_variable('kalman.varPZ', 'float')
+
+    var_y_history = [1000] * 10
+    var_x_history = [1000] * 10
+    var_z_history = [1000] * 10
+
+    threshold = 0.001
+
+    with SyncLogger(scf, log_config) as logger:
+        for log_entry in logger:
+            data = log_entry[1]
+
+            var_x_history.append(data['kalman.varPX'])
+            var_x_history.pop(0)
+            var_y_history.append(data['kalman.varPY'])
+            var_y_history.pop(0)
+            var_z_history.append(data['kalman.varPZ'])
+            var_z_history.pop(0)
+
+            min_x = min(var_x_history)
+            max_x = max(var_x_history)
+            min_y = min(var_y_history)
+            max_y = max(var_y_history)
+            min_z = min(var_z_history)
+            max_z = max(var_z_history)
+
+            # print("{} {} {}".
+            #       format(max_x - min_x, max_y - min_y, max_z - min_z))
+
+            if (max_x - min_x) < threshold and (
+                    max_y - min_y) < threshold and (
+                    max_z - min_z) < threshold:
+                break
+
+
+def reset_estimator(scf):
+    cf = scf.cf
+    cf.param.set_value('kalman.resetEstimation', '1')
+    time.sleep(0.1)
+    cf.param.set_value('kalman.resetEstimation', '0')
+
+    wait_for_position_estimator(cf)
+
+
+def position_callback(timestamp, data, logconf):
+    x = data['kalman.stateX']
+    y = data['kalman.stateY']
+    z = data['kalman.stateZ']
+    print('pos: ({}, {}, {})'.format(x, y, z))
+
+
+def start_position_printing(scf):
+    log_conf = LogConfig(name='Position', period_in_ms=500)
+    log_conf.add_variable('kalman.stateX', 'float')
+    log_conf.add_variable('kalman.stateY', 'float')
+    log_conf.add_variable('kalman.stateZ', 'float')
+
+    scf.cf.log.add_config(log_conf)
+    log_conf.data_received_cb.add_callback(position_callback)
+    log_conf.start()
+
+
+def vector_substract(v0, v1):
+    return [v0[0] - v1[0], v0[1] - v1[1], v0[2] - v1[2]]
+
+
+def vector_add(v0, v1):
+    return [v0[0] + v1[0], v0[1] + v1[1], v0[2] + v1[2]]
+
+
+def vector_norm(v0):
+    return math.sqrt((v0[0] * v0[0]) + (v0[1] * v0[1]) + (v0[2] * v0[2]))
+
+
+def run_sequence(scf0, scf1):
+    cf0 = scf0.cf
+    cf1 = scf1.cf
+
+    poses = vr.getDeviceToAbsoluteTrackingPose(
+        openvr.TrackingUniverseStanding, 0, openvr.k_unMaxTrackedDeviceCount)
+    controller_pose = poses[controllerId]
+    pose = controller_pose.mDeviceToAbsoluteTracking
+    setpoints = [[-1*pose[2][3], -1*pose[0][3] - 0.5, pose[1][3] + 0.3],
+                 [-1*pose[2][3], -1*pose[0][3] + 0.5, pose[1][3] + 0.3]]
+
+    closest = 0
+
+    grabbed = False
+    grab_controller_start = [0, 0, 0]
+    grab_setpoint_start = [0, 0, 0]
+
+    while True:
+        poses = vr.getDeviceToAbsoluteTrackingPose(
+            openvr.TrackingUniverseStanding, 0,
+            openvr.k_unMaxTrackedDeviceCount)
+        controller_state = vr.getControllerState(controllerId)[1]
+
+        trigger = ((controller_state.ulButtonPressed & 0x200000000) != 0)
+
+        controller_pose = poses[controllerId]
+        pose = controller_pose.mDeviceToAbsoluteTracking
+
+        if not grabbed and trigger:
+            print('Grab started')
+            grab_controller_start = [-1*pose[2][3], -1*pose[0][3], pose[1][3]]
+
+            dist0 = vector_norm(vector_substract(grab_controller_start,
+                                                 setpoints[0]))
+            dist1 = vector_norm(vector_substract(grab_controller_start,
+                                                 setpoints[1]))
+
+            if dist0 < dist1:
+                closest = 0
+            else:
+                closest = 1
+
+            grab_setpoint_start = setpoints[closest]
+
+        if grabbed and not trigger:
+            print('Grab ended')
+
+        grabbed = trigger
+
+        if trigger:
+            curr = [-1*pose[2][3], -1*pose[0][3], pose[1][3]]
+            setpoints[closest] = vector_add(
+                grab_setpoint_start, vector_substract(curr,
+                                                      grab_controller_start))
+
+        cf0.commander.send_position_setpoint(setpoints[0][0],
+                                             setpoints[0][1],
+                                             setpoints[0][2],
+                                             0)
+        cf1.commander.send_position_setpoint(setpoints[1][0],
+                                             setpoints[1][1],
+                                             setpoints[1][2],
+                                             0)
+
+        time.sleep(0.02)
+
+    cf0.commander.send_setpoint(0, 0, 0, 0)
+    cf1.commander.send_setpoint(0, 0, 0, 0)
+    # Make sure that the last packet leaves before the link is closed
+    # since the message queue is not flushed before closing
+    time.sleep(0.1)
+
+
+if __name__ == '__main__':
+    cflib.crtp.init_drivers(enable_debug_driver=False)
+
+    with SyncCrazyflie(uri0, cf=Crazyflie(rw_cache='./cache')) as scf0:
+        reset_estimator(scf0)
+        with SyncCrazyflie(uri1, cf=Crazyflie(rw_cache='./cache')) as scf1:
+            reset_estimator(scf1)
+            run_sequence(scf0, scf1)
+
+    openvr.shutdown()

pps_ws/src/d_fall_pps/crazyradio/test/support/asyncCallbackCaller.py → dfall_ws/src/dfall_pkg/crazyradio/examples/lighthouse/read-geometry-mem.py

@@ -6,7 +6,7 @@
 #  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
 #   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
 #
-#  Copyright (C) 2016 Bitcraze AB
+#  Copyright (C) 2019 Bitcraze AB
 #
 #  This program is free software; you can redistribute it and/or
 #  modify it under the terms of the GNU General Public License
@@ -21,26 +21,49 @@
 #  along with this program; if not, write to the Free Software
 #  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
 #  MA  02110-1301, USA.
+"""
+Example of how to read the Lighthouse base station geometry memory from a
+Crazyflie
+"""
+import logging
 import time
-from threading import Thread
 
+import cflib.crtp  # noqa
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.mem import MemoryElement
+from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
+# Only output errors from the logging framework
 
-class AsyncCallbackCaller:
+logging.basicConfig(level=logging.ERROR)
 
-    def __init__(self, cb=None, delay=0, args=(), kwargs={}):
-        self.caller = cb
-        self.delay = delay
-        self.args = args
-        self.kwargs = kwargs
 
-    def trigger(self, *args, **kwargs):
-        Thread(target=self._make_call).start()
+class ReadMem:
+    def __init__(self, uri):
+        self.got_data = False
 
-    def call_and_wait(self):
-        thread = Thread(target=self._make_call)
-        thread.start()
-        thread.join()
+        with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
+            mems = scf.cf.mem.get_mems(MemoryElement.TYPE_LH)
 
-    def _make_call(self):
-        time.sleep(self.delay)
-        self.caller.call(*self.args, **self.kwargs)
+            count = len(mems)
+            if count != 1:
+                raise Exception('Unexpected nr of memories found:', count)
+
+            print('Rquesting data')
+            mems[0].update(self._data_updated)
+
+            while not self.got_data:
+                time.sleep(1)
+
+    def _data_updated(self, mem):
+        mem.dump()
+        self.got_data = True
+
+
+if __name__ == '__main__':
+    # URI to the Crazyflie to connect to
+    uri = 'radio://0/80/2M'
+
+    # Initialize the low-level drivers (don't list the debug drivers)
+    cflib.crtp.init_drivers(enable_debug_driver=False)
+
+    ReadMem(uri)

pps_ws/src/d_fall_pps/crazyradio/build/lib/lpslib/lopoanchor.py → dfall_ws/src/dfall_pkg/crazyradio/examples/lighthouse/write-geometry-mem.py

@@ -6,7 +6,7 @@
 #  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
 #   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
 #
-#  Copyright (C) 2017 Bitcraze AB
+#  Copyright (C) 2019 Bitcraze AB
 #
 #  This program is free software; you can redistribute it and/or
 #  modify it under the terms of the GNU General Public License
@@ -22,38 +22,67 @@
 #  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
 #  MA  02110-1301, USA.
 """
-This class represents the connection to one or more Loco Positioning Anchors
+Example of how to write to the Lighthouse base station geometry memory in a
+Crazyflie
 """
-import struct
-
-
-class LoPoAnchor():
-    LPP_TYPE_POSITION = 1
-    LPP_TYPE_REBOOT = 2
-
-    REBOOT_TO_BOOTLOADER = 0
-    REBOOT_TO_FIRMWARE = 1
-
-    def __init__(self, crazyflie):
-        """
-        :param crazyflie: A crazyflie object to be used as a bridge to the LoPo
-         system."""
-        self.crazyflie = crazyflie
-
-    def set_position(self, anchor_id, position):
-        """
-        Send a packet with a position to one anchor.
-        :param anchor_id: The id of the targeted anchor. This is the first byte
-        of the anchor address.
-        :param position: The position of the anchor, as an array
-        """
-        x = position[0]
-        y = position[1]
-        z = position[2]
-        data = struct.pack('<Bfff', LoPoAnchor.LPP_TYPE_POSITION, x, y, z)
-
-        self.crazyflie.loc.send_short_lpp_packet(anchor_id, data)
-
-    def reboot(self, anchor_id, mode):
-        data = struct.pack('<BB', LoPoAnchor.LPP_TYPE_REBOOT, mode)
-        self.crazyflie.loc.send_short_lpp_packet(anchor_id, data)
+import logging
+import time
+
+import cflib.crtp  # noqa
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.mem import LighthouseBsGeometry
+from cflib.crazyflie.mem import MemoryElement
+from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
+# Only output errors from the logging framework
+
+logging.basicConfig(level=logging.ERROR)
+
+
+class WriteMem:
+    def __init__(self, uri, bs1, bs2):
+        self.data_written = False
+
+        with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
+            mems = scf.cf.mem.get_mems(MemoryElement.TYPE_LH)
+
+            count = len(mems)
+            if count != 1:
+                raise Exception('Unexpected nr of memories found:', count)
+
+            mems[0].geometry_data = [bs1, bs2]
+
+            print('Writing data')
+            mems[0].write_data(self._data_written)
+
+            while not self.data_written:
+                time.sleep(1)
+
+    def _data_written(self, mem, addr):
+        self.data_written = True
+        print('Data written')
+
+
+if __name__ == '__main__':
+    # URI to the Crazyflie to connect to
+    uri = 'radio://0/80/2M'
+
+    # Initialize the low-level drivers (don't list the debug drivers)
+    cflib.crtp.init_drivers(enable_debug_driver=False)
+
+    bs1 = LighthouseBsGeometry()
+    bs1.origin = [1.0, 2.0, 3.0]
+    bs1.rotation_matrix = [
+        [4.0, 5.0, 6.0],
+        [7.0, 8.0, 9.0],
+        [10.0, 11.0, 12.0],
+    ]
+
+    bs2 = LighthouseBsGeometry()
+    bs2.origin = [21.0, 22.0, 23.0]
+    bs2.rotation_matrix = [
+        [24.0, 25.0, 26.0],
+        [27.0, 28.0, 29.0],
+        [30.0, 31.0, 32.0],
+    ]
+
+    WriteMem(uri, bs1, bs2)

pps_ws/src/d_fall_pps/crazyradio/build/lib/cflib/crazyflie/syncCrazyflie.py → dfall_ws/src/dfall_pkg/crazyradio/examples/motion_commander_demo.py

@@ -6,7 +6,7 @@
 #  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
 #   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
 #
-#  Copyright (C) 2016 Bitcraze AB
+#  Copyright (C) 2017 Bitcraze AB
 #
 #  Crazyflie Nano Quadcopter Client
 #
@@ -24,74 +24,77 @@
 #  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
 #  MA  02110-1301, USA.
 """
-The syncronous Crazyflie class is a wrapper around the "normal" Crazyflie
-class. It handles the asynchronous nature of the Crazyflie API and turns it
-into blocking function. It is useful for simple scripts that performs tasks
-as a sequence of events.
+This script shows the basic use of the MotionCommander class.
+
+Simple example that connects to the crazyflie at `URI` and runs a
+sequence. This script requires some kind of location system, it has been
+tested with (and designed for) the flow deck.
+
+The MotionCommander uses velocity setpoints.
+
+Change the URI variable to your Crazyflie configuration.
 """
-from threading import Event
+import logging
+import time
 
+import cflib.crtp
 from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
+from cflib.positioning.motion_commander import MotionCommander
 
+URI = 'radio://0/70/2M'
 
-class SyncCrazyflie:
+# Only output errors from the logging framework
+logging.basicConfig(level=logging.ERROR)
 
-    def __init__(self, link_uri, cf=None):
-        """ Create a synchronous Crazyflie instance with the specified
-        link_uri """
 
-        if cf:
-            self.cf = cf
-        else:
-            self.cf = Crazyflie()
+if __name__ == '__main__':
+    # Initialize the low-level drivers (don't list the debug drivers)
+    cflib.crtp.init_drivers(enable_debug_driver=False)
 
-        self._link_uri = link_uri
-        self._connect_event = Event()
-        self._is_link_open = False
-        self._error_message = None
+    with SyncCrazyflie(URI, cf=Crazyflie(rw_cache='./cache')) as scf:
+        # We take off when the commander is created
+        with MotionCommander(scf) as mc:
+            time.sleep(1)
 
-        self.cf.connected.add_callback(self._connected)
-        self.cf.connection_failed.add_callback(self._connection_failed)
-        self.cf.disconnected.add_callback(self._disconnected)
+            # There is a set of functions that move a specific distance
+            # We can move in all directions
+            mc.forward(0.8)
+            mc.back(0.8)
+            time.sleep(1)
 
-    def open_link(self):
-        if (self.is_link_open()):
-            raise Exception('Link already open')
+            mc.up(0.5)
+            mc.down(0.5)
+            time.sleep(1)
 
-        print('Connecting to %s' % self._link_uri)
-        self.cf.open_link(self._link_uri)
-        self._connect_event.wait()
-        if not self._is_link_open:
-            raise Exception(self._error_message)
+            # We can also set the velocity
+            mc.right(0.5, velocity=0.8)
+            time.sleep(1)
+            mc.left(0.5, velocity=0.4)
+            time.sleep(1)
 
-    def __enter__(self):
-        self.open_link()
-        return self
+            # We can do circles or parts of circles
+            mc.circle_right(0.5, velocity=0.5, angle_degrees=180)
 
-    def close_link(self):
-        self.cf.close_link()
-        self._is_link_open = False
+            # Or turn
+            mc.turn_left(90)
+            time.sleep(1)
 
-    def __exit__(self, exc_type, exc_val, exc_tb):
-        self.close_link()
+            # We can move along a line in 3D space
+            mc.move_distance(-1, 0.0, 0.5, velocity=0.6)
+            time.sleep(1)
 
-    def is_link_open(self):
-        return self._is_link_open
+            # There is also a set of functions that start a motion. The
+            # Crazyflie will keep on going until it gets a new command.
 
-    def _connected(self, link_uri):
-        """ This callback is called form the Crazyflie API when a Crazyflie
-        has been connected and the TOCs have been downloaded."""
-        print('Connected to %s' % link_uri)
-        self._is_link_open = True
-        self._connect_event.set()
+            mc.start_left(velocity=0.5)
+            # The motion is started and we can do other stuff, printing for
+            # instance
+            for _ in range(5):
+                print('Doing other work')
+                time.sleep(0.2)
 
-    def _connection_failed(self, link_uri, msg):
-        """Callback when connection initial connection fails (i.e no Crazyflie
-        at the specified address)"""
-        print('Connection to %s failed: %s' % (link_uri, msg))
-        self._is_link_open = False
-        self._error_message = msg
-        self._connect_event.set()
+            # And we can stop
+            mc.stop()
 
-    def _disconnected(self, link_uri):
-        self._is_link_open = False
+            # We land when the MotionCommander goes out of scope

pps_ws/src/d_fall_pps/crazyradio/examples/multiramp.py → dfall_ws/src/dfall_pkg/crazyradio/examples/multiramp.py

@@ -44,7 +44,7 @@ class MotorRampExample:
     def __init__(self, link_uri):
         """ Initialize and run the example with the specified link_uri """
 
-        self._cf = Crazyflie()
+        self._cf = Crazyflie(rw_cache='./cache')
 
         self._cf.connected.add_callback(self._connected)
         self._cf.disconnected.add_callback(self._disconnected)

dfall_ws/src/dfall_pkg/crazyradio/examples/multiranger_pointcloud.py

+#!/usr/bin/env python2
+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __
+#  +------+      / __ )(_) /_______________ _____  ___
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2018 Bitcraze AB
+#
+#  Crazyflie Python Library
+#
+#  This program is free software; you can redistribute it and/or
+#  modify it under the terms of the GNU General Public License
+#  as published by the Free Software Foundation; either version 2
+#  of the License, or (at your option) any later version.
+#
+#  This program is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#  GNU General Public License for more details.
+#  You should have received a copy of the GNU General Public License
+#  along with this program; if not, write to the Free Software
+#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+#  MA  02110-1301, USA.
+"""
+Example script that plots the output ranges from the Multiranger and Flow
+deck in a 3D plot.
+
+When the application is started the Crazyflie will hover at 0.3 m. The
+Crazyflie can then be controlled by using keyboard input:
+ * Move by using the arrow keys (left/right/forward/backwards)
+ * Adjust the right with w/s (0.1 m for each keypress)
+ * Yaw slowly using a/d (CCW/CW)
+ * Yaw fast using z/x (CCW/CW)
+
+There's additional setting for (see constants below):
+ * Plotting the downwards sensor
+ * Plotting the estimated Crazyflie postition
+ * Max threashold for sensors
+ * Speed factor that set's how fast the Crazyflie moves
+
+The demo is ended by either closing the graph window.
+
+For the example to run the following hardware is needed:
+ * Crazyflie 2.0
+ * Crazyradio PA
+ * Flow deck
+ * Multiranger deck
+"""
+import logging
+import math
+import sys
+
+import numpy as np
+from vispy import scene
+from vispy.scene import visuals
+from vispy.scene.cameras import TurntableCamera
+
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.log import LogConfig
+
+try:
+    from sip import setapi
+    setapi('QVariant', 2)
+    setapi('QString', 2)
+except ImportError:
+    pass
+
+from PyQt5 import QtCore, QtWidgets
+
+logging.basicConfig(level=logging.INFO)
+
+URI = 'radio://0/80/2M'
+
+if len(sys.argv) > 1:
+    URI = sys.argv[1]
+
+# Enable plotting of Crazyflie
+PLOT_CF = False
+# Enable plotting of down sensor
+PLOT_SENSOR_DOWN = False
+# Set the sensor threashold (in mm)
+SENSOR_TH = 2000
+# Set the speed factor for moving and rotating
+SPEED_FACTOR = 0.3
+
+
+class MainWindow(QtWidgets.QMainWindow):
+
+    def __init__(self, URI):
+        QtWidgets.QMainWindow.__init__(self)
+
+        self.resize(700, 500)
+        self.setWindowTitle('Multi-ranger point cloud')
+
+        self.canvas = Canvas(self.updateHover)
+        self.canvas.create_native()
+        self.canvas.native.setParent(self)
+
+        self.setCentralWidget(self.canvas.native)
+
+        cflib.crtp.init_drivers(enable_debug_driver=False)
+        self.cf = Crazyflie(ro_cache=None, rw_cache='cache')
+
+        # Connect callbacks from the Crazyflie API
+        self.cf.connected.add_callback(self.connected)
+        self.cf.disconnected.add_callback(self.disconnected)
+
+        # Connect to the Crazyflie
+        self.cf.open_link(URI)
+
+        self.hover = {'x': 0.0, 'y': 0.0, 'z': 0.0, 'yaw': 0.0, 'height': 0.3}
+
+        self.hoverTimer = QtCore.QTimer()
+        self.hoverTimer.timeout.connect(self.sendHoverCommand)
+        self.hoverTimer.setInterval(100)
+        self.hoverTimer.start()
+
+    def sendHoverCommand(self):
+        self.cf.commander.send_hover_setpoint(
+            self.hover['x'], self.hover['y'], self.hover['yaw'],
+            self.hover['height'])
+
+    def updateHover(self, k, v):
+        if (k != 'height'):
+            self.hover[k] = v * SPEED_FACTOR
+        else:
+            self.hover[k] += v
+
+    def disconnected(self, URI):
+        print('Disconnected')
+
+    def connected(self, URI):
+        print('We are now connected to {}'.format(URI))
+
+        # The definition of the logconfig can be made before connecting
+        lpos = LogConfig(name='Position', period_in_ms=100)
+        lpos.add_variable('stateEstimate.x')
+        lpos.add_variable('stateEstimate.y')
+        lpos.add_variable('stateEstimate.z')
+
+        try:
+            self.cf.log.add_config(lpos)
+            lpos.data_received_cb.add_callback(self.pos_data)
+            lpos.start()
+        except KeyError as e:
+            print('Could not start log configuration,'
+                  '{} not found in TOC'.format(str(e)))
+        except AttributeError:
+            print('Could not add Position log config, bad configuration.')
+
+        lmeas = LogConfig(name='Meas', period_in_ms=100)
+        lmeas.add_variable('range.front')
+        lmeas.add_variable('range.back')
+        lmeas.add_variable('range.up')
+        lmeas.add_variable('range.left')
+        lmeas.add_variable('range.right')
+        lmeas.add_variable('range.zrange')
+        lmeas.add_variable('stabilizer.roll')
+        lmeas.add_variable('stabilizer.pitch')
+        lmeas.add_variable('stabilizer.yaw')
+
+        try:
+            self.cf.log.add_config(lmeas)
+            lmeas.data_received_cb.add_callback(self.meas_data)
+            lmeas.start()
+        except KeyError as e:
+            print('Could not start log configuration,'
+                  '{} not found in TOC'.format(str(e)))
+        except AttributeError:
+            print('Could not add Measurement log config, bad configuration.')
+
+    def pos_data(self, timestamp, data, logconf):
+        position = [
+            data['stateEstimate.x'],
+            data['stateEstimate.y'],
+            data['stateEstimate.z']
+        ]
+        self.canvas.set_position(position)
+
+    def meas_data(self, timestamp, data, logconf):
+        measurement = {
+            'roll': data['stabilizer.roll'],
+            'pitch': data['stabilizer.pitch'],
+            'yaw': data['stabilizer.yaw'],
+            'front': data['range.front'],
+            'back': data['range.back'],
+            'up': data['range.up'],
+            'down': data['range.zrange'],
+            'left': data['range.left'],
+            'right': data['range.right']
+        }
+        self.canvas.set_measurement(measurement)
+
+    def closeEvent(self, event):
+        if (self.cf is not None):
+            self.cf.close_link()
+
+
+class Canvas(scene.SceneCanvas):
+    def __init__(self, keyupdateCB):
+        scene.SceneCanvas.__init__(self, keys=None)
+        self.size = 800, 600
+        self.unfreeze()
+        self.view = self.central_widget.add_view()
+        self.view.bgcolor = '#ffffff'
+        self.view.camera = TurntableCamera(
+            fov=10.0, distance=30.0, up='+z', center=(0.0, 0.0, 0.0))
+        self.last_pos = [0, 0, 0]
+        self.pos_markers = visuals.Markers()
+        self.meas_markers = visuals.Markers()
+        self.pos_data = np.array([0, 0, 0], ndmin=2)
+        self.meas_data = np.array([0, 0, 0], ndmin=2)
+        self.lines = []
+
+        self.view.add(self.pos_markers)
+        self.view.add(self.meas_markers)
+        for i in range(6):
+            line = visuals.Line()
+            self.lines.append(line)
+            self.view.add(line)
+
+        self.keyCB = keyupdateCB
+
+        self.freeze()
+
+        scene.visuals.XYZAxis(parent=self.view.scene)
+
+    def on_key_press(self, event):
+        if (not event.native.isAutoRepeat()):
+            if (event.native.key() == QtCore.Qt.Key_Left):
+                self.keyCB('y', 1)
+            if (event.native.key() == QtCore.Qt.Key_Right):
+                self.keyCB('y', -1)
+            if (event.native.key() == QtCore.Qt.Key_Up):
+                self.keyCB('x', 1)
+            if (event.native.key() == QtCore.Qt.Key_Down):
+                self.keyCB('x', -1)
+            if (event.native.key() == QtCore.Qt.Key_A):
+                self.keyCB('yaw', -70)
+            if (event.native.key() == QtCore.Qt.Key_D):
+                self.keyCB('yaw', 70)
+            if (event.native.key() == QtCore.Qt.Key_Z):
+                self.keyCB('yaw', -200)
+            if (event.native.key() == QtCore.Qt.Key_X):
+                self.keyCB('yaw', 200)
+            if (event.native.key() == QtCore.Qt.Key_W):
+                self.keyCB('height', 0.1)
+            if (event.native.key() == QtCore.Qt.Key_S):
+                self.keyCB('height', -0.1)
+
+    def on_key_release(self, event):
+        if (not event.native.isAutoRepeat()):
+            if (event.native.key() == QtCore.Qt.Key_Left):
+                self.keyCB('y', 0)
+            if (event.native.key() == QtCore.Qt.Key_Right):
+                self.keyCB('y', 0)
+            if (event.native.key() == QtCore.Qt.Key_Up):
+                self.keyCB('x', 0)
+            if (event.native.key() == QtCore.Qt.Key_Down):
+                self.keyCB('x', 0)
+            if (event.native.key() == QtCore.Qt.Key_A):
+                self.keyCB('yaw', 0)
+            if (event.native.key() == QtCore.Qt.Key_D):
+                self.keyCB('yaw', 0)
+            if (event.native.key() == QtCore.Qt.Key_W):
+                self.keyCB('height', 0)
+            if (event.native.key() == QtCore.Qt.Key_S):
+                self.keyCB('height', 0)
+            if (event.native.key() == QtCore.Qt.Key_Z):
+                self.keyCB('yaw', 0)
+            if (event.native.key() == QtCore.Qt.Key_X):
+                self.keyCB('yaw', 0)
+
+    def set_position(self, pos):
+        self.last_pos = pos
+        if (PLOT_CF):
+            self.pos_data = np.append(self.pos_data, [pos], axis=0)
+            self.pos_markers.set_data(self.pos_data, face_color='red', size=5)
+
+    def rot(self, roll, pitch, yaw, origin, point):
+        cosr = math.cos(math.radians(roll))
+        cosp = math.cos(math.radians(pitch))
+        cosy = math.cos(math.radians(yaw))
+
+        sinr = math.sin(math.radians(roll))
+        sinp = math.sin(math.radians(pitch))
+        siny = math.sin(math.radians(yaw))
+
+        roty = np.array([[cosy, -siny, 0],
+                         [siny, cosy, 0],
+                         [0, 0,    1]])
+
+        rotp = np.array([[cosp, 0, sinp],
+                         [0, 1, 0],
+                         [-sinp, 0, cosp]])
+
+        rotr = np.array([[1, 0,   0],
+                         [0, cosr, -sinr],
+                         [0, sinr,  cosr]])
+
+        rotFirst = np.dot(rotr, rotp)
+
+        rot = np.array(np.dot(rotFirst, roty))
+
+        tmp = np.subtract(point, origin)
+        tmp2 = np.dot(rot, tmp)
+        return np.add(tmp2, origin)
+
+    def rotate_and_create_points(self, m):
+        data = []
+        o = self.last_pos
+        roll = m['roll']
+        pitch = -m['pitch']
+        yaw = m['yaw']
+
+        if (m['up'] < SENSOR_TH):
+            up = [o[0], o[1], o[2] + m['up'] / 1000.0]
+            data.append(self.rot(roll, pitch, yaw, o, up))
+
+        if (m['down'] < SENSOR_TH and PLOT_SENSOR_DOWN):
+            down = [o[0], o[1], o[2] - m['down'] / 1000.0]
+            data.append(self.rot(roll, pitch, yaw, o, down))
+
+        if (m['left'] < SENSOR_TH):
+            left = [o[0], o[1] + m['left'] / 1000.0, o[2]]
+            data.append(self.rot(roll, pitch, yaw, o, left))
+
+        if (m['right'] < SENSOR_TH):
+            right = [o[0], o[1] - m['right'] / 1000.0, o[2]]
+            data.append(self.rot(roll, pitch, yaw, o, right))
+
+        if (m['front'] < SENSOR_TH):
+            front = [o[0] + m['front'] / 1000.0, o[1], o[2]]
+            data.append(self.rot(roll, pitch, yaw, o, front))
+
+        if (m['back'] < SENSOR_TH):
+            back = [o[0] - m['back'] / 1000.0, o[1], o[2]]
+            data.append(self.rot(roll, pitch, yaw, o, back))
+
+        return data
+
+    def set_measurement(self, measurements):
+        data = self.rotate_and_create_points(measurements)
+        o = self.last_pos
+        for i in range(6):
+            if (i < len(data)):
+                o = self.last_pos
+                self.lines[i].set_data(np.array([o, data[i]]))
+            else:
+                self.lines[i].set_data(np.array([o, o]))
+
+        if (len(data) > 0):
+            self.meas_data = np.append(self.meas_data, data, axis=0)
+        self.meas_markers.set_data(self.meas_data, face_color='blue', size=5)
+
+
+if __name__ == '__main__':
+    appQt = QtWidgets.QApplication(sys.argv)
+    win = MainWindow(URI)
+    win.show()
+    appQt.exec_()

dfall_ws/src/dfall_pkg/crazyradio/examples/multiranger_push.py

+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __
+#  +------+      / __ )(_) /_______________ _____  ___
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2017 Bitcraze AB
+#
+#  Crazyflie Python Library
+#
+#  This program is free software; you can redistribute it and/or
+#  modify it under the terms of the GNU General Public License
+#  as published by the Free Software Foundation; either version 2
+#  of the License, or (at your option) any later version.
+#
+#  This program is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#  GNU General Public License for more details.
+#  You should have received a copy of the GNU General Public License
+#  along with this program; if not, write to the Free Software
+#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+#  MA  02110-1301, USA.
+"""
+Example scipts that allows a user to "push" the Crazyflie 2.0 around
+using your hands while it's hovering.
+
+This examples uses the Flow and Multi-ranger decks to measure distances
+in all directions and tries to keep away from anything that comes closer
+than 0.2m by setting a velocity in the opposite direction.
+
+The demo is ended by either pressing Ctrl-C or by holding your hand above the
+Crazyflie.
+
+For the example to run the following hardware is needed:
+ * Crazyflie 2.0
+ * Crazyradio PA
+ * Flow deck
+ * Multiranger deck
+"""
+import logging
+import sys
+import time
+
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
+from cflib.positioning.motion_commander import MotionCommander
+from cflib.utils.multiranger import Multiranger
+
+URI = 'radio://0/80/2M'
+
+if len(sys.argv) > 1:
+    URI = sys.argv[1]
+
+# Only output errors from the logging framework
+logging.basicConfig(level=logging.ERROR)
+
+
+def is_close(range):
+    MIN_DISTANCE = 0.2  # m
+
+    if range is None:
+        return False
+    else:
+        return range < MIN_DISTANCE
+
+
+if __name__ == '__main__':
+    # Initialize the low-level drivers (don't list the debug drivers)
+    cflib.crtp.init_drivers(enable_debug_driver=False)
+
+    cf = Crazyflie(rw_cache='./cache')
+    with SyncCrazyflie(URI, cf=cf) as scf:
+        with MotionCommander(scf) as motion_commander:
+            with Multiranger(scf) as multiranger:
+                keep_flying = True
+
+                while keep_flying:
+                    VELOCITY = 0.5
+                    velocity_x = 0.0
+                    velocity_y = 0.0
+
+                    if is_close(multiranger.front):
+                        velocity_x -= VELOCITY
+                    if is_close(multiranger.back):
+                        velocity_x += VELOCITY
+
+                    if is_close(multiranger.left):
+                        velocity_y -= VELOCITY
+                    if is_close(multiranger.right):
+                        velocity_y += VELOCITY
+
+                    if is_close(multiranger.up):
+                        keep_flying = False
+
+                    motion_commander.start_linear_motion(
+                        velocity_x, velocity_y, 0)
+
+                    time.sleep(0.1)
+
+            print('Demo terminated!')

dfall_ws/src/dfall_pkg/crazyradio/examples/position_commander_demo.py

+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __
+#  +------+      / __ )(_) /_______________ _____  ___
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2018 Bitcraze AB
+#
+#  This program is free software; you can redistribute it and/or
+#  modify it under the terms of the GNU General Public License
+#  as published by the Free Software Foundation; either version 2
+#  of the License, or (at your option) any later version.
+#
+#  This program is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#  GNU General Public License for more details.
+#  You should have received a copy of the GNU General Public License
+#  along with this program; if not, write to the Free Software
+#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+#  MA  02110-1301, USA.
+"""
+This script shows the basic use of the PositionHlCommander class.
+
+Simple example that connects to the crazyflie at `URI` and runs a
+sequence. This script requires some kind of location system.
+
+The PositionHlCommander uses position setpoints.
+
+Change the URI variable to your Crazyflie configuration.
+"""
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
+from cflib.positioning.position_hl_commander import PositionHlCommander
+
+# URI to the Crazyflie to connect to
+uri = 'radio://0/80/2M/E7E7E7E7E7'
+
+
+def slightly_more_complex_usage():
+    with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
+        with PositionHlCommander(
+                scf,
+                x=0.0, y=0.0, z=0.0,
+                default_velocity=0.3,
+                default_height=0.5,
+                controller=PositionHlCommander.CONTROLLER_MELLINGER) as pc:
+            # Go to a coordinate
+            pc.go_to(1.0, 1.0, 1.0)
+
+            # Move relative to the current position
+            pc.right(1.0)
+
+            # Go to a coordinate and use default height
+            pc.go_to(0.0, 0.0)
+
+            # Go slowly to a coordinate
+            pc.go_to(1.0, 1.0, velocity=0.2)
+
+            # Set new default velocity and height
+            pc.set_default_velocity(0.3)
+            pc.set_default_height(1.0)
+            pc.go_to(0.0, 0.0)
+
+
+def simple_sequence():
+    with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
+        with PositionHlCommander(scf) as pc:
+            pc.forward(1.0)
+            pc.left(1.0)
+            pc.back(1.0)
+            pc.go_to(0.0, 0.0, 1.0)
+
+
+if __name__ == '__main__':
+    cflib.crtp.init_drivers(enable_debug_driver=False)
+
+    simple_sequence()
+    # slightly_more_complex_usage()

dfall_ws/src/dfall_pkg/crazyradio/examples/positioning/bezier_trajectory.py

+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __
+#  +------+      / __ )(_) /_______________ _____  ___
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2019 Bitcraze AB
+#
+#  This program is free software; you can redistribute it and/or
+#  modify it under the terms of the GNU General Public License
+#  as published by the Free Software Foundation; either version 2
+#  of the License, or (at your option) any later version.
+#
+#  This program is distributed in the hope that it will be useful,
+#  but WITHOUT ANY WARRANTY; without even the implied warranty of
+#  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#  GNU General Public License for more details.
+#  You should have received a copy of the GNU General Public License
+#  along with this program; if not, write to the Free Software
+#  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
+#  MA  02110-1301, USA.
+"""
+Example of how to generate trajectories for the High Level commander using
+Bezier curves. The output from this script is intended to be pasted into the
+autonomous_sequence_high_level.py example.
+
+This code uses Bezier curves of degree 7, that is with 8 control points.
+See https://en.wikipedia.org/wiki/B%C3%A9zier_curve
+
+All coordinates are (x, y, z, yaw)
+"""
+import math
+
+import numpy as np
+
+
+class Node:
+    """
+    A node represents the connection point between two Bezier curves
+    (called Segments).
+    It holds 4 control points for each curve and the positions of the control
+    points are set to join the curves with continuity in c0, c1, c2, c3.
+    See https://www.cl.cam.ac.uk/teaching/2000/AGraphHCI/SMEG/node3.html
+
+    The control points are named
+    p4, p5, p6 and p7 for the tail of the first curve
+    q0, q1, q2, q3 for the head of the second curve
+    """
+
+    def __init__(self, q0, q1=None, q2=None, q3=None):
+        """
+        Create a Node. Pass in control points to define the shape of the
+        two segments that share the Node. The control points are for the
+        second segment, that is the four first control points of the Bezier
+        curve after the node. The control points for the Bezier curve before
+        the node are calculated from the existing control points.
+        The control points are for scale = 1, that is if the Bezier curve
+        after the node has scale = 1 it will have exactly these handles. If the
+        curve after the node has a different scale the handles will be moved
+        accordingly when the Segment is created.
+
+        q0 is required, the other points are optional.
+        if q1 is missing it will be set to generate no velocity in q0.
+        If q2 is missing it will be set to generate no acceleration in q0.
+        If q3 is missing it will be set to generate no jerk in q0.
+
+        If only q0 is set, the node will represent a point where the Crazyflie
+        has no velocity. Good for starting and stopping.
+
+        To get a fluid motion between segments, q1 must be set.
+
+        :param q0: The position of the node
+        :param q1: The position of the first control point
+        :param q2: The position of the second control point
+        :param q3: The position of the third control point
+        """
+        self._control_points = np.zeros([2, 4, 4])
+
+        q0 = np.array(q0)
+
+        if q1 is None:
+            q1 = q0
+        else:
+            q1 = np.array(q1)
+            # print('q1 generated:', q1)
+
+        d = q1 - q0
+
+        if q2 is None:
+            q2 = q0 + 2 * d
+            # print('q2 generated:', q2)
+        else:
+            q2 = np.array(q2)
+
+        e = 3 * q2 - 2 * q0 - 6 * d
+
+        if q3 is None:
+            q3 = e + 3 * d
+            # print('q3 generated:', q3)
+        else:
+            q3 = np.array(q3)
+
+        p7 = q0
+        p6 = q1 - 2 * d
+        p5 = q2 - 4 * d
+        p4 = 2 * e - q3
+
+        self._control_points[0][0] = q0
+        self._control_points[0][1] = q1
+        self._control_points[0][2] = q2
+        self._control_points[0][3] = q3
+
+        self._control_points[1][3] = p7
+        self._control_points[1][2] = p6
+        self._control_points[1][1] = p5
+        self._control_points[1][0] = p4
+
+    def get_head_points(self):
+        return self._control_points[0]
+
+    def get_tail_points(self):
+        return self._control_points[1]
+
+    def draw_unscaled_controlpoints(self, visualizer):
+        color = (0.8, 0.8, 0.8)
+        for p in self._control_points[0]:
+            visualizer.marker(p[0:3], color=color)
+        for p in self._control_points[1]:
+            visualizer.marker(p[0:3], color=color)
+
+    def print(self):
+        print('Node ---')
+        print('Tail:')
+        for c in self._control_points[1]:
+            print(c)
+        print('Head:')
+        for c in self._control_points[0]:
+            print(c)
+
+
+class Segment:
+    """
+    A Segment represents a Bezier curve of degree 7. It uses two Nodes to
+    define the shape. The scaling of the segment will move the handles compared
+    to the Node to maintain continuous position, velocity, acceleration and
+    jerk through the Node.
+    A Segment can generate a polynomial that is compatible with the High Level
+    Commander, either in python to be sent to the Crazyflie, or as C code to be
+    used in firmware.
+    A Segment can also be rendered in Vispy.
+    """
+
+    def __init__(self, head_node, tail_node, scale):
+        self._scale = scale
+
+        unscaled_points = np.concatenate(
+            [head_node.get_head_points(), tail_node.get_tail_points()])
+
+        self._points = self._scale_control_points(unscaled_points, self._scale)
+
+        polys = self._convert_to_polys()
+        self._polys = self._stretch_polys(polys, self._scale)
+
+        self._vel = self._deriv(self._polys)
+        self._acc = self._deriv(self._vel)
+        self._jerk = self._deriv(self._acc)
+
+    def _convert_to_polys(self):
+        n = len(self._points) - 1
+        result = np.zeros([4, 8])
+
+        for d in range(4):
+            for j in range(n + 1):
+                s = 0.0
+                for i in range(j + 1):
+                    s += ((-1) ** (i + j)) * self._points[i][d] / (
+                        math.factorial(i) * math.factorial(j - i))
+
+                c = s * math.factorial(n) / math.factorial(n - j)
+                result[d][j] = c
+
+        return result
+
+    def draw_trajectory(self, visualizer):
+        self._draw(self._polys, 'black', visualizer)
+
+    def draw_vel(self, visualizer):
+        self._draw(self._vel, 'green', visualizer)
+
+    def draw_acc(self, visualizer):
+        self._draw(self._acc, 'red', visualizer)
+
+    def draw_jerk(self, visualizer):
+        self._draw(self._jerk, 'blue', visualizer)
+
+    def draw_control_points(self, visualizer):
+        for p in self._points:
+            visualizer.marker(p[0:3])
+
+    def _draw(self, polys, color, visualizer):
+        step = self._scale / 32
+        prev = None
+        for t in np.arange(0.0, self._scale + step, step):
+            p = self._eval_xyz(polys, t)
+
+            if prev is not None:
+                visualizer.line(p, prev, color=color)
+
+            prev = p
+
+    def velocity(self, t):
+        return self._eval_xyz(self._vel, t)
+
+    def acceleration(self, t):
+        return self._eval_xyz(self._acc, t)
+
+    def jerk(self, t):
+        return self._eval_xyz(self._jerk, t)
+
+    def _deriv(self, p):
+        result = []
+        for i in range(3):
+            result.append([
+                1 * p[i][1],
+                2 * p[i][2],
+                3 * p[i][3],
+                4 * p[i][4],
+                5 * p[i][5],
+                6 * p[i][6],
+                7 * p[i][7],
+                0
+            ])
+
+        return result
+
+    def _eval(self, p, t):
+        result = 0
+        for part in range(8):
+            result += p[part] * (t ** part)
+        return result
+
+    def _eval_xyz(self, p, t):
+        return np.array(
+            [self._eval(p[0], t), self._eval(p[1], t), self._eval(p[2], t)])
+
+    def print_poly_python(self):
+        s = '  [' + str(self._scale) + ', '
+
+        for axis in range(4):
+            for d in range(8):
+                s += str(self._polys[axis][d]) + ', '
+
+        s += '],  # noqa'
+        print(s)
+
+    def print_poly_c(self):
+        s = ''
+
+        for axis in range(4):
+            for d in range(8):
+                s += str(self._polys[axis][d]) + ', '
+
+        s += str(self._scale)
+        print(s)
+
+    def print_points(self):
+        print(self._points)
+
+    def print_peak_vals(self):
+        peak_v = 0.0
+        peak_a = 0.0
+        peak_j = 0.0
+
+        step = 0.05
+        for t in np.arange(0.0, self._scale + step, step):
+            peak_v = max(peak_v, np.linalg.norm(self._eval_xyz(self._vel, t)))
+            peak_a = max(peak_a, np.linalg.norm(self._eval_xyz(self._acc, t)))
+            peak_j = max(peak_j, np.linalg.norm(self._eval_xyz(self._jerk, t)))
+
+        print('Peak v:', peak_v, 'a:', peak_a, 'j:', peak_j)
+
+    def _stretch_polys(self, polys, time):
+        result = np.zeros([4, 8])
+
+        recip = 1.0 / time
+
+        for p in range(4):
+            scale = 1.0
+            for t in range(8):
+                result[p][t] = polys[p][t] * scale
+                scale *= recip
+
+        return result
+
+    def _scale_control_points(self, unscaled_points, scale):
+        s = scale
+        l_s = 1 - s
+        p = unscaled_points
+
+        result = [None] * 8
+
+        result[0] = p[0]
+        result[1] = l_s * p[0] + s * p[1]
+        result[2] = l_s ** 2 * p[0] + 2 * l_s * s * p[1] + s ** 2 * p[2]
+        result[3] = l_s ** 3 * p[0] + 3 * l_s ** 2 * s * p[
+            1] + 3 * l_s * s ** 2 * p[2] + s ** 3 * p[3]
+        result[4] = l_s ** 3 * p[7] + 3 * l_s ** 2 * s * p[
+            6] + 3 * l_s * s ** 2 * p[5] + s ** 3 * p[4]
+        result[5] = l_s ** 2 * p[7] + 2 * l_s * s * p[6] + s ** 2 * p[5]
+        result[6] = l_s * p[7] + s * p[6]
+        result[7] = p[7]
+
+        return result
+
+
+class Visualizer:
+    def __init__(self):
+        self.canvas = scene.SceneCanvas(keys='interactive', size=(800, 600),
+                                        show=True)
+        view = self.canvas.central_widget.add_view()
+        view.bgcolor = '#ffffff'
+        view.camera = TurntableCamera(fov=10.0, distance=40.0, up='+z',
+                                      center=(0.0, 0.0, 1.0))
+        XYZAxis(parent=view.scene)
+        self.scene = view.scene
+
+    def marker(self, pos, color='black', size=8):
+        Markers(pos=np.array(pos, ndmin=2), face_color=color,
+                parent=self.scene, size=size)
+
+    def lines(self, points, color='black'):
+        LinePlot(points, color=color, parent=self.scene)
+
+    def line(self, a, b, color='black'):
+        self.lines([a, b], color)
+
+    def run(self):
+        self.canvas.app.run()
+
+
+segment_time = 2
+z = 1
+yaw = 0
+
+segments = []
+
+# Nodes with one control point has not velocity, this is similar to calling
+# goto in the High-level commander
+
+n0 = Node((0, 0, z, yaw))
+n1 = Node((1, 0, z, yaw))
+n2 = Node((1, 1, z, yaw))
+n3 = Node((0, 1, z, yaw))
+
+segments.append(Segment(n0, n1, segment_time))
+segments.append(Segment(n1, n2, segment_time))
+segments.append(Segment(n2, n3, segment_time))
+segments.append(Segment(n3, n0, segment_time))
+
+
+# By setting the q1 control point we get velocity through the nodes
+# Increase d to 0.7 to get some more action
+d = 0.1
+
+n5 = Node((1, 0, z, yaw), q1=(1 + d, 0 + d, z, yaw))
+n6 = Node((1, 1, z, yaw), q1=(1 - d, 1 + d, z, yaw))
+n7 = Node((0, 1, z, yaw), q1=(0 - d, 1 - d, z, yaw))
+
+segments.append(Segment(n0, n5, segment_time))
+segments.append(Segment(n5, n6, segment_time))
+segments.append(Segment(n6, n7, segment_time))
+segments.append(Segment(n7, n0, segment_time))
+
+
+# When setting q2 we can also control acceleration and get more action.
+# Yaw also adds to the fun.
+
+d2 = 0.2
+dyaw = 2
+f = -0.3
+
+n8 = Node(
+    (1, 0, z, yaw),
+    q1=(1 + d2, 0 + d2, z, yaw),
+    q2=(1 + 2 * d2, 0 + 2 * d2 + 0*f * d2, 1, yaw))
+n9 = Node(
+    (1, 1, z, yaw + dyaw),
+    q1=(1 - d2, 1 + d2, z, yaw + dyaw),
+    q2=(1 - 2 * d2 + f * d2, 1 + 2 * d2 + f * d2, 1, yaw + dyaw))
+n10 = Node(
+    (0, 1, z, yaw - dyaw),
+    q1=(0 - d2, 1 - d2, z, yaw - dyaw),
+    q2=(0 - 2 * d2,  1 - 2 * d2,  1, yaw - dyaw))
+
+segments.append(Segment(n0, n8, segment_time))
+segments.append(Segment(n8, n9, segment_time))
+segments.append(Segment(n9, n10, segment_time))
+segments.append(Segment(n10, n0, segment_time))
+
+
+print('Paste this code into the autonomous_sequence_high_level.py example to '
+      'see it fly')
+for s in segments:
+    s.print_poly_python()
+
+
+# Enable this if you have Vispy installed and want a visualization of the
+# trajectory
+if False:
+    # Import here to avoid problems for users that do not have Vispy
+    from vispy import scene
+    from vispy.scene import XYZAxis, LinePlot, TurntableCamera, Markers
+
+    visualizer = Visualizer()
+    for s in segments:
+        s.draw_trajectory(visualizer)
+        # s.draw_vel(visualizer)
+        # s.draw_control_points(visualizer)
+
+    for n in [n0, n1, n2, n3, n5, n6, n7, n8, n9, n10]:
+        n.draw_unscaled_controlpoints(visualizer)
+
+    visualizer.run()

pps_ws/src/d_fall_pps/crazyradio/examples/swarmSequence.py → dfall_ws/src/dfall_pkg/crazyradio/examples/positioning/initial_position.py

@@ -6,7 +6,7 @@
 #  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
 #   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
 #
-#  Copyright (C) 2017 Bitcraze AB
+#  Copyright (C) 2019 Bitcraze AB
 #
 #  Crazyflie Nano Quadcopter Client
 #
@@ -24,40 +24,37 @@
 #  Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
 #  MA  02110-1301, USA.
 """
-Version of the AutonomousSequence.py example connecting to 2 Crazyflies and
-flying them throught a list of setpoints at the same time. This shows how to
-control more than one Crazyflie autonomously.
+Simple example that connects to one crazyflie, sets the initial position/yaw
+and flies a trajectory.
+
+The initial pose (x, y, z, yaw) is configured in a number of variables and
+the trajectory is flown relative to this position, using the initial yaw.
+
+This example is intended to work with any absolute positioning system.
+It aims at documenting how to take off with the Crazyflie in an orientation
+that is different from the standard positive X orientation and how to set the
+initial position of the kalman estimator.
 """
+import math
 import time
 
 import cflib.crtp
+from cflib.crazyflie import Crazyflie
 from cflib.crazyflie.log import LogConfig
-from cflib.crazyflie.swarm import Swarm
+from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
 from cflib.crazyflie.syncLogger import SyncLogger
 
-# Change uris and sequences according to your setup
-URI0 = 'radio://0/70/2M'
-URI1 = 'radio://0/90/2M'
-
-uris = {URI0, URI1}
-
-#             x    y    z  YAW
-sequence0 = [(2.5, 2.5, 1.0, 0),
-             (2.5, 2.3, 1.0, 0),
-             (2.0, 2.3, 1.0, 0),
-             (2.0, 2.5, 1.0, 0),
-             (2.0, 2.5, 0.5, 0)]
+# URI to the Crazyflie to connect to
+uri = 'radio://0/80/2M'
 
-sequence1 = [(2.0, 2.5, 1.0, 0),
-             (2.0, 2.7, 1.0, 0),
-             (2.5, 2.7, 1.0, 0),
-             (2.5, 2.5, 1.0, 0),
-             (2.5, 2.5, 0.5, 0)]
-
-seq_args = {
-    URI0: [sequence0],
-    URI1: [sequence1],
-}
+# Change the sequence according to your setup
+#             x    y    z
+sequence = [
+    (0, 0, 0.7),
+    (-0.7, 0, 0.7),
+    (0, 0, 0.7),
+    (0, 0, 0.2),
+]
 
 
 def wait_for_position_estimator(scf):
@@ -101,6 +98,15 @@ def wait_for_position_estimator(scf):
                 break
 
 
+def set_initial_position(scf, x, y, z, yaw_deg):
+    scf.cf.param.set_value('kalman.initialX', x)
+    scf.cf.param.set_value('kalman.initialY', y)
+    scf.cf.param.set_value('kalman.initialZ', z)
+
+    yaw_radians = math.radians(yaw_deg)
+    scf.cf.param.set_value('kalman.initialYaw', yaw_radians)
+
+
 def reset_estimator(scf):
     cf = scf.cf
     cf.param.set_value('kalman.resetEstimation', '1')
@@ -110,38 +116,21 @@ def reset_estimator(scf):
     wait_for_position_estimator(cf)
 
 
-def position_callback(timestamp, data, logconf):
-    x = data['kalman.stateX']
-    y = data['kalman.stateY']
-    z = data['kalman.stateZ']
-    print('pos: ({}, {}, {})'.format(x, y, z))
-
-
-def start_position_printing(scf):
-    log_conf = LogConfig(name='Position', period_in_ms=500)
-    log_conf.add_variable('kalman.stateX', 'float')
-    log_conf.add_variable('kalman.stateY', 'float')
-    log_conf.add_variable('kalman.stateZ', 'float')
-
-    scf.cf.log.add_config(log_conf)
-    log_conf.data_received_cb.add_callback(position_callback)
-    log_conf.start()
-
-
-def run_sequence(scf, sequence):
+def run_sequence(scf, sequence, base_x, base_y, base_z, yaw):
     cf = scf.cf
 
-    cf.param.set_value('flightmode.posSet', '1')
-
     for position in sequence:
         print('Setting position {}'.format(position))
+
+        x = position[0] + base_x
+        y = position[1] + base_y
+        z = position[2] + base_z
+
         for i in range(50):
-            cf.commander.send_setpoint(position[1], position[0],
-                                       position[3],
-                                       int(position[2] * 1000))
+            cf.commander.send_position_setpoint(x, y, z, yaw)
             time.sleep(0.1)
 
-    cf.commander.send_setpoint(0, 0, 0, 0)
+    cf.commander.send_stop_setpoint()
     # Make sure that the last packet leaves before the link is closed
     # since the message queue is not flushed before closing
     time.sleep(0.1)
@@ -150,7 +139,19 @@ def run_sequence(scf, sequence):
 if __name__ == '__main__':
     cflib.crtp.init_drivers(enable_debug_driver=False)
 
-    with Swarm(uris) as swarm:
-        swarm.parallel(reset_estimator)
-        # swarm.parallel(start_position_printing)
-        swarm.parallel(run_sequence, args_dict=seq_args)
+    # Set these to the position and yaw based on how your Crazyflie is placed
+    # on the floor
+    initial_x = 1.0
+    initial_y = 1.0
+    initial_z = 0.0
+    initial_yaw = 90  # In degrees
+    # 0: positive X direction
+    # 90: positive Y direction
+    # 180: negative X direction
+    # 270: negative Y direction
+
+    with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
+        set_initial_position(scf, initial_x, initial_y, initial_z, initial_yaw)
+        reset_estimator(scf)
+        run_sequence(scf, sequence,
+                     initial_x, initial_y, initial_z, initial_yaw)