# This file is part of CodeAIR, and includes portions of code derived from the
# Crazyflie Python client library.
#
# Copyright (c) 2018-2020 Bitcraze AB
# Modifications Copyright (c) 2024 Firia, Inc.
#
# 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 high level setpoints to the Crazyflie
"""
import struct
from cflib.crtpstack import CRTPPort
# from cflib.crtp.crtpstack import CRTPPacket
# from cflib.crtp.crtpstack import CRTPPort
# __author__ = 'Bitcraze AB'
# __all__ = ['HighLevelCommander']
[docs]class HighLevelCommander():
"""
Used for sending high level setpoints to the Crazyflie
"""
COMMAND_SET_GROUP_MASK = 0
COMMAND_STOP = 3
COMMAND_GO_TO = 4
COMMAND_START_TRAJECTORY = 5
COMMAND_DEFINE_TRAJECTORY = 6
COMMAND_TAKEOFF_2 = 7
COMMAND_LAND_2 = 8
ALL_GROUPS = 0
TRAJECTORY_LOCATION_MEM = 1
TRAJECTORY_TYPE_POLY4D = 0
TRAJECTORY_TYPE_POLY4D_COMPRESSED = 1
def __init__(self, crazyflie=None):
"""
Initialize the object.
"""
self._cf = crazyflie
[docs] def set_group_mask(self, group_mask=ALL_GROUPS):
"""
Set the group mask that the Crazyflie belongs to
:param group_mask: Mask for which groups this CF belongs to
"""
self._send_packet(struct.pack('<BB',
self.COMMAND_SET_GROUP_MASK,
group_mask))
[docs] def takeoff(self, absolute_height_m, duration_s, group_mask=ALL_GROUPS,
yaw=0.0):
"""
vertical takeoff from current x-y position to given height
:param absolute_height_m: Absolute (m)
:param duration_s: Time it should take until target height is
reached (s)
:param group_mask: Mask for which CFs this should apply to
:param yaw: Yaw (rad). Use current yaw if set to None.
"""
target_yaw = yaw
useCurrentYaw = False
if yaw is None:
target_yaw = 0.0
useCurrentYaw = True
self._send_packet(struct.pack('<BBff?f',
self.COMMAND_TAKEOFF_2,
group_mask,
absolute_height_m,
target_yaw,
useCurrentYaw,
duration_s))
[docs] def land(self, absolute_height_m, duration_s, group_mask=ALL_GROUPS,
yaw=0.0):
"""
vertical land from current x-y position to given height
:param absolute_height_m: Absolute (m)
:param duration_s: Time it should take until target height is
reached (s)
:param group_mask: Mask for which CFs this should apply to
:param yaw: Yaw (rad). Use current yaw if set to None.
"""
target_yaw = yaw
useCurrentYaw = False
if yaw is None:
target_yaw = 0.0
useCurrentYaw = True
self._send_packet(struct.pack('<BBff?f',
self.COMMAND_LAND_2,
group_mask,
absolute_height_m,
target_yaw,
useCurrentYaw,
duration_s))
[docs] def stop(self, group_mask=ALL_GROUPS):
"""
stops the current trajectory (turns off the motors)
:param group_mask: Mask for which CFs this should apply to
:return:
"""
self._send_packet(struct.pack('<BB',
self.COMMAND_STOP,
group_mask))
[docs] def go_to(self, x, y, z, yaw, duration_s, relative=False,
group_mask=ALL_GROUPS):
"""
Go to an absolute or relative position
:param x: X (m)
:param y: Y (m)
:param z: Z (m)
:param yaw: Yaw (radians)
:param duration_s: Time it should take to reach the position (s)
:param relative: True if x, y, z is relative to the current position
:param group_mask: Mask for which CFs this should apply to
"""
self._send_packet(struct.pack('<BBBfffff',
self.COMMAND_GO_TO,
group_mask,
relative,
x, y, z,
yaw,
duration_s))
[docs] def start_trajectory(self, trajectory_id, time_scale=1.0, relative=False,
reversed=False, group_mask=ALL_GROUPS):
"""
starts executing a specified trajectory
:param trajectory_id: Id of the trajectory (previously defined by
define_trajectory)
:param time_scale: Time factor; 1.0 = original speed;
>1.0: slower;
<1.0: faster
:param relative: Set to True, if trajectory should be shifted to
current setpoint
:param reversed: Set to True, if trajectory should be executed in
reverse
:param group_mask: Mask for which CFs this should apply to
:return:
"""
self._send_packet(struct.pack('<BBBBBf',
self.COMMAND_START_TRAJECTORY,
group_mask,
relative,
reversed,
trajectory_id,
time_scale))
[docs] def define_trajectory(self, trajectory_id, offset, n_pieces, type=TRAJECTORY_TYPE_POLY4D):
"""
Define a trajectory that has previously been uploaded to memory.
:param trajectory_id: The id of the trajectory
:param offset: Offset in uploaded memory
:param n_pieces: Nr of pieces in the trajectory
:param type: The type of trajectory data; TRAJECTORY_TYPE_POLY4D or TRAJECTORY_TYPE_POLY4D_COMPRESSED
:return:
"""
self._send_packet(struct.pack('<BBBBIB',
self.COMMAND_DEFINE_TRAJECTORY,
trajectory_id,
self.TRAJECTORY_LOCATION_MEM,
type,
offset,
n_pieces))
def _send_packet(self, data):
# pk = CRTPPacket()
# pk.port = CRTPPort.SETPOINT_HL
# pk.data = data
# self._cf.send_packet(pk)
# Firia Labs: simple crtp send
crtp.send(CRTPPort.SETPOINT_HL, 0, data)