1 /* ----------------------------------------------------------------------
2  * Project:      CMSIS DSP Python Wrapper
3  * Title:        cmsismodule.h
4  * Description:  C code for the CMSIS-DSP Python wrapper
5  *
6  * $Date:        27 April 2021
7  * $Revision:    V1.0
8  *
9  * Target Processor: Cortex-M cores
10  * -------------------------------------------------------------------- */
11 /*
12  * Copyright (C) 2010-2021 ARM Limited or its affiliates. All rights reserved.
13  *
14  * SPDX-License-Identifier: Apache-2.0
15  *
16  * Licensed under the Apache License, Version 2.0 (the License); you may
17  * not use this file except in compliance with the License.
18  * You may obtain a copy of the License at
19  *
20  * www.apache.org/licenses/LICENSE-2.0
21  *
22  * Unless required by applicable law or agreed to in writing, software
23  * distributed under the License is distributed on an AS IS BASIS, WITHOUT
24  * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
25  * See the License for the specific language governing permissions and
26  * limitations under the License.
27  */
28 
29 #define MODNAME "cmsisdsp_quaternion"
30 #define MODINITNAME cmsisdsp_quaternion
31 
32 #include "cmsisdsp_module.h"
33 
34 
35 NUMPYVECTORFROMBUFFER(f32,float32_t,NPY_FLOAT);
36 
37 
typeRegistration(PyObject * module)38 void typeRegistration(PyObject *module) {
39 
40 
41 }
42 
43 
44 
45 
46 
47 
48 
49 static PyObject *
cmsis_arm_quaternion_product_f32(PyObject * obj,PyObject * args)50 cmsis_arm_quaternion_product_f32(PyObject *obj, PyObject *args)
51 {
52 
53   PyObject *pSrcA=NULL; // input
54   float32_t *pSrcA_converted=NULL; // input
55   PyObject *pSrcB=NULL; // input
56   float32_t *pSrcB_converted=NULL; // input
57   float32_t *pDst=NULL; // output
58   uint32_t nbQuaternions; // input
59 
60   if (PyArg_ParseTuple(args,"OO",&pSrcA,&pSrcB))
61   {
62 
63     GETARGUMENT(pSrcA,NPY_DOUBLE,double,float32_t);
64     GETARGUMENT(pSrcB,NPY_DOUBLE,double,float32_t);
65     nbQuaternions = arraySizepSrcA / 4 ;
66 
67     pDst=PyMem_Malloc(4*sizeof(float32_t)*nbQuaternions);
68 
69 
70     arm_quaternion_product_f32(pSrcA_converted,pSrcB_converted,pDst,nbQuaternions);
71  FLOATARRAY1(pDstOBJ,4*nbQuaternions,pDst);
72 
73     PyObject *pythonResult = Py_BuildValue("O",pDstOBJ);
74 
75     FREEARGUMENT(pSrcA_converted);
76     FREEARGUMENT(pSrcB_converted);
77     Py_DECREF(pDstOBJ);
78     return(pythonResult);
79 
80   }
81   return(NULL);
82 }
83 
84 static PyObject *
cmsis_arm_quaternion_product_single_f32(PyObject * obj,PyObject * args)85 cmsis_arm_quaternion_product_single_f32(PyObject *obj, PyObject *args)
86 {
87 
88   PyObject *pSrcA=NULL; // input
89   float32_t *pSrcA_converted=NULL; // input
90   PyObject *pSrcB=NULL; // input
91   float32_t *pSrcB_converted=NULL; // input
92   float32_t *pDst=NULL; // output
93 
94   if (PyArg_ParseTuple(args,"OO",&pSrcA,&pSrcB))
95   {
96 
97     GETARGUMENT(pSrcA,NPY_DOUBLE,double,float32_t);
98     GETARGUMENT(pSrcB,NPY_DOUBLE,double,float32_t);
99 
100     pDst=PyMem_Malloc(4*sizeof(float32_t));
101 
102 
103     arm_quaternion_product_single_f32(pSrcA_converted,pSrcB_converted,pDst);
104  FLOATARRAY1(pDstOBJ,4,pDst);
105 
106     PyObject *pythonResult = Py_BuildValue("O",pDstOBJ);
107 
108     FREEARGUMENT(pSrcA_converted);
109     FREEARGUMENT(pSrcB_converted);
110     Py_DECREF(pDstOBJ);
111     return(pythonResult);
112 
113   }
114   return(NULL);
115 }
116 
117 
118 
119 static PyObject *
cmsis_arm_quaternion2rotation_f32(PyObject * obj,PyObject * args)120 cmsis_arm_quaternion2rotation_f32(PyObject *obj, PyObject *args)
121 {
122 
123   PyObject *pSrc=NULL; // input
124   float32_t *pSrc_converted=NULL; // input
125   float32_t *pDst=NULL; // output
126   uint32_t nbQuaternions; // input
127 
128   if (PyArg_ParseTuple(args,"O",&pSrc))
129   {
130 
131     GETARGUMENT(pSrc,NPY_DOUBLE,double,float32_t);
132     nbQuaternions = arraySizepSrc / 4 ;
133 
134     pDst=PyMem_Malloc(9*sizeof(float32_t)*nbQuaternions);
135 
136 
137     arm_quaternion2rotation_f32(pSrc_converted,pDst,nbQuaternions);
138  FLOATARRAY1(pDstOBJ,9*nbQuaternions,pDst);
139 
140     PyObject *pythonResult = Py_BuildValue("O",pDstOBJ);
141 
142     FREEARGUMENT(pSrc_converted);
143     Py_DECREF(pDstOBJ);
144     return(pythonResult);
145 
146   }
147   return(NULL);
148 }
149 
150 static PyObject *
cmsis_arm_rotation2quaternion_f32(PyObject * obj,PyObject * args)151 cmsis_arm_rotation2quaternion_f32(PyObject *obj, PyObject *args)
152 {
153 
154   PyObject *pSrc=NULL; // input
155   float32_t *pSrc_converted=NULL; // input
156   float32_t *pDst=NULL; // output
157   uint32_t nbQuaternions; // input
158 
159   if (PyArg_ParseTuple(args,"O",&pSrc))
160   {
161 
162     GETARGUMENT(pSrc,NPY_DOUBLE,double,float32_t);
163     nbQuaternions = arraySizepSrc / 9 ;
164 
165     pDst=PyMem_Malloc(4*sizeof(float32_t)*nbQuaternions);
166 
167 
168     arm_rotation2quaternion_f32(pSrc_converted,pDst,nbQuaternions);
169  FLOATARRAY1(pDstOBJ,4*nbQuaternions,pDst);
170 
171     PyObject *pythonResult = Py_BuildValue("O",pDstOBJ);
172 
173     FREEARGUMENT(pSrc_converted);
174     Py_DECREF(pDstOBJ);
175     return(pythonResult);
176 
177   }
178   return(NULL);
179 }
180 
181 static PyObject *
cmsis_arm_quaternion_normalize_f32(PyObject * obj,PyObject * args)182 cmsis_arm_quaternion_normalize_f32(PyObject *obj, PyObject *args)
183 {
184 
185   PyObject *pSrc=NULL; // input
186   float32_t *pSrc_converted=NULL; // input
187   float32_t *pDst=NULL; // output
188   uint32_t nbQuaternions; // input
189 
190   if (PyArg_ParseTuple(args,"O",&pSrc))
191   {
192 
193     GETARGUMENT(pSrc,NPY_DOUBLE,double,float32_t);
194     nbQuaternions = arraySizepSrc / 4 ;
195 
196     pDst=PyMem_Malloc(4*sizeof(float32_t)*nbQuaternions);
197 
198 
199     arm_quaternion_normalize_f32(pSrc_converted,pDst,nbQuaternions);
200  FLOATARRAY1(pDstOBJ,4*nbQuaternions,pDst);
201 
202     PyObject *pythonResult = Py_BuildValue("O",pDstOBJ);
203 
204     FREEARGUMENT(pSrc_converted);
205     Py_DECREF(pDstOBJ);
206     return(pythonResult);
207 
208   }
209   return(NULL);
210 }
211 
212 static PyObject *
cmsis_arm_quaternion_norm_f32(PyObject * obj,PyObject * args)213 cmsis_arm_quaternion_norm_f32(PyObject *obj, PyObject *args)
214 {
215 
216   PyObject *pSrc=NULL; // input
217   float32_t *pSrc_converted=NULL; // input
218   float32_t *pDst=NULL; // output
219   uint32_t nbQuaternions; // input
220 
221   if (PyArg_ParseTuple(args,"O",&pSrc))
222   {
223 
224     GETARGUMENT(pSrc,NPY_DOUBLE,double,float32_t);
225     nbQuaternions = arraySizepSrc / 4 ;
226 
227     pDst=PyMem_Malloc(sizeof(float32_t)*nbQuaternions);
228 
229 
230     arm_quaternion_norm_f32(pSrc_converted,pDst,nbQuaternions);
231  FLOATARRAY1(pDstOBJ,nbQuaternions,pDst);
232 
233     PyObject *pythonResult = Py_BuildValue("O",pDstOBJ);
234 
235     FREEARGUMENT(pSrc_converted);
236     Py_DECREF(pDstOBJ);
237     return(pythonResult);
238 
239   }
240   return(NULL);
241 }
242 
243 static PyObject *
cmsis_arm_quaternion_conjugate_f32(PyObject * obj,PyObject * args)244 cmsis_arm_quaternion_conjugate_f32(PyObject *obj, PyObject *args)
245 {
246 
247   PyObject *pSrc=NULL; // input
248   float32_t *pSrc_converted=NULL; // input
249   float32_t *pDst=NULL; // output
250   uint32_t nbQuaternions; // input
251 
252   if (PyArg_ParseTuple(args,"O",&pSrc))
253   {
254 
255     GETARGUMENT(pSrc,NPY_DOUBLE,double,float32_t);
256     nbQuaternions = arraySizepSrc / 4 ;
257 
258     pDst=PyMem_Malloc(4*sizeof(float32_t)*nbQuaternions);
259 
260 
261     arm_quaternion_conjugate_f32(pSrc_converted,pDst,nbQuaternions);
262  FLOATARRAY1(pDstOBJ,4*nbQuaternions,pDst);
263 
264     PyObject *pythonResult = Py_BuildValue("O",pDstOBJ);
265 
266     FREEARGUMENT(pSrc_converted);
267     Py_DECREF(pDstOBJ);
268     return(pythonResult);
269 
270   }
271   return(NULL);
272 }
273 
274 static PyObject *
cmsis_arm_quaternion_inverse_f32(PyObject * obj,PyObject * args)275 cmsis_arm_quaternion_inverse_f32(PyObject *obj, PyObject *args)
276 {
277 
278   PyObject *pSrc=NULL; // input
279   float32_t *pSrc_converted=NULL; // input
280   float32_t *pDst=NULL; // output
281   uint32_t nbQuaternions; // input
282 
283   if (PyArg_ParseTuple(args,"O",&pSrc))
284   {
285 
286     GETARGUMENT(pSrc,NPY_DOUBLE,double,float32_t);
287     nbQuaternions = arraySizepSrc / 4 ;
288 
289     pDst=PyMem_Malloc(4*sizeof(float32_t)*nbQuaternions);
290 
291 
292     arm_quaternion_inverse_f32(pSrc_converted,pDst,nbQuaternions);
293  FLOATARRAY1(pDstOBJ,4*nbQuaternions,pDst);
294 
295     PyObject *pythonResult = Py_BuildValue("O",pDstOBJ);
296 
297     FREEARGUMENT(pSrc_converted);
298     Py_DECREF(pDstOBJ);
299     return(pythonResult);
300 
301   }
302   return(NULL);
303 }
304 
305 
306 
307 
308 
309 
310 static PyMethodDef CMSISDSPMethods[] = {
311 
312 
313 
314 
315 
316 
317 {"arm_quaternion_normalize_f32" ,  cmsis_arm_quaternion_normalize_f32, METH_VARARGS,""},
318 {"arm_quaternion_conjugate_f32" ,  cmsis_arm_quaternion_conjugate_f32, METH_VARARGS,""},
319 {"arm_quaternion_inverse_f32" ,  cmsis_arm_quaternion_inverse_f32, METH_VARARGS,""},
320 {"arm_quaternion_norm_f32" ,  cmsis_arm_quaternion_norm_f32, METH_VARARGS,""},
321 {"arm_quaternion2rotation_f32" ,  cmsis_arm_quaternion2rotation_f32, METH_VARARGS,""},
322 {"arm_rotation2quaternion_f32" ,  cmsis_arm_rotation2quaternion_f32, METH_VARARGS,""},
323 {"arm_quaternion_product_f32" ,  cmsis_arm_quaternion_product_f32, METH_VARARGS,""},
324 {"arm_quaternion_product_single_f32" ,  cmsis_arm_quaternion_product_single_f32, METH_VARARGS,""},
325 
326 
327 
328     {"error_out", (PyCFunction)error_out, METH_NOARGS, NULL},
329     {NULL, NULL, 0, NULL}        /* Sentinel */
330 };
331 
332 #ifdef IS_PY3K
cmsisdsp_traverse(PyObject * m,visitproc visit,void * arg)333 static int cmsisdsp_traverse(PyObject *m, visitproc visit, void *arg) {
334     Py_VISIT(GETSTATE(m)->error);
335     return 0;
336 }
337 
cmsisdsp_clear(PyObject * m)338 static int cmsisdsp_clear(PyObject *m) {
339     Py_CLEAR(GETSTATE(m)->error);
340     return 0;
341 }
342 
343 
344 static struct PyModuleDef moduledef = {
345         PyModuleDef_HEAD_INIT,
346         MODNAME,
347         NULL,
348         sizeof(struct module_state),
349         CMSISDSPMethods,
350         NULL,
351         cmsisdsp_traverse,
352         cmsisdsp_clear,
353         NULL
354 };
355 
356 #define INITERROR return NULL
357 
358 PyMODINIT_FUNC
CAT(PyInit_,MODINITNAME)359 CAT(PyInit_,MODINITNAME)(void)
360 
361 
362 #else
363 #define INITERROR return
364 
365 void CAT(init,MODINITNAME)(void)
366 #endif
367 {
368     import_array();
369 
370   #ifdef IS_PY3K
371     PyObject *module = PyModule_Create(&moduledef);
372   #else
373     PyObject *module = Py_InitModule(MODNAME, CMSISDSPMethods);
374   #endif
375 
376   if (module == NULL)
377       INITERROR;
378   struct module_state *st = GETSTATE(module);
379 
380   st->error = PyErr_NewException(MODNAME".Error", NULL, NULL);
381   if (st->error == NULL) {
382       Py_DECREF(module);
383       INITERROR;
384   }
385 
386 
387   typeRegistration(module);
388 
389   #ifdef IS_PY3K
390     return module;
391   #endif
392 }