Fly 3 crazyflies using swarm

Discussions and questions about the Crazyflie Nano Quadcopter
Post Reply
yousba
Beginner
Posts: 11
Joined: Tue Jan 28, 2020 1:54 pm

Fly 3 crazyflies using swarm

Post by yousba »

Hello,

After flying 2 drones using swarm I modified the script to try with 3 Drones but when I run the script all 3 drones become completely unstable knowing that I just added a new Uri. Here is my script :

Code: Select all

#!/usr/bin/env python3
# -*- 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.
"""
Simple example of a synchronized swarm choreography using the High level
commander.

The swarm takes off and flies a synchronous choreography before landing.
The take-of is relative to the start position but the Goto are absolute.
The sequence contains a list of commands to be executed at each step.

This example is intended to work with any absolute positioning system.
It aims at documenting how to use the High Level Commander together with
the Swarm class to achieve synchronous sequences.
"""
import threading
import time
from collections import namedtuple
from queue import Queue

import cflib.crtp
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.swarm import CachedCfFactory
from cflib.crazyflie.swarm import Swarm
from cflib.crazyflie.syncLogger import SyncLogger

# Time for one step in second
STEP_TIME = 1

# Possible commands, all times are in seconds
Takeoff = namedtuple('Takeoff', ['height', 'time'])
Land = namedtuple('Land', ['time'])
Goto = namedtuple('Goto', ['x', 'y', 'z', 'time'])
# RGB [0-255], Intensity [0.0-1.0]
Ring = namedtuple('Ring', ['r', 'g', 'b', 'intensity', 'time'])
# Reserved for the control loop, do not use in sequence
Quit = namedtuple('Quit', [])

uris = [
    'radio://0/80/2M/E7E7E7E701',  # cf_id 0, startup position [0.70, 1.27]
    'radio://0/80/2M/E7E7E7E702',   # cf_id 1, startup position [ 0.70, 0.84]
    'radio://0/80/2M/E7E7E7E703',
   # 'radio://0/10/2M/E7E7E7E703',  # cf_id 3, startup position [0.5, 0.5]
    # Add more URIs if you want more copters in the swarm
]

sequence = [
    # Step, CF_id,  action
    (0,    0,      Takeoff(1, 5)),
    (0,    1,      Takeoff(0.5, 5)),
    (0,    2,      Takeoff(0.5, 5)),







    (1,    0,      Land(5)),
    (1,    1,      Land(5)),
    (1,    2,      Land(5)),



]


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(scf)


def activate_high_level_commander(scf):
    scf.cf.param.set_value('commander.enHighLevel', '1')


def activate_mellinger_controller(scf, use_mellinger):
    controller = 1
    if use_mellinger:
        controller = 2
    scf.cf.param.set_value('stabilizer.controller', str(controller))


def set_ring_color(cf, r, g, b, intensity, time):
    cf.param.set_value('ring.fadeTime', str(time))

    r *= intensity
    g *= intensity
    b *= intensity

    color = (int(r) << 16) | (int(g) << 8) | int(b)

    cf.param.set_value('ring.fadeColor', str(color))


def crazyflie_control(scf):
    cf = scf.cf
    control = controlQueues[uris.index(cf.link_uri)]

    activate_mellinger_controller(scf, True)

    commander = scf.cf.high_level_commander

    # Set fade to color effect and reset to Led-ring OFF
    set_ring_color(cf, 0, 0, 0, 0, 0)
    cf.param.set_value('ring.effect', '14')

    while True:
        command = control.get()
        if type(command) is Quit:
            return
        elif type(command) is Takeoff:
            commander.takeoff(command.height, command.time)
        elif type(command) is Land:
            commander.land(0.0, command.time)
        elif type(command) is Goto:
            commander.go_to(command.x, command.y, command.z, 0, command.time)
        elif type(command) is Ring:
            set_ring_color(cf, command.r, command.g, command.b,
                           command.intensity, command.time)
            pass
        else:
            print('Warning! unknown command {} for uri {}'.format(command,
                                                                  cf.uri))


def control_thread():
    pointer = 0
    step = 0
    stop = False

    while not stop:
        print('Step {}:'.format(step))
        while sequence[pointer][0] <= step:
            cf_id = sequence[pointer][1]
            command = sequence[pointer][2]

            print(' - Running: {} on {}'.format(command, cf_id))
            controlQueues[cf_id].put(command)
            pointer += 1

            if pointer >= len(sequence):
                print('Reaching the end of the sequence, stopping!')
                stop = True
                break

        step += 1
        time.sleep(STEP_TIME)

    for ctrl in controlQueues:
        ctrl.put(Quit())


if __name__ == '__main__':
    controlQueues = [Queue() for _ in range(len(uris))]

    cflib.crtp.init_drivers(enable_debug_driver=False)
    factory = CachedCfFactory(rw_cache='./cache')
    with Swarm(uris, factory=factory) as swarm:
        swarm.parallel_safe(activate_high_level_commander)
        swarm.parallel_safe(reset_estimator)

        print('Starting sequence!')

        threading.Thread(target=control_thread).start()

        swarm.parallel_safe(crazyflie_control)

        time.sleep(1)
I even tried this script: https://github.com/bitcraze/crazyflie-l ... equence.py but still the same problem. can anyone help me, thanks in advance

kimberly
Bitcraze
Posts: 465
Joined: Fri Jul 06, 2018 11:13 am

Re: Fly 3 crazyflies using swarm

Post by kimberly »

Hi! I can try to recreate your problem here. Could give us more info about your setup? For example: Which deck or positioning system you are using, with version of the cflib you are using and if the crazyflies are flashed to the latest release, on which OS you are running this one.

yousba
Beginner
Posts: 11
Joined: Tue Jan 28, 2020 1:54 pm

Re: Fly 3 crazyflies using swarm

Post by yousba »

Hello,
Thank you for your answer,

I work with a Loco positioning deck. in the last version of Cflib.

I fly my crazyflies in the Loco positioning, mode tdoa2 with 8 anchors, for the crazyflies I flashed them with the latest version.

My OS is ubuntu.

kimberly
Bitcraze
Posts: 465
Joined: Fri Jul 06, 2018 11:13 am

Re: Fly 3 crazyflies using swarm

Post by kimberly »

Ah oke great, thanks for letting me know. Currently I can not go to the lab(which has a LPS system) so I won't be able to test this out until next week. However, since you are using the LPS system with multiple drones, it might be useful for you to test out different LPS modes. It is not possible to fly multiple with TWR but it is with TDOA2 and TDOA3.

The bottom of this tutorial explains how to change mode: https://www.bitcraze.io/documentation/t ... ng-system/. Let us know if this works when you change the mode.

yousba
Beginner
Posts: 11
Joined: Tue Jan 28, 2020 1:54 pm

Re: Fly 3 crazyflies using swarm

Post by yousba »

Unfortunately I tried the two modes tdoa2 and tdoa3, but still the same problem I even reconfigured the anchors in tdoa2 and tdoa3 mode

kimberly
Bitcraze
Posts: 465
Joined: Fri Jul 06, 2018 11:13 am

Re: Fly 3 crazyflies using swarm

Post by kimberly »

So I tried out your script with 3 crazyflies 2.1 with an LPS deck. I made some adjustments to your script thought and I'm flying in TDOA3 (but that shouldn't make a huge difference, Only TWR is not advised)

Code: Select all

    (0,    0,      Takeoff(1.5, 5)),
    (0,    1,      Takeoff(1.0, 5)),
    (0,    2,      Takeoff(1.5, 5)),
and the step_time is set to 5 seconds.

the takeoff(1.5, 5) says that you need to get to that height in 5 seconds, however the step_time of 1 second like in your previous script is not giving enough time to do that, so it stays near the ground which there is the ground effect which will make the crazyflies unstable, especially when there are others around. The crazyflies need to take off the ground immediately.

So I tried it out and it worked fine for me! Maybe also try with the changed heights as well and the increased time step? Let us know this improves anything for you

yousba
Beginner
Posts: 11
Joined: Tue Jan 28, 2020 1:54 pm

Re: Fly 3 crazyflies using swarm

Post by yousba »

I changed the time and heights, I did several tests but still the same problem.

kimberly
Bitcraze
Posts: 465
Joined: Fri Jul 06, 2018 11:13 am

Re: Fly 3 crazyflies using swarm

Post by kimberly »

Are you able to provide a video of the flight? I can compare them to the test I've done here

kimberly
Bitcraze
Posts: 465
Joined: Fri Jul 06, 2018 11:13 am

Re: Fly 3 crazyflies using swarm

Post by kimberly »

Just give us an update if you keep having problems or if you have fixed your issue.

Post Reply