Threading multiple crazyflies (not using swarm class), erratic flying. How to fix?

[john-amadora]

I coded in control of 3 synchronous drones via the python wrapper for the crazyflie and using only the motion commander class for movement. I did this instead of the swarm class because each drone will be having its own command and the swarm class is limited to sending setpoints, synchronous movement, and sequential movement.

I implemented my system using threading in python. That is, the crazyflie and the motion commander is initialized in a function and the thread is enabled three times in main all with its own URI.

When one drone is enabled, hovering is steady, but when 3 drones are enabled, the drones tend to drift by a lot.

How can I fix this? I am thinking that perhaps they are sharing state estimates. Any ideas?

Here's what the function looks like. This is called 3 times in main to thread together. For more context, I want to control each drone based on command submitted by an external controller (handled by another thread). May not be the smartest implementation, I am not a compsci major after all. I just develop as needed.

with SyncCrazyflie(URI, cf) as scf:
        scf.cf.param.add_update_callback(group='deck', name='bcFlow2',
                                         cb=param_deck_flow)
        logconf = LogConfig(name=drone, period_in_ms=500)
        logconf.add_variable('stateEstimate.x', 'float')
        logconf.add_variable('stateEstimate.y', 'float')
        logconf.add_variable('stateEstimate.z', 'float')
        logconf.add_variable('pm.vbat', 'float')
        scf.cf.log.add_config(logconf)
        logconf.data_received_cb.add_callback(log_pos_callback)
        if not deck_attached_event.wait(timeout=3):
            print(f"{drone}:No flow deck detected!")
            enable = False
        mc = MotionCommander(scf, default_height = DEFAULT_HEIGHT)
        mc._reset_position_estimator()
        logconf.start()
        with Multiranger(scf) as mr:
            print(f"{drone}: Ready to Fly!")
            while not enable:
                #Checks every 0.2 second
                enable = flight_enable
                time.sleep(0.2)
            if enable == True:  
                print(f"{drone}: Flying Started!")
                mc.take_off(height = 0.5, velocity = 0.5)
            mc.stop()
                # Drone 1 Logic
                if drone == "Drone 1" and (drone1 or groupDrone) and control_enable:
                    local_movement = movement
                    drone1_command = movement
                    printDroneMovementWhenChanged(movement_prev, local_movement, drone)
                    if local_movement == 'up':
                        mc.up(cmd_dist)
                        mc.stop()
                    elif local_movement == 'down':
                        mc.down(cmd_dist)
                        mc.stop()
                    elif local_movement == 'left':
                        mc.left(cmd_dist)
                        mc.stop()
                    elif local_movement == 'right':
                        mc.right(cmd_dist)
                        mc.stop()
                    elif local_movement == 'fward':
                        mc.forward(cmd_dist)
                        mc.stop()
                    elif local_movement == 'bward':
                        mc.back(cmd_dist)
                        mc.stop()
                    movement_prev = local_movement
                elif drone == "Drone 1":
                    drone1_command = 'hover'
                    
            print(f"{drone}: Stopping")
            mc.land()
        
        print(f"{drone}: Stopped")
        time.sleep(2)
        logconf.stop()

[gemenerik]

Hi John. Thanks for sharing your code. Before we dive in, can you verify that your CFs that share a radio are on the same radio channel? Channel switching takes a significant amount of time and can slow communications.

[john-amadora]

Yes,

URI1 = uri_helper.uri_from_env(default='radio://0/70/2M/E5E5E5E5E5')
URI2 = uri_helper.uri_from_env(default='radio://0/70/2M/E7E7E7E7E4')
URI3 = uri_helper.uri_from_env(default='radio://0/70/2M/E5E5E5E5E3')

Then called in the program via

    cf1=Crazyflie(rw_cache='./cache')
    cf2=Crazyflie(rw_cache='./cache')
    cf3=Crazyflie(rw_cache='./cache')  
    thread1 = threading.Thread(target=flydrone, args=(URI1, cf1, "Drone 1",))
    thread2 = threading.Thread(target=flydrone, args=(URI2, cf2, "Drone 2",))
    thread3 = threading.Thread(target=flydrone, args=(URI3, cf3, "Drone 3",))

[ArisMorgens]

Hello. I would suggest to do the following:

1. Move _reset_position_estimator before defining the logconf. Your code should look like this:

with SyncCrazyflie(URI, cf) as scf:
	mc = MotionCommander(scf, default_height = DEFAULT_HEIGHT)
        mc._reset_position_estimator()
        scf.cf.param.add_update_callback(group='deck', name='bcFlow2',
                                         cb=param_deck_flow)
        logconf = LogConfig(name=drone, period_in_ms=500)
        logconf.add_variable('stateEstimate.x', 'float')
        logconf.add_variable('stateEstimate.y', 'float')
        logconf.add_variable('stateEstimate.z', 'float')
        logconf.add_variable('pm.vbat', 'float')
        scf.cf.log.add_config(logconf)
        logconf.data_received_cb.add_callback(log_pos_callback)
        if not deck_attached_event.wait(timeout=3):
            print(f"{drone}:No flow deck detected!")
            enable = False
        logconf.start()

2. Replace uri_helper.uri_from_env with the Crazyflie id for a more direct connection to your Crazyflies:

URI1 = uri_helper.uri_from_env(default='radio://0/70/2M/E5E5E5E5E5')
URI1 = 'radio://0/70/2M/E5E5E5E5E5'

[john-amadora]

Hi! Tried this and still the same issue.Another question though, what is the difference of using uri_helper.uri_from_env and not using it? arent they supposed to be the same?

[ArisMorgens]

Hello. So, uri_helper.uri_from_env is used to connect to the first Crazyflie found and is not really used in swarm scenarios.
Regarding your problem, there might be some bug where the motion commander does not interact properly with threading. I opened a new issue here as it sounds like an interesting addition to have an example of multiple Crazyflies flying with the motion commander. I will keep you updated here if we get an example running, but please let us know if you find a solution to the problem.

As a temporary solution: I think it is possible to feed each drone with a different command at different timestamps through the high level commander and the swarm class. You can maybe try them out and let us know if you need any help.

[Nika250]

great, thank you!