CrazyFlie autonomous flight and control problem with the Python API

[danielcmdeveloper]

Good morning,
We have been trying for days to perform autonomous flights with the Python API provided.
We have installed Loco Positioning System and although there is a slight deviation of 10 cm positioning the drone is positioned relatively well.

Below, we show the examples we have used and finally the trajectories that are being presented. In this case the main problem is that the Crazyflie loses control after take-off.

Even the basic examples of movement in the BasicMotion.py file generate this problem.

We thought it might be a problem with 1 CrazyFlie in particular, but we have tried two others and they all behave the same way in the same way.

Performing test with take-off function (take_off_simple)

def take_off_simple(scf):
    with MotionCommander(scf, default_height=DEFAULT_HEIGHT) as mc:
        time.sleep(3)
        mc.stop()

In this case, the drone takes off and then goes out of control and does not maintain the hover.

It could be logical since this function is not performing any positioning control, so any external disturbance or the drone itself may cause the drone to move.

For this reason, a function has been implemented that controls the position and makes corrections to try to maintain the hover, but even so the drone does not manage to do so. (Below are some examples).

Example 1:

import time
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.positioning.motion_commander import MotionCommander

# URI de conexión - asegúrate de que coincide con la configuración de tu Crazyflie
# Inicializar drivers de Crazyflie
uri = 'radio://0/80/2M/E7E7E7E703'

# Velocidad deseada para las correcciones de vuelo estacionario en los ejes X e Y
desired_speed_x = 0.01  # Muy lento en el eje X
desired_speed_y = 0.01  # Muy lento en el eje Y


def connected_callback(link_uri):
    print(f'Conectado a {link_uri}')


def disconnected_callback(link_uri):
    print(f'Desconectado de {link_uri}')


def main():
    # Inicializar los controladores de radio
    cflib.crtp.init_drivers()

    # Crear una instancia de Crazyflie
    cf = Crazyflie(rw_cache='./cache')

    # Configurar los callbacks para conexión y desconexión
    cf.connected.add_callback(connected_callback)
    cf.disconnected.add_callback(disconnected_callback)

    # Intentar conectar
    print("Intentando conectar al Crazyflie...")
    cf.open_link(uri)

    # Esperar unos segundos para asegurar la conexión
    time.sleep(2)

    # Verificar si el Crazyflie está conectado antes de continuar
    if cf.is_connected():
        print("Conexión exitosa, iniciando vuelo estacionario...")

        # Establecer la altura predeterminada para el vuelo estacionario
        with MotionCommander(cf, default_height=0.5) as mc:
            # Mantener el vuelo estacionario con correcciones lentas
            flight_time = 10  # Duración del vuelo estacionario en segundos
            start_time = time.time()
            while time.time() - start_time < flight_time:
                # Aplicar velocidades muy lentas para mantener la posición
                mc.start_linear_motion(desired_speed_x, desired_speed_y, 0)
                time.sleep(0.1)  # Intervalo breve para ajustar la posición
    else:
        print("Crazyflie no está conectado. Revisa la conexión e intenta de nuevo.")


if __name__ == '__main__':
    main()

Example 2

import time
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.positioning.motion_commander import MotionCommander

# Inicializar drivers de Crazyflie
cflib.crtp.init_drivers()
uri = 'radio://0/80/2M/E7E7E7E703'

# Velocidad lenta de corrección en los ejes X e Y para mantener posición
desired_speed_x = 0.01
desired_speed_y = 0.01

# Conectar y desconectar callbacks
def connected_callback(link_uri):
    print(f'Conectado a {link_uri}')

def disconnected_callback(link_uri):
    print(f'Desconectado de {link_uri}')

def main():
    # Crear instancia Crazyflie
    cf = Crazyflie(rw_cache='./cache')

    # Agregar callbacks
    cf.connected.add_callback(connected_callback)
    cf.disconnected.add_callback(disconnected_callback)

    # Conectar al Crazyflie
    print("Intentando conectar al Crazyflie...")
    cf.open_link(uri)
    time.sleep(2)  # Espera para confirmar conexión

    # Verificar conexión antes de comenzar
    if cf.is_connected():
        print("Conexión exitosa, iniciando vuelo estacionario...")

        # Configurar altura y mantener vuelo estacionario con MotionCommander
        with MotionCommander(cf, default_height=0.5) as mc:
            # Despegar y alcanzar la altura predeterminada
            mc.take_off(height=0.5)
            time.sleep(2)  # Esperar a que el dron alcance la altura

            # Establecer una posición inicial estable
            cf.param.set_value('flightmode.posSet', 'true')
            flight_time = 20  # Tiempo de vuelo estacionario

            start_time = time.time()
            while time.time() - start_time < flight_time:
                # Corrección muy lenta para mantener hover
                mc.start_linear_motion(desired_speed_x, desired_speed_y, 0)
                time.sleep(0.05)  # Intervalo para reacción rápida

            # Iniciar aterrizaje
            mc.land()
            print("Aterrizaje completado.")
    else:
        print("Crazyflie no está conectado. Verifica e intenta nuevamente.")

if __name__ == '__main__':
    main()

Example 3:

import cflib
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.positioning.position_hl_commander import PositionHlCommander
from cflib.crazyflie.log import LogConfig
import time

cflib.crtp.init_drivers()

# URI de conexión y configuración
uri = 'radio://0/80/2M/E7E7E7E701'
hover_altitude = 0.5  # Altitud de vuelo deseada en metros
log_interval = 0.2  # Intervalo de logging en segundos (200 ms)
log_file = "drone_trajectory.txt"  # Nombre del archivo de registro


def hover_flight_with_logging(scf, log_file):
    # Configurar los parámetros de logging para obtener las coordenadas de vuelo
    log_config = LogConfig(name='Position', period_in_ms=int(log_interval * 1000))
    log_config.add_variable('stateEstimate.x', 'float')
    log_config.add_variable('stateEstimate.y', 'float')
    log_config.add_variable('stateEstimate.z', 'float')

    # Habilitar el control de posición y altitud
    scf.cf.param.set_value('flightmode.posSet', '1')
    scf.cf.param.set_value('flightmode.althold', '1')

    # Abrir archivo en modo escritura y añadir el encabezado
    with open(log_file, "w") as file:
        file.write("Time(s), X(m), Y(m), Z(m)\n")

        def log_callback(timestamp, data, logconf):
            # Guardar las coordenadas en el archivo
            file.write(
                f"{timestamp / 1000.0}, {data['stateEstimate.x']}, {data['stateEstimate.y']}, {data['stateEstimate.z']}\n")

        # Añadir y activar el registro de datos
        scf.cf.log.add_config(log_config)
        log_config.data_received_cb.add_callback(log_callback)
        log_config.start()

        # Configurar el PositionHlCommander para el vuelo estacionario
        with PositionHlCommander(scf.cf, default_height=hover_altitude,
                                 controller=PositionHlCommander.CONTROLLER_PID) as pc:
            # Despegar
            pc.take_off()
            print("Despegando y estabilizando posición")

            # Mantener la posición durante 5 segundos
            time.sleep(5)

            # Aterrizar suavemente
            pc.land()
            print("Aterrizaje completado")

        # Detener el logging después del vuelo
        log_config.stop()


# Ejecutar el vuelo estacionario con logging de coordenadas
with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf:
    hover_flight_with_logging(scf, log_file)

With all these examples and those provided as native examples, the drone neither maintains stability nor performs controlled translations; it directly leaves the flight area until it crashes.

Below I show the graphs of the trajectories within the airfield so that you can see how he loses control.

We need to know where we are going wrong and how to perform this kind of examples and drone control, both hovering and controlled translations within the flight area established with Loco Positioning System.

Thank you very much for everything.
Best regards
Daniel Caballero :-)

[Bananazooka]

Hi Daniel,
First of all, you should determine if you're able to take off without crashing. You could use the flight controller tab (shown here) to try and hover in a stable way.
What you describe could be caused by the Crazyflie being started looking at the wrong direction. Have you followed the first steps described here?

[danielcmdeveloper]

Good morning,

Finally we have managed to perform the hover flight correctly ( hover ).
Thanks to the help of @Bananazooka we managed to see that the aircraft's take-off orientation was not correct, which caused a loss of control and subsequent collision.

By setting the original orientation, we have managed to keep the aircraft in hovering flight as shown in the video below.

As a help for the rest of the people, we show below the code that performs a hovering flight for about 40 seconds approximately.

VERY IMPORTANT: THE DRONE MUST BE SET IN THE CORRECT ORIENTATION AT TAKE-OFF AS SPECIFIED HERE. : https://www.bitcraze.io/documentation/tutorials/getting-started-with-flying-using-lps/

import time
import cflib.crtp
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
from cflib.crazyflie.high_level_commander import HighLevelCommander

# Inicializar drivers de Crazyflie
cflib.crtp.init_drivers()
uri = 'radio://0/80/2M/E7E7E7E703'

# Variables para almacenar la posición inicial y final
initial_x = None
initial_y = None
final_x = None
final_y = None
final_z = None

def position_callback(timestamp, data, logconf):
    global initial_x, initial_y, final_x, final_y, final_z
    x = data['stateEstimate.x']
    y = data['stateEstimate.y']
    z = data['stateEstimate.z']

    if initial_x is None and initial_y is None:
        # Guardar la posición inicial solo una vez, al iniciar
        initial_x = x
        initial_y = y
        print(f"Posición inicial registrada: X={initial_x}, Y={initial_y}")

    # Actualizar la posición final en cada callback
    final_x = x
    final_y = y
    final_z = z

def main():
    # Crear una instancia de Crazyflie
    cf = Crazyflie(rw_cache='./cache')

    # Configurar el registro de posición
    logconf = LogConfig(name='Position', period_in_ms=100)
    logconf.add_variable('stateEstimate.x', 'float')
    logconf.add_variable('stateEstimate.y', 'float')
    logconf.add_variable('stateEstimate.z', 'float')

    # Usar SyncCrazyflie para manejar la conexión
    with SyncCrazyflie(uri, cf=cf) as scf:


        # Configurar y comenzar el registro de datos
        scf.cf.log.add_config(logconf)
        logconf.data_received_cb.add_callback(position_callback)
        logconf.start()

        # Esperar a que la posición inicial esté registrada
        while initial_x is None or initial_y is None:
            time.sleep(0.1)

        print("Posición inicial registrada, iniciando vuelo estacionario...")

        # Crear un objeto HighLevelCommander para usar `go_to`
        commander = scf.cf.high_level_commander

        # Altura de despegue
        commander.takeoff(1.0, 5.0)  # Despegar a 1 m de altura en 2 segundos
        time.sleep(40)
    
        # Posición objetivo para la traslación frontal en coordenadas absolutas
        #target_x = initial_x + 0.5  # Avance de 10 cm en el eje X
        #target_y = initial_y
        #print("1")
        # Trasladarse a la posición objetivo
        #commander.go_to(target_x, target_y, 1.0, 0.2, 10, relative=False)  # Moverse a la nueva coordenada
        #time.sleep(3)
        #print("2")
        # Regreso a la posición inicial registrada
        #commander.go_to(initial_x, initial_y, 1.0, 0.2, 10, relative=False)  # Volver a la posición inicial
        #time.sleep(3)
        #print("3")
        # Aterrizar
        commander.land(0.0, 5.0)  # Aterrizar en 2 segundos
        time.sleep(3)

        print("Vuelo completado.")
        print(f"Posición final registrada: X={final_x}, Y={final_y}, Z={final_z}")

if __name__ == '__main__':
    main()

Thanks again for your help @Bananazooka.
Best regards
Daniel Caballero

[Bananazooka]

Happy that it was solved!