Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension


Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
33 changes: 33 additions & 0 deletions Fusion/FusionConvention.c
Original file line number Diff line number Diff line change
@@ -0,0 +1,33 @@
/**
* @file FusionConvention.c
* @author Seb Madgwick
* @brief Earth axes convention.
*/

//------------------------------------------------------------------------------
// Includes

#include "FusionConvention.h"

//------------------------------------------------------------------------------
// Functions

/**
* @brief Returns a string representation of the earth axes convention.
* @param convention Earth axes convention.
* @return String representation of the earth axes convention.
*/
const char *FusionConventionToString(const FusionConvention convention) {
switch (convention) {
case FusionConventionNwu:
return "North, West, Up (NWU)";
case FusionConventionEnu:
return "East, North, Up (ENU)";
case FusionConventionNed:
return "North, East, Down (NED)";
}
return ""; // avoid compiler warning
}

//------------------------------------------------------------------------------
// End of file
5 changes: 5 additions & 0 deletions Fusion/FusionConvention.h
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,11 @@ typedef enum {
FusionConventionNed, /* North, East, Down (NED) */
} FusionConvention;

//------------------------------------------------------------------------------
// Function declarations

const char *FusionConventionToString(const FusionConvention convention);

#endif

//------------------------------------------------------------------------------
Expand Down
75 changes: 75 additions & 0 deletions Fusion/FusionRemap.c
Original file line number Diff line number Diff line change
@@ -0,0 +1,75 @@
/**
* @file FusionRemap.c
* @author Seb Madgwick
* @brief Remaps the sensor axes to the body frame.
*/

//------------------------------------------------------------------------------
// Includes

#include "FusionRemap.h"

//------------------------------------------------------------------------------
// Functions

/**
* @brief Returns a string representation of the alignment.
* @param alignment Alignment.
* @return String representation of the alignment.
*/
const char *FusionRemapAlignmentToString(const FusionRemapAlignment alignment) {
switch (alignment) {
case FusionRemapAlignmentPXPYPZ:
return "+X+Y+Z";
case FusionRemapAlignmentPXPZNY:
return "+X+Z-Y";
case FusionRemapAlignmentPXNZPY:
return "+X-Z+Y";
case FusionRemapAlignmentPXNYNZ:
return "+X-Y-Z";
case FusionRemapAlignmentPYPXNZ:
return "+Y+X-Z";
case FusionRemapAlignmentPYPZPX:
return "+Y+Z+X";
case FusionRemapAlignmentPYNZNX:
return "+Y-Z-X";
case FusionRemapAlignmentPYNXPZ:
return "+Y-X+Z";
case FusionRemapAlignmentPZPXPY:
return "+Z+X+Y";
case FusionRemapAlignmentPZPYNX:
return "+Z+Y-X";
case FusionRemapAlignmentPZNYPX:
return "+Z-Y+X";
case FusionRemapAlignmentPZNXNY:
return "+Z-X-Y";
case FusionRemapAlignmentNZPXNY:
return "-Z+X-Y";
case FusionRemapAlignmentNZPYPX:
return "-Z+Y+X";
case FusionRemapAlignmentNZNYNX:
return "-Z-Y-X";
case FusionRemapAlignmentNZNXPY:
return "-Z-X+Y";
case FusionRemapAlignmentNYPXPZ:
return "-Y+X+Z";
case FusionRemapAlignmentNYPZNX:
return "-Y+Z-X";
case FusionRemapAlignmentNYNZPX:
return "-Y-Z+X";
case FusionRemapAlignmentNYNXNZ:
return "-Y-X-Z";
case FusionRemapAlignmentNXPYNZ:
return "-X+Y-Z";
case FusionRemapAlignmentNXPZPY:
return "-X+Z+Y";
case FusionRemapAlignmentNXNZNY:
return "-X-Z-Y";
case FusionRemapAlignmentNXNYPZ:
return "-X-Y+Z";
}
return ""; // avoid compiler warning
}

//------------------------------------------------------------------------------
// End of file
5 changes: 5 additions & 0 deletions Fusion/FusionRemap.h
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,11 @@ typedef enum {
FusionRemapAlignmentNXNYPZ, /* -X-Y+Z */
} FusionRemapAlignment;

//------------------------------------------------------------------------------
// Function declarations

const char *FusionRemapAlignmentToString(const FusionRemapAlignment alignment);

//------------------------------------------------------------------------------
// Inline functions

Expand Down
4 changes: 4 additions & 0 deletions Python/imufusion-stubs/__init__.pyi
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,9 @@ CONVENTION_NED: int
# compass_methods
def compass(accelerometer: np.ndarray, magnetometer: np.ndarray, convention: int = ...) -> float: ...

# convention_methods
def convention_to_string(convention: int) -> str: ...

# convert_methods
def quaternion_to_matrix(quaternion: np.ndarray) -> np.ndarray: ...
def quaternion_to_euler(quaternion: np.ndarray) -> np.ndarray: ...
Expand All @@ -50,6 +53,7 @@ def model_magnetic(

# remap_methods
def remap(sensor: np.ndarray, alignment: int) -> np.ndarray: ...
def alignment_to_string(alignment: int) -> str: ...

# ahrs_object
class Ahrs:
Expand Down
15 changes: 15 additions & 0 deletions Python/imufusion/Convention.h
Original file line number Diff line number Diff line change
Expand Up @@ -17,4 +17,19 @@ static int convention_from(FusionConvention *const convention, const int convent
return -1;
}

static PyObject *convention_to_string(PyObject *null, PyObject *arg) {
const long convention = PyLong_AsLong(arg);

if (PyErr_Occurred()) {
return NULL;
}

return PyUnicode_FromString(FusionConventionToString((FusionConvention) convention));
}

static PyMethodDef convention_methods[] = {
{"convention_to_string", (PyCFunction) convention_to_string, METH_O, ""},
{NULL} /* sentinel */
};

#endif
11 changes: 11 additions & 0 deletions Python/imufusion/Remap.h
Original file line number Diff line number Diff line change
Expand Up @@ -64,8 +64,19 @@ static PyObject *remap(PyObject *self, PyObject *args) {
return np_array_1x3_from(remapped_sensor.array);
}

static PyObject *alignment_to_string(PyObject *null, PyObject *arg) {
const long alignment = PyLong_AsLong(arg);

if (PyErr_Occurred()) {
return NULL;
}

return PyUnicode_FromString(FusionRemapAlignmentToString((FusionRemapAlignment) alignment));
}

static PyMethodDef remap_methods[] = {
{"remap", (PyCFunction) remap, METH_VARARGS, ""},
{"alignment_to_string", (PyCFunction) alignment_to_string, METH_O, ""},
{NULL} /* sentinel */
};

Expand Down
2 changes: 2 additions & 0 deletions Python/imufusion/imufusion.c
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@
#include "Bias.h"
#include "BiasSettings.h"
#include "Compass.h"
#include "Convention.h"
#include "Convert.h"
#include "Model.h"
#include <Python.h>
Expand Down Expand Up @@ -66,6 +67,7 @@ PyMODINIT_FUNC PyInit_imufusion() {
(PyModule_AddIntConstant(module, "CONVENTION_ENU", FusionConventionEnu) == 0) &&
(PyModule_AddIntConstant(module, "CONVENTION_NED", FusionConventionNed) == 0) &&
(PyModule_AddFunctions(module, compass_methods) == 0) &&
(PyModule_AddFunctions(module, convention_methods) == 0) &&
(PyModule_AddFunctions(module, convert_methods) == 0) &&
(PyModule_AddFunctions(module, model_methods) == 0) &&
(PyModule_AddFunctions(module, remap_methods) == 0) &&
Expand Down
2 changes: 2 additions & 0 deletions setup.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,8 @@
"Fusion/FusionAhrs.c",
"Fusion/FusionBias.c",
"Fusion/FusionCompass.c",
"Fusion/FusionConvention.c",
"Fusion/FusionRemap.c",
"Python/imufusion/imufusion.c",
],
include_dirs=[numpy.get_include()],
Expand Down