Swarm and UAV Trajectory

Discussions about autonomous flight in general, regardless of positioning method
Post Reply
gabrielalima
Beginner
Posts: 10
Joined: Thu Jan 24, 2019 12:15 pm

Swarm and UAV Trajectory

Post by gabrielalima »

Hi!

In CF 2.0, I'm used to swarm flying using high level functions.

And recently, I tested on an aircraft the UAV trajectory package, where you unload a trajectory based on polynomials ...

Now I would like to join these programs. Swarm and trajectory. But I'm having problems ...

I created a file to upload on each aircraft the trajectory. Okay, apparently it worked.

And I tried to adapt an example of swarm, to work with the command: commander.start_trajectory

But it doesn't work well ... Questions about what to send in run_sequence ..

Code: Select all

"""
Swarm
"""

import time
import csv
import datetime
import numpy as np

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

# Change uris and sequences according to your setup
URI1 = 'radio://0/81/2M'
URI2 = 'radio://1/82/2M'

###    x   y   z  time
##sequence1 = [
##    (-0.7,-1.2,0.8,2),
##    (-0.7,0.0,0.8,2),
##    (0.0,0.0,0.8,2),
##    (0.8,0.0,0.8,2),
##    (0.8,-1.2,0.8,2),
##    (0.0,-1.2,0.8,2),
##    (-0.7,-1.2,0.8,2),
##]
##sequence2 = [
##    (-0.7,0.0,0.8,2),
##    (-0.7,1.5,0.8,2),
##    (0.0,1.5,0.8,2),
##    (0.8,1.5,0.8,2),
##    (0.8,0.0,0.8,2),
##    (0.0,0.0,0.8,2),
##    (-0.7,0.0,0.8,2),
##]

##seq_args = {
##    URI1: [sequence1],
##    URI2: [sequence2],
##    
##}

# List of URIs, comment the one you do not want to fly
uris = {
    URI1,
    URI2,    
}


x1_vetor = []
y1_vetor = []
z1_vetor = []
x2_vetor = []
y2_vetor = [] 
z2_vetor = []


def wait_for_position_estimator(scf):
    print('Waiting for estimator to find position...')

    log_config = LogConfig(name='Kalman Variance', period_in_ms=100)
    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 activate_high_level_commander(scf):
    scf.cf.param.set_value('commander.enHighLevel', '1')


def activate_controller(scf):
    # 1-PID 2-Mellinger
    controller = 2
    scf.cf.param.set_value('stabilizer.controller', controller)

def run_sequence(scf, args_dict=seq_args):

    global x1_vetor
    global y1_vetor
    global z1_vetor
    global x2_vetor
    global y2_vetor
    global z2_vetor
    
    trajectory_id = 1
    commander = scf.cf.high_level_commander

    commander.takeoff(0.8, 2.0)
    time.sleep(3.0)

    relative = True
    commander.start_trajectory(trajectory_id, 1.0, relative)
    time.sleep(duration)
    
    commander.land(0.0, 2.0)
    time.sleep(2)
    commander.stop()



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(activate_controller)
        swarm.parallel_safe(reset_estimator)
        swarm.parallel_safe(start_position_printing)
        swarm.parallel_safe(run_sequence, args_dict=seq_args)


gabrielalima
Beginner
Posts: 10
Joined: Thu Jan 24, 2019 12:15 pm

Re: Swarm and UAV Trajectory

Post by gabrielalima »

Problem solved!! :D
Post Reply