From 82f39e06ba5c635c833054df3cec380907c539f1 Mon Sep 17 00:00:00 2001
From: sfrana <sfrana@iastate.edu>
Date: Wed, 25 Oct 2023 22:19:57 +0200
Subject: [PATCH] started crazyflie_connection implementation
---
.../CrazyflieProtoConnection.py | 221 +++++++++++++++
cflib_groundstation/SetpointHandler.py | 215 +++++++++++++++
cflib_groundstation/crazyflie_connection.py | 115 +++++++-
cflib_groundstation/uCartCommander.py | 258 ++++++++++++++++++
mp4Params.json | 24 +-
5 files changed, 813 insertions(+), 20 deletions(-)
create mode 100644 cflib_groundstation/CrazyflieProtoConnection.py
create mode 100644 cflib_groundstation/SetpointHandler.py
create mode 100644 cflib_groundstation/uCartCommander.py
diff --git a/cflib_groundstation/CrazyflieProtoConnection.py b/cflib_groundstation/CrazyflieProtoConnection.py
new file mode 100644
index 000000000..b2a0ed79e
--- /dev/null
+++ b/cflib_groundstation/CrazyflieProtoConnection.py
@@ -0,0 +1,221 @@
+"""
+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()
diff --git a/cflib_groundstation/SetpointHandler.py b/cflib_groundstation/SetpointHandler.py
new file mode 100644
index 000000000..d8f7e56de
--- /dev/null
+++ b/cflib_groundstation/SetpointHandler.py
@@ -0,0 +1,215 @@
+"""
+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)
diff --git a/cflib_groundstation/crazyflie_connection.py b/cflib_groundstation/crazyflie_connection.py
index 01aa89e97..5f3e7d8ca 100644
--- a/cflib_groundstation/crazyflie_connection.py
+++ b/cflib_groundstation/crazyflie_connection.py
@@ -1,6 +1,72 @@
+"""
+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
diff --git a/cflib_groundstation/uCartCommander.py b/cflib_groundstation/uCartCommander.py
new file mode 100644
index 000000000..f74bc7751
--- /dev/null
+++ b/cflib_groundstation/uCartCommander.py
@@ -0,0 +1,258 @@
+#!/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
diff --git a/mp4Params.json b/mp4Params.json
index 67951cbe6..64bb92c23 100644
--- a/mp4Params.json
+++ b/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
}
}
--
GitLab