started crazyflie_connection implementation

cflib_groundstation/CrazyflieProtoConnection.py

+"""
+Crazyflie Proto Connection handles the actual interaction with the crazyflie.
+The only reason it exists is to serve as an intermediary between the frontend
+and the crazyflie itself so that it can handle all interactions with the
+cflib library and that the GUI doesn't have to.
+
+If one wanted to slot this GUI into the existing microcart infrastructure,
+CrazyflieProtoConnection could be rewritten to interface to frontend
+commands, and to connect to the backend, and the crazyflie adapter,
+and crazyflie groundstation.
+
+"""
+
+
+from time import time
+from typing import List
+import time
+
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
+from queue import Queue
+
+from cflib.crazyflie.log import LogConfig
+
+
+class CrazyflieProtoConnection:
+    """
+    Handles all interactions with cflib.
+    """
+
+    def __init__(self):
+        """
+        Initialize the start time, the logging queue which is given to the
+        plotting window, and set the synchronous crazyflie connection by
+        default to None. This should be set to the crazyflie when connecting
+        to one.
+        """
+
+        # Get the start time, so that the timestamp returned to the user will
+        # be in seconds since startup.
+        self.start_time = time.time()
+
+        self.logging_queue = Queue()
+
+        self.scf = None
+        self.is_connected = False
+        self.param_callback_count = 0
+        self.logging_configs = []
+
+        # self.timer = QTimer()
+        # self.timer.timeout.connect(self.update_plot)
+        # self.timer.start(50)
+
+    def check_link_is_alive_callback(self):
+        """ Periodically investigate if the link is alive """
+        if self.scf:
+            if not self.scf.is_link_open():
+                self.is_connected = False
+
+    def logging_callback(self, _timestamp, data, _logconf):
+        """ Whenever data comes in from the logging, it is sent here,
+        which routes it into our specific format for the logging queue. """
+
+        timestamp1 = time.time() - self.start_time
+
+        for key in data.keys():
+            value_pair = {'timestamp': timestamp1, 'data': data[key],
+                          'signal': key}
+            self.logging_queue.put(value_pair)
+
+    def param_set_value(self, group: str, name: str, value: float):
+        """ Set a crazyflie parameter value. """
+
+        try:
+            if self.scf.is_link_open():
+                full_name = group + "." + name
+                cf = self.scf.cf
+                cf.param.add_update_callback(group=group, name=name,
+                                             cb=self.done_setting_param_value)
+
+                cf.param.set_value(full_name, value)
+
+                # Don't return until the parameter is done getting set
+                while self.param_callback_count < 1:
+                    time.sleep(0.01)
+
+                self.param_callback_count = 0
+        except AttributeError:
+            print("Nothing connected")
+
+    def done_setting_param_value(self, *_args):
+        """ Callback when done setting a parameter value. """
+        print("Done setting param")
+        self.param_callback_count += 1
+
+    def param_get_value(self, group: str, name: str):
+        """ Retrieve parameter value from crazyflie toc. """
+        try:
+            if self.scf.is_link_open():
+                return self.scf.cf.param.values[group][name]
+
+        except AttributeError:
+            pass
+        return -1.234567890  # 1234567890 should be pretty obvious that
+        # something has gone wrong.
+
+    def get_logging_toc(self):
+        """ Retrieve entire logging table of contents. Used in order to
+        display list in logging tab. """
+
+        try:
+            if self.scf.is_link_open():
+                tocFull = self.scf.cf.log.toc.toc
+                toc = []
+                for key in tocFull.keys():
+                    for inner_key in tocFull[key].keys():
+                        # concatenate group name with parameter name.
+                        full_name = key + "." + inner_key
+                        toc.append(full_name)
+
+                return toc
+            else:
+                return []
+        except AttributeError:
+            pass
+        return []
+
+    def get_param_toc(self):
+        """ Get the names of all groups available for parameters on the
+        crazyflie. Used to populate parameter group list on parameter tab. """
+
+        try:
+            if self.scf.is_link_open():
+                toc = self.scf.cf.param.values
+
+                return toc
+        except AttributeError:
+            pass
+        return {}
+
+    def add_log_config(self, name: str, period: int, variables: List[str]):
+        """ Add a logging config. Used from logging tab when refreshing
+        logging variables. Add callback to route logged data to logging
+        queue. """
+
+        print("Name: " + name + ", period: " + str(period) + ", variables: "
+              + str(variables))
+        logging_group = LogConfig(name=name, period_in_ms=period)
+
+        for variable in variables:
+            logging_group.add_variable(variable, 'float')
+
+        self.logging_configs.append(logging_group)
+        self.logging_configs[-1].data_received_cb.add_callback(
+            self.logging_callback)
+        self.scf.cf.log.add_config(self.logging_configs[-1])
+
+    def clear_logging_configs(self):
+        """ Stop logging and clear configuration. Used when refreshing
+        logging to stop anything that has configured to be logged from
+        logging. """
+
+        self.stop_logging()
+        self.logging_configs = []
+        # done refreshing toc is a callback function that is triggered when
+        # refresh toc is done executing.
+        self.scf.cf.log.refresh_toc(
+            self.done_refreshing_toc, self.scf.cf.log._toc_cache)
+        # Blocks until toc is done refreshing.
+        while self.param_callback_count < 1:
+            time.sleep(0.01)
+        self.param_callback_count = 0
+
+        # grabs new toc values
+        self.scf.wait_for_params()
+
+    def done_refreshing_toc(self, *_args):
+        """ Callback for flow control, increments param callback count to
+        allow exit of while loop. """
+        self.param_callback_count = 1
+
+    def start_logging(self):
+        """ Begins logging all configured logging blocks. This is used from
+        the controls tab when hitting begin logging. """
+        for i in range(0, len(self.logging_configs)):
+            self.logging_configs[i].start()
+
+    def stop_logging(self):
+        """ Stops logging all configured logging blocks. This is used from
+        the controls tab when hitting pause logging. """
+        for i in range(0, len(self.logging_configs)):
+            self.logging_configs[i].stop()
+
+    def connect(self, uri: str):
+        """
+        Handles connecting to a crazyflie. Bitcraze has excellent
+        documentation on how to use the synchronous crazyflie object in order
+        to send setpoints, set parameters or retrieve logging.
+        :param uri: Radio channel
+        """
+        cflib.crtp.init_drivers()
+        self.scf = SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache'))
+        self.scf.open_link()
+        self.scf.wait_for_params()
+        self.is_connected = True
+
+    def disconnect(self):
+        """ Disconnect from crazyflie. """
+        print("Disconnect quad")
+        if self.is_connected:
+            self.scf.close_link()
+            self.scf = None
+            self.is_connected = False
+
+    @staticmethod
+    def list_available_crazyflies():
+        """ Lists crazyflies that are on and within range. """
+        cflib.crtp.init_drivers()  # run this again just in case you plug the
+        # dongle in
+        return cflib.crtp.scan_interfaces()

cflib_groundstation/SetpointHandler.py

+"""
+Setpoint handler holds classes used to send setpoints to the crazyflie proto
+connection in an organized fashion. It is used by the setpoint menu found on
+the controls tab.
+
+"""
+
+from enum import Enum
+from PyQt5.QtCore import QTimer
+from uCartCommander import Commander
+from threading import Semaphore
+
+
+class FlightMode(Enum):
+    """ Available flight modes found in crazyflie firmware. """
+    TYPE_STOP: int = 0
+    TYPE_VELOCITY_WORLD: int = 1
+    TYPE_ZDISTANCE: int = 2
+    altHoldType: int = 4
+    TYPE_HOVER: int = 5
+    FULL_STATE_TYPE: int = 6
+    TYPE_POSITION: int = 7
+    # ------ Custom modes added to firmware by uCart -----------
+    ATTITUDE_RATE_TYPE: int = 8
+    ATTITUDE_TYPE: int = 9
+    MIXED_ATTITUDE_TYPE: int = 10
+
+
+class Setpoint:
+    """ Used in order to hold setpoint easily. """
+    def __init__(self):
+        self.yaw: float = 0
+        self.pitch: float = 0
+        self.roll: float = 0
+        self.thrust: int = 0
+
+
+class SetpointHandler:
+    """
+    Setpoint handler is used to send setpoints to the crazyflie proto
+    connection in an organized manner. It holds direct access to the
+    crazyflie's commander, which means that whenever connecting to a
+    crazyflie, that commander must be configured in the setpoint handler.
+    This commander is intended to by the custom uCartCommander which is a
+    modified low level setpoint commander. Currently, no modified version of
+    the high level commander exists.
+
+    """
+
+    def __init__(self):
+        """
+        Initialize timer,
+
+        :param commander: uCartCommander taken directly from the synchronous
+        crazyflie.
+        """
+
+        self.setpoint = Setpoint()
+        self.commander = None
+
+        self.setpoint_semaphore = Semaphore(1)
+        self._flight_mode = FlightMode.TYPE_STOP
+
+        # Send setpoints to crazyflie every 20 ms.
+        self.timer = QTimer()
+        self.timer.timeout.connect(self.update)
+        self.timer.start(20)
+
+    def setCommander(self, commander: Commander):
+        """ When the crazyflie is not connected, there will be no valid
+        commander. Set it during runtime. Enables the use of if commander:
+        on other functions to check if it is valid. """
+        self.commander = commander
+
+    def disconnectCommander(self):
+        """ Set the commander equal to none so that it won't be called when
+        we are not actually connected to the crazyflie. """
+        self.commander = None
+
+    def update(self):
+        """ If the flight mode is not stopped, send the current setpoint. """
+        if self._flight_mode != FlightMode.TYPE_STOP:
+            self.sendSetpoint()
+
+    def getFlightMode(self):
+        """ Returns current flight mode. """
+        return self._flight_mode
+
+    def setAttitudeMode(self):
+        """ Safely set to attitude mode. Uses semaphore in case a callback
+                happens in the middle of the function. """
+        self.setpoint_semaphore.acquire()
+        self._flight_mode = FlightMode.ATTITUDE_TYPE
+        self.setpoint_semaphore.release()
+
+    def setMixedAttitudeMode(self):
+        """ Safely set to mixed attitude mode. Uses semaphore in case a
+                callback happens in the middle of the function. """
+        self.setpoint_semaphore.acquire()
+        self._flight_mode = FlightMode.MIXED_ATTITUDE_TYPE
+        self.setpoint_semaphore.release()
+
+    def setRateMode(self):
+        """ Safely set to rate mode. Uses semaphore in case a callback
+                happens in the middle of the function. """
+        self.setpoint_semaphore.acquire()
+        self._flight_mode = FlightMode.ATTITUDE_RATE_TYPE
+        self.setpoint_semaphore.release()
+
+    def startFlying(self):
+        """ Sends an all 0's setpoint which is the convention to tell the
+            crazyflie to listen for setpoints. """
+        if self.commander:
+            self.commander.start_flying()
+
+    def stopFlying(self):
+        """ Tells the crazyflie to stop flying. """
+        if self.commander:
+            self.setpoint_semaphore.acquire()
+            self._flight_mode = FlightMode.TYPE_STOP
+
+            self.commander.send_notify_setpoint_stop(0)
+            self.setpoint_semaphore.release()
+
+    def setSetpoint(self, yaw: float, pitch: float, roll: float, thrust: float):
+        """ Safely sets the crazyflie setpoint. Utilizes semaphore to avoid
+                reading and writing at the same time due to callbacks. """
+        self.setpoint_semaphore.acquire()
+
+        self.setpoint.yaw = yaw
+        self.setpoint.pitch = pitch
+        self.setpoint.roll = roll
+        self.setpoint.thrust = thrust
+        self.sendSetpointUnsafe()
+
+        self.setpoint_semaphore.release()
+
+    def sendSetpoint(self):
+        """ Uses commander to send setpoints to crazyflie depending upon the
+                current flight mode. """
+        if self.commander:
+            self.setpoint_semaphore.acquire()
+
+            if self._flight_mode == FlightMode.ATTITUDE_TYPE:
+
+                # scales thrust from 100 for slider control.
+                thrust = self.setpoint.thrust * 60000 / 100
+                print(f"Set attitude: {self.setpoint.yaw}, {self.setpoint.pitch}, "
+                      f"{self.setpoint.roll}, {thrust}")
+
+                self.commander.send_attitude_setpoint(
+                    self.setpoint.yaw, self.setpoint.pitch, self.setpoint.roll,
+                    thrust)
+
+            elif self._flight_mode == FlightMode.ATTITUDE_RATE_TYPE:
+
+                thrust = self.setpoint.thrust * 60000 / 100
+                print(f"Set attitude rate: {self.setpoint.yaw},"
+                      f" {self.setpoint.pitch}, "
+                      f"{self.setpoint.roll}, {thrust}")
+
+                self.commander.send_attitude_rate_setpoint(
+                    self.setpoint.yaw, self.setpoint.pitch, self.setpoint.roll,
+                    thrust)
+
+            elif self._flight_mode == FlightMode.MIXED_ATTITUDE_TYPE:
+
+                # scales thrust from 1000 for more fine-grained gamepad control.
+                thrust = self.setpoint.thrust * 60000 / 1000
+                print(f"Set mixed attitude: {self.setpoint.yaw},"
+                      f" {self.setpoint.pitch}, "
+                      f"{self.setpoint.roll}, {thrust}")
+
+                self.commander.send_mixed_attitude_setpoint(
+                    self.setpoint.yaw, self.setpoint.pitch, self.setpoint.roll,
+                    thrust)
+
+            self.setpoint_semaphore.release()
+
+    def sendSetpointUnsafe(self):
+        """ Exactly the same as send setpoint but no semaphore is used. """
+
+        print("Unsafe mode activate :)")
+        if self.commander:
+            if self._flight_mode == FlightMode.ATTITUDE_TYPE:
+                # scales thrust from 100 for slider control.
+                thrust = self.setpoint.thrust * 60000 / 100
+                print(f"Set attitude: {self.setpoint.yaw}, {self.setpoint.pitch}, "
+                      f"{self.setpoint.roll}, {self.setpoint.thrust}")
+
+                self.commander.send_attitude_setpoint(
+                    self.setpoint.yaw, self.setpoint.pitch, self.setpoint.roll,
+                    int(thrust))
+
+            elif self._flight_mode == FlightMode.ATTITUDE_RATE_TYPE:
+
+                thrust = self.setpoint.thrust * 60000 / 100
+                print(f"Set attitude rate: {self.setpoint.yaw},"
+                      f" {self.setpoint.pitch}, "
+                      f"{self.setpoint.roll}, {thrust}")
+
+                self.commander.send_attitude_rate_setpoint(
+                    self.setpoint.yaw, self.setpoint.pitch, self.setpoint.roll,
+                    thrust)
+
+            elif self._flight_mode == FlightMode.MIXED_ATTITUDE_TYPE:
+                # scales thrust from 1000 for more fine-grained gamepad control.
+                thrust = self.setpoint.thrust * 60000 / 1000
+                print(f"Set mixed attitude: {self.setpoint.yaw},"
+                      f" {self.setpoint.pitch}, "
+                      f"{self.setpoint.roll}, {thrust}")
+
+                self.commander.send_mixed_attitude_setpoint(
+                    self.setpoint.yaw, self.setpoint.pitch, self.setpoint.roll,
+                    thrust)

cflib_groundstation/crazyflie_connection.py

+"""
+crazyflie_connection handles the actual interaction with the crazyflie.
+The only reason it exists is to serve as an intermediary between the groundstation
+and the crazyflie itself so that it can handle all interactions with the
+cflib library.
+"""
 
+from time import time
+from typing import List
+import time
+
+import cflib.crtp
+from cflib.crazyflie import Crazyflie
+from cflib.crazyflie.syncCrazyflie import SyncCrazyflie
+from queue import Queue
+
+from cflib.crazyflie.log import LogConfig
+
+
+class CrazyflieConnection:
+
+    """
+    Handles all interactions with cflib.
+    """
+
+    def __init__(self):
+        """
+        Initialize the start time, the logging queue which is given to the
+        plotting window, and set the synchronous crazyflie connection by
+        default to None. This should be set to the crazyflie when connecting
+        to one.
+        """
+
+        # Get the start time, so that the timestamp returned to the user will
+        # be in seconds since startup.
+        self.start_time = time.time()
+
+        self.logging_queue = Queue()
+
+        self.scf = None
+        self.is_connected = False
+        self.param_callback_count = 0
+        self.logging_configs = []
+
+        # self.timer = QTimer()
+        # self.timer.timeout.connect(self.update_plot)
+        # self.timer.start(50)
+
+    def connect(self, uri: str):
+        """
+        Handles connecting to a crazyflie. Bitcraze has excellent
+        documentation on how to use the synchronous crazyflie object in order
+        to send setpoints, set parameters or retrieve logging.
+        :param uri: Radio channel
+        """
+        cflib.crtp.init_drivers()
+        self.scf = SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache'))
+        self.scf.open_link()
+        self.scf.wait_for_params()
+        self.is_connected = True
+
+    def disconnect(self):
+        """ Disconnect from crazyflie. """
+        print("Disconnect quad")
+        if self.is_connected:
+            self.scf.close_link()
+            self.scf = None
+            self.is_connected = False
 
-class CrazyflieConnection():
     def Debug():
         raise Exception
     def PacketLog():
@@ -11,14 +77,45 @@ class CrazyflieConnection():
         raise Exception
     def BeginUpdate():
         raise Exception
-    def OverrideOuput(): #TODO
+    def OverrideOuput(): 
+        #TODO
         raise Exception
     def GetNodeIds():
         raise Exception
-    def SetParam(): #TODO
-        raise Exception
-    def GetParam(): #TODO
-        raise Exception
+    def SetParam(self, group: str, name: str, value: float):
+        """ Set a crazyflie parameter value. """
+        try:
+            if self.scf.is_link_open():
+                full_name = group + "." + name
+                cf = self.scf.cf
+                cf.param.add_update_callback(group=group, name=name,
+                                             cb=self.done_setting_param_value)
+
+                cf.param.set_value(full_name, value)
+
+                # Don't return until the parameter is done getting set
+                while self.param_callback_count < 1:
+                    time.sleep(0.01)
+
+                self.param_callback_count = 0
+        except AttributeError:
+            print("Nothing connected")
+
+    def done_setting_param_value(self, *_args):
+        """ Callback when done setting a parameter value. """
+        print("Done setting param")
+        self.param_callback_count += 1
+
+    def GetParam(self, group: str, name: str):
+        """ Retrieve parameter value from crazyflie toc. """
+        try:
+            if self.scf.is_link_open():
+                return self.scf.cf.param.values[group][name]
+
+        except AttributeError:
+            pass
+        return -1.234567890  # 1234567890 should be pretty obvious that
+        # something has gone wrong.
     def SetSource():
         raise Exception
     def GetSource():
@@ -31,7 +128,9 @@ class CrazyflieConnection():
         raise Exception
     def AddNode():
         raise Exception
-    def GetLogFile(): #TODO
+    def GetLogFile(): 
+        #TODO
         raise Exception
-    def LogBlockCommand(): #TODO
+    def LogBlockCommand(): 
+        #TODO
         raise Exception
\ No newline at end of file

cflib_groundstation/uCartCommander.py

+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+#
+#     ||          ____  _ __
+#  +------+      / __ )(_) /_______________ _____  ___
+#  | 0xBC |     / __  / / __/ ___/ ___/ __ `/_  / / _ \
+#  +------+    / /_/ / / /_/ /__/ /  / /_/ / / /_/  __/
+#   ||  ||    /_____/_/\__/\___/_/   \__,_/ /___/\___/
+#
+#  Copyright (C) 2011-2013 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, see <https://www.gnu.org/licenses/>.
+"""
+Used for sending control setpoints to the Crazyflie
+
+- 4/7/2023 - Austin Beinder uCart 23:
+    modified to support uCart 22's custom packet types
+
+"""
+import struct
+import time
+
+from cflib.crtp.crtpstack import CRTPPacket
+from cflib.crtp.crtpstack import CRTPPort
+from cflib.crazyflie.commander import Commander
+
+__author__ = 'Bitcraze AB'
+__all__ = ['Commander']
+
+SET_SETPOINT_CHANNEL = 0
+META_COMMAND_CHANNEL = 1
+
+TYPE_STOP = 0
+TYPE_VELOCITY_WORLD = 1
+TYPE_ZDISTANCE = 2
+altHoldType = 4
+TYPE_HOVER = 5
+FULL_STATE_TYPE = 6
+TYPE_POSITION = 7
+# Custom modes -----------
+ATTITUDE_RATE_TYPE = 8
+ATTITUDE_TYPE = 9
+MIXED_ATTITUDE_TYPE = 10  # Considering the default commander has a mixed
+# attitude setpoint sender, it is unclear to me why this exists, unless it
+# didn't exist in the version of the firmware this is based upon.
+# ------------------------
+
+TYPE_META_COMMAND_NOTIFY_SETPOINT_STOP = 0
+
+
+class Commander:
+    """
+    Used for sending control setpoints to the Crazyflie
+    """
+
+    def __init__(self, crazyflie=None):
+        """
+        Initialize the commander object. By default, the commander is in
+        +-mode (not x-mode).
+        """
+        self._cf = crazyflie
+        self._x_mode = False
+
+    def set_client_xmode(self, enabled):
+        """
+        Enable/disable the client side X-mode. When enabled this recalculates
+        the setpoints before sending them to the Crazyflie.
+        """
+        self._x_mode = enabled
+
+    def send_setpoint(self, roll, pitch, yawrate, thrust):
+        """
+        Send a new control setpoint for roll/pitch/yaw_Rate/thrust to the
+        copter.
+        The meaning of these values is depended on the mode of the RPYT
+        commander in the firmware
+        Default settings are Roll, pitch, yawrate and thrust
+        roll,  pitch are in degrees
+        yawrate is in degrees/s
+        thrust is an integer value ranging from 10001 (next to no power) to
+        60000 (full power)
+        """
+        if thrust > 0xFFFF or thrust < 0:
+            raise ValueError('Thrust must be between 0 and 0xFFFF')
+
+        if self._x_mode:
+            roll, pitch = 0.707 * (roll - pitch), 0.707 * (roll + pitch)
+
+        pk = CRTPPacket()
+        pk.port = CRTPPort.COMMANDER
+        pk.data = struct.pack('<fffH', roll, pitch, yawrate, thrust)
+        self._cf.send_packet(pk)
+        print()
+
+    def start_flying(self):
+        self.send_setpoint(0, 0, 0, 0)
+
+    def send_notify_setpoint_stop(self, remain_valid_milliseconds=0):
+        """
+        Sends a packet so that the priority of the current setpoint to the
+        lowest non-disabled value,
+        so any new setpoint regardless of source will overwrite it.
+        """
+        pk = CRTPPacket()
+        pk.port = CRTPPort.COMMANDER_GENERIC
+        pk.channel = META_COMMAND_CHANNEL
+        pk.data = struct.pack('<BI', TYPE_META_COMMAND_NOTIFY_SETPOINT_STOP,
+                              remain_valid_milliseconds)
+        self._cf.send_packet(pk)
+
+    def send_stop_setpoint(self):
+        """
+        Send STOP setpoint, stopping the motors and (potentially) falling.
+        """
+        pk = CRTPPacket()
+        pk.port = CRTPPort.COMMANDER_GENERIC
+        pk.data = struct.pack('<B', TYPE_STOP)
+        self._cf.send_packet(pk)
+
+    def send_velocity_world_setpoint(self, vx, vy, vz, yawrate):
+        """
+        Send Velocity in the world frame of reference setpoint with yawrate
+        commands
+        vx, vy, vz are in m/s
+        yawrate is in degrees/s
+        """
+        pk = CRTPPacket()
+        pk.port = CRTPPort.COMMANDER_GENERIC
+        pk.channel = SET_SETPOINT_CHANNEL
+        pk.data = struct.pack('<Bffff', TYPE_VELOCITY_WORLD,
+                              vx, vy, vz, yawrate)
+        self._cf.send_packet(pk)
+
+    def send_zdistance_setpoint(self, roll, pitch, yawrate, zdistance):
+        """
+        Control mode where the height is sent as an absolute setpoint (intended
+        to be the distance to the surface under the Crazflie),
+        while giving roll,
+        pitch and yaw rate commands
+        roll, pitch are in degrees
+        yawrate is in degrees/s
+        zdistance is in meters
+        """
+        pk = CRTPPacket()
+        pk.port = CRTPPort.COMMANDER_GENERIC
+        pk.channel = SET_SETPOINT_CHANNEL
+        pk.data = struct.pack('<Bffff', TYPE_ZDISTANCE,
+                              roll, pitch, yawrate, zdistance)
+        self._cf.send_packet(pk)
+
+    def send_hover_setpoint(self, vx, vy, yawrate, zdistance):
+        """
+        Control mode where the height is sent as an absolute setpoint (intended
+        to be the distance to the surface under the Crazflie), while giving x,
+        y velocity
+        commands in body-fixed coordinates.
+        vx,  vy are in m/s
+        yawrate is in degrees/s
+        zdistance is in meters
+        """
+        pk = CRTPPacket()
+        pk.port = CRTPPort.COMMANDER_GENERIC
+        pk.channel = SET_SETPOINT_CHANNEL
+        pk.data = struct.pack('<Bffff', TYPE_HOVER,
+                              vx, vy, yawrate, zdistance)
+        self._cf.send_packet(pk)
+
+    def send_position_setpoint(self, x, y, z, yaw):
+        """
+        Control mode where the position is sent as absolute (world) x,y,z
+        coordinate in
+        meter and the yaw is the absolute orientation.
+        x, y, z are in m
+        yaw is in degrees
+        """
+        pk = CRTPPacket()
+        pk.port = CRTPPort.COMMANDER_GENERIC
+        pk.channel = SET_SETPOINT_CHANNEL
+        pk.data = struct.pack('<Bffff', TYPE_POSITION,
+                              x, y, z, yaw)
+        self._cf.send_packet(pk)
+
+    # ---------------- Custom changes ----------------------------
+    def send_attitude_rate_setpoint(self, yawRate: float, pitchRate: float,
+                                    rollRate: float, thrust: float) -> None:
+        """
+        Control mode where the attitude rates are set directly. Rates are in
+        deg/s.
+
+        :param yawRate: Yaw rotational velocity setpoint
+        :param pitchRate: Pitch rotational velocity setpoint
+        :param rollRate: Roll rotational velocity setpoint
+        :param thrust: Directly set thrust, no PID required
+        :return: None
+        """
+        pk = CRTPPacket()
+        pk.port = CRTPPort.COMMANDER_GENERIC
+        pk.channel = SET_SETPOINT_CHANNEL
+        pk.data = struct.pack('<Bffff', ATTITUDE_RATE_TYPE, rollRate,
+                              pitchRate,
+                              yawRate, thrust)  # todo confirm struct is
+        # right, I tried to format it using H for the int of thrust and f for
+        # floats otherwise. Kinda looks like what is above
+        self._cf.send_packet(pk)
+
+    def send_attitude_setpoint(self, yaw: float, pitch: float, roll: float,
+                               thrust: float) -> None:
+        """
+        Control mode where the attitude angles are set directly. Values are
+        in deg.
+
+        :param yaw: Yaw rotation angle setpoint
+        :param pitch: Pitch rotation angle setpoint
+        :param roll: Roll rotation angle setpoint
+        :param thrust: Directly set thrust, no PID required
+        :return: None
+        """
+        pk = CRTPPacket()
+        pk.port = CRTPPort.COMMANDER_GENERIC
+        pk.channel = SET_SETPOINT_CHANNEL
+        pk.data = struct.pack('<Bffff', ATTITUDE_TYPE, roll, pitch, yaw,
+         thrust)
+        self._cf.send_packet(pk)  # todo confirm struct is right , I tried to
+        # format it using H for the int of thrust and f for
+        # floats otherwise. Kinda looks like what is above
+
+    def send_mixed_attitude_setpoint(self, yaw: float, pitch: float,
+                                     roll: float, thrust: float) -> None:
+        """
+        Control mode where the attitude angles are set directly, but yaw is
+        controlled via rate. Values are in deg and deg/s.
+
+        :param yaw: Yaw rotation angular velocity setpoint
+        :param pitch: Pitch rotation angle setpoint
+        :param roll: Roll rotation angle setpoint
+        :param thrust: Directly set thrust, no PID required
+        :return: None
+        """
+        pk = CRTPPacket()
+        pk.port = CRTPPort.COMMANDER_GENERIC
+        pk.channel = SET_SETPOINT_CHANNEL
+        pk.data = struct.pack('<Bffff', MIXED_ATTITUDE_TYPE, roll, pitch, yaw,
+         thrust)
+        self._cf.send_packet(pk)  # todo confirm struct is right , I tried to
+        # format it using H for the int of thrust and f for
+        # floats otherwise. Kinda looks like what is above

mp4Params.json

@@ -5,26 +5,26 @@
     },
     "s_pid_rate":
     {
-        "roll_kp" : 12,
+        "roll_kp" : 250,
         "roll_ki" : 0,
-        "roll_kd" : 0,
-        "pitch_kp": 0,
+        "roll_kd" : 1.5,
+        "pitch_kp": 250,
         "pitch_ki": 0,
-        "pitch_kd": 0,
-        "yaw_kp"  : 0,
+        "pitch_kd": 1.5,
+        "yaw_kp"  : 1060,
         "yaw_ki"  : 0,
-        "yaw_kd"  : 0
+        "yaw_kd"  : 3
     },
     "s_pid_attitude":
     {
-        "roll_kp" : 0,
+        "roll_kp" : 5,
         "roll_ki" : 0,
-        "roll_kd" : 0,
-        "pitch_kp": 0,
+        "roll_kd" : 0.2,
+        "pitch_kp": 5,
         "pitch_ki": 0,
-        "pitch_kd": 0,
-        "yaw_kp"  : 0,
+        "pitch_kd": 0.2,
+        "yaw_kp"  : 100,
         "yaw_ki"  : 0,
-        "yaw_kd"  : 0
+        "yaw_kd"  : 1.5
     }
 }