TestingV1.py

"""
Simple example that connects to the first Crazyflie found, logs the Stabilizer
and prints it to the console.

The Crazyflie is controlled using the commander interface

Press q to Kill the drone in case of emergency

After 50s the application disconnects and exits.
"""
import logging
import time
from threading import Timer
import threading
import math

from pynput import keyboard # Import the keyboard module for key press detection

import cflib.crtp  # noqa
from cflib.crazyflie import Crazyflie
from cflib.crazyflie.log import LogConfig
from cflib.utils import uri_helper

# TODO: CHANGE THIS URI TO YOUR CRAZYFLIE & YOUR RADIO CHANNEL
uri = uri_helper.uri_from_env(default='radio://0/10/2M/E7E7E7E701') #Example for group 17

# Only output errors from the logging framework
logging.basicConfig(level=logging.ERROR)

# CONSTANTS
DISTANCE_TRESHOLD = 0.05
TAKEOFF_HEIGHT = 0.4

class LoggingExample:
    """
    Simple logging example class that logs the Stabilizer from a supplied
    link uri and disconnects after 10s.
    """

    def __init__(self, link_uri):
        """ Initialize and run the example with the specified link_uri """

        self._cf = Crazyflie(rw_cache='./cache')

        self.prev_z_ = [None, None, None, None, None]

        # Connect some callbacks from the Crazyflie API
        self._cf.connected.add_callback(self._connected)
        self._cf.disconnected.add_callback(self._disconnected)
        self._cf.connection_failed.add_callback(self._connection_failed)
        self._cf.connection_lost.add_callback(self._connection_lost)

        print('Connecting to %s' % link_uri)

        # Try to connect to the Crazyflie
        self._cf.open_link(link_uri)

        # Variable used to keep main loop occupied until disconnect
        self.is_connected = True

        # Init states variables
        self.sensor_data = {}

        self.sensor_data['t'] = 0
        self.sensor_data["x"] = 0
        self.sensor_data["y"] = 0
        self.sensor_data["z"] = 0
        self.sensor_data["yaw"] = 0

        # Accumulators for smoothing / hand detection
        self.accumulator_z = [0]*10

        # Boolean states
        self.emergency_stop = False
        self.block_callback = False

    def _connected(self, link_uri):
        """ This callback is called form the Crazyflie API when a Crazyflie
        has been connected and the TOCs have been downloaded."""
        print('Connected to %s' % link_uri)

        # The definition of the logconfig can be made before connecting
        self._lg_stab = LogConfig(name='Stabilizer', period_in_ms=50)
        self._lg_stab.add_variable('stateEstimate.x', 'float')
        self._lg_stab.add_variable('stateEstimate.y', 'float')
        self._lg_stab.add_variable('stateEstimate.z', 'float')
        self._lg_stab.add_variable('stabilizer.yaw', 'float')

        # The fetch-as argument can be set to FP16 to save space in the log packet
        # self._lg_stab.add_variable('pm.vbat', 'FP16')

        # Adding the configuration cannot be done until a Crazyflie is
        # connected, since we need to check that the variables we
        # would like to log are in the TOC.
        try:
            self._cf.log.add_config(self._lg_stab)
            # This callback will receive the data
            self._lg_stab.data_received_cb.add_callback(self._stab_log_data)
            # This callback will be called on errors
            self._lg_stab.error_cb.add_callback(self._stab_log_error)
            # Start the logging
            self._lg_stab.start()
        except KeyError as e:
            print('Could not start log configuration,'
                  '{} not found in TOC'.format(str(e)))
        except AttributeError:
            print('Could not add Stabilizer log config, bad configuration.')

        # Start a timer to disconnect in 50s     TODO: CHANGE THIS TO YOUR NEEDS
        t = Timer(50, self._cf.close_link)
        t.start()

    def _stab_log_error(self, logconf, msg):
        """Callback from the log API when an error occurs"""
        print('Error when logging %s: %s' % (logconf.name, msg))

    def _stab_log_data(self, timestamp, data, logconf):
        """Callback from a the log API when data arrives"""

        # Print the data to the console to see what the Logger is getting !
        #print(f'[{timestamp}][{logconf.name}]: ', end='')
        # for name, value in data.items():
        #     print(f'{name}: {value:3.3f} ', end='')
        # print()

        # Store the state estimate data you need in a dictionary
        if not(self.block_callback):
            self.block_callback = True  # Prevents the callback from being called again while processing

            # Update the other states
            for name, value in data.items():
                if name == 'stateEstimate.x':
                    self.sensor_data['x'] = value
                if name == 'stateEstimate.y':
                    self.sensor_data['y'] = value
                if name == 'stateEstimate.z':
                    self.sensor_data['z'] = value
                    # Update the accumulator
                    self.accumulator_z.append(value)
                    self.accumulator_z.pop(0)
                if name == 'stabilizer.yaw':
                    self.sensor_data['yaw'] = value

            self.block_callback = False

    def _connection_failed(self, link_uri, msg):
        """Callback when connection initial connection fails (i.e no Crazyflie
        at the specified address)"""
        print('Connection to %s failed: %s' % (link_uri, msg))
        self.is_connected = False

    def _connection_lost(self, link_uri, msg):
        """Callback when disconnected after a connection has been made (i.e
        Crazyflie moves out of range)"""
        print('Connection to %s lost: %s' % (link_uri, msg))

    def _disconnected(self, link_uri):
        """Callback when the Crazyflie is disconnected (called in all cases)"""
        print('Disconnected from %s' % link_uri)
        self.is_connected = False


# Define your custom callback function
def emergency_stop_callback(le):
    cf = le._cf  # Access the Crazyflie instance from the LoggingExample

    def on_press(key):
        try:
            if key.char == 'q':  # Check if the "space" key is pressed
                print("Emergency stop triggered!")
                le.emergency_stop = True
                return False     # Stop the listener
        except AttributeError:
            pass

    # Start listening for key presses
    with keyboard.Listener(on_press=on_press) as listener:
        listener.join()

    # Send the stop setpoint to the Crazyflie if the emergency stop is triggered
    if le.emergency_stop:
        cf.commander.send_stop_setpoint()
        cf.close_link()

def Get_Current_Setpoint(setpoints, setpoint_index):
    return setpoints[setpoint_index]

def On_Setpoint_Reached(setpoint_index):
    setpoint_index += 1
    return setpoint_index

def Check_If_Lap_Completed(setpoints, setpoint_index):
    return setpoint_index >= len(setpoints) - 1

def Check_If_Setpoint_Reached(current_setpoint, state_estimate):
    x = current_setpoint["x"] - state_estimate["x"]
    y = current_setpoint["y"] - state_estimate["y"]
    z = current_setpoint["z"] - state_estimate["z"]
    distance = math.sqrt( x**2 + y**2 + z**2 )
    return distance <= DISTANCE_TRESHOLD

def Get_State_Estimate(cf):
    return cf.sensor_data

def Fly_Towards_Setpoint(cf, current_setpoint):
    cf.commander.send_position_setpoint(current_setpoint["x"], current_setpoint["y"], current_setpoint["z"], current_setpoint["yaw"])

def Takeoff(cf):
    for y in range(10):
        cf.commander.send_hover_setpoint(0, 0, 0, y / 25)
        time.sleep(0.1)
    for _ in range(20):
        cf.commander.send_hover_setpoint(0, 0, 0, 0.4)
        time.sleep(0.1)

if __name__ == '__main__':
    # Initialize the low-level drivers
    cflib.crtp.init_drivers()

    le = LoggingExample(uri)
    cf = le._cf

    cf.param.set_value('kalman.resetEstimation', '1')
    time.sleep(0.1)
    cf.param.set_value('kalman.resetEstimation', '0')
    time.sleep(2)

    # Replace the thread creation with the updated function
    emergency_stop_thread = threading.Thread(target=emergency_stop_callback, args=(le,))
    emergency_stop_thread.start()

    setpoints = {}
    setpoint_index = 0
    lap = 0

    # TODO : CHANGE THIS TO YOUR NEEDS
    print("Starting control")
    while le.is_connected:
        time.sleep(0.01)
        Takeoff(cf)
        lap = 1
        while lap == 1:
            state_estimate = Get_State_Estimate(cf)
            current_setpoint = Get_Current_Setpoint(setpoints, setpoint_index)
            Fly_Towards_Setpoint(cf, current_setpoint)
            if Check_If_Setpoint_Reached(current_setpoint, state_estimate):
                setpoint_index = On_Setpoint_Reached(setpoint_index)
            if Check_If_Lap_Completed(setpoints, setpoint_index):
                lap = 2
            time.sleep(0.1)

        while lap == 2:
            # Lap 2
            pass

        cf.commander.send_stop_setpoint()
        break

"""
        for _ in range(50):
            cf.commander.send_hover_setpoint(0, 0, 0, 0.4)
            time.sleep(0.1)
        for _ in range(50):
            cf.commander.send_hover_setpoint(0, 0, 0, 0.4)
            time.sleep(0.1)
"""