add Python bindings

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"

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",

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