From fc95d953b8d22738ad0ae0f22d3acb47c106818a Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 15 Feb 2023 14:18:20 +0100 Subject: [PATCH 01/12] rename fp16 to encoding utils --- cflib/crazyflie/localization.py | 2 +- cflib/utils/encoding.py | 53 +++++++++++++++++++++++++++++++++ 2 files changed, 54 insertions(+), 1 deletion(-) create mode 100644 cflib/utils/encoding.py diff --git a/cflib/crazyflie/localization.py b/cflib/crazyflie/localization.py index 072c6be95..8134956cf 100644 --- a/cflib/crazyflie/localization.py +++ b/cflib/crazyflie/localization.py @@ -32,7 +32,7 @@ from cflib.crtp.crtpstack import CRTPPacket from cflib.crtp.crtpstack import CRTPPort from cflib.utils.callbacks import Caller -from cflib.utils.fp16 import fp16_to_float +from cflib.utils.encoding import fp16_to_float __author__ = 'Bitcraze AB' __all__ = ['Localization', 'LocalizationPacket'] diff --git a/cflib/utils/encoding.py b/cflib/utils/encoding.py new file mode 100644 index 000000000..5eb47ee5b --- /dev/null +++ b/cflib/utils/encoding.py @@ -0,0 +1,53 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2020 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 . +import struct + + +# Code from davidejones at https://gamedev.stackexchange.com/a/28756 +def fp16_to_float(float16): + s = int((float16 >> 15) & 0x00000001) # sign + e = int((float16 >> 10) & 0x0000001f) # exponent + f = int(float16 & 0x000003ff) # fraction + + if e == 0: + if f == 0: + return int(s << 31) + else: + while not (f & 0x00000400): + f <<= 1 + e -= 1 + e += 1 + f &= ~0x00000400 + # print(s,e,f) + elif e == 31: + if f == 0: + return int((s << 31) | 0x7f800000) + else: + return int((s << 31) | 0x7f800000 | (f << 13)) + + e += 127 - 15 + f <<= 13 + result = int((s << 31) | (e << 23) | f) + return struct.unpack('f', struct.pack('I', result))[0] From d5d58050ae0927f643ab5b2a236c525808151508 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 15 Feb 2023 14:44:08 +0100 Subject: [PATCH 02/12] add compression quaterion --- cflib/utils/encoding.py | 27 +++++++++++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/cflib/utils/encoding.py b/cflib/utils/encoding.py index 5eb47ee5b..ba7b6f042 100644 --- a/cflib/utils/encoding.py +++ b/cflib/utils/encoding.py @@ -24,6 +24,7 @@ # along with this program. If not, see . import struct +from math import sqrt # Code from davidejones at https://gamedev.stackexchange.com/a/28756 def fp16_to_float(float16): @@ -51,3 +52,29 @@ def fp16_to_float(float16): f <<= 13 result = int((s << 31) | (e << 23) | f) return struct.unpack('f', struct.pack('I', result))[0] + + + +# compress a quaternion, see quatcompress.h in firmware +# input: q = [x,y,z,w], output: 32-bit number +def compress_quaternion(qx, qy, qz, qw): + + q = [qx, qy, qz, qw] + + i_largest = 0 + for i in range(1, 4): + if abs(q[i]) > abs(q[i_largest]): + i_largest = i + + negate = q[i_largest] < 0 + + comp = i_largest + m_sqrt_2 = 1.0 / np.sqrt(2) + + for i in range(0,4): + if i != i_largest: + negbit = (q[i] < 0) ^ negate + mag = ((1 << 9) - 1) * (abs(q[i]) / m_sqrt_2) * 0.5 + comp = (comp << 10) | (negbit << 9) | mag + + return comp From 0ec1006e561d155aab0f7e75665bc6def9ca3598 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Wed, 15 Feb 2023 16:12:17 +0100 Subject: [PATCH 03/12] add full state setpoint in commander --- cflib/crazyflie/commander.py | 24 ++++++++++++++++++++++++ cflib/utils/encoding.py | 3 ++- 2 files changed, 26 insertions(+), 1 deletion(-) diff --git a/cflib/crazyflie/commander.py b/cflib/crazyflie/commander.py index c75128ee5..f616f82cb 100644 --- a/cflib/crazyflie/commander.py +++ b/cflib/crazyflie/commander.py @@ -29,6 +29,7 @@ from cflib.crtp.crtpstack import CRTPPacket from cflib.crtp.crtpstack import CRTPPort +from cflib.utils.encoding import compress_quaternion __author__ = 'Bitcraze AB' __all__ = ['Commander'] @@ -37,6 +38,7 @@ TYPE_VELOCITY_WORLD = 1 TYPE_ZDISTANCE = 2 TYPE_HOVER = 5 +TYPE_FULL_STATE = 6 TYPE_POSITION = 7 @@ -136,6 +138,28 @@ def send_hover_setpoint(self, vx, vy, yawrate, zdistance): vx, vy, yawrate, zdistance) self._cf.send_packet(pk) + def send_full_state_setpoint(self, x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, rollrate, pitchrate, yawrate): + """ + Control mode where the position, velocity, acceleration, orientation, angular + velocity are sent as absolute (world) values. + + x, y, z are in m + vx, vy, vz are in m/s + ax, ay, az are in m/s^2 + qx, qy, qz, qw are the quaternion components of the orientation + rollrate, pitchrate, yawrate are in degrees/s + """ + + pk = CRTPPacket() + pk.port = CRTPPort.COMMANDER_GENERIC + pk.data = struct.pack(' Date: Wed, 15 Feb 2023 16:12:25 +0100 Subject: [PATCH 04/12] make a demo script --- examples/autonomy/full_state_setpoint_demo.py | 165 ++++++++++++++++++ 1 file changed, 165 insertions(+) create mode 100644 examples/autonomy/full_state_setpoint_demo.py diff --git a/examples/autonomy/full_state_setpoint_demo.py b/examples/autonomy/full_state_setpoint_demo.py new file mode 100644 index 000000000..342cc02f9 --- /dev/null +++ b/examples/autonomy/full_state_setpoint_demo.py @@ -0,0 +1,165 @@ +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2016 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 . +""" +Used for sending full state control setpoints to the Crazyflie +""" +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.syncLogger import SyncLogger +from cflib.utils import uri_helper +import math + + + +# URI to the Crazyflie to connect to +uri = uri_helper.uri_from_env(default='radio://0/65/2M/E7E7E7E7F2') + +def quaternion_from_euler(roll, pitch, yaw): + + cy = math.cos(yaw * 0.5) + sy = math.sin(yaw * 0.5) + cp = math.cos(pitch * 0.5) + sp = math.sin(pitch * 0.5) + cr = math.cos(roll * 0.5) + sr = math.sin(roll * 0.5) + + q = [0] * 4 + q[0] = cy * cp * cr + sy * sp * sr + q[1] = cy * cp * sr - sy * sp * cr + q[2] = sy * cp * sr + cy * sp * cr + q[3] = sy * cp * cr - cy * sp * sr + + return q + + +def wait_for_position_estimator(scf): + print('Waiting for estimator to find position...') + + log_config = LogConfig(name='Kalman Variance', period_in_ms=500) + log_config.add_variable('kalman.varPX', 'float') + log_config.add_variable('kalman.varPY', 'float') + log_config.add_variable('kalman.varPZ', 'float') + + var_y_history = [1000] * 10 + var_x_history = [1000] * 10 + var_z_history = [1000] * 10 + + threshold = 0.001 + + with SyncLogger(scf, log_config) as logger: + for log_entry in logger: + data = log_entry[1] + + var_x_history.append(data['kalman.varPX']) + var_x_history.pop(0) + var_y_history.append(data['kalman.varPY']) + var_y_history.pop(0) + var_z_history.append(data['kalman.varPZ']) + var_z_history.pop(0) + + min_x = min(var_x_history) + max_x = max(var_x_history) + min_y = min(var_y_history) + max_y = max(var_y_history) + min_z = min(var_z_history) + max_z = max(var_z_history) + + # print("{} {} {}". + # format(max_x - min_x, max_y - min_y, max_z - min_z)) + + if (max_x - min_x) < threshold and ( + max_y - min_y) < threshold and ( + max_z - min_z) < threshold: + break + + +def reset_estimator(scf): + cf = scf.cf + cf.param.set_value('kalman.resetEstimation', '1') + time.sleep(0.1) + cf.param.set_value('kalman.resetEstimation', '0') + + wait_for_position_estimator(cf) + + +def position_callback(timestamp, data, logconf): + x = data['kalman.stateX'] + y = data['kalman.stateY'] + z = data['kalman.stateZ'] + print('pos: ({}, {}, {})'.format(x, y, z)) + + +def start_position_printing(scf): + log_conf = LogConfig(name='Position', period_in_ms=500) + log_conf.add_variable('kalman.stateX', 'float') + log_conf.add_variable('kalman.stateY', 'float') + log_conf.add_variable('kalman.stateZ', 'float') + + scf.cf.log.add_config(log_conf) + log_conf.data_received_cb.add_callback(position_callback) + log_conf.start() + + +def run_sequence(scf): + cf = scf.cf + + # quaternion from roll pitch yaw + roll = 0.0 + pitch = 0.0 + yaw = 0.0 + q = quaternion_from_euler(roll, pitch, yaw) + + cf.commander.send_full_state_setpoint(0.0,1.0,0.0, + 0.0,0.0,0.0, + 0.0,0.0,0.0, + q[0],q[1],q[2],q[3], + 0.0,0.0,0.0) + + time.sleep(2.0) + + cf.commander.send_full_state_setpoint(0.0,0.2,0.0, + 0.0,0.0,0.0, + 0.0,0.0,0.0, + q[0],q[1],q[2],q[3], + 0.0,0.0,0.0) + + time.sleep(2.0) + + + cf.commander.send_stop_setpoint() + # Make sure that the last packet leaves before the link is closed + # since the message queue is not flushed before closing + time.sleep(0.1) + + +if __name__ == '__main__': + cflib.crtp.init_drivers() + + with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: + reset_estimator(scf) + run_sequence(scf) From f54ad87cb44649463573ea0a1e76860dd1c554a9 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 16 Feb 2023 09:54:55 +0100 Subject: [PATCH 05/12] fix flake8 and structure symbols --- cflib/crazyflie/commander.py | 2 +- cflib/utils/encoding.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/cflib/crazyflie/commander.py b/cflib/crazyflie/commander.py index f616f82cb..dcf5d378b 100644 --- a/cflib/crazyflie/commander.py +++ b/cflib/crazyflie/commander.py @@ -152,7 +152,7 @@ def send_full_state_setpoint(self, x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, pk = CRTPPacket() pk.port = CRTPPort.COMMANDER_GENERIC - pk.data = struct.pack(' Date: Thu, 16 Feb 2023 10:00:26 +0100 Subject: [PATCH 06/12] remove unused package --- cflib/utils/encoding.py | 1 - 1 file changed, 1 deletion(-) diff --git a/cflib/utils/encoding.py b/cflib/utils/encoding.py index 2f1939029..43aeff101 100644 --- a/cflib/utils/encoding.py +++ b/cflib/utils/encoding.py @@ -25,7 +25,6 @@ import struct from math import sqrt -import numpy as np # Code from davidejones at https://gamedev.stackexchange.com/a/28756 def fp16_to_float(float16): From 5a70fd7ab219ea3a0c310a3512b7aaa1a15beea0 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 16 Feb 2023 12:11:09 +0100 Subject: [PATCH 07/12] fix order of quarternion --- examples/autonomy/full_state_setpoint_demo.py | 22 ++++++++++++------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/examples/autonomy/full_state_setpoint_demo.py b/examples/autonomy/full_state_setpoint_demo.py index 342cc02f9..7e89d7ef5 100644 --- a/examples/autonomy/full_state_setpoint_demo.py +++ b/examples/autonomy/full_state_setpoint_demo.py @@ -49,10 +49,10 @@ def quaternion_from_euler(roll, pitch, yaw): sr = math.sin(roll * 0.5) q = [0] * 4 - q[0] = cy * cp * cr + sy * sp * sr - q[1] = cy * cp * sr - sy * sp * cr - q[2] = sy * cp * sr + cy * sp * cr - q[3] = sy * cp * cr - cy * sp * sr + q[0] = sr * cp * cy - cr * sp * sy + q[1] = cr * sp * cy + sr * cp * sy + q[2] = cr * cp * sy - sr * sp * cy + q[3] = cr * cp * cy + sr * sp * sy return q @@ -133,16 +133,22 @@ def run_sequence(scf): pitch = 0.0 yaw = 0.0 q = quaternion_from_euler(roll, pitch, yaw) - - cf.commander.send_full_state_setpoint(0.0,1.0,0.0, + print('takeoff') + cf.commander.send_full_state_setpoint(0.0,0.0,1.0, 0.0,0.0,0.0, 0.0,0.0,0.0, q[0],q[1],q[2],q[3], 0.0,0.0,0.0) time.sleep(2.0) - - cf.commander.send_full_state_setpoint(0.0,0.2,0.0, + cf.commander.send_full_state_setpoint(0.0,0.0,1.0, + 0.0,0.0,0.0, + 0.0,0.0,0.0, + q[0],q[1],q[2],q[3], + 0.0,0.0,0.0) + time.sleep(2.0) + print('land') + cf.commander.send_full_state_setpoint(0.0,0.0,0.2, 0.0,0.0,0.0, 0.0,0.0,0.0, q[0],q[1],q[2],q[3], From c173effa74e8c5e8d7c999e5095fe12a478d9877 Mon Sep 17 00:00:00 2001 From: knmcguire Date: Thu, 16 Feb 2023 16:32:55 +0100 Subject: [PATCH 08/12] more testing, why weird angles in PID? --- cflib/crazyflie/commander.py | 3 +- cflib/utils/encoding.py | 37 +++++++++++++------ examples/autonomy/full_state_setpoint_demo.py | 29 +++++++++++++-- 3 files changed, 53 insertions(+), 16 deletions(-) diff --git a/cflib/crazyflie/commander.py b/cflib/crazyflie/commander.py index dcf5d378b..f4c8f4a23 100644 --- a/cflib/crazyflie/commander.py +++ b/cflib/crazyflie/commander.py @@ -149,10 +149,9 @@ def send_full_state_setpoint(self, x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qx, qy, qz, qw are the quaternion components of the orientation rollrate, pitchrate, yawrate are in degrees/s """ - pk = CRTPPacket() pk.port = CRTPPort.COMMANDER_GENERIC - pk.data = struct.pack('> 30 + sum_squares = 0 + for i in range(3, -1, -1): + if i != i_largest: + mag = comp & mask + negbit = (comp >> 9) & 0x1 + comp = comp >> 10 + q[i] = mag / mask / np.sqrt(2) + if negbit == 1: + q[i] = -q[i] + sum_squares += q[i] * q[i] + q[i_largest] = np.sqrt(1.0 - sum_squares) + return q # compress a quaternion, see quatcompress.h in firmware -# input: q = [x,y,z,w], output: 32-bit number +# input: 32-bit number, output q = [x,y,z,w] def compress_quaternion(qx, qy, qz, qw): - q = [qx, qy, qz, qw] - i_largest = 0 for i in range(1, 4): if abs(q[i]) > abs(q[i_largest]): i_largest = i - negate = q[i_largest] < 0 - comp = i_largest - m_sqrt_2 = 1.0 / sqrt(2) + M_SQRT1_2 = 1.0 / np.sqrt(2) - for i in range(0,4): + comp = i_largest + for i in range(4): if i != i_largest: negbit = (q[i] < 0) ^ negate - mag = ((1 << 9) - 1) * (abs(q[i]) / m_sqrt_2) * 0.5 - comp = (comp << 10) | (negbit << 9) | int(mag) + mag = int(((1 << 9) - 1) * (abs(q[i]) / M_SQRT1_2) + 0.5) + comp = (comp << 10) | (negbit << 9) | mag - return comp + return comp \ No newline at end of file diff --git a/examples/autonomy/full_state_setpoint_demo.py b/examples/autonomy/full_state_setpoint_demo.py index 7e89d7ef5..a25bbdc0d 100644 --- a/examples/autonomy/full_state_setpoint_demo.py +++ b/examples/autonomy/full_state_setpoint_demo.py @@ -25,6 +25,7 @@ Used for sending full state control setpoints to the Crazyflie """ import time +import logging import cflib.crtp from cflib.crazyflie import Crazyflie @@ -33,6 +34,7 @@ from cflib.crazyflie.syncLogger import SyncLogger from cflib.utils import uri_helper import math +from cflib.crazyflie.log import LogConfig @@ -128,10 +130,13 @@ def start_position_printing(scf): def run_sequence(scf): cf = scf.cf + # Set to mellinger controller + # cf.param.set_value('stabilizer.controller', '2') + # quaternion from roll pitch yaw roll = 0.0 pitch = 0.0 - yaw = 0.0 + yaw = 0.0 #20.0*math.pi/180.0 q = quaternion_from_euler(roll, pitch, yaw) print('takeoff') cf.commander.send_full_state_setpoint(0.0,0.0,1.0, @@ -162,10 +167,28 @@ def run_sequence(scf): # since the message queue is not flushed before closing time.sleep(0.1) - +def _stab_log_data(timestamp, data, logconf): + print('roll: {}, pitch: {}, yaw: {}'.format(data['controller.roll'], + data['controller.pitch'], + data['controller.yaw'])) + print('ctrltarget.x: {}, ctrltarget.y: {}, ctrltarget.z: {}'.format(data['ctrltarget.x'], + data['ctrltarget.y'], + data['ctrltarget.z'])) if __name__ == '__main__': cflib.crtp.init_drivers() with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - reset_estimator(scf) + _lg_stab = LogConfig(name='Stabilizer', period_in_ms=500) + _lg_stab.add_variable('controller.roll', 'float') + _lg_stab.add_variable('controller.pitch', 'float') + _lg_stab.add_variable('controller.yaw', 'float') + _lg_stab.add_variable('ctrltarget.x', 'float') + _lg_stab.add_variable('ctrltarget.y', 'float') + _lg_stab.add_variable('ctrltarget.z', 'float') + + scf.cf.log.add_config(_lg_stab) + _lg_stab.data_received_cb.add_callback(_stab_log_data) + _lg_stab.start() + + #reset_estimator(scf) run_sequence(scf) From 8d2769237dae4513c4e69e58f3b5789239b24d13 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 29 Aug 2023 12:39:45 +0200 Subject: [PATCH 09/12] Removed old fp16 file --- cflib/utils/fp16.py | 53 --------------------------------------------- 1 file changed, 53 deletions(-) delete mode 100644 cflib/utils/fp16.py diff --git a/cflib/utils/fp16.py b/cflib/utils/fp16.py deleted file mode 100644 index 5eb47ee5b..000000000 --- a/cflib/utils/fp16.py +++ /dev/null @@ -1,53 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- -# -# || ____ _ __ -# +------+ / __ )(_) /_______________ _____ ___ -# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ -# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ -# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ -# -# Copyright (C) 2020 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 . -import struct - - -# Code from davidejones at https://gamedev.stackexchange.com/a/28756 -def fp16_to_float(float16): - s = int((float16 >> 15) & 0x00000001) # sign - e = int((float16 >> 10) & 0x0000001f) # exponent - f = int(float16 & 0x000003ff) # fraction - - if e == 0: - if f == 0: - return int(s << 31) - else: - while not (f & 0x00000400): - f <<= 1 - e -= 1 - e += 1 - f &= ~0x00000400 - # print(s,e,f) - elif e == 31: - if f == 0: - return int((s << 31) | 0x7f800000) - else: - return int((s << 31) | 0x7f800000 | (f << 13)) - - e += 127 - 15 - f <<= 13 - result = int((s << 31) | (e << 23) | f) - return struct.unpack('f', struct.pack('I', result))[0] From f7963c4460fbf57acb0829b722e97ddd7294d629 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 29 Aug 2023 12:52:57 +0200 Subject: [PATCH 10/12] Corrections --- cflib/crazyflie/commander.py | 33 ++++--- cflib/crazyflie/localization.py | 2 +- cflib/utils/encoding.py | 48 +++++++--- examples/autonomy/full_state_setpoint_demo.py | 93 +++++++++---------- test/utils/test_encoding.py | 66 +++++++++++++ 5 files changed, 166 insertions(+), 76 deletions(-) create mode 100644 test/utils/test_encoding.py diff --git a/cflib/crazyflie/commander.py b/cflib/crazyflie/commander.py index f4c8f4a23..df5a37752 100644 --- a/cflib/crazyflie/commander.py +++ b/cflib/crazyflie/commander.py @@ -7,7 +7,7 @@ # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2011-2013 Bitcraze AB +# Copyright (C) 2011-2023 Bitcraze AB # # Crazyflie Nano Quadcopter Client # @@ -138,25 +138,34 @@ def send_hover_setpoint(self, vx, vy, yawrate, zdistance): vx, vy, yawrate, zdistance) self._cf.send_packet(pk) - def send_full_state_setpoint(self, x, y, z, vx, vy, vz, ax, ay, az, qx, qy, qz, qw, rollrate, pitchrate, yawrate): + def send_full_state_setpoint(self, pos, vel, acc, orientation, rollrate, pitchrate, yawrate): """ - Control mode where the position, velocity, acceleration, orientation, angular + Control mode where the position, velocity, acceleration, orientation and angular velocity are sent as absolute (world) values. - x, y, z are in m - vx, vy, vz are in m/s - ax, ay, az are in m/s^2 - qx, qy, qz, qw are the quaternion components of the orientation + position [x, y, z] are in m + velocity [vx, vy, vz] are in m/s + acceleration [ax, ay, az] are in m/s^2 + orientation [qx, qy, qz, qw] are the quaternion components of the orientation rollrate, pitchrate, yawrate are in degrees/s """ + def vector_to_mm_16bit(vec): + return int(vec[0] * 1000), int(vec[1] * 1000), int(vec[2] * 1000) + + x, y, z = vector_to_mm_16bit(pos) + vx, vy, vz = vector_to_mm_16bit(vel) + ax, ay, az = vector_to_mm_16bit(acc) + rr, pr, yr = vector_to_mm_16bit([rollrate, pitchrate, yawrate]) + orient_comp = compress_quaternion(orientation) + pk = CRTPPacket() pk.port = CRTPPort.COMMANDER_GENERIC pk.data = struct.pack('. import struct -from math import sqrt import numpy as np + # Code from davidejones at https://gamedev.stackexchange.com/a/28756 def fp16_to_float(float16): s = int((float16 >> 15) & 0x00000001) # sign @@ -54,9 +52,18 @@ def fp16_to_float(float16): result = int((s << 31) | (e << 23) | f) return struct.unpack('f', struct.pack('I', result))[0] -# decompress a quaternion, see quatcompress.h in firmware -# input: 32-bit number, output q = [x,y,z,w] + def decompress_quaternion(comp): + """Decompress a quaternion + + see quatcompress.h in the firmware for definitions + + Args: + comp int: A 32-bit number + + Returns: + np array: q = [x, y, z, w] + """ q = np.zeros(4) mask = (1 << 9) - 1 i_largest = comp >> 30 @@ -73,23 +80,34 @@ def decompress_quaternion(comp): q[i_largest] = np.sqrt(1.0 - sum_squares) return q -# compress a quaternion, see quatcompress.h in firmware -# input: 32-bit number, output q = [x,y,z,w] -def compress_quaternion(qx, qy, qz, qw): - q = [qx, qy, qz, qw] + +def compress_quaternion(quat): + """Compress a quaternion. + assumes input quaternion is normalized. will fail if not. + + see quatcompress.h in firmware the for definitions + + Args: + quat : An array of floats representing a quaternion [x, y, z, w] + + Returns: 32-bit integer + """ + # Normalize the quaternion + quat_n = np.array(quat) / np.linalg.norm(quat) + i_largest = 0 for i in range(1, 4): - if abs(q[i]) > abs(q[i_largest]): + if abs(quat_n[i]) > abs(quat_n[i_largest]): i_largest = i - negate = q[i_largest] < 0 + negate = quat_n[i_largest] < 0 M_SQRT1_2 = 1.0 / np.sqrt(2) comp = i_largest for i in range(4): if i != i_largest: - negbit = (q[i] < 0) ^ negate - mag = int(((1 << 9) - 1) * (abs(q[i]) / M_SQRT1_2) + 0.5) + negbit = int((quat_n[i] < 0) ^ negate) + mag = int(((1 << 9) - 1) * (abs(quat_n[i]) / M_SQRT1_2) + 0.5) comp = (comp << 10) | (negbit << 9) | mag - return comp \ No newline at end of file + return comp diff --git a/examples/autonomy/full_state_setpoint_demo.py b/examples/autonomy/full_state_setpoint_demo.py index a25bbdc0d..f1920863d 100644 --- a/examples/autonomy/full_state_setpoint_demo.py +++ b/examples/autonomy/full_state_setpoint_demo.py @@ -6,9 +6,7 @@ # +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ # || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ # -# Copyright (C) 2016 Bitcraze AB -# -# Crazyflie Nano Quadcopter Client +# Copyright (C) 2023 Bitcraze AB # # This program is free software; you can redistribute it and/or # modify it under the terms of the GNU General Public License @@ -22,10 +20,11 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . """ -Used for sending full state control setpoints to the Crazyflie +Shows how to send full state control setpoints to the Crazyflie """ import time -import logging + +from scipy.spatial.transform import Rotation import cflib.crtp from cflib.crazyflie import Crazyflie @@ -33,30 +32,24 @@ from cflib.crazyflie.syncCrazyflie import SyncCrazyflie from cflib.crazyflie.syncLogger import SyncLogger from cflib.utils import uri_helper -import math -from cflib.crazyflie.log import LogConfig - # URI to the Crazyflie to connect to uri = uri_helper.uri_from_env(default='radio://0/65/2M/E7E7E7E7F2') -def quaternion_from_euler(roll, pitch, yaw): - cy = math.cos(yaw * 0.5) - sy = math.sin(yaw * 0.5) - cp = math.cos(pitch * 0.5) - sp = math.sin(pitch * 0.5) - cr = math.cos(roll * 0.5) - sr = math.sin(roll * 0.5) +def quaternion_from_euler(roll: float, pitch: float, yaw: float): + """Convert Euler angles to quaternion - q = [0] * 4 - q[0] = sr * cp * cy - cr * sp * sy - q[1] = cr * sp * cy + sr * cp * sy - q[2] = cr * cp * sy - sr * sp * cy - q[3] = cr * cp * cy + sr * sp * sy + Args: + roll (float): roll, in radians + pitch (float): pitch, in radians + yaw (float): yaw, in radians - return q + Returns: + array: the quaternion [x, y, z, w] + """ + return Rotation.from_euler(seq='xyz', angles=(roll, pitch, yaw), degrees=False).as_quat() def wait_for_position_estimator(scf): @@ -127,46 +120,48 @@ def start_position_printing(scf): log_conf.start() +def send_setpoint(cf, duration, pos, vel, acc, orientation, rollrate, pitchrate, yawrate): + # Set points must be sent continuously to the Crazyflie, if not it will think that connection is lost + end_time = time.time() + duration + while time.time() < end_time: + cf.commander.send_full_state_setpoint(pos, vel, acc, orientation, rollrate, pitchrate, yawrate) + time.sleep(0.2) + + def run_sequence(scf): cf = scf.cf # Set to mellinger controller # cf.param.set_value('stabilizer.controller', '2') - # quaternion from roll pitch yaw - roll = 0.0 - pitch = 0.0 - yaw = 0.0 #20.0*math.pi/180.0 - q = quaternion_from_euler(roll, pitch, yaw) print('takeoff') - cf.commander.send_full_state_setpoint(0.0,0.0,1.0, - 0.0,0.0,0.0, - 0.0,0.0,0.0, - q[0],q[1],q[2],q[3], - 0.0,0.0,0.0) - - time.sleep(2.0) - cf.commander.send_full_state_setpoint(0.0,0.0,1.0, - 0.0,0.0,0.0, - 0.0,0.0,0.0, - q[0],q[1],q[2],q[3], - 0.0,0.0,0.0) - time.sleep(2.0) + send_setpoint(cf, 4.0, + [0.0, -1.0, 1.0], + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0], + quaternion_from_euler(0.0, 0.0, 0.0), + 0.0, 0.0, 0.0) + + send_setpoint(cf, 2.0, + [0.0, -1.0, 1.0], + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0], + quaternion_from_euler(0.0, 0.0, 0.0), + 0.0, 0.0, 0.0) print('land') - cf.commander.send_full_state_setpoint(0.0,0.0,0.2, - 0.0,0.0,0.0, - 0.0,0.0,0.0, - q[0],q[1],q[2],q[3], - 0.0,0.0,0.0) - - time.sleep(2.0) - + send_setpoint(cf, 2.0, + [0.0, -1.0, 0.2], + [0.0, 0.0, 0.0], + [0.0, 0.0, 0.0], + quaternion_from_euler(0.0, 0.0, 0.0), + 0.0, 0.0, 0.0) cf.commander.send_stop_setpoint() # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing time.sleep(0.1) + def _stab_log_data(timestamp, data, logconf): print('roll: {}, pitch: {}, yaw: {}'.format(data['controller.roll'], data['controller.pitch'], @@ -174,6 +169,8 @@ def _stab_log_data(timestamp, data, logconf): print('ctrltarget.x: {}, ctrltarget.y: {}, ctrltarget.z: {}'.format(data['ctrltarget.x'], data['ctrltarget.y'], data['ctrltarget.z'])) + + if __name__ == '__main__': cflib.crtp.init_drivers() @@ -190,5 +187,5 @@ def _stab_log_data(timestamp, data, logconf): _lg_stab.data_received_cb.add_callback(_stab_log_data) _lg_stab.start() - #reset_estimator(scf) + reset_estimator(scf) run_sequence(scf) diff --git a/test/utils/test_encoding.py b/test/utils/test_encoding.py new file mode 100644 index 000000000..1d1904eda --- /dev/null +++ b/test/utils/test_encoding.py @@ -0,0 +1,66 @@ +# -*- coding: utf-8 -*- +# +# || ____ _ __ +# +------+ / __ )(_) /_______________ _____ ___ +# | 0xBC | / __ / / __/ ___/ ___/ __ `/_ / / _ \ +# +------+ / /_/ / / /_/ /__/ / / /_/ / / /_/ __/ +# || || /_____/_/\__/\___/_/ \__,_/ /___/\___/ +# +# Copyright (C) 2023 Bitcraze AB +# +# 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 . +import unittest +import numpy as np +from cflib.utils.encoding import compress_quaternion, decompress_quaternion + + +class EncodingTest(unittest.TestCase): + + def test_compress_decompress(self): + # Fixture + expected = self._normalize_quat([1, 2, 3, 4]) + + # Test + compressed = compress_quaternion(expected) + actual = decompress_quaternion(compressed) + + # Assert + np.testing.assert_allclose(actual, expected, 0.001) + + def test_compress_decompress_not_normalized(self): + # Fixture + quat = [1, 2, 3, 4] + expected = self._normalize_quat(quat) + + # Test + compressed = compress_quaternion(quat) + actual = decompress_quaternion(compressed) + + # Assert + np.testing.assert_allclose(actual, expected, 0.001) + + def test_other_largest_component(self): + # Fixture + quat = [5, 10, 3, 4] + expected = self._normalize_quat(quat) + + # Test + compressed = compress_quaternion(quat) + actual = decompress_quaternion(compressed) + + # Assert + np.testing.assert_allclose(actual, expected, 0.001) + + def _normalize_quat(self, quat): + quat = np.array(quat) + return quat / np.linalg.norm(quat) From 5e94b13b7467f9bbf6e46f9f2e0464b3c6f8a134 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 29 Aug 2023 13:00:15 +0200 Subject: [PATCH 11/12] Reorder imports --- test/utils/test_encoding.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/test/utils/test_encoding.py b/test/utils/test_encoding.py index 1d1904eda..57d97e72a 100644 --- a/test/utils/test_encoding.py +++ b/test/utils/test_encoding.py @@ -20,8 +20,11 @@ # You should have received a copy of the GNU General Public License # along with this program. If not, see . import unittest + import numpy as np -from cflib.utils.encoding import compress_quaternion, decompress_quaternion + +from cflib.utils.encoding import compress_quaternion +from cflib.utils.encoding import decompress_quaternion class EncodingTest(unittest.TestCase): From 1920ee7f338cda64ce062e9b57da2adc29acb129 Mon Sep 17 00:00:00 2001 From: Kristoffer Richardsson Date: Tue, 29 Aug 2023 13:17:08 +0200 Subject: [PATCH 12/12] Updated demo --- examples/autonomy/full_state_setpoint_demo.py | 38 +++++++++++-------- 1 file changed, 22 insertions(+), 16 deletions(-) diff --git a/examples/autonomy/full_state_setpoint_demo.py b/examples/autonomy/full_state_setpoint_demo.py index f1920863d..9b6ea00c6 100644 --- a/examples/autonomy/full_state_setpoint_demo.py +++ b/examples/autonomy/full_state_setpoint_demo.py @@ -136,27 +136,30 @@ def run_sequence(scf): print('takeoff') send_setpoint(cf, 4.0, - [0.0, -1.0, 1.0], + [0.0, 0.0, 1.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], quaternion_from_euler(0.0, 0.0, 0.0), 0.0, 0.0, 0.0) send_setpoint(cf, 2.0, - [0.0, -1.0, 1.0], + [0.0, 0.0, 1.0], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], - quaternion_from_euler(0.0, 0.0, 0.0), + quaternion_from_euler(0.0, 0.0, 0.7), 0.0, 0.0, 0.0) print('land') send_setpoint(cf, 2.0, - [0.0, -1.0, 0.2], + [0.0, 0.0, 0.2], [0.0, 0.0, 0.0], [0.0, 0.0, 0.0], quaternion_from_euler(0.0, 0.0, 0.0), 0.0, 0.0, 0.0) cf.commander.send_stop_setpoint() + # Hand control over to the high level commander to avoid timeout and locking of the Crazyflie + cf.commander.send_notify_setpoint_stop() + # Make sure that the last packet leaves before the link is closed # since the message queue is not flushed before closing time.sleep(0.1) @@ -171,21 +174,24 @@ def _stab_log_data(timestamp, data, logconf): data['ctrltarget.z'])) +def set_up_logging(scf): + _lg_stab = LogConfig(name='Stabilizer', period_in_ms=500) + _lg_stab.add_variable('controller.roll', 'float') + _lg_stab.add_variable('controller.pitch', 'float') + _lg_stab.add_variable('controller.yaw', 'float') + _lg_stab.add_variable('ctrltarget.x', 'float') + _lg_stab.add_variable('ctrltarget.y', 'float') + _lg_stab.add_variable('ctrltarget.z', 'float') + + scf.cf.log.add_config(_lg_stab) + _lg_stab.data_received_cb.add_callback(_stab_log_data) + _lg_stab.start() + + if __name__ == '__main__': cflib.crtp.init_drivers() with SyncCrazyflie(uri, cf=Crazyflie(rw_cache='./cache')) as scf: - _lg_stab = LogConfig(name='Stabilizer', period_in_ms=500) - _lg_stab.add_variable('controller.roll', 'float') - _lg_stab.add_variable('controller.pitch', 'float') - _lg_stab.add_variable('controller.yaw', 'float') - _lg_stab.add_variable('ctrltarget.x', 'float') - _lg_stab.add_variable('ctrltarget.y', 'float') - _lg_stab.add_variable('ctrltarget.z', 'float') - - scf.cf.log.add_config(_lg_stab) - _lg_stab.data_received_cb.add_callback(_stab_log_data) - _lg_stab.start() - + # set_up_logging(scf) reset_estimator(scf) run_sequence(scf)