One drone following another drone. [Solved]

Topics related to the Lighthouse positioning system, configuration and use
cafeciaojoe
Member
Posts: 83
Joined: Mon Jun 18, 2018 2:37 am

One drone following another drone. [Solved]

Post by cafeciaojoe »

Hi!

I am looking for some general guidance of a problem I have just started tackling.

I would like a drone to follow another drone, but the leading drone is not flying. it is being held in the hands.

My first hack at this was to start a second context manager to activate the 2nd drone but the 2nd context manager did not eventuate.
(Keep in mind that I still a beginner in python so I don't even know if context managers work within context managers lol)
Eventually linking the position estimate from the logger to the pc.go_to.

Code: Select all

import logging
import time
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.positioning.position_hl_commander import PositionHlCommander
from cflib.crazyflie.log import LogConfig

# URI to the Crazyflie to connect to
uri =   'radio://0/80/2M/E7E7E7E7E7'
uri_2 = 'radio://0/80/2M/E7E7E7E7ED'

position_estimate = [0, 0, 0]

def log_pos_callback(timestamp, data, logconf):
    print(data)
    global position_estimate
    position_estimate[0] = data['stateEstimate.x']
    position_estimate[1] = data['stateEstimate.y']
    position_estimate[2] = data['stateEstimate.z']

def simple_sequence():
    with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
        with PositionHlCommander(scf) as pc:
            # TODO link position_estimate to pc.go_to

if __name__ == '__main__':
    cflib.crtp.init_drivers(enable_debug_driver=False)

    with SyncCrazyflie(uri_2, cf=Crazyflie(rw_cache='./cache')) as scf:
        logconf = LogConfig(name='Position', period_in_ms=10)
        logconf.add_variable('stateEstimate.x', 'float')
        logconf.add_variable('stateEstimate.y', 'float')
        logconf.add_variable('stateEstimate.z', 'float')
        scf.cf.log.add_config(logconf)
        logconf.data_received_cb.add_callback(log_pos_callback)

        logconf.start()

        with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:

            simple_sequence()

        logconf.stop()
Thanks :D
Last edited by cafeciaojoe on Mon Jan 11, 2021 7:01 am, edited 1 time in total.
PhD Student
RMIT University
School of Design
http://www.cafeciaojoe.com
Image
kimberly
Bitcraze
Posts: 1050
Joined: Fri Jul 06, 2018 11:13 am

Re: One drone following another drone.

Post by kimberly »

Hi!

I'm not exactly sure what you mean with the context managers. but you might be able to something with some if statements with setting up it with swarm parallels. You can get the uris of each instance like this

Code: Select all

 print(scf.cf.link_uri)  
and only do logging with one, save it in a global variable and use that value in the other thread. Not sure how well the global variables will do in threads though....

By the way, I believe that Kristoffer did something similar to this recently, so I can ask him to give you more tips. Unfortunately he will not be back until the 4th of January. Let us know if you are willing to wait until then.
cafeciaojoe
Member
Posts: 83
Joined: Mon Jun 18, 2018 2:37 am

Re: One drone following another drone.

Post by cafeciaojoe »

HI Kimberly,

I also just got back on the 4th. I managed to get my hack working. The drone is only stable if it is following the stateestimate of one axis at a time. eg:

Code: Select all

pc.go_to(0, 0, set_point_z)
Any way to make this more stable in all thee axes?

Also. Yes I would love some more tips from Kristoffer.

Full code below.

Code: Select all

import logging
import time
import cflib.crtp

from cflib.crazyflie import Crazyflie
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.positioning.position_hl_commander import PositionHlCommander
from cflib.crazyflie.log import LogConfig

# URI to the Crazyflie to connect to
uri =   'radio://0/80/2M/E7E7E7E7E7'
uri_2 = 'radio://0/80/2M/E7E7E7E7ED'


position_estimate = [0, 0, 0]

def log_pos_callback(timestamp, data, logconf):
    # print(data)
    global position_estimate
    position_estimate[0] = data['stateEstimate.x']
    position_estimate[1] = data['stateEstimate.y']
    position_estimate[2] = data['stateEstimate.z']

def follow():
    with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf_2:
        with PositionHlCommander(scf_2) as pc:
            while True:
                set_point_x = position_estimate[0] -.4
                set_point_y = position_estimate[1]
                set_point_z = position_estimate[2] +.2
                pc.go_to(set_point_x, set_point_y, set_point_z)

if __name__ == '__main__':
    cflib.crtp.init_drivers(enable_debug_driver=False)

    with SyncCrazyflie(uri_2, cf=Crazyflie(rw_cache='./cache')) as scf:
        logconf = LogConfig(name='Position', period_in_ms=10)
        logconf.add_variable('stateEstimate.x', 'float')
        logconf.add_variable('stateEstimate.y', 'float')
        logconf.add_variable('stateEstimate.z', 'float')
        scf.cf.log.add_config(logconf)
        logconf.data_received_cb.add_callback(log_pos_callback)

        logconf.start()
        follow()
        logconf.stop()
Can post video of instability if needed.
Cheers
Joe
PhD Student
RMIT University
School of Design
http://www.cafeciaojoe.com
Image
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: One drone following another drone.

Post by kristoffer »

Hi Joe!

It is possible to use one context manager within another one like you do. I think your code looks pretty OK, I would suggest adding a sleep in the while loop inside the follow() function to avoid pushing too many set points to the Crazyflie. Maybe add a time.sleep(0.1)?

I don't know why you get stability problems if you use more than one axis. You should probably use the PID controller, but that is the default one and unless you have done something funky elsewhere that is probably what you already have.

Have you tried to add a print to see if the position values look reasonable?

Which positioning system are you using?
cafeciaojoe
Member
Posts: 83
Joined: Mon Jun 18, 2018 2:37 am

Re: One drone following another drone.

Post by cafeciaojoe »

Hi Kristoffer thanks for the reply! I have a couple of questions.

Q1
I have the main drone flying at the midpoint of the two motorless drone now, I just used an addition with statement. Is it advisable to nest instances of the CF like this? I don't plan to have any more than three drones really.

Code: Select all

    with SyncCrazyflie(uri_2, cf=Crazyflie(rw_cache='./cache')) as scf_2:
        logconf_2 = LogConfig(name='Position', period_in_ms=10)
        logconf_2.add_variable('stateEstimate.x', 'float')
        logconf_2.add_variable('stateEstimate.y', 'float')
        logconf_2.add_variable('stateEstimate.z', 'float')
        scf_2.cf.log.add_config(logconf_2)
        logconf_2.data_received_cb.add_callback(log_pos_callback_L)

        with SyncCrazyflie(uri_3, cf=Crazyflie(rw_cache='./cache')) as scf_3:
            logconf_3 = LogConfig(name='Position', period_in_ms=10)
            logconf_3.add_variable('stateEstimate.x', 'float')
            logconf_3.add_variable('stateEstimate.y', 'float')
            logconf_3.add_variable('stateEstimate.z', 'float')
            scf_3.cf.log.add_config(logconf_3)
            logconf_3.data_received_cb.add_callback(log_pos_callback_R)

            logconf_2.start()
            logconf_3.start()
            follow()
Q2
In this hack I used the PositionHlCommander (https://github.com/bitcraze/crazyflie-l ... mmander.py) but if I was to incorporate yaw, like the HighLevelCommander class has in the Crazyflie module. What is the most reliable way to go about this? I have tried modifying the go_to() function in the PositionGlCoimmander to accept yaw as an argument:

Code: Select all

    def go_to(self, x, y, z, yaw, velocity=DEFAULT):
        """
        Go to a position

        :param x: X coordinate
        :param y: Y coordinate
        :param z: Z coordinate
        :param velocity: the velocity (meters/second)
        :return:
        """

        z = self._height(z)

        dx = x - self._x
        dy = y - self._y
        dz = z - self._z
        distance = math.sqrt(dx * dx + dy * dy + dz * dz)

        if distance > 0.0:
            duration_s = distance / self._velocity(velocity)
            self._hl_commander.go_to(x, y, z, yaw, duration_s)
            time.sleep(duration_s)

            self._x = x
            self._y = y
            self._z = z
then changed the yaw periodically, but the drone does not turn? Am I doing something simple wrong here?

Code: Select all

def follow():
    with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf_2:
        yaw = 0
        with PositionHlCommander(scf_2) as pc:
            while True:
                pc.go_to(0,0,1,yaw)
                yaw += 1
                print(yaw)
                time.sleep(5)
PS
The stability issues actually come from the drone being too high (we are using the lighthouses, so I assume the angle is too shallow) not from the multiple axis set points. It is a lot better down low (<1.6m for a 2.5m. high lighthouse arena, this improves as we move the lighthouses closer together.)

Thanks again
Joe
PhD Student
RMIT University
School of Design
http://www.cafeciaojoe.com
Image
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: One drone following another drone.

Post by kristoffer »

Hi!

Q1
It should be fine to nest yet another SyncCrazyflie. You can take a look in https://github.com/bitcraze/crazyflie-l ... py#L71-L73 to see what is actually going on. When you enter the context it pretty much just opens the link to the Crazyflie and waits for the callback that signals that the connection is ready.

Q2
Ah! I see you are using the PositionHlCommander, I missed that yesterday, sorry!
Your modifications for the yaw looks OK to me, but as you are pushing in the same x, y, z they will be discarded by

Code: Select all

if distance > 0.0:
Also note that the PositionHlCommander is doing a sleep to wait until the CF has reached the destination based on distance to target and velocity
(so disregard my comment yesterday about a sleep)

It is possible to use the high level commander for this type of tasks but you have to trick it a bit. When you send a goto to the HL commander it will plan a trajectory from its current state to the desired position. If the CF is hovering this means an acceleration phase + travel + deceleration, and it becomes "jerky" since it is starting and stopping al the time. You probably want a fluid motion and this can be achieved by using the fact that the HL commander will plan the trajectory from the current state (position, velocity, acceleration) so if the CF is going in some direction and you send a goto to the HL commander, it will replan from the current pos/vel in a smooth way. So the trick is to send new gotos before it reaches the target position and stops.

As you can see in the goto you modified, you send a go_to() to the _hl_commander with a duration and then sleep the same duration. The duration in the go_to is used to plan the trajectory while the sleep (obviously) waits for a while. You want the sleep to be shorter than the duration in the go_to

Another option is to skip the high level commander and send set points directly to the commander in the crazyflie, see https://github.com/bitcraze/crazyflie-l ... equence.py an example of this.
The drawback is that you must handle take off and landing as well.
cafeciaojoe
Member
Posts: 83
Joined: Mon Jun 18, 2018 2:37 am

Re: One drone following another drone.

Post by cafeciaojoe »

Great! Thanks.

Any appreciable difference in performance (ie latency or stability) between these three options?

Cheers
Joe
PhD Student
RMIT University
School of Design
http://www.cafeciaojoe.com
Image
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: One drone following another drone.

Post by kristoffer »

The High level commander is pretty cool and generates nice trajectories with lots of control information for the undrlying controller, unfortunately the PID controller is not as competent and will discard most of if. To really get the best out of the HL commander you need the Mellinger controller, but on the other hand it is much more sensitive to errors and is mainly usful in a good mo-cap system. The PID controller is much more forgiving and robust, so in your case I think it is a better choise.

I think the three options are similar in performance but the last one is easier to implement and debug.

The part of our Christmas video that I made (https://youtu.be/ypJecS1PrdA?t=76) was implemented with setpoints to the PID controller (it is running a bit faster in the video compared to real life though). I used two scripts: one to record the position/yaw when moving the CF around by hand, and one to play it back and retrace the movements. Let me know if you want the escripts for inspiration.
cafeciaojoe
Member
Posts: 83
Joined: Mon Jun 18, 2018 2:37 am

Re: One drone following another drone.

Post by cafeciaojoe »

I think the three options are similar in performance but the last one is easier to implement and debug.
To be clear, you think the positionalHLcommander is easier to implement and debug or sending setpoints to the PID?

If it is the latter then yes I would love to see the scripts for inspirartion.

Cheers
Joe
PhD Student
RMIT University
School of Design
http://www.cafeciaojoe.com
Image
kristoffer
Bitcraze
Posts: 630
Joined: Tue Jun 30, 2015 7:47 am

Re: One drone following another drone.

Post by kristoffer »

Sorry for being unclear, I meant that it is easier to send the setpoints directly to the controller and skip the high level commander.
One thing to note is that you must continuously send setpoints (atleast 4-5/s) after take off, otherwise you will trigger a safety mechanism in the Crazyflie that will shut off the motors (assuming we lost contact with the ground station).

Script to record position and write data to stdout:

Code: Select all

"""
Record position

"""
import logging
import time

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 = 'usb://0'
sample_rate = 10  # Hz


# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)

x_name = 'stateEstimate.x'
y_name = 'stateEstimate.y'
z_name = 'stateEstimate.z'
yaw_name = 'stateEstimate.yaw'


# Initialize the low-level drivers (don't list the debug drivers)
cflib.crtp.init_drivers(enable_debug_driver=False)

lg_stab = LogConfig(name='pos', period_in_ms=int(1000 / sample_rate))
lg_stab.add_variable(x_name, 'float')
lg_stab.add_variable(y_name, 'float')
lg_stab.add_variable(z_name, 'float')
lg_stab.add_variable(yaw_name, 'float')

cf = Crazyflie(rw_cache='./cache')
with SyncCrazyflie(uri, cf=cf) as scf:
    with SyncLogger(scf, lg_stab) as logger:
        start = -1
        for log_entry in logger:

            data = log_entry[1]

            x = data[x_name]
            y = data[y_name]
            z = data[z_name]
            yaw = data[yaw_name]
            time = log_entry[0]
            if start < 0:
                start = time

            adjusted_time = (time - start) / 1000.0

            if z > 0.1:
                print('    [%f, %f, %f, %f, %f],' % (adjusted_time, x, y, z, yaw))
Script to retrace recorded trajectory. Note that the output from the record script should be pasted into this script.

Code: Select all

"""
Replay a trajectory
"""

import time

import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.mem import MemoryElement
from cflib.crazyflie.mem import Poly4D
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.crazyflie.syncLogger import SyncLogger
from cflib.crazyflie.swarm import CachedCfFactory
from cflib.crazyflie.swarm import Swarm


# Output from recording is pasted here
trajectory = [
    [5.400000, -0.046623, -1.483068, 0.762471, 82.846436],
    [5.500000, -0.047361, -1.393556, 0.813164, 84.603775],
    [5.600000, -0.059637, -1.363293, 0.820030, 84.172287],
    [5.700000, -0.066751, -1.336190, 0.816958, 81.779724],
    [5.800000, -0.069864, -1.308472, 0.821256, 75.042160],
    [5.900000, -0.080665, -1.279595, 0.824106, 69.356316],
    [6.000000, -0.080370, -1.251695, 0.828467, 65.894135],
    [6.100000, -0.082539, -1.222107, 0.829532, 62.976616],
    [6.200000, -0.082787, -1.189730, 0.829509, 60.504341],
    [6.300000, -0.081539, -1.156130, 0.826049, 58.877171],
    [6.400000, -0.077907, -1.121137, 0.819377, 58.178196],
    [6.500000, -0.066879, -1.083180, 0.812653, 59.252960],
    [6.600000, -0.053675, -1.043357, 0.809948, 61.132259],
    [6.700000, -0.041428, -1.004326, 0.814144, 64.737968],
    [6.800000, -0.029411, -0.968979, 0.821937, 69.534546],
    [6.900000, -0.020582, -0.936722, 0.824898, 74.548950],
    [7.000000, -0.014653, -0.904782, 0.823062, 76.927361],
    [7.100000, -0.000151, -0.875139, 0.824938, 77.834595],
    [7.200000, -0.001970, -0.839843, 0.810099, 79.992210],
    [7.300000, 0.001109, -0.802715, 0.800625, 80.943268],
    [7.400000, 0.001555, -0.764795, 0.793533, 79.931564],
    [7.500000, 0.002797, -0.724406, 0.787105, 74.448074],
    [7.600000, -0.000397, -0.688022, 0.782962, 64.401520],
    [7.700000, 0.018339, -0.658532, 0.788613, 59.271442],
    [7.800000, 0.029360, -0.590794, 0.817208, 55.675495],
    [7.900000, 0.050684, -0.563149, 0.823424, 51.057396],
    [8.000000, 0.079647, -0.540493, 0.825355, 49.538200],
    [8.100000, 0.109896, -0.526162, 0.824579, 51.219215],
    [8.200000, 0.142115, -0.519066, 0.816511, 54.697765],
    [8.300000, 0.175884, -0.512573, 0.804499, 58.219784],
    [8.400000, 0.203641, -0.478185, 0.808745, 64.146255],
    [8.500000, 0.229068, -0.461136, 0.802763, 70.117943],
    [8.600000, 0.248275, -0.441172, 0.800030, 76.210114],
    [8.700000, 0.268882, -0.415872, 0.801275, 80.397133],
    [8.800000, 0.280903, -0.385706, 0.800784, 85.776161],
    [8.900000, 0.290677, -0.353264, 0.799559, 91.427910],
    [9.000000, 0.296246, -0.321653, 0.793055, 97.325310],
    [9.100000, 0.300180, -0.285427, 0.790114, 102.996185],
    [9.200000, 0.297368, -0.256180, 0.792900, 108.499481],
    [9.300000, 0.291420, -0.232984, 0.794600, 117.379250],
    [9.400000, 0.285174, -0.217094, 0.796162, 123.318878],
    [9.500000, 0.282441, -0.205182, 0.796854, 125.983597],
    [9.601000, 0.281805, -0.196767, 0.797027, 130.582062],
    [9.701000, 0.289993, -0.194549, 0.798496, 136.410538],
    [9.801000, 0.302597, -0.194705, 0.799659, 139.695297],
    [9.901000, 0.310636, -0.193411, 0.804258, 141.941925],
    [10.002000, 0.315161, -0.191946, 0.808310, 144.142685],
    [10.100000, 0.319253, -0.188847, 0.813771, 144.897217],
    [10.202000, 0.321448, -0.185337, 0.817889, 144.746872],
    [10.300000, 0.321749, -0.184425, 0.820635, 145.460281],
    [10.400000, 0.322916, -0.182957, 0.821464, 144.902496],
    [10.500000, 0.325141, -0.180800, 0.820596, 144.089630],
    [10.600000, 0.324196, -0.177482, 0.820580, 143.311401],
    [10.700000, 0.314568, -0.171069, 0.822845, 142.640778],
    [10.800000, 0.301888, -0.174088, 0.814857, 143.367004],
    [10.900000, 0.274989, -0.159648, 0.818286, 143.417664],
    [11.000000, 0.238710, -0.135476, 0.818622, 147.435715],
    [11.100000, 0.192609, -0.102869, 0.828788, 149.457138],
    [11.202000, 0.121393, -0.086206, 0.857264, 150.869476],
    [11.300000, 0.064492, -0.060692, 0.867345, 152.213165],
    [11.402000, 0.009199, -0.035161, 0.877946, 152.123779],
    [11.500000, -0.042365, -0.013294, 0.886184, 152.419479],
    [11.600000, -0.084726, 0.008046, 0.896372, 152.041611],
    [11.700000, -0.130026, 0.026541, 0.901281, 152.393127],
    [11.800000, -0.162568, 0.040818, 0.906327, 153.610229],
    [11.900000, -0.187910, 0.051822, 0.907942, 153.907425],
    [12.000000, -0.208461, 0.058913, 0.908455, 154.950348],
    [12.100000, -0.222268, 0.065125, 0.911239, 155.066055],
    [12.200000, -0.230135, 0.069143, 0.911024, 155.248764],
    [12.302000, -0.233711, 0.071800, 0.909383, 155.441055],
    [12.400000, -0.234907, 0.074022, 0.907196, 155.764938],
    [12.502000, -0.234188, 0.077555, 0.906003, 155.466400],
    [12.600000, -0.233200, 0.078978, 0.904372, 155.537643],
    [12.702000, -0.231589, 0.078735, 0.902779, 156.045410],
    [12.800000, -0.228161, 0.078074, 0.902425, 155.926010],
    [12.902000, -0.222553, 0.073920, 0.901006, 156.608032],
    [13.000000, -0.215126, 0.067212, 0.899366, 157.604004],
    [13.101000, -0.200705, 0.059126, 0.900632, 158.011826],
    [13.201000, -0.184218, 0.052638, 0.903326, 157.605682],
    [13.300000, -0.160767, 0.046944, 0.905363, 158.107254],
    [13.400000, -0.132374, 0.039068, 0.901664, 159.609528],
    [13.500000, -0.091360, 0.035059, 0.898739, 160.820358],
    [13.600000, -0.042628, 0.029065, 0.895779, 162.295654],
    [13.701000, 0.000598, 0.020691, 0.897583, 162.223877],
    [13.801000, 0.047377, 0.013213, 0.898069, 161.282150],
    [13.900000, 0.088747, 0.004695, 0.898055, 159.553955],
    [14.001000, 0.127440, -0.001550, 0.900602, 155.896774],
    [14.100000, 0.151998, 0.002863, 0.898810, 150.351532],
    [14.200000, 0.168572, 0.020592, 0.891069, 141.137253],
    [14.300000, 0.181331, 0.053950, 0.889098, 130.363144],
    [14.400000, 0.185751, 0.089106, 0.891717, 122.277512],
    [14.500000, 0.179911, 0.126473, 0.889913, 118.228111],
    [14.600000, 0.168139, 0.168882, 0.884054, 118.967461],
    [14.700000, 0.162218, 0.221941, 0.873269, 117.599953],
    [14.800000, 0.152207, 0.276901, 0.867829, 117.023293],
    [14.900000, 0.138454, 0.325850, 0.863850, 118.322395],
    [15.000000, 0.126450, 0.373337, 0.866681, 120.539169],
    [15.100000, 0.113888, 0.417951, 0.870066, 124.454376],
    [15.200000, 0.096409, 0.463486, 0.872850, 128.330917],
    [15.300000, 0.084632, 0.507698, 0.870167, 134.123459],
    [15.400000, 0.075949, 0.551666, 0.868117, 139.664139],
    [15.500000, 0.065718, 0.591515, 0.866254, 147.888779],
    [15.600000, 0.057157, 0.625083, 0.862384, 156.325424],
    [15.700000, 0.050263, 0.653542, 0.861792, 165.931412],
    [15.800000, 0.044840, 0.672475, 0.869083, 174.995865],
    [15.900000, 0.035987, 0.679698, 0.873232, -176.889206],
    [16.000000, 0.031933, 0.681341, 0.877860, -170.199432],
    [16.100000, 0.017795, 0.679226, 0.878745, -164.821014],
    [16.200000, 0.008859, 0.672684, 0.879141, -158.626328],
    [16.300000, -0.001810, 0.668458, 0.882887, -155.356888],
    [16.400000, -0.013572, 0.661749, 0.890053, -153.303375],
    [16.500000, -0.025374, 0.652693, 0.894960, -151.227417],
    [16.600000, -0.037357, 0.646304, 0.896470, -149.784592],
    [16.700000, -0.044247, 0.638797, 0.896294, -148.500870],
    [16.800000, -0.054011, 0.627448, 0.897501, -147.173615],
    [16.900000, -0.059676, 0.610932, 0.897475, -145.524857],
    [17.000000, -0.076153, 0.595791, 0.894804, -143.534500],
    [17.100000, -0.087931, 0.576382, 0.895319, -141.276001],
    [17.200000, -0.101048, 0.556051, 0.899925, -140.048920],
    [17.300000, -0.115180, 0.535109, 0.903920, -139.913788],
    [17.400000, -0.128533, 0.515344, 0.908306, -141.996094],
    [17.500000, -0.147858, 0.501066, 0.912917, -145.767319],
    [17.600000, -0.171879, 0.497260, 0.914510, -149.234802],
    [17.700000, -0.192439, 0.497547, 0.913693, -150.653290],
    [17.801000, -0.208069, 0.503069, 0.912844, -150.336227],
    [17.901000, -0.232794, 0.515735, 0.912051, -151.592957],
    [18.000000, -0.253801, 0.528238, 0.911665, -151.858078],
    [18.100000, -0.272340, 0.541164, 0.911718, -152.077774],
    [18.200000, -0.293113, 0.555201, 0.910980, -153.201019],
    [18.300000, -0.307615, 0.565918, 0.912698, -154.127853],
    [18.400000, -0.321831, 0.571500, 0.909084, -154.144882],
    [18.500000, -0.334011, 0.569999, 0.907208, -154.404739],
    [18.600000, -0.343095, 0.565756, 0.913959, -152.992371],
    [18.700000, -0.345451, 0.551376, 0.914896, -150.311935],
    [18.800000, -0.341044, 0.537976, 0.918523, -148.353973],
    [18.900000, -0.334784, 0.523252, 0.919487, -147.916214],
    [19.000000, -0.325640, 0.507924, 0.916097, -147.409164],
    [19.100000, -0.317081, 0.496668, 0.915371, -148.369537],
    [19.200000, -0.307171, 0.486724, 0.915962, -149.271133],
    [19.300000, -0.298579, 0.479550, 0.916771, -150.826462],
    [19.400000, -0.292454, 0.473091, 0.918505, -152.418198],
    [19.500000, -0.289246, 0.467184, 0.921805, -154.928223],
    [19.600000, -0.283680, 0.458309, 0.927418, -156.329483],
    [19.700000, -0.278370, 0.453534, 0.932164, -158.291855],
    [19.800000, -0.275078, 0.452329, 0.937822, -160.249664],
    [19.900000, -0.275693, 0.449275, 0.942925, -162.023880],
    [20.000000, -0.281309, 0.443927, 0.944006, -162.342407],
    [20.100000, -0.292613, 0.440254, 0.943939, -162.436584],
    [20.200000, -0.305672, 0.436720, 0.941985, -162.284637],
    [20.300000, -0.323377, 0.433207, 0.936215, -162.369873],
    [20.400000, -0.341929, 0.429356, 0.928638, -162.314011],
    [20.500000, -0.360774, 0.426056, 0.922264, -162.134308],
    [20.600000, -0.377360, 0.423401, 0.918520, -161.751389],
    [20.700000, -0.391521, 0.420324, 0.915428, -160.701645],
    [20.800000, -0.402444, 0.417258, 0.911941, -158.409195],
    [20.900000, -0.410922, 0.417973, 0.910166, -157.781342],
    [21.000000, -0.419157, 0.420354, 0.908463, -156.921661],
    [21.100000, -0.424450, 0.422737, 0.906261, -155.502808],
    [21.200000, -0.429944, 0.427150, 0.902388, -153.458130],
    [21.300000, -0.435902, 0.431193, 0.895661, -151.366211],
    [21.400000, -0.440796, 0.433938, 0.890285, -149.417755],
    [21.500000, -0.445341, 0.432129, 0.880908, -146.601395],
    [21.600000, -0.442475, 0.449230, 0.899447, -144.710129],
    [21.700000, -0.445497, 0.451509, 0.899618, -142.111404],
    [21.800000, -0.444956, 0.450667, 0.900012, -139.143295],
    [21.900000, -0.449435, 0.450733, 0.898642, -135.112747],
    [22.000000, -0.456917, 0.455062, 0.900622, -131.801346],
    [22.100000, -0.463418, 0.456522, 0.899665, -129.603836],
    [22.200000, -0.466760, 0.457840, 0.895979, -127.527031],
    [22.300000, -0.475486, 0.461157, 0.897568, -125.008247],
    [22.400000, -0.487764, 0.464076, 0.897628, -122.349541],
    [22.500000, -0.492463, 0.464052, 0.900481, -119.331963],
    [22.600000, -0.498622, 0.465022, 0.902053, -117.793381],
    [22.700000, -0.503581, 0.466047, 0.898336, -116.492683],
    [22.800000, -0.507393, 0.468721, 0.894880, -115.528450],
    [22.900000, -0.508886, 0.469871, 0.890539, -114.258926],
    [23.000000, -0.510318, 0.469329, 0.888048, -112.877457],
    [23.100000, -0.512463, 0.467951, 0.887714, -111.788788],
    [23.200000, -0.517202, 0.465377, 0.892095, -111.043152],
    [23.300000, -0.522087, 0.465298, 0.895361, -110.423584],
    [23.400000, -0.525428, 0.467577, 0.899576, -110.283272],
    [23.500000, -0.527589, 0.467972, 0.899472, -110.294510],
    [23.600000, -0.527789, 0.467816, 0.899079, -110.239861],
    [23.700000, -0.527635, 0.468650, 0.899528, -110.608620],
    [23.800000, -0.525360, 0.470913, 0.899101, -111.264130],
    [23.900000, -0.521762, 0.472963, 0.897648, -112.033623],
    [24.000000, -0.518507, 0.473502, 0.896078, -112.839180],
    [24.100000, -0.515085, 0.474178, 0.896094, -113.729500],
    [24.200000, -0.511506, 0.474851, 0.896650, -115.006584],
    [24.300000, -0.507173, 0.474946, 0.895814, -116.882454],
    [24.400000, -0.502559, 0.474459, 0.896814, -119.984970],
    [24.500000, -0.497302, 0.472021, 0.900273, -122.877502],
    [24.600000, -0.491228, 0.468080, 0.903080, -125.034584],
    [24.700000, -0.483110, 0.464161, 0.906682, -126.890823],
    [24.800000, -0.476857, 0.462277, 0.912308, -129.374451],
    [24.900000, -0.471988, 0.461673, 0.915945, -130.782654],
    [25.000000, -0.464826, 0.463808, 0.920092, -132.726181],
    [25.100000, -0.454295, 0.460184, 0.927616, -134.630920],
    [25.200000, -0.446902, 0.459114, 0.934398, -138.021286],
    [25.300000, -0.438180, 0.457808, 0.942661, -140.441345],
    [25.400000, -0.428586, 0.455538, 0.950296, -141.654510],
    [25.500000, -0.417578, 0.453981, 0.958315, -143.204773],
    [25.600000, -0.411773, 0.454847, 0.964336, -145.615250],
    [25.700000, -0.399030, 0.451764, 0.971577, -147.556000],
    [25.800000, -0.390161, 0.449143, 0.975474, -149.770386],
    [25.900000, -0.381391, 0.445979, 0.978363, -151.616348],
    [26.000000, -0.375257, 0.444479, 0.979279, -155.008713],
    [26.100000, -0.371485, 0.444311, 0.979954, -159.193130],
    [26.200000, -0.372066, 0.443676, 0.977613, -161.477005],
    [26.300000, -0.368032, 0.437399, 0.978775, -162.898911],
    [26.400000, -0.371852, 0.437876, 0.976464, -165.982040],
    [26.500000, -0.374992, 0.434435, 0.973554, -166.109589],
    [26.600000, -0.379728, 0.433049, 0.971891, -165.771408],
    [26.700000, -0.387189, 0.431129, 0.971397, -166.313248],
    [26.800000, -0.395911, 0.430379, 0.970693, -166.715408],
    [26.900000, -0.405527, 0.429396, 0.966257, -166.831711],
    [27.000000, -0.413944, 0.427412, 0.962656, -166.568436],
    [27.100000, -0.423543, 0.425709, 0.957094, -166.786774],
    [27.200000, -0.433399, 0.422405, 0.951179, -167.028564],
    [27.300000, -0.442000, 0.420086, 0.946355, -167.785492],
    [27.400000, -0.449406, 0.415657, 0.939222, -167.902786],
    [27.500000, -0.455403, 0.411057, 0.930943, -168.468842],
    [27.600000, -0.460190, 0.404735, 0.919994, -168.239838],
    [27.700000, -0.464305, 0.398163, 0.907833, -168.139709],
    [27.800000, -0.467552, 0.392361, 0.896193, -168.436493],
    [27.900000, -0.469289, 0.388015, 0.887649, -168.995651],
    [28.000000, -0.469569, 0.383832, 0.881809, -169.550079],
    [28.100000, -0.468701, 0.379538, 0.878221, -170.599197],
    [28.200000, -0.467161, 0.375380, 0.875951, -171.601318],
    [28.300000, -0.464608, 0.371546, 0.875124, -172.909729],
    [28.400000, -0.461016, 0.367284, 0.875254, -174.162628],
    [28.500000, -0.440110, 0.407415, 0.924563, -171.495010],
    [28.600000, -0.435611, 0.410871, 0.931705, -170.983246],
    [28.700000, -0.431164, 0.411971, 0.938610, -169.405228],
    [28.800000, -0.430665, 0.412623, 0.944380, -166.658020],
    [28.900000, -0.425064, 0.415045, 0.952103, -164.151688],
    [29.000000, -0.427240, 0.421557, 0.961185, -165.475266],
    [29.100000, -0.426900, 0.423370, 0.974769, -165.773697],
    [29.200000, -0.424892, 0.420869, 0.990924, -165.443207],
    [29.300000, -0.426775, 0.420017, 1.007705, -165.591965],
    [29.400000, -0.433260, 0.420795, 1.024368, -166.218994],
    [29.500000, -0.435997, 0.420950, 1.038688, -166.297455],
    [29.600000, -0.445871, 0.421503, 1.049848, -166.942841],
    [29.700000, -0.456370, 0.421962, 1.058090, -167.507675],
    [29.800000, -0.462014, 0.420604, 1.060297, -167.147690],
    [29.900000, -0.471665, 0.418350, 1.060835, -165.494812],
    [30.000000, -0.483893, 0.416253, 1.061958, -164.633286],
    [30.100000, -0.499154, 0.398175, 1.074498, -163.295273],
    [30.200000, -0.512847, 0.395647, 1.080874, -162.584595],
    [30.300000, -0.527056, 0.391161, 1.087653, -161.492188],
    [30.400000, -0.542780, 0.388828, 1.093485, -161.208206],
    [30.500000, -0.560633, 0.387485, 1.099806, -161.282547],
    [30.600000, -0.575754, 0.386549, 1.104162, -161.065155],
    [30.700000, -0.592633, 0.385481, 1.105130, -160.937469],
    [30.800000, -0.608893, 0.384480, 1.104031, -161.165131],
    [30.900000, -0.623569, 0.381321, 1.100413, -160.968369],
    [31.000000, -0.637320, 0.379125, 1.092797, -160.987198],
    [31.100000, -0.649553, 0.377852, 1.081211, -161.421616],
    [31.200000, -0.659161, 0.375144, 1.067424, -160.989227],
    [31.300000, -0.666067, 0.372101, 1.051442, -160.607620],
    [31.400000, -0.669467, 0.367780, 1.034234, -159.550034],
    [31.500000, -0.667677, 0.366218, 1.018367, -159.480240],
    [31.600000, -0.660411, 0.365100, 1.004314, -160.076126],
    [31.700000, -0.650226, 0.364043, 0.992483, -161.283722],
    [31.800000, -0.637634, 0.362683, 0.983479, -162.214386],
    [31.900000, -0.616668, 0.369934, 0.978507, -162.077881],
    [32.000000, -0.592629, 0.372811, 0.968966, -160.220413],
    [32.100000, -0.566684, 0.376912, 0.959580, -158.874298],
    [32.200000, -0.538083, 0.379907, 0.950346, -156.683731],
    [32.300000, -0.508783, 0.385710, 0.936716, -155.459686],
    [32.400000, -0.478993, 0.390328, 0.920076, -153.598389],
    [32.500000, -0.451700, 0.394269, 0.904986, -151.731827],
    [32.600000, -0.430844, 0.398511, 0.887833, -150.976746],
    [32.700000, -0.417489, 0.397769, 0.862780, -148.362473],
    [32.800000, -0.412943, 0.399168, 0.826656, -149.118500],
    [32.900000, -0.417921, 0.399357, 0.777750, -149.047836],
    [33.000000, -0.429065, 0.395261, 0.733175, -153.046478],
    [33.100000, -0.431353, 0.397026, 0.711700, -163.547089],
    [33.200000, -0.428411, 0.395765, 0.705513, -164.345154],
    [33.300000, -0.427238, 0.392024, 0.667885, -161.053085],
    [33.400000, -0.439706, 0.386841, 0.625672, -133.894974],
    [33.500000, -0.452514, 0.363602, 0.618029, -120.610451],
    [33.600000, -0.473528, 0.353555, 0.615272, -143.069855],
    [33.700000, -0.472243, 0.356287, 0.607969, -160.453415],
    [33.800000, -0.454168, 0.354941, 0.601848, -154.408691],
    [33.900000, -0.419579, 0.347439, 0.597786, -144.856598],
    [34.000000, -0.373342, 0.335019, 0.592088, -142.925858],
]


def goto(scf, x, y, z, yaw):
    scf.cf.commander.send_position_setpoint(x, y, z, yaw)
    print(time.time(), x, y, z, yaw)

cflib.crtp.init_drivers(enable_debug_driver=False)
with SyncCrazyflie("radio://0/30") as scf:

    scf.cf.param.set_value("ring.effect", 14)
    scf.cf.param.set_value("ring.fadeColor", 0)
    scf.cf.param.set_value("ring.fadeTime", 1.0)


    # Take off
    steps = 10
    start_x = 0.0
    start_y = -1.5
    for i in range(steps):
        x = (i * trajectory[0][1] + (steps - i) * start_x) / steps
        y = (i * trajectory[0][2] + (steps - i) * start_y) / steps
        z = i * trajectory[0][3] / steps
        yaw = i * trajectory[0][4] / steps
        goto(scf, x, y, z, yaw)
        time.sleep(0.2)

    for i in range(5):
        goto(scf, trajectory[0][1], trajectory[0][2], trajectory[0][3], trajectory[0][4])
        time.sleep(0.2)

    offset_time = trajectory[0][0]
    start_time = time.time()
    last_led_time = 0

    for row in trajectory:
        next_time = row[0] - offset_time + start_time
        delay = next_time - time.time()
        if delay > 0:
            time.sleep(delay)

        x = row[1]
        y = row[2]
        z = row[3]
        yaw = row[4]

        goto(scf, x, y, z, yaw)

        now_int = int(time.time())
        if last_led_time != now_int:
            col = 0x000000ff * (now_int & 1)
            scf.cf.param.set_value("ring.fadeColor", col)

            last_led_time = now_int


    # Land for now
    # steps = 10
    # for i in range(steps):
    #     x = trajectory[-1][1]
    #     y = trajectory[-1][2]
    #     z = (steps - i) * trajectory[-1][3] / steps
    #     yaw = trajectory[-1][4]
    #     goto(scf, x, y, z, yaw)
    #     time.sleep(0.2)

    # time.sleep(0.3)
    scf.cf.commander.send_stop_setpoint()

    for i in range(10):
        scf.cf.param.set_value("ring.fadeColor", 0x00ff0000)
        time.sleep(2)
        scf.cf.param.set_value("ring.fadeColor", 0x0000ff00)
        time.sleep(2)
Post Reply