[test] playing around with cython wrappers

2026-05-30 03:27:33 +08:00 · 2013-10-29 09:55:28 +01:00
parent 083ec6be83
commit f6d2b476f5
14 changed files with 595 additions and 0 deletions
@@ -0,0 +1,6 @@
+*.so
+algebra_int.c
+algebra_float.c
+ref_quat_float.c
+ref_quat_int.c
+build
@@ -0,0 +1,11 @@
+Some Cython wrappers for stabilization reference generation testing.
+
+Tested with Cython 0.19
+
+To build execute in this directory:
+
+    python setup.py build_ext --inplace
+
+Run the example script to compare float and int ref quat implementations:
+
+    ./compare_ref_quat.py
@@ -0,0 +1,10 @@
+cimport pprz_algebra_float_c
+
+cdef class FloatEulers:
+    cdef pprz_algebra_float_c.FloatEulers *ptr
+
+cdef class FloatQuat:
+    cdef pprz_algebra_float_c.FloatQuat *ptr
+
+cdef class FloatRates:
+    cdef pprz_algebra_float_c.FloatRates *ptr
@@ -0,0 +1,118 @@
+cdef extern from "stdint.h":
+    ctypedef unsigned long int uintptr_t
+
+cimport pprz_algebra_float_c
+cimport algebra_float
+cimport numpy as np
+import numpy as np
+
+
+cdef class FloatRates:
+    # already declared in pxd
+    #cdef pprz_algebra_float_c.FloatRates *ptr
+
+    def __cinit__(self, uintptr_t p):
+        self.ptr = <pprz_algebra_float_c.FloatRates*> p
+
+    property array:
+        def __get__(self):
+            return np.asarray([self.ptr[0].p, self.ptr[0].q, self.ptr[0].r])
+        def __set__(self, np.ndarray a):
+            self.ptr[0].p = a[0]
+            self.ptr[0].q = a[1]
+            self.ptr[0].r = a[2]
+
+    property p:
+        def __get__(self):
+            return self.ptr[0].p
+        def __set__(self, v):
+            self.ptr[0].p = v
+
+    property q:
+        def __get__(self):
+            return self.ptr[0].q
+        def __set__(self, v):
+            self.ptr[0].q = v
+
+    property r:
+        def __get__(self):
+            return self.ptr[0].r
+        def __set__(self, v):
+            self.ptr[0].r = v
+
+
+cdef class FloatEulers:
+    # already declared in pxd
+    #cdef pprz_algebra_float_c.FloatEulers *ptr
+
+    def __cinit__(self, uintptr_t p):
+        self.ptr = <pprz_algebra_float_c.FloatEulers*> p
+
+    property array:
+        def __get__(self):
+            return np.asarray([self.ptr[0].phi, self.ptr[0].theta,
+                               self.ptr[0].psi])
+        def __set__(self, np.ndarray a):
+            self.ptr[0].phi = a[0]
+            self.ptr[0].theta = a[1]
+            self.ptr[0].psi = a[2]
+
+    property phi:
+        def __get__(self):
+            return self.ptr[0].phi
+        def __set__(self, v):
+            self.ptr[0].phi = v
+
+    property theta:
+        def __get__(self):
+            return self.ptr[0].theta
+        def __set__(self, v):
+            self.ptr[0].theta = v
+
+    property psi:
+        def __get__(self):
+            return self.ptr[0].psi
+        def __set__(self, v):
+            self.ptr[0].psi = v
+
+
+cdef class FloatQuat:
+    # already declared in pxd
+    #cdef pprz_algebra_float_c.FloatQuat *ptr
+
+    def __cinit__(self, uintptr_t p):
+        self.ptr = <pprz_algebra_float_c.FloatQuat*> p
+
+    property array:
+        def __get__(self):
+            return np.asarray([self.ptr[0].qi, self.ptr[0].qx,
+                               self.ptr[0].qy, self.ptr[0].qz])
+        def __set__(self, np.ndarray a):
+            self.ptr[0].qi = a[0]
+            self.ptr[0].qx = a[1]
+            self.ptr[0].qy = a[2]
+            self.ptr[0].qz = a[3]
+
+    property qi:
+        def __get__(self):
+            return self.ptr[0].qi
+        def __set__(self, v):
+            self.ptr[0].qi = v
+
+    property qx:
+        def __get__(self):
+            return self.ptr[0].qx
+        def __set__(self, v):
+            self.ptr[0].qx = v
+
+    property qy:
+        def __get__(self):
+            return self.ptr[0].qy
+        def __set__(self, v):
+            self.ptr[0].qy = v
+
+    property qz:
+        def __get__(self):
+            return self.ptr[0].qz
+        def __set__(self, v):
+            self.ptr[0].qz = v
@@ -0,0 +1,10 @@
+cimport pprz_algebra_int_c
+
+cdef class Int32Eulers:
+    cdef pprz_algebra_int_c.Int32Eulers *ptr
+
+cdef class Int32Quat:
+    cdef pprz_algebra_int_c.Int32Quat *ptr
+
+cdef class Int32Rates:
+    cdef pprz_algebra_int_c.Int32Rates *ptr
@@ -0,0 +1,112 @@
+cdef extern from "stdint.h":
+    ctypedef unsigned long int uintptr_t
+
+cimport pprz_algebra_int_c
+cimport algebra_int
+cimport numpy as np
+import numpy as np
+
+
+cdef class Int32Rates:
+
+    def __cinit__(self, uintptr_t p):
+        self.ptr = <pprz_algebra_int_c.Int32Rates*> p
+
+    property array:
+        def __get__(self):
+            return np.asarray([self.ptr[0].p, self.ptr[0].q, self.ptr[0].r])
+        def __set__(self, np.ndarray a):
+            self.ptr[0].p = a[0]
+            self.ptr[0].q = a[1]
+            self.ptr[0].r = a[2]
+
+    property p:
+        def __get__(self):
+            return self.ptr[0].p
+        def __set__(self, v):
+            self.ptr[0].p = v
+
+    property q:
+        def __get__(self):
+            return self.ptr[0].q
+        def __set__(self, v):
+            self.ptr[0].q = v
+
+    property r:
+        def __get__(self):
+            return self.ptr[0].r
+        def __set__(self, v):
+            self.ptr[0].r = v
+
+
+cdef class Int32Eulers:
+
+    def __cinit__(self, uintptr_t p):
+        self.ptr = <pprz_algebra_int_c.Int32Eulers*> p
+
+    property array:
+        def __get__(self):
+            return np.asarray([self.ptr[0].phi, self.ptr[0].theta,
+                               self.ptr[0].psi])
+        def __set__(self, np.ndarray a):
+            self.ptr[0].phi = a[0]
+            self.ptr[0].theta = a[1]
+            self.ptr[0].psi = a[2]
+
+    property phi:
+        def __get__(self):
+            return self.ptr[0].phi
+        def __set__(self, v):
+            self.ptr[0].phi = v
+
+    property theta:
+        def __get__(self):
+            return self.ptr[0].theta
+        def __set__(self, v):
+            self.ptr[0].theta = v
+
+    property psi:
+        def __get__(self):
+            return self.ptr[0].psi
+        def __set__(self, v):
+            self.ptr[0].psi = v
+
+
+cdef class Int32Quat:
+
+    def __cinit__(self, uintptr_t p):
+        self.ptr = <pprz_algebra_int_c.Int32Quat*> p
+
+    property array:
+        def __get__(self):
+            return np.asarray([self.ptr[0].qi, self.ptr[0].qx,
+                               self.ptr[0].qy, self.ptr[0].qz])
+        def __set__(self, np.ndarray a):
+            self.ptr[0].qi = a[0]
+            self.ptr[0].qx = a[1]
+            self.ptr[0].qy = a[2]
+            self.ptr[0].qz = a[3]
+
+    property qi:
+        def __get__(self):
+            return self.ptr[0].qi
+        def __set__(self, v):
+            self.ptr[0].qi = v
+
+    property qx:
+        def __get__(self):
+            return self.ptr[0].qx
+        def __set__(self, v):
+            self.ptr[0].qx = v
+
+    property qy:
+        def __get__(self):
+            return self.ptr[0].qy
+        def __set__(self, v):
+            self.ptr[0].qy = v
+
+    property qz:
+        def __get__(self):
+            return self.ptr[0].qz
+        def __set__(self, v):
+            self.ptr[0].qz = v
@@ -0,0 +1,50 @@
+#! /usr/bin/env python
+
+from __future__ import division, print_function, absolute_import
+
+import numpy as np
+import matplotlib.pyplot as plt
+
+import ref_quat_float
+import ref_quat_int
+
+steps = 512 * 2
+ref_eul_res = np.zeros((steps, 3))
+ref_quat_res = np.zeros((steps, 3))
+
+ref_quat_float.init()
+ref_quat_int.init()
+
+# reset psi and update_ref_quat_from_eulers
+ref_quat_float.enter()
+ref_quat_int.enter()
+
+q_sp = np.array([0.92387956, 0.38268346, 0., 0.])
+ref_quat_float.sp_quat.array = q_sp
+ref_quat_int.sp_quat.array = q_sp * (1 << 15)
+
+for i in range(0, steps):
+    ref_quat_float.update()
+    ref_eul_res[i, :] = ref_quat_float.ref_euler.array
+    ref_quat_int.update()
+    ref_quat_res[i, :] = ref_quat_int.ref_euler.array / (1 << 20)
+
+
+plt.figure(1)
+plt.subplot(311)
+plt.title("reference in euler angles")
+plt.plot(np.degrees(ref_eul_res[:, 0]), 'g')
+plt.plot(np.degrees(ref_quat_res[:, 0]), 'r')
+plt.ylabel("phi [deg]")
+
+plt.subplot(312)
+plt.plot(np.degrees(ref_eul_res[:, 1]), 'g')
+plt.plot(np.degrees(ref_quat_res[:, 1]), 'r')
+plt.ylabel("theta [deg]")
+
+plt.subplot(313)
+plt.plot(np.degrees(ref_eul_res[:, 2]), 'g')
+plt.plot(np.degrees(ref_quat_res[:, 2]), 'r')
+plt.ylabel("psi [deg]")
+
+plt.show()
@@ -0,0 +1,51 @@
+/* fake generated airframe file */
+
+#ifndef AIRFRAME_H
+#define AIRFRAME_H
+
+#define PERIODIC_FREQUENCY 512
+
+#if defined STABILIZATION_ATTITUDE_TYPE_FLOAT
+
+#define SECTION_STABILIZATION_ATTITUDE 1
+#define STABILIZATION_ATTITUDE_SP_MAX_PHI 0.7853981625
+#define STABILIZATION_ATTITUDE_SP_MAX_THETA 0.7853981625
+#define STABILIZATION_ATTITUDE_SP_MAX_R 1.570796325
+
+#define STABILIZATION_ATTITUDE_REF_OMEGA_P {RadOfDeg(800)}
+#define STABILIZATION_ATTITUDE_REF_ZETA_P {0.85}
+#define STABILIZATION_ATTITUDE_REF_MAX_P 6.981317
+#define STABILIZATION_ATTITUDE_REF_MAX_PDOT RadOfDeg(8000.)
+#define STABILIZATION_ATTITUDE_REF_OMEGA_Q {RadOfDeg(800)}
+#define STABILIZATION_ATTITUDE_REF_ZETA_Q {0.85}
+#define STABILIZATION_ATTITUDE_REF_MAX_Q 6.981317
+#define STABILIZATION_ATTITUDE_REF_MAX_QDOT RadOfDeg(8000.)
+#define STABILIZATION_ATTITUDE_REF_OMEGA_R {RadOfDeg(500)}
+#define STABILIZATION_ATTITUDE_REF_ZETA_R {0.85}
+#define STABILIZATION_ATTITUDE_REF_MAX_R 3.14159265
+#define STABILIZATION_ATTITUDE_REF_MAX_RDOT RadOfDeg(1800.)
+
+#elif defined STABILIZATION_ATTITUDE_TYPE_INT
+
+#define SECTION_STABILIZATION_ATTITUDE 1
+#define STABILIZATION_ATTITUDE_SP_MAX_PHI 0.7853981625
+#define STABILIZATION_ATTITUDE_SP_MAX_THETA 0.7853981625
+#define STABILIZATION_ATTITUDE_SP_MAX_R 1.570796325
+
+#define STABILIZATION_ATTITUDE_REF_OMEGA_P 13.962634
+#define STABILIZATION_ATTITUDE_REF_ZETA_P 0.85
+#define STABILIZATION_ATTITUDE_REF_MAX_P 6.981317
+#define STABILIZATION_ATTITUDE_REF_MAX_PDOT RadOfDeg(8000.)
+#define STABILIZATION_ATTITUDE_REF_OMEGA_Q 13.962634
+#define STABILIZATION_ATTITUDE_REF_ZETA_Q 0.85
+#define STABILIZATION_ATTITUDE_REF_MAX_Q 6.981317
+#define STABILIZATION_ATTITUDE_REF_MAX_QDOT RadOfDeg(8000.)
+#define STABILIZATION_ATTITUDE_REF_OMEGA_R 8.72664625
+#define STABILIZATION_ATTITUDE_REF_ZETA_R 0.85
+#define STABILIZATION_ATTITUDE_REF_MAX_R 3.14159265
+#define STABILIZATION_ATTITUDE_REF_MAX_RDOT RadOfDeg(1800.)
+
+#endif
+
+
+#endif // AIRFRAME_H
@@ -0,0 +1,29 @@
+cdef extern from "math/pprz_algebra_double.h":
+
+    struct DoubleVect2
+        double x
+        double y
+
+    struct DoubleVect3
+        double x
+        double y
+        double z
+
+    struct DoubleEulers:
+        double phi
+        double theta
+        double psi
+
+    struct DoubleQuat:
+        double qi
+        double qx
+        double qy
+        double qz
+
+    struct DoubleRates:
+        double p
+        double q
+        double r
+
+    struct DoubleRMat:
+        double m[3*3]
@@ -0,0 +1,29 @@
+cdef extern from "math/pprz_algebra_float.h":
+
+    struct FloatVect2:
+        float x
+        float y
+
+    struct FloatVect3:
+        float x
+        float y
+        float z
+
+    struct FloatEulers:
+        float phi
+        float theta
+        float psi
+
+    struct FloatQuat:
+        float qi
+        float qx
+        float qy
+        float qz
+
+    struct FloatRates:
+        float p
+        float q
+        float r
+
+    struct FloatRMat:
+        float m[3*3]
@@ -0,0 +1,34 @@
+from libc.stdint cimport uint16_t, int16_t, uint32_t, int32_t, uint64_t, int64_t
+
+cdef extern from "math/pprz_algebra_int.h":
+
+    struct Int32Vect2:
+        int32_t x
+        int32_t y
+
+    struct Int32Vect3:
+        int32_t x
+        int32_t y
+        int32_t z
+
+    struct Int32Eulers:
+        int32_t phi
+        int32_t theta
+        int32_t psi
+
+    struct Int32Quat:
+        int32_t qi
+        int32_t qx
+        int32_t qy
+        int32_t qz
+
+    struct Int32Rates:
+        int32_t p
+        int32_t q
+        int32_t r
+
+    struct Int32RMat:
+        int32_t m[3*3]
+
+cdef extern from "math/pprz_trig_int.h":
+    int16_t pprz_trig_int[]
@@ -0,0 +1,60 @@
+cdef extern from "stdint.h":
+    ctypedef unsigned long int uintptr_t
+
+
+import numpy as np
+cimport numpy as np
+
+cimport pprz_algebra_float_c
+from algebra_float import FloatQuat, FloatEulers, FloatRates
+
+
+cdef extern from "stabilization/stabilization_attitude_ref_quat_float.h":
+    void stabilization_attitude_ref_init()
+    void stabilization_attitude_ref_update()
+    void stabilization_attitude_ref_enter()
+    void stabilization_attitude_ref_schedule(int idx)
+
+    pprz_algebra_float_c.FloatEulers stab_att_sp_euler
+    pprz_algebra_float_c.FloatQuat   stab_att_sp_quat
+    pprz_algebra_float_c.FloatEulers stab_att_ref_euler
+    pprz_algebra_float_c.FloatQuat   stab_att_ref_quat
+    pprz_algebra_float_c.FloatRates  stab_att_ref_rate
+    pprz_algebra_float_c.FloatRates  stab_att_ref_accel
+
+
+sp_euler = FloatEulers(<uintptr_t> &stab_att_sp_euler)
+sp_quat = FloatQuat(<uintptr_t> &stab_att_sp_quat)
+ref_euler = FloatEulers(<uintptr_t> &stab_att_ref_euler)
+ref_quat = FloatQuat(<uintptr_t> &stab_att_ref_quat)
+ref_rate = FloatRates(<uintptr_t> &stab_att_ref_rate)
+ref_accel = FloatRates(<uintptr_t> &stab_att_ref_accel)
+
+
+def init():
+    stabilization_attitude_ref_init()
+
+def enter():
+    stabilization_attitude_ref_enter()
+
+def update():
+    stabilization_attitude_ref_update()
+
+"""
+# test some different set methods
+def set_sp_euler(**kwds):
+    global stab_att_sp_euler
+    cdef pprz_algebra_float_c.FloatEulers c_sp = kwds
+    stab_att_sp_euler = c_sp
+
+def set_sp_euler(phi=0.0, theta=0.0, psi=0.0):
+    global sp_euler
+    sp_euler.phi = phi
+    sp_euler.theta = theta
+    sp_euler.psi = psi
+
+def set_sp_quat(**kwds):
+    global stab_att_sp_quat
+    cdef pprz_algebra_float_c.FloatQuat c_sp = kwds
+    stab_att_sp_quat = c_sp
+"""
@@ -0,0 +1,37 @@
+cdef extern from "stdint.h":
+    ctypedef unsigned long int uintptr_t
+
+
+cimport pprz_algebra_int_c
+from algebra_int import Int32Quat, Int32Eulers, Int32Rates
+
+
+cdef extern from "stabilization/stabilization_attitude_ref_quat_int.h":
+    void stabilization_attitude_ref_init()
+    void stabilization_attitude_ref_update()
+    void stabilization_attitude_ref_enter()
+
+    pprz_algebra_int_c.Int32Eulers stab_att_sp_euler
+    pprz_algebra_int_c.Int32Quat   stab_att_sp_quat
+    pprz_algebra_int_c.Int32Eulers stab_att_ref_euler
+    pprz_algebra_int_c.Int32Quat   stab_att_ref_quat
+    pprz_algebra_int_c.Int32Rates  stab_att_ref_rate
+    pprz_algebra_int_c.Int32Rates  stab_att_ref_accel
+
+
+sp_euler = Int32Eulers(<uintptr_t> &stab_att_sp_euler)
+sp_quat = Int32Quat(<uintptr_t> &stab_att_sp_quat)
+ref_euler = Int32Eulers(<uintptr_t> &stab_att_ref_euler)
+ref_quat = Int32Quat(<uintptr_t> &stab_att_ref_quat)
+ref_rate = Int32Rates(<uintptr_t> &stab_att_ref_rate)
+ref_accel = Int32Rates(<uintptr_t> &stab_att_ref_accel)
+
+
+def init():
+    stabilization_attitude_ref_init()
+
+def enter():
+    stabilization_attitude_ref_enter()
+
+def update():
+    stabilization_attitude_ref_update()
@@ -0,0 +1,38 @@
+from distutils.core import setup, Extension
+from Cython.Build import cythonize
+from distutils.extension import Extension
+import numpy
+
+from os import path, getenv
+
+# if PAPARAZZI_SRC not set, then assume the tree containing this
+# file is a reasonable substitute
+pprz_src = getenv("PAPARAZZI_SRC", path.normpath(path.join(path.dirname(path.abspath(__file__)), '../../../../')))
+pprz_airborne = path.join(pprz_src, "sw/airborne")
+
+common_inc_dirs = ["./", path.join(pprz_src, "sw/include"), pprz_airborne, numpy.get_include()]
+
+algebra_float = Extension("algebra_float", sources=['algebra_float.pyx'],
+                          include_dirs=common_inc_dirs)
+
+algebra_int = Extension("algebra_int", sources=['algebra_int.pyx', path.join(pprz_airborne, 'math/pprz_trig_int.c')],
+                        include_dirs=common_inc_dirs)
+
+includedirs = common_inc_dirs + [path.join(pprz_airborne, "firmwares/rotorcraft")]
+ext_quat_float = Extension("ref_quat_float",
+                           sources=['ref_quat_float.pyx',
+                                    path.join(pprz_airborne, "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_float.c")],
+                           include_dirs=includedirs,
+                           extra_compile_args=["-std=c99", "-DSTABILIZATION_ATTITUDE_TYPE_FLOAT"])
+ext_quat_int = Extension("ref_quat_int",
+                         sources=['ref_quat_int.pyx',
+                                  path.join(pprz_airborne, 'math/pprz_trig_int.c'),
+                                  path.join(pprz_airborne, "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.c")],
+                         include_dirs=includedirs,
+                         extra_compile_args=["-std=c99", "-DSTABILIZATION_ATTITUDE_TYPE_INT"])
+
+extensions = [algebra_float, algebra_int, ext_quat_float, ext_quat_int]
+
+setup(
+    ext_modules=cythonize(extensions)
+)