From cb6b75d3a98bed55ea72f19480178eda532ab643 Mon Sep 17 00:00:00 2001 From: Wolfgang Hoenig Date: Mon, 22 Jan 2024 12:30:53 +0100 Subject: [PATCH] add Python bindings --- bindings/cffirmware.i | 2 ++ bindings/setup.py | 1 + test_python/test_controller_lee.py | 45 ++++++++++++++++++++++++++++++ 3 files changed, 48 insertions(+) create mode 100644 test_python/test_controller_lee.py diff --git a/bindings/cffirmware.i b/bindings/cffirmware.i index 289706ba69..3b8ea9a8d4 100755 --- a/bindings/cffirmware.i +++ b/bindings/cffirmware.i @@ -19,6 +19,7 @@ #include "num.h" #include "controller_mellinger.h" #include "controller_brescianini.h" +#include "controller_lee.h" #include "power_distribution.h" #include "axis3fSubSampler.h" #include "outlierFilterTdoa.h" @@ -35,6 +36,7 @@ %include "imu_types.h" %include "controller_mellinger.h" %include "controller_brescianini.h" +%include "controller_lee.h" %include "power_distribution.h" %include "axis3fSubSampler.h" %include "outlierFilterTdoa.h" diff --git a/bindings/setup.py b/bindings/setup.py index a543e31c62..4f67015624 100644 --- a/bindings/setup.py +++ b/bindings/setup.py @@ -41,6 +41,7 @@ "src/modules/src/controller/attitude_pid_controller.c", "src/modules/src/controller/controller_mellinger.c", "src/modules/src/controller/controller_brescianini.c", + "src/modules/src/controller/controller_lee.c", "src/utils/src/pid.c", "src/utils/src/filter.c", "src/utils/src/num.c", diff --git a/test_python/test_controller_lee.py b/test_python/test_controller_lee.py new file mode 100644 index 0000000000..eed35560b7 --- /dev/null +++ b/test_python/test_controller_lee.py @@ -0,0 +1,45 @@ +#!/usr/bin/env python + +import cffirmware + +def test_controller_lee(): + + ctrl = cffirmware.controllerLee_t() + + cffirmware.controllerLeeInit(ctrl) + + control = cffirmware.control_t() + setpoint = cffirmware.setpoint_t() + setpoint.mode.z = cffirmware.modeAbs + setpoint.position.z = 0 + setpoint.mode.x = cffirmware.modeVelocity + setpoint.velocity.x = 0 + setpoint.mode.y = cffirmware.modeVelocity + setpoint.velocity.y = 0 + setpoint.mode.yaw = cffirmware.modeVelocity + setpoint.attitudeRate.yaw = 0 + + state = cffirmware.state_t() + state.attitude.roll = 0 + state.attitude.pitch = -0 # WARNING: This needs to be negated + state.attitude.yaw = 0 + state.position.x = 0 + state.position.y = 0 + state.position.z = 0 + state.velocity.x = 0 + state.velocity.y = 0 + state.velocity.z = 0 + + sensors = cffirmware.sensorData_t() + sensors.gyro.x = 0 + sensors.gyro.y = 0 + sensors.gyro.z = 0 + + step = 100 + + cffirmware.controllerLee(ctrl, control, setpoint,sensors,state,step) + assert control.controlMode == cffirmware.controlModeForceTorque + # control.thrust will be at a (tuned) hover-state + assert control.torqueX == 0 + assert control.torqueY == 0 + assert control.torqueZ == 0