Demo that makes the crazyflie with the LED-ring fly 3 concentric circles in the colors: red, blue and green.

lighthouse_concentric_circles.py

#!/usr/bin/env python3
#Demo that makes the crazyflie fly 3 concentric circles in the colors: red, blue and green.


import sys
import time
import math
import numpy

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
from cflib.positioning.position_hl_commander import PositionHlCommander

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


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


def position_callback(timestamp, data, logconf):
    x = data['kalman.stateX']
    y = data['kalman.stateY']
    z = data['kalman.stateZ']
    print('pos: ({}, {}, {})'.format(x, y, z))


def start_position_printing(scf):
    log_conf = LogConfig(name='Position', period_in_ms=500)
    log_conf.add_variable('kalman.stateX', 'float')
    log_conf.add_variable('kalman.stateY', 'float')
    log_conf.add_variable('kalman.stateZ', 'float')

    scf.cf.log.add_config(log_conf)
    log_conf.data_received_cb.add_callback(position_callback)
    log_conf.start()


def vector_substract(v0, v1):
    return [v0[0] - v1[0], v0[1] - v1[1], v0[2] - v1[2]]


def vector_add(v0, v1):
    return [v0[0] + v1[0], v0[1] + v1[1], v0[2] + v1[2]]


def run_sequence(scf):
    cf = scf.cf

    cf.param.set_value('ring.effect','0')


    radii = [0.5, 0.3, 0.1]
    center = 1.3


    with PositionHlCommander(
            scf,
            x=0.0, y=0.0, z=0.0,
            default_velocity=0.3,
            default_height=1.0,
            controller=PositionHlCommander.CONTROLLER_PID) as pc:
        
        time.sleep(1.0)

        setpoint = [0.0, 0.0, center-radii[0]]
        pc.go_to(setpoint[0], setpoint[1],setpoint[2])

        time.sleep(10.0)

        # make outer circle (green)
        cf.param.set_value('ring.effect','7')
        cf.param.set_value('ring.solidBlue','20')
        cf.param.set_value('ring.solidRed','20')
        cf.param.set_value('ring.solidGreen','100')
    
        time.sleep(1.0)

        for it_pi in numpy.arange(-math.pi+0.1,math.pi+0.2,0.1):
            z_circle = radii[0]*math.sin(it_pi+0.5*math.pi)+center
            y_circle = radii[0]*math.cos(it_pi+0.5*math.pi)
            print(z_circle,y_circle)
    
            pc.go_to(setpoint[0], y_circle, z_circle)
            time.sleep(0.1)
    

        # make inner circle
        cf.param.set_value('ring.effect','0')
        setpoint = [0.0, 0.0, center-radii[1]]
        pc.go_to(setpoint[0], setpoint[1],setpoint[2])
        time.sleep(1.0)
    

        cf.param.set_value('ring.effect','7')
        cf.param.set_value('ring.solidBlue','20')
        cf.param.set_value('ring.solidRed','100')
        cf.param.set_value('ring.solidGreen','20')
        time.sleep(1)

    
        for it_pi in numpy.arange(-math.pi+0.2,math.pi+0.4,0.2):
            z_circle = radii[1]*math.sin(it_pi+0.5*math.pi)+center
            y_circle = radii[1]*math.cos(it_pi+0.5*math.pi)
            print(z_circle,y_circle)
    
            pc.go_to(setpoint[0], y_circle, z_circle)
            time.sleep(0.1)
    
        cf.param.set_value('ring.effect','0')
   
    
        # make inner inner circle
        setpoint = [0.0, 0.0, center-radii[2]]
        pc.go_to(setpoint[0], setpoint[1],setpoint[2])
        time.sleep(1.0)
        
        cf.param.set_value('ring.effect','7')
        cf.param.set_value('ring.solidBlue','100')
        cf.param.set_value('ring.solidRed','20')
        cf.param.set_value('ring.solidGreen','20')
        
        time.sleep(1)
    
        
        for it_pi in numpy.arange(-math.pi+0.3,math.pi+0.6,0.3):
            z_circle = radii[2]*math.sin(it_pi+0.5*math.pi)+center
            y_circle = radii[2]*math.cos(it_pi+0.5*math.pi)
            print(z_circle,y_circle)
    
            pc.go_to(setpoint[0], y_circle, z_circle)
            time.sleep(0.1)

        cf.param.set_value('ring.effect','0')
    
        
        pc.go_to(0.0, 0.0, 0.1)
    
        # Make sure that the last packet leaves before the link is closed
        # since the message queue is not flushed before closing
        time.sleep(0.5)
        
        


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

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