Drone Swarming with HTC Vive

Topics related to the Lighthouse positioning system, configuration and use
skyfall
Beginner
Posts: 14
Joined: Thu Aug 06, 2020 8:04 am

Re: Drone Swarming with HTC Vive

Post by skyfall »

Tried running the example code already with 2 drones, but then only manage to get one of the drones flying properly. The other one managed to take off, but then it lost control after a while and fell eventually.

In the code, it states that the layout must be as follows:

Code: Select all

     >

^    +    v

     <
How do I know which drone to place at which specific position within that formation? I assume URI2 is the middle one since it is the only one with a different altitude of 0.5m.

By the way, quick question, for this example code, do I need both the flow deck and the lighthouse deck in order to execute this code? Or only the LH Deck?
kimberly
Bitcraze
Posts: 518
Joined: Fri Jul 06, 2018 11:13 am

Re: Drone Swarming with HTC Vive

Post by kimberly »

Hi,

About the second drone falling; was this with the flowdeck or with the lighthouse deck? There are two reasons why it will fall like that: 1. battery is too low (LED1 red LED is fully red). 2. The drone is outside of the tracking area (if lighthousedeck) or flies over low texture area (if flowdeck).

URI2 is indeed in the middle. Where the others are does not make a difference, since the following command is relative of where they start:

Code: Select all

cf.commander.send_hover_setpoint(d * comp * math.pi / circle_time, 0, 360.0 / circle_time, z) 
So URI0,1,3,4 can be placed wherever you want, as long as they are facing the circle as indicated in the drawing.

You can use either the flowdeck or the lighthouse deck, but there is no need to use them at the same time for your application
skyfall
Beginner
Posts: 14
Joined: Thu Aug 06, 2020 8:04 am

Re: Drone Swarming with HTC Vive

Post by skyfall »

Previously, I was using both the LH deck and Flow Deck, which only 1 was working. I tried to remove the Flow deck, utilizing only the LH deck, but the results seemed to be worst. Both the drone didn't execute as expected. Following is the modified code for 2 drones for the swarmSequenceCircle.py:

Code: Select all


# -*- coding: utf-8 -*-
#
#     ||          ____  _ __
#  +------+      / __ )(_) /_______________ _____  ___
#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
#
#  Copyright (C) 2017 Bitcraze AB
#
#  Crazyflie Nano Quadcopter Client
#
#  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.
"""
A script to fly 5 Crazyflies in formation. One stays in the center and the
other four fly around it in a circle. Mainly intended to be used with the
Flow deck.
The starting positions are vital and should be oriented like this

     >

^    +    v

     <

The distance from the center to the perimeter of the circle is around 0.5 m

"""
import math
import time

import cflib.crtp
from cflib.crazyflie.swarm import CachedCfFactory
from cflib.crazyflie.swarm import Swarm

# Change uris according to your setup
URI0 = 'radio://0/80/2M/E7E7E7E7EB'
#URI1 = 'radio://0/80/2M/E7E7E7E7E'
URI2 = 'radio://0/80/2M/E7E7E7E7EA'
#URI3 = 'radio://0/80/2M/E7E7E7E7EA'
#URI4 = 'radio://0/110/2M/E7E7E7E703'

# d: diameter of circle
# z: altitude
params0 = {'d': 1.0, 'z': 0.3}
#params1 = {'d': 1.0, 'z': 0.3}
params2 = {'d': 0.0, 'z': 0.5}
#params3 = {'d': 1.0, 'z': 0.3}
#params4 = {'d': 1.0, 'z': 0.3}


uris = {
    URI0,
    #URI1,
    URI2,
    #URI3,
    #URI4,
}

params = {
    URI0: [params0],
    #URI1: [params1],
    URI2: [params2],
    #URI3: [params3],
    #URI4: [params4],
}


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')
    time.sleep(2)


def poshold(cf, t, z):
    steps = t * 10

    for r in range(steps):
        cf.commander.send_hover_setpoint(0, 0, 0, z)
        time.sleep(0.1)


def run_sequence(scf, params):
    cf = scf.cf

    # Number of setpoints sent per second
    fs = 4
    fsi = 1.0 / fs

    # Compensation for unknown error :-(
    comp = 1.3

    # Base altitude in meters
    base = 0.15

    d = params['d']
    z = params['z']

    poshold(cf, 2, base)

    ramp = fs * 2
    for r in range(ramp):
        cf.commander.send_hover_setpoint(0, 0, 0, base + r * (z - base) / ramp)
        time.sleep(fsi)

    poshold(cf, 2, z)

    for _ in range(2):
        # The time for one revolution
        circle_time = 8

        steps = circle_time * fs
        for _ in range(steps):
            cf.commander.send_hover_setpoint(d * comp * math.pi / circle_time,
                                             0, 360.0 / circle_time, z)
            time.sleep(fsi)

    poshold(cf, 2, z)

    for r in range(ramp):
        cf.commander.send_hover_setpoint(0, 0, 0,
                                         base + (ramp - r) * (z - base) / ramp)
        time.sleep(fsi)

    poshold(cf, 1, base)

    cf.commander.send_stop_setpoint()


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

    factory = CachedCfFactory(rw_cache='./cache')
    with Swarm(uris, factory=factory) as swarm:
        swarm.parallel(reset_estimator)
        swarm.parallel(run_sequence, args_dict=params)

I am only using URI0 (Drone on the outer circle) and URI2 (Middle Drone). Is there any mistake in the modified example code?
kimberly
Bitcraze
Posts: 518
Joined: Fri Jul 06, 2018 11:13 am

Re: Drone Swarming with HTC Vive

Post by kimberly »

It does not seem like it there is anything wrong in the code.

It would be good to get back to one again and see what is happening. You did replace the geometry data for your lighthouse system in the firmware for the crazyflies right?

Could you check with the plotter and position estimate (stateEstimate.x/y/z) if the drones are showing a position estimate that is correct and not diverting?
skyfall
Beginner
Posts: 14
Joined: Thu Aug 06, 2020 8:04 am

Re: Drone Swarming with HTC Vive

Post by skyfall »

Here is the result of the stateEstimate.x/y/z that I get when the drone is stationary at the centre of the play area. Not too sure how to interpret the graph though.
StateEstimate.JPG
kimberly
Bitcraze
Posts: 518
Joined: Fri Jul 06, 2018 11:13 am

Re: Drone Swarming with HTC Vive

Post by kimberly »

Thanks for sharing! This is making a lot of things clear. what you see is the values diverting, which means it is not getting enough information to estimate position. You have to step back to when you were still setting up the lighthouse positioning system because this has failed somewhere. Probably when you tried out the single crazyflie scripts, it worked for you because of the flowdeck, but not because of the lighthouse deck.

Go carefully through the guide again until you see those values on the plotter of the state estimate become stable. Also check the console tab of the cfclient if the deck is detected at all.
skyfall
Beginner
Posts: 14
Joined: Thu Aug 06, 2020 8:04 am

Re: Drone Swarming with HTC Vive

Post by skyfall »

I have read through the guide again already. Here is the data that I got from the plotter:
StateEstimate2.JPG
Everything seems constant, but when I run the code, the drone still doesn't seem to work still. An 'Error no LogEntry' problem also came out while executing the script.

Code: Select all

bitcraze@ubuntu:~/projects/crazyflie-lib-python/examples/swarm$ python3 swarmSequenceCircle2Drones.py 
Connecting to radio://0/80/2M/E7E7E7E7EB
Connecting to radio://0/80/2M/E7E7E7E7EA
Error no LogEntry to handle id=24
Error no LogEntry to handle id=26
Error no LogEntry to handle id=15
Error no LogEntry to handle id=16
Connected to radio://0/80/2M/E7E7E7E7EB
Connected to radio://0/80/2M/E7E7E7E7EA
bitcraze@ubuntu:~/projects/crazyflie-lib-python/examples/swarm$ 
kimberly
Bitcraze
Posts: 518
Joined: Fri Jul 06, 2018 11:13 am

Re: Drone Swarming with HTC Vive

Post by kimberly »

that plot looks much better.

With the drone not working, do you mean that they still crash or just don't take off at all? Have you tried again the commander scripts for just one crazyflie?
skyfall
Beginner
Posts: 14
Joined: Thu Aug 06, 2020 8:04 am

Re: Drone Swarming with HTC Vive

Post by skyfall »

They do take off, but then it will fly randomly and crash eventually. Since the swarmcircle involves 2 drones, I moved on to work on the hl-commander-swarm example first with 1 drone to see if the positioning working or not. It produces same outcome though unfortunately. Here is the code:

Code: Select all

# -*- 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 swarm using the High level commander.

The swarm takes off and flies a synchronous square shape before landing.
The trajectories are relative to the starting positions and the Crazyflies can
be at any position on the floor when the script is started.

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.
"""
import time

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


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')
    time.sleep(2)
    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', controller)


def run_shared_sequence(scf):
    activate_mellinger_controller(scf, False)

    box_size = 1
    flight_time = 2

    commander = scf.cf.high_level_commander

    commander.takeoff(1.0, 2.0)
    time.sleep(3)
    print('Take Off')

    commander.go_to(box_size, 0, 0, 0, flight_time, relative=True)
    time.sleep(flight_time)
    print('Movement1')

    commander.go_to(0, box_size, 0, 0, flight_time, relative=True)
    time.sleep(flight_time)
    print('Movement2')

    commander.go_to(-box_size, 0, 0, 0, flight_time, relative=True)
    time.sleep(flight_time)
    print('Movement3')

    commander.go_to(0, -box_size, 0, 0, flight_time, relative=True)
    time.sleep(flight_time)
    print('Movement4')

    commander.land(0.0, 2.0)
    time.sleep(2)
    print('Landing')

    commander.stop()
    print('Stop')


uris = {
    'radio://0/80/2M/E7E7E7E7EB',
    # Add more URIs if you want more copters in the swarm
}

if __name__ == '__main__':
    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)
        swarm.parallel_safe(run_shared_sequence)
I decided to print a text after executing every movement to see if they actually execute it.
kimberly
Bitcraze
Posts: 518
Joined: Fri Jul 06, 2018 11:13 am

Re: Drone Swarming with HTC Vive

Post by kimberly »

This is very strange indeed.

Do you know which version of the firmware you have flashed on the crazyflies? Is the latest release according to the getting started guide. You mentioned that your crazyflies were already used a while.
Post Reply