From b6f09c27de66886a751ec3b72852c7b20b2c2739 Mon Sep 17 00:00:00 2001 From: Changjo Kim Date: Fri, 21 Mar 2025 17:51:01 +0900 Subject: [PATCH 1/8] Add batch update methods for Ahrs and Offset classes --- Python/Python-C-API/Ahrs.h | 173 ++++++++++++++++++++++++++++++++++- Python/Python-C-API/Offset.h | 57 +++++++++++- 2 files changed, 225 insertions(+), 5 deletions(-) diff --git a/Python/Python-C-API/Ahrs.h b/Python/Python-C-API/Ahrs.h index dd44b92..2ddc596 100644 --- a/Python/Python-C-API/Ahrs.h +++ b/Python/Python-C-API/Ahrs.h @@ -137,6 +137,92 @@ static PyObject *ahrs_update(Ahrs *self, PyObject *args) { return Py_None; } +static PyObject *ahrs_update_batch(Ahrs *self, PyObject *args) { + PyArrayObject *gyroscope_array; + PyArrayObject *accelerometer_array; + PyArrayObject *magnetometer_array; + PyArrayObject *delta_time_array; + + const char *error = PARSE_TUPLE(args, "O!O!O!O!", + &PyArray_Type, &gyroscope_array, + &PyArray_Type, &accelerometer_array, + &PyArray_Type, &magnetometer_array, + &PyArray_Type, &delta_time_array); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + + if (PyArray_TYPE(gyroscope_array) != NPY_FLOAT) { + PyErr_SetString(PyExc_TypeError, "gyroscope array must be np.float32"); + return NULL; + } + if (PyArray_TYPE(accelerometer_array) != NPY_FLOAT) { + PyErr_SetString(PyExc_TypeError, "accelerometer array must be np.float32"); + return NULL; + } + if (PyArray_TYPE(magnetometer_array) != NPY_FLOAT) { + PyErr_SetString(PyExc_TypeError, "magnetometer array must be np.float32"); + return NULL; + } + if (PyArray_TYPE(delta_time_array) != NPY_FLOAT) { + PyErr_SetString(PyExc_TypeError, "delta_time array must be np.float32"); + return NULL; + } + + npy_intp n = PyArray_SIZE(gyroscope_array) / 3; + if ((PyArray_SIZE(accelerometer_array) / 3) != n || + (PyArray_SIZE(magnetometer_array) / 3) != n || + (PyArray_SIZE(delta_time_array)) != n) { + PyErr_SetString(PyExc_ValueError, "모든 입력 배열의 크기가 일치해야 합니다."); + return NULL; + } + + npy_intp dims[2] = { n, 3 }; + PyObject *euler_array = PyArray_SimpleNew(2, dims, NPY_FLOAT); + float *euler_data = (float *) PyArray_DATA((PyArrayObject *)euler_array); + PyObject *earth_accel_array = PyArray_SimpleNew(2, dims, NPY_FLOAT); + float *earth_accel_data = (float *) PyArray_DATA((PyArrayObject *)earth_accel_array); + + float *gyro_data = (float *) PyArray_DATA(gyroscope_array); + float *accel_data = (float *) PyArray_DATA(accelerometer_array); + float *mag_data = (float *) PyArray_DATA(magnetometer_array); + float *dt_data = (float *) PyArray_DATA(delta_time_array); + + FusionVector gyro_vec, accel_vec, mag_vec, earth_accel; + FusionEuler euler; + + for (npy_intp i = 0; i < n; i++) { + for (int j = 0; j < 3; j++) { + gyro_vec.array[j] = gyro_data[i * 3 + j]; + accel_vec.array[j] = accel_data[i * 3 + j]; + mag_vec.array[j] = mag_data[i * 3 + j]; + } + + float dt = dt_data[i]; + + FusionAhrsUpdate(&self->ahrs, gyro_vec, accel_vec, mag_vec, dt); + + euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&self->ahrs)); + earth_accel = FusionAhrsGetEarthAcceleration(&self->ahrs); + + euler_data[i * 3 + 0] = euler.angle.roll; + euler_data[i * 3 + 1] = euler.angle.pitch; + euler_data[i * 3 + 2] = euler.angle.yaw; + + earth_accel_data[i * 3 + 0] = earth_accel.axis.x; + earth_accel_data[i * 3 + 1] = earth_accel.axis.y; + earth_accel_data[i * 3 + 2] = earth_accel.axis.z; + } + + PyObject *result = PyTuple_New(2); + PyTuple_SET_ITEM(result, 0, euler_array); + PyTuple_SET_ITEM(result, 1, earth_accel_array); + + return result; +} + + static PyObject *ahrs_update_no_magnetometer(Ahrs *self, PyObject *args) { PyArrayObject *gyroscope_array; PyArrayObject *accelerometer_array; @@ -168,6 +254,83 @@ static PyObject *ahrs_update_no_magnetometer(Ahrs *self, PyObject *args) { return Py_None; } + +static PyObject *ahrs_update_no_magnetometer_batch(Ahrs *self, PyObject *args) { + PyArrayObject *gyroscope_array; + PyArrayObject *accelerometer_array; + PyArrayObject *delta_time_array; + + const char *error = PARSE_TUPLE(args, "O!O!O!", + &PyArray_Type, &gyroscope_array, + &PyArray_Type, &accelerometer_array, + &PyArray_Type, &delta_time_array); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + if (PyArray_TYPE(gyroscope_array) != NPY_FLOAT) { + PyErr_SetString(PyExc_TypeError, "gyroscope array must be np.float32"); + return NULL; + } + if (PyArray_TYPE(accelerometer_array) != NPY_FLOAT) { + PyErr_SetString(PyExc_TypeError, "accelerometer array must be np.float32"); + return NULL; + } + if (PyArray_TYPE(delta_time_array) != NPY_FLOAT) { + PyErr_SetString(PyExc_TypeError, "delta_time array must be np.float32"); + return NULL; + } + + npy_intp n = PyArray_SIZE(gyroscope_array) / 3; + if ((PyArray_SIZE(accelerometer_array) / 3) != n || + (PyArray_SIZE(delta_time_array)) != n) { + PyErr_SetString(PyExc_ValueError, "모든 입력 배열의 크기가 일치해야 합니다."); + return NULL; + } + + npy_intp dims[2] = { n, 3 }; + PyObject *euler_array = PyArray_SimpleNew(2, dims, NPY_FLOAT); + float *euler_data = (float *) PyArray_DATA((PyArrayObject *)euler_array); + PyObject *earth_accel_array = PyArray_SimpleNew(2, dims, NPY_FLOAT); + float *earth_accel_data = (float *) PyArray_DATA((PyArrayObject *)earth_accel_array); + + float *gyro_data = (float *) PyArray_DATA(gyroscope_array); + float *accel_data = (float *) PyArray_DATA(accelerometer_array); + float *dt_data = (float *) PyArray_DATA(delta_time_array); + + FusionVector gyro_vec, accel_vec, earth_accel; + FusionEuler euler; + + for (npy_intp i = 0; i < n; i++) { + for (int j = 0; j < 3; j++) { + gyro_vec.array[j] = gyro_data[i * 3 + j]; + accel_vec.array[j] = accel_data[i * 3 + j]; + } + + float dt = dt_data[i]; + + FusionAhrsUpdateNoMagnetometer(&self->ahrs, gyro_vec, accel_vec, dt); + + euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&self->ahrs)); + earth_accel = FusionAhrsGetEarthAcceleration(&self->ahrs); + + euler_data[i * 3 + 0] = euler.angle.roll; + euler_data[i * 3 + 1] = euler.angle.pitch; + euler_data[i * 3 + 2] = euler.angle.yaw; + + earth_accel_data[i * 3 + 0] = earth_accel.axis.x; + earth_accel_data[i * 3 + 1] = earth_accel.axis.y; + earth_accel_data[i * 3 + 2] = earth_accel.axis.z; + } + + PyObject *result = PyTuple_New(2); + PyTuple_SET_ITEM(result, 0, euler_array); + PyTuple_SET_ITEM(result, 1, earth_accel_array); + + return result; +} + + static PyObject *ahrs_update_external_heading(Ahrs *self, PyObject *args) { PyArrayObject *gyroscope_array; PyArrayObject *accelerometer_array; @@ -224,10 +387,12 @@ static PyGetSetDef ahrs_get_set[] = { }; static PyMethodDef ahrs_methods[] = { - {"reset", (PyCFunction) ahrs_reset, METH_NOARGS, ""}, - {"update", (PyCFunction) ahrs_update, METH_VARARGS, ""}, - {"update_no_magnetometer", (PyCFunction) ahrs_update_no_magnetometer, METH_VARARGS, ""}, - {"update_external_heading", (PyCFunction) ahrs_update_external_heading, METH_VARARGS, ""}, + {"reset", (PyCFunction) ahrs_reset, METH_NOARGS, ""}, + {"update", (PyCFunction) ahrs_update, METH_VARARGS, ""}, + {"update_no_magnetometer", (PyCFunction) ahrs_update_no_magnetometer, METH_VARARGS, ""}, + {"update_external_heading", (PyCFunction) ahrs_update_external_heading, METH_VARARGS, ""}, + {"update_batch", (PyCFunction) ahrs_update_batch, METH_VARARGS, ""}, + {"update_no_magnetometer_batch", (PyCFunction) ahrs_update_no_magnetometer_batch, METH_VARARGS, ""}, {NULL} /* sentinel */ }; diff --git a/Python/Python-C-API/Offset.h b/Python/Python-C-API/Offset.h index 8873f64..37fde03 100644 --- a/Python/Python-C-API/Offset.h +++ b/Python/Python-C-API/Offset.h @@ -56,8 +56,63 @@ static PyObject *offset_update(Offset *self, PyObject *args) { return output_array; } + +static PyObject *offset_update_batch(Offset *self, PyObject *args) { + PyArrayObject *input_array; + + const char *error = PARSE_TUPLE(args, "O!", &PyArray_Type, &input_array); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + + if (PyArray_TYPE(input_array) != NPY_FLOAT) { + PyErr_SetString(PyExc_TypeError, "input_array must be np.float32"); + return NULL; + } + + if (PyArray_NDIM(input_array) != 2) { + PyErr_SetString(PyExc_ValueError, "Input array must be 2-dimensional (n,3)."); + return NULL; + } + + npy_intp *dims = PyArray_DIMS(input_array); + if (dims[1] != 3) { + PyErr_SetString(PyExc_ValueError, "Input array's second dimension must be 3."); + return NULL; + } + npy_intp n = dims[0]; + + FusionVector input_vector, output_vector; + + const npy_intp out_dims[2] = { n, 3 }; + PyObject *output_array = PyArray_SimpleNew(2, out_dims, NPY_FLOAT); + float *output_data = (float *) PyArray_DATA((PyArrayObject *)output_array); + + float *input_data = (float *) PyArray_DATA(input_array); + + for (npy_intp i = 0; i < n; i++) { + for (int j = 0; j < 3; j++) { + input_vector.array[j] = input_data[i * 3 + j]; + } + + // FusionVector *const output_vector = malloc(sizeof(FusionVector)); + // *output_vector = FusionOffsetUpdate(&self->offset, input_vector); + output_vector = FusionOffsetUpdate(&self->offset, input_vector); + + for (int j = 0; j < 3; j++) { + output_data[i * 3 + j] = output_vector.array[j]; + } + // free(output_vector); + } + + PyArray_ENABLEFLAGS((PyArrayObject *) output_array, NPY_ARRAY_OWNDATA); + return output_array; +} + static PyMethodDef offset_methods[] = { - {"update", (PyCFunction) offset_update, METH_VARARGS, ""}, + {"update", (PyCFunction) offset_update, METH_VARARGS, ""}, + {"update_batch", (PyCFunction) offset_update_batch, METH_VARARGS, ""}, {NULL} /* sentinel */ }; From 950a611cb77f775ecae3e579dfb4b9c33b7e27a4 Mon Sep 17 00:00:00 2001 From: Changjo Kim Date: Fri, 21 Mar 2025 17:52:24 +0900 Subject: [PATCH 2/8] Bump version to 1.2.8+1 in pyproject.toml --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 94b536c..6b51e87 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "imufusion" -version = "1.2.8" +version = "1.2.8+1" description = "Fusion Python package" readme = { text = "See [github](https://github.com/xioTechnologies/Fusion) for documentation and examples.", content-type = "text/markdown" } authors = [{ name = "x-io Technologies Limited", email = "info@x-io.co.uk" }] From f90d57567002b681ebc4f93e864d0db24aa9c6a5 Mon Sep 17 00:00:00 2001 From: Changjo Kim Date: Wed, 7 May 2025 18:46:02 +0900 Subject: [PATCH 3/8] Update `ahrs_update_batch` and `ahrs_update_no_magnetometer_batch` to return quaternion data as part of the result of a dictionary type --- Python/Python-C-API/Ahrs.h | 42 ++++++++++++++++++++++++++++---------- 1 file changed, 31 insertions(+), 11 deletions(-) diff --git a/Python/Python-C-API/Ahrs.h b/Python/Python-C-API/Ahrs.h index 2ddc596..72863ec 100644 --- a/Python/Python-C-API/Ahrs.h +++ b/Python/Python-C-API/Ahrs.h @@ -178,7 +178,10 @@ static PyObject *ahrs_update_batch(Ahrs *self, PyObject *args) { return NULL; } + npy_intp quaternion_dims[2] = { n, 4 }; npy_intp dims[2] = { n, 3 }; + PyObject *quaternion_array = PyArray_SimpleNew(2, quaternion_dims, NPY_FLOAT); + float *quaternion_data = (float *) PyArray_DATA((PyArrayObject *)quaternion_array); PyObject *euler_array = PyArray_SimpleNew(2, dims, NPY_FLOAT); float *euler_data = (float *) PyArray_DATA((PyArrayObject *)euler_array); PyObject *earth_accel_array = PyArray_SimpleNew(2, dims, NPY_FLOAT); @@ -190,6 +193,7 @@ static PyObject *ahrs_update_batch(Ahrs *self, PyObject *args) { float *dt_data = (float *) PyArray_DATA(delta_time_array); FusionVector gyro_vec, accel_vec, mag_vec, earth_accel; + FusionQuaternion quaternion; FusionEuler euler; for (npy_intp i = 0; i < n; i++) { @@ -202,10 +206,15 @@ static PyObject *ahrs_update_batch(Ahrs *self, PyObject *args) { float dt = dt_data[i]; FusionAhrsUpdate(&self->ahrs, gyro_vec, accel_vec, mag_vec, dt); - - euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&self->ahrs)); + quaternion = FusionAhrsGetQuaternion(&self->ahrs); + euler = FusionQuaternionToEuler(quaternion); earth_accel = FusionAhrsGetEarthAcceleration(&self->ahrs); + quaternion_data[i * 4 + 0] = quaternion.element.w; + quaternion_data[i * 4 + 1] = quaternion.element.x; + quaternion_data[i * 4 + 2] = quaternion.element.y; + quaternion_data[i * 4 + 3] = quaternion.element.z; + euler_data[i * 3 + 0] = euler.angle.roll; euler_data[i * 3 + 1] = euler.angle.pitch; euler_data[i * 3 + 2] = euler.angle.yaw; @@ -215,9 +224,10 @@ static PyObject *ahrs_update_batch(Ahrs *self, PyObject *args) { earth_accel_data[i * 3 + 2] = earth_accel.axis.z; } - PyObject *result = PyTuple_New(2); - PyTuple_SET_ITEM(result, 0, euler_array); - PyTuple_SET_ITEM(result, 1, earth_accel_array); + PyObject *result = PyDict_New(); + PyDict_SetItemString(result, "quaternion", quaternion_array); + PyDict_SetItemString(result, "euler", euler_array); + PyDict_SetItemString(result, "earth_acceleration", earth_accel_array); return result; } @@ -288,7 +298,10 @@ static PyObject *ahrs_update_no_magnetometer_batch(Ahrs *self, PyObject *args) { return NULL; } + npy_intp quaternion_dims[2] = { n, 4 }; npy_intp dims[2] = { n, 3 }; + PyObject *quaternion_array = PyArray_SimpleNew(2, quaternion_dims, NPY_FLOAT); + float *quaternion_data = (float *) PyArray_DATA((PyArrayObject *)quaternion_array); PyObject *euler_array = PyArray_SimpleNew(2, dims, NPY_FLOAT); float *euler_data = (float *) PyArray_DATA((PyArrayObject *)euler_array); PyObject *earth_accel_array = PyArray_SimpleNew(2, dims, NPY_FLOAT); @@ -299,6 +312,7 @@ static PyObject *ahrs_update_no_magnetometer_batch(Ahrs *self, PyObject *args) { float *dt_data = (float *) PyArray_DATA(delta_time_array); FusionVector gyro_vec, accel_vec, earth_accel; + FusionQuaternion quaternion; FusionEuler euler; for (npy_intp i = 0; i < n; i++) { @@ -310,10 +324,15 @@ static PyObject *ahrs_update_no_magnetometer_batch(Ahrs *self, PyObject *args) { float dt = dt_data[i]; FusionAhrsUpdateNoMagnetometer(&self->ahrs, gyro_vec, accel_vec, dt); - - euler = FusionQuaternionToEuler(FusionAhrsGetQuaternion(&self->ahrs)); + quaternion = FusionAhrsGetQuaternion(&self->ahrs); + euler = FusionQuaternionToEuler(quaternion); earth_accel = FusionAhrsGetEarthAcceleration(&self->ahrs); - + + quaternion_data[i * 4 + 0] = quaternion.element.w; + quaternion_data[i * 4 + 1] = quaternion.element.x; + quaternion_data[i * 4 + 2] = quaternion.element.y; + quaternion_data[i * 4 + 3] = quaternion.element.z; + euler_data[i * 3 + 0] = euler.angle.roll; euler_data[i * 3 + 1] = euler.angle.pitch; euler_data[i * 3 + 2] = euler.angle.yaw; @@ -323,9 +342,10 @@ static PyObject *ahrs_update_no_magnetometer_batch(Ahrs *self, PyObject *args) { earth_accel_data[i * 3 + 2] = earth_accel.axis.z; } - PyObject *result = PyTuple_New(2); - PyTuple_SET_ITEM(result, 0, euler_array); - PyTuple_SET_ITEM(result, 1, earth_accel_array); + PyObject *result = PyDict_New(); + PyDict_SetItemString(result, "quaternion", quaternion_array); + PyDict_SetItemString(result, "euler", euler_array); + PyDict_SetItemString(result, "earth_acceleration", earth_accel_array); return result; } From bdcd04d872931be94d2af05424ec5127138756c7 Mon Sep 17 00:00:00 2001 From: Changjo Kim Date: Wed, 7 May 2025 18:50:35 +0900 Subject: [PATCH 4/8] Bump version to 1.2.8+2 in pyproject.toml --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index 6b51e87..6553d2b 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta" [project] name = "imufusion" -version = "1.2.8+1" +version = "1.2.8+2" description = "Fusion Python package" readme = { text = "See [github](https://github.com/xioTechnologies/Fusion) for documentation and examples.", content-type = "text/markdown" } authors = [{ name = "x-io Technologies Limited", email = "info@x-io.co.uk" }] From e243669f8e5036086ab5c9a81d17352a25558950 Mon Sep 17 00:00:00 2001 From: Changjo Kim Date: Thu, 8 May 2025 17:14:30 +0900 Subject: [PATCH 5/8] Add memory allocation checks in batch update methods for quaternion and acceleration arrays --- Python/Python-C-API/Ahrs.h | 30 ++++++++++++++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/Python/Python-C-API/Ahrs.h b/Python/Python-C-API/Ahrs.h index 72863ec..a2f5394 100644 --- a/Python/Python-C-API/Ahrs.h +++ b/Python/Python-C-API/Ahrs.h @@ -181,10 +181,25 @@ static PyObject *ahrs_update_batch(Ahrs *self, PyObject *args) { npy_intp quaternion_dims[2] = { n, 4 }; npy_intp dims[2] = { n, 3 }; PyObject *quaternion_array = PyArray_SimpleNew(2, quaternion_dims, NPY_FLOAT); + if (!quaternion_array) { + PyErr_NoMemory(); + return NULL; + } float *quaternion_data = (float *) PyArray_DATA((PyArrayObject *)quaternion_array); PyObject *euler_array = PyArray_SimpleNew(2, dims, NPY_FLOAT); + if (!euler_array) { + PyErr_NoMemory(); + Py_DECREF(quaternion_array); + return NULL; + } float *euler_data = (float *) PyArray_DATA((PyArrayObject *)euler_array); PyObject *earth_accel_array = PyArray_SimpleNew(2, dims, NPY_FLOAT); + if (!earth_accel_array) { + PyErr_NoMemory(); + Py_DECREF(quaternion_array); + Py_DECREF(euler_array); + return NULL; + } float *earth_accel_data = (float *) PyArray_DATA((PyArrayObject *)earth_accel_array); float *gyro_data = (float *) PyArray_DATA(gyroscope_array); @@ -301,10 +316,25 @@ static PyObject *ahrs_update_no_magnetometer_batch(Ahrs *self, PyObject *args) { npy_intp quaternion_dims[2] = { n, 4 }; npy_intp dims[2] = { n, 3 }; PyObject *quaternion_array = PyArray_SimpleNew(2, quaternion_dims, NPY_FLOAT); + if (!quaternion_array) { + PyErr_NoMemory(); + return NULL; + } float *quaternion_data = (float *) PyArray_DATA((PyArrayObject *)quaternion_array); PyObject *euler_array = PyArray_SimpleNew(2, dims, NPY_FLOAT); + if (!euler_array) { + PyErr_NoMemory(); + Py_DECREF(quaternion_array); + return NULL; + } float *euler_data = (float *) PyArray_DATA((PyArrayObject *)euler_array); PyObject *earth_accel_array = PyArray_SimpleNew(2, dims, NPY_FLOAT); + if (!earth_accel_array) { + PyErr_NoMemory(); + Py_DECREF(quaternion_array); + Py_DECREF(euler_array); + return NULL; + } float *earth_accel_data = (float *) PyArray_DATA((PyArrayObject *)earth_accel_array); float *gyro_data = (float *) PyArray_DATA(gyroscope_array); From e6f78c8815cdb135ff1ba5d92322282e3d99219e Mon Sep 17 00:00:00 2001 From: Changjo Kim Date: Fri, 9 May 2025 16:26:23 +0900 Subject: [PATCH 6/8] Update error message in ahrs_update_no_magnetometer_batch for input array size validation --- Python/Python-C-API/Ahrs.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Python/Python-C-API/Ahrs.h b/Python/Python-C-API/Ahrs.h index a2f5394..514f293 100644 --- a/Python/Python-C-API/Ahrs.h +++ b/Python/Python-C-API/Ahrs.h @@ -309,7 +309,7 @@ static PyObject *ahrs_update_no_magnetometer_batch(Ahrs *self, PyObject *args) { npy_intp n = PyArray_SIZE(gyroscope_array) / 3; if ((PyArray_SIZE(accelerometer_array) / 3) != n || (PyArray_SIZE(delta_time_array)) != n) { - PyErr_SetString(PyExc_ValueError, "모든 입력 배열의 크기가 일치해야 합니다."); + PyErr_SetString(PyExc_ValueError, "All input arrays must have the same size."); return NULL; } From 7662fd8deadeb97a9b46760bb64f063342b3ea58 Mon Sep 17 00:00:00 2001 From: Changjo Kim Date: Thu, 15 May 2025 14:55:03 +0900 Subject: [PATCH 7/8] Add memory checks and error handling in ahrs_update_batch and ahrs_update_no_magnetometer_batch --- Python/Python-C-API/Ahrs.h | 48 +++++++++++++++++++++++++++++++++----- 1 file changed, 42 insertions(+), 6 deletions(-) diff --git a/Python/Python-C-API/Ahrs.h b/Python/Python-C-API/Ahrs.h index 514f293..be39b8a 100644 --- a/Python/Python-C-API/Ahrs.h +++ b/Python/Python-C-API/Ahrs.h @@ -240,9 +240,27 @@ static PyObject *ahrs_update_batch(Ahrs *self, PyObject *args) { } PyObject *result = PyDict_New(); - PyDict_SetItemString(result, "quaternion", quaternion_array); - PyDict_SetItemString(result, "euler", euler_array); - PyDict_SetItemString(result, "earth_acceleration", earth_accel_array); + if (!result) { + PyErr_NoMemory(); + Py_DECREF(quaternion_array); + Py_DECREF(euler_array); + Py_DECREF(earth_accel_array); + return NULL; + } + + if (PyDict_SetItemString(result, "quaternion", quaternion_array) < 0 || + PyDict_SetItemString(result, "euler", euler_array) < 0 || + PyDict_SetItemString(result, "earth_acceleration", earth_accel_array) < 0) { + Py_DECREF(quaternion_array); + Py_DECREF(euler_array); + Py_DECREF(earth_accel_array); + Py_DECREF(result); + return NULL; + } + + Py_DECREF(quaternion_array); + Py_DECREF(euler_array); + Py_DECREF(earth_accel_array); return result; } @@ -373,9 +391,27 @@ static PyObject *ahrs_update_no_magnetometer_batch(Ahrs *self, PyObject *args) { } PyObject *result = PyDict_New(); - PyDict_SetItemString(result, "quaternion", quaternion_array); - PyDict_SetItemString(result, "euler", euler_array); - PyDict_SetItemString(result, "earth_acceleration", earth_accel_array); + if (!result) { + PyErr_NoMemory(); + Py_DECREF(quaternion_array); + Py_DECREF(euler_array); + Py_DECREF(earth_accel_array); + return NULL; + } + + if (PyDict_SetItemString(result, "quaternion", quaternion_array) < 0 || + PyDict_SetItemString(result, "euler", euler_array) < 0 || + PyDict_SetItemString(result, "earth_acceleration", earth_accel_array) < 0) { + Py_DECREF(quaternion_array); + Py_DECREF(euler_array); + Py_DECREF(earth_accel_array); + Py_DECREF(result); + return NULL; + } + + Py_DECREF(quaternion_array); + Py_DECREF(euler_array); + Py_DECREF(earth_accel_array); return result; } From 39d9792f5ee71c0237a18e11df4cebe1ada50ce5 Mon Sep 17 00:00:00 2001 From: Changjo Kim Date: Fri, 20 Feb 2026 15:39:04 +0900 Subject: [PATCH 8/8] Improves AHRS algorithm performance Optimizes the AHRS algorithm for batch processing by integrating vector and quaternion calculations and providing access to linear acceleration, earth acceleration, and Euler angles, enhancing its efficiency and accuracy in dynamic motion tracking. Includes functions for magnetometer-less operation. --- Fusion/FusionAhrs.c | 602 +++++++++++++------------ Fusion/FusionAhrs.h | 73 ++-- Fusion/FusionAxes.h | 293 ++++++------- Fusion/FusionCalibration.h | 16 +- Fusion/FusionCompass.c | 48 +- Fusion/FusionCompass.h | 4 +- Fusion/FusionConvention.h | 6 +- Fusion/FusionMath.h | 341 ++++++++------- Fusion/FusionOffset.c | 40 +- Fusion/FusionOffset.h | 8 +- Python/Python-C-API/Ahrs.h | 630 ++++++++++++++++++++------- Python/Python-C-API/Axes.h | 53 ++- Python/Python-C-API/Compass.h | 60 +-- Python/Python-C-API/Flags.h | 40 +- Python/Python-C-API/Helpers.h | 114 ++--- Python/Python-C-API/InternalStates.h | 51 ++- Python/Python-C-API/Offset.h | 135 ++++-- Python/Python-C-API/Quaternion.h | 118 +++-- Python/Python-C-API/Settings.h | 139 +++--- Python/Python-C-API/imufusion.c | 109 +++-- 20 files changed, 1656 insertions(+), 1224 deletions(-) diff --git a/Fusion/FusionAhrs.c b/Fusion/FusionAhrs.c index b353c44..e8bb360 100644 --- a/Fusion/FusionAhrs.c +++ b/Fusion/FusionAhrs.c @@ -8,9 +8,9 @@ //------------------------------------------------------------------------------ // Includes -#include // FLT_MAX #include "FusionAhrs.h" -#include // atan2f, cosf, fabsf, powf, sinf +#include // FLT_MAX +#include // atan2f, cosf, fabsf, powf, sinf //------------------------------------------------------------------------------ // Definitions @@ -44,16 +44,16 @@ static inline int Clamp(const int value, const int min, const int max); * @param ahrs AHRS algorithm structure. */ void FusionAhrsInitialise(FusionAhrs *const ahrs) { - const FusionAhrsSettings settings = { - .convention = FusionConventionNwu, - .gain = 0.5f, - .gyroscopeRange = 0.0f, - .accelerationRejection = 90.0f, - .magneticRejection = 90.0f, - .recoveryTriggerPeriod = 0, - }; - FusionAhrsSetSettings(ahrs, &settings); - FusionAhrsReset(ahrs); + const FusionAhrsSettings settings = { + .convention = FusionConventionNwu, + .gain = 0.5f, + .gyroscopeRange = 0.0f, + .accelerationRejection = 90.0f, + .magneticRejection = 90.0f, + .recoveryTriggerPeriod = 0, + }; + FusionAhrsSetSettings(ahrs, &settings); + FusionAhrsReset(ahrs); } /** @@ -62,19 +62,19 @@ void FusionAhrsInitialise(FusionAhrs *const ahrs) { * @param ahrs AHRS algorithm structure. */ void FusionAhrsReset(FusionAhrs *const ahrs) { - ahrs->quaternion = FUSION_IDENTITY_QUATERNION; - ahrs->accelerometer = FUSION_VECTOR_ZERO; - ahrs->initialising = true; - ahrs->rampedGain = INITIAL_GAIN; - ahrs->angularRateRecovery = false; - ahrs->halfAccelerometerFeedback = FUSION_VECTOR_ZERO; - ahrs->halfMagnetometerFeedback = FUSION_VECTOR_ZERO; - ahrs->accelerometerIgnored = false; - ahrs->accelerationRecoveryTrigger = 0; - ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; - ahrs->magnetometerIgnored = false; - ahrs->magneticRecoveryTrigger = 0; - ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; + ahrs->quaternion = FUSION_IDENTITY_QUATERNION; + ahrs->accelerometer = FUSION_VECTOR_ZERO; + ahrs->initialising = true; + ahrs->rampedGain = INITIAL_GAIN; + ahrs->angularRateRecovery = false; + ahrs->halfAccelerometerFeedback = FUSION_VECTOR_ZERO; + ahrs->halfMagnetometerFeedback = FUSION_VECTOR_ZERO; + ahrs->accelerometerIgnored = false; + ahrs->accelerationRecoveryTrigger = 0; + ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; + ahrs->magnetometerIgnored = false; + ahrs->magneticRecoveryTrigger = 0; + ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; } /** @@ -83,22 +83,31 @@ void FusionAhrsReset(FusionAhrs *const ahrs) { * @param settings Settings. */ void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings) { - ahrs->settings.convention = settings->convention; - ahrs->settings.gain = settings->gain; - ahrs->settings.gyroscopeRange = settings->gyroscopeRange == 0.0f ? FLT_MAX : 0.98f * settings->gyroscopeRange; - ahrs->settings.accelerationRejection = settings->accelerationRejection == 0.0f ? FLT_MAX : powf(0.5f * sinf(FusionDegreesToRadians(settings->accelerationRejection)), 2); - ahrs->settings.magneticRejection = settings->magneticRejection == 0.0f ? FLT_MAX : powf(0.5f * sinf(FusionDegreesToRadians(settings->magneticRejection)), 2); - ahrs->settings.recoveryTriggerPeriod = settings->recoveryTriggerPeriod; - ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; - ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; - if ((settings->gain == 0.0f) || (settings->recoveryTriggerPeriod == 0)) { // disable acceleration and magnetic rejection features if gain is zero - ahrs->settings.accelerationRejection = FLT_MAX; - ahrs->settings.magneticRejection = FLT_MAX; - } - if (ahrs->initialising == false) { - ahrs->rampedGain = ahrs->settings.gain; - } - ahrs->rampedGainStep = (INITIAL_GAIN - ahrs->settings.gain) / INITIALISATION_PERIOD; + ahrs->settings.convention = settings->convention; + ahrs->settings.gain = settings->gain; + ahrs->settings.gyroscopeRange = + settings->gyroscopeRange == 0.0f ? FLT_MAX : 0.98f * settings->gyroscopeRange; + ahrs->settings.accelerationRejection = + settings->accelerationRejection == 0.0f + ? FLT_MAX + : powf(0.5f * sinf(FusionDegreesToRadians(settings->accelerationRejection)), 2); + ahrs->settings.magneticRejection = + settings->magneticRejection == 0.0f + ? FLT_MAX + : powf(0.5f * sinf(FusionDegreesToRadians(settings->magneticRejection)), 2); + ahrs->settings.recoveryTriggerPeriod = settings->recoveryTriggerPeriod; + ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; + ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; + if ((settings->gain == 0.0f) || + (settings->recoveryTriggerPeriod == + 0)) { // disable acceleration and magnetic rejection features if gain is zero + ahrs->settings.accelerationRejection = FLT_MAX; + ahrs->settings.magneticRejection = FLT_MAX; + } + if (ahrs->initialising == false) { + ahrs->rampedGain = ahrs->settings.gain; + } + ahrs->rampedGainStep = (INITIAL_GAIN - ahrs->settings.gain) / INITIALISATION_PERIOD; } /** @@ -110,109 +119,125 @@ void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *con * @param magnetometer Magnetometer measurement in arbitrary units. * @param deltaTime Delta time in seconds. */ -void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const FusionVector magnetometer, const float deltaTime) { +void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, + const FusionVector accelerometer, const FusionVector magnetometer, + const float deltaTime) { #define Q ahrs->quaternion.element - // Store accelerometer - ahrs->accelerometer = accelerometer; + // Store accelerometer + ahrs->accelerometer = accelerometer; - // Reinitialise if gyroscope range exceeded - if ((fabsf(gyroscope.axis.x) > ahrs->settings.gyroscopeRange) || (fabsf(gyroscope.axis.y) > ahrs->settings.gyroscopeRange) || (fabsf(gyroscope.axis.z) > ahrs->settings.gyroscopeRange)) { - const FusionQuaternion quaternion = ahrs->quaternion; - FusionAhrsReset(ahrs); - ahrs->quaternion = quaternion; - ahrs->angularRateRecovery = true; + // Reinitialise if gyroscope range exceeded + if ((fabsf(gyroscope.axis.x) > ahrs->settings.gyroscopeRange) || + (fabsf(gyroscope.axis.y) > ahrs->settings.gyroscopeRange) || + (fabsf(gyroscope.axis.z) > ahrs->settings.gyroscopeRange)) { + const FusionQuaternion quaternion = ahrs->quaternion; + FusionAhrsReset(ahrs); + ahrs->quaternion = quaternion; + ahrs->angularRateRecovery = true; + } + + // Ramp down gain during initialisation + if (ahrs->initialising) { + ahrs->rampedGain -= ahrs->rampedGainStep * deltaTime; + if ((ahrs->rampedGain < ahrs->settings.gain) || (ahrs->settings.gain == 0.0f)) { + ahrs->rampedGain = ahrs->settings.gain; + ahrs->initialising = false; + ahrs->angularRateRecovery = false; } - - // Ramp down gain during initialisation - if (ahrs->initialising) { - ahrs->rampedGain -= ahrs->rampedGainStep * deltaTime; - if ((ahrs->rampedGain < ahrs->settings.gain) || (ahrs->settings.gain == 0.0f)) { - ahrs->rampedGain = ahrs->settings.gain; - ahrs->initialising = false; - ahrs->angularRateRecovery = false; - } + } + + // Calculate direction of gravity indicated by algorithm + const FusionVector halfGravity = HalfGravity(ahrs); + + // Calculate accelerometer feedback + FusionVector halfAccelerometerFeedback = FUSION_VECTOR_ZERO; + ahrs->accelerometerIgnored = true; + if (FusionVectorIsZero(accelerometer) == false) { + + // Calculate accelerometer feedback scaled by 0.5 + ahrs->halfAccelerometerFeedback = Feedback(FusionVectorNormalise(accelerometer), halfGravity); + + // Don't ignore accelerometer if acceleration error below threshold + if (ahrs->initialising || ((FusionVectorMagnitudeSquared(ahrs->halfAccelerometerFeedback) <= + ahrs->settings.accelerationRejection))) { + ahrs->accelerometerIgnored = false; + ahrs->accelerationRecoveryTrigger -= 9; + } else { + ahrs->accelerationRecoveryTrigger += 1; } - // Calculate direction of gravity indicated by algorithm - const FusionVector halfGravity = HalfGravity(ahrs); - - // Calculate accelerometer feedback - FusionVector halfAccelerometerFeedback = FUSION_VECTOR_ZERO; - ahrs->accelerometerIgnored = true; - if (FusionVectorIsZero(accelerometer) == false) { - - // Calculate accelerometer feedback scaled by 0.5 - ahrs->halfAccelerometerFeedback = Feedback(FusionVectorNormalise(accelerometer), halfGravity); - - // Don't ignore accelerometer if acceleration error below threshold - if (ahrs->initialising || ((FusionVectorMagnitudeSquared(ahrs->halfAccelerometerFeedback) <= ahrs->settings.accelerationRejection))) { - ahrs->accelerometerIgnored = false; - ahrs->accelerationRecoveryTrigger -= 9; - } else { - ahrs->accelerationRecoveryTrigger += 1; - } - - // Don't ignore accelerometer during acceleration recovery - if (ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout) { - ahrs->accelerationRecoveryTimeout = 0; - ahrs->accelerometerIgnored = false; - } else { - ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; - } - ahrs->accelerationRecoveryTrigger = Clamp(ahrs->accelerationRecoveryTrigger, 0, ahrs->settings.recoveryTriggerPeriod); - - // Apply accelerometer feedback - if (ahrs->accelerometerIgnored == false) { - halfAccelerometerFeedback = ahrs->halfAccelerometerFeedback; - } + // Don't ignore accelerometer during acceleration recovery + if (ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout) { + ahrs->accelerationRecoveryTimeout = 0; + ahrs->accelerometerIgnored = false; + } else { + ahrs->accelerationRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; } + ahrs->accelerationRecoveryTrigger = + Clamp(ahrs->accelerationRecoveryTrigger, 0, ahrs->settings.recoveryTriggerPeriod); - // Calculate magnetometer feedback - FusionVector halfMagnetometerFeedback = FUSION_VECTOR_ZERO; - ahrs->magnetometerIgnored = true; - if (FusionVectorIsZero(magnetometer) == false) { - - // Calculate direction of magnetic field indicated by algorithm - const FusionVector halfMagnetic = HalfMagnetic(ahrs); - - // Calculate magnetometer feedback scaled by 0.5 - ahrs->halfMagnetometerFeedback = Feedback(FusionVectorNormalise(FusionVectorCrossProduct(halfGravity, magnetometer)), halfMagnetic); - - // Don't ignore magnetometer if magnetic error below threshold - if (ahrs->initialising || ((FusionVectorMagnitudeSquared(ahrs->halfMagnetometerFeedback) <= ahrs->settings.magneticRejection))) { - ahrs->magnetometerIgnored = false; - ahrs->magneticRecoveryTrigger -= 9; - } else { - ahrs->magneticRecoveryTrigger += 1; - } - - // Don't ignore magnetometer during magnetic recovery - if (ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout) { - ahrs->magneticRecoveryTimeout = 0; - ahrs->magnetometerIgnored = false; - } else { - ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; - } - ahrs->magneticRecoveryTrigger = Clamp(ahrs->magneticRecoveryTrigger, 0, ahrs->settings.recoveryTriggerPeriod); - - // Apply magnetometer feedback - if (ahrs->magnetometerIgnored == false) { - halfMagnetometerFeedback = ahrs->halfMagnetometerFeedback; - } + // Apply accelerometer feedback + if (ahrs->accelerometerIgnored == false) { + halfAccelerometerFeedback = ahrs->halfAccelerometerFeedback; + } + } + + // Calculate magnetometer feedback + FusionVector halfMagnetometerFeedback = FUSION_VECTOR_ZERO; + ahrs->magnetometerIgnored = true; + if (FusionVectorIsZero(magnetometer) == false) { + + // Calculate direction of magnetic field indicated by algorithm + const FusionVector halfMagnetic = HalfMagnetic(ahrs); + + // Calculate magnetometer feedback scaled by 0.5 + ahrs->halfMagnetometerFeedback = Feedback( + FusionVectorNormalise(FusionVectorCrossProduct(halfGravity, magnetometer)), halfMagnetic); + + // Don't ignore magnetometer if magnetic error below threshold + if (ahrs->initialising || ((FusionVectorMagnitudeSquared(ahrs->halfMagnetometerFeedback) <= + ahrs->settings.magneticRejection))) { + ahrs->magnetometerIgnored = false; + ahrs->magneticRecoveryTrigger -= 9; + } else { + ahrs->magneticRecoveryTrigger += 1; } - // Convert gyroscope to radians per second scaled by 0.5 - const FusionVector halfGyroscope = FusionVectorMultiplyScalar(gyroscope, FusionDegreesToRadians(0.5f)); - - // Apply feedback to gyroscope - const FusionVector adjustedHalfGyroscope = FusionVectorAdd(halfGyroscope, FusionVectorMultiplyScalar(FusionVectorAdd(halfAccelerometerFeedback, halfMagnetometerFeedback), ahrs->rampedGain)); - - // Integrate rate of change of quaternion - ahrs->quaternion = FusionQuaternionAdd(ahrs->quaternion, FusionQuaternionMultiplyVector(ahrs->quaternion, FusionVectorMultiplyScalar(adjustedHalfGyroscope, deltaTime))); + // Don't ignore magnetometer during magnetic recovery + if (ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout) { + ahrs->magneticRecoveryTimeout = 0; + ahrs->magnetometerIgnored = false; + } else { + ahrs->magneticRecoveryTimeout = ahrs->settings.recoveryTriggerPeriod; + } + ahrs->magneticRecoveryTrigger = + Clamp(ahrs->magneticRecoveryTrigger, 0, ahrs->settings.recoveryTriggerPeriod); - // Normalise quaternion - ahrs->quaternion = FusionQuaternionNormalise(ahrs->quaternion); + // Apply magnetometer feedback + if (ahrs->magnetometerIgnored == false) { + halfMagnetometerFeedback = ahrs->halfMagnetometerFeedback; + } + } + + // Convert gyroscope to radians per second scaled by 0.5 + const FusionVector halfGyroscope = + FusionVectorMultiplyScalar(gyroscope, FusionDegreesToRadians(0.5f)); + + // Apply feedback to gyroscope + const FusionVector adjustedHalfGyroscope = FusionVectorAdd( + halfGyroscope, + FusionVectorMultiplyScalar( + FusionVectorAdd(halfAccelerometerFeedback, halfMagnetometerFeedback), ahrs->rampedGain)); + + // Integrate rate of change of quaternion + ahrs->quaternion = FusionQuaternionAdd( + ahrs->quaternion, + FusionQuaternionMultiplyVector(ahrs->quaternion, + FusionVectorMultiplyScalar(adjustedHalfGyroscope, deltaTime))); + + // Normalise quaternion + ahrs->quaternion = FusionQuaternionNormalise(ahrs->quaternion); #undef Q } @@ -223,26 +248,28 @@ void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, cons */ static inline FusionVector HalfGravity(const FusionAhrs *const ahrs) { #define Q ahrs->quaternion.element - switch (ahrs->settings.convention) { - case FusionConventionNwu: - case FusionConventionEnu: { - const FusionVector halfGravity = {.axis = { - .x = Q.x * Q.z - Q.w * Q.y, - .y = Q.y * Q.z + Q.w * Q.x, - .z = Q.w * Q.w - 0.5f + Q.z * Q.z, - }}; // third column of transposed rotation matrix scaled by 0.5 - return halfGravity; - } - case FusionConventionNed: { - const FusionVector halfGravity = {.axis = { - .x = Q.w * Q.y - Q.x * Q.z, - .y = -1.0f * (Q.y * Q.z + Q.w * Q.x), - .z = 0.5f - Q.w * Q.w - Q.z * Q.z, - }}; // third column of transposed rotation matrix scaled by -0.5 - return halfGravity; - } - } - return FUSION_VECTOR_ZERO; // avoid compiler warning + switch (ahrs->settings.convention) { + case FusionConventionNwu: + case FusionConventionEnu: { + const FusionVector halfGravity = { + .axis = { + .x = Q.x * Q.z - Q.w * Q.y, + .y = Q.y * Q.z + Q.w * Q.x, + .z = Q.w * Q.w - 0.5f + Q.z * Q.z, + }}; // third column of transposed rotation matrix scaled by 0.5 + return halfGravity; + } + case FusionConventionNed: { + const FusionVector halfGravity = { + .axis = { + .x = Q.w * Q.y - Q.x * Q.z, + .y = -1.0f * (Q.y * Q.z + Q.w * Q.x), + .z = 0.5f - Q.w * Q.w - Q.z * Q.z, + }}; // third column of transposed rotation matrix scaled by -0.5 + return halfGravity; + } + } + return FUSION_VECTOR_ZERO; // avoid compiler warning #undef Q } @@ -253,33 +280,36 @@ static inline FusionVector HalfGravity(const FusionAhrs *const ahrs) { */ static inline FusionVector HalfMagnetic(const FusionAhrs *const ahrs) { #define Q ahrs->quaternion.element - switch (ahrs->settings.convention) { - case FusionConventionNwu: { - const FusionVector halfMagnetic = {.axis = { - .x = Q.x * Q.y + Q.w * Q.z, - .y = Q.w * Q.w - 0.5f + Q.y * Q.y, - .z = Q.y * Q.z - Q.w * Q.x, - }}; // second column of transposed rotation matrix scaled by 0.5 - return halfMagnetic; - } - case FusionConventionEnu: { - const FusionVector halfMagnetic = {.axis = { - .x = 0.5f - Q.w * Q.w - Q.x * Q.x, - .y = Q.w * Q.z - Q.x * Q.y, - .z = -1.0f * (Q.x * Q.z + Q.w * Q.y), - }}; // first column of transposed rotation matrix scaled by -0.5 - return halfMagnetic; - } - case FusionConventionNed: { - const FusionVector halfMagnetic = {.axis = { - .x = -1.0f * (Q.x * Q.y + Q.w * Q.z), - .y = 0.5f - Q.w * Q.w - Q.y * Q.y, - .z = Q.w * Q.x - Q.y * Q.z, - }}; // second column of transposed rotation matrix scaled by -0.5 - return halfMagnetic; - } - } - return FUSION_VECTOR_ZERO; // avoid compiler warning + switch (ahrs->settings.convention) { + case FusionConventionNwu: { + const FusionVector halfMagnetic = { + .axis = { + .x = Q.x * Q.y + Q.w * Q.z, + .y = Q.w * Q.w - 0.5f + Q.y * Q.y, + .z = Q.y * Q.z - Q.w * Q.x, + }}; // second column of transposed rotation matrix scaled by 0.5 + return halfMagnetic; + } + case FusionConventionEnu: { + const FusionVector halfMagnetic = { + .axis = { + .x = 0.5f - Q.w * Q.w - Q.x * Q.x, + .y = Q.w * Q.z - Q.x * Q.y, + .z = -1.0f * (Q.x * Q.z + Q.w * Q.y), + }}; // first column of transposed rotation matrix scaled by -0.5 + return halfMagnetic; + } + case FusionConventionNed: { + const FusionVector halfMagnetic = { + .axis = { + .x = -1.0f * (Q.x * Q.y + Q.w * Q.z), + .y = 0.5f - Q.w * Q.w - Q.y * Q.y, + .z = Q.w * Q.x - Q.y * Q.z, + }}; // second column of transposed rotation matrix scaled by -0.5 + return halfMagnetic; + } + } + return FUSION_VECTOR_ZERO; // avoid compiler warning #undef Q } @@ -290,10 +320,10 @@ static inline FusionVector HalfMagnetic(const FusionAhrs *const ahrs) { * @return Feedback. */ static inline FusionVector Feedback(const FusionVector sensor, const FusionVector reference) { - if (FusionVectorDotProduct(sensor, reference) < 0.0f) { // if error is >90 degrees - return FusionVectorNormalise(FusionVectorCrossProduct(sensor, reference)); - } - return FusionVectorCrossProduct(sensor, reference); + if (FusionVectorDotProduct(sensor, reference) < 0.0f) { // if error is >90 degrees + return FusionVectorNormalise(FusionVectorCrossProduct(sensor, reference)); + } + return FusionVectorCrossProduct(sensor, reference); } /** @@ -304,13 +334,13 @@ static inline FusionVector Feedback(const FusionVector sensor, const FusionVecto * @return Value limited to maximum and minimum. */ static inline int Clamp(const int value, const int min, const int max) { - if (value < min) { - return min; - } - if (value > max) { - return max; - } - return value; + if (value < min) { + return min; + } + if (value > max) { + return max; + } + return value; } /** @@ -321,15 +351,16 @@ static inline int Clamp(const int value, const int min, const int max) { * @param accelerometer Accelerometer measurement in g. * @param deltaTime Delta time in seconds. */ -void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float deltaTime) { +void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, + const FusionVector accelerometer, const float deltaTime) { - // Update AHRS algorithm - FusionAhrsUpdate(ahrs, gyroscope, accelerometer, FUSION_VECTOR_ZERO, deltaTime); + // Update AHRS algorithm + FusionAhrsUpdate(ahrs, gyroscope, accelerometer, FUSION_VECTOR_ZERO, deltaTime); - // Zero heading during initialisation - if (ahrs->initialising) { - FusionAhrsSetHeading(ahrs, 0.0f); - } + // Zero heading during initialisation + if (ahrs->initialising) { + FusionAhrsSetHeading(ahrs, 0.0f); + } } /** @@ -341,23 +372,25 @@ void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector g * @param heading Heading measurement in degrees. * @param deltaTime Delta time in seconds. */ -void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float heading, const float deltaTime) { +void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, + const FusionVector accelerometer, const float heading, + const float deltaTime) { #define Q ahrs->quaternion.element - // Calculate roll - const float roll = atan2f(Q.w * Q.x + Q.y * Q.z, 0.5f - Q.y * Q.y - Q.x * Q.x); + // Calculate roll + const float roll = atan2f(Q.w * Q.x + Q.y * Q.z, 0.5f - Q.y * Q.y - Q.x * Q.x); - // Calculate magnetometer - const float headingRadians = FusionDegreesToRadians(heading); - const float sinHeadingRadians = sinf(headingRadians); - const FusionVector magnetometer = {.axis = { - .x = cosf(headingRadians), - .y = -1.0f * cosf(roll) * sinHeadingRadians, - .z = sinHeadingRadians * sinf(roll), - }}; + // Calculate magnetometer + const float headingRadians = FusionDegreesToRadians(heading); + const float sinHeadingRadians = sinf(headingRadians); + const FusionVector magnetometer = {.axis = { + .x = cosf(headingRadians), + .y = -1.0f * cosf(roll) * sinHeadingRadians, + .z = sinHeadingRadians * sinf(roll), + }}; - // Update AHRS algorithm - FusionAhrsUpdate(ahrs, gyroscope, accelerometer, magnetometer, deltaTime); + // Update AHRS algorithm + FusionAhrsUpdate(ahrs, gyroscope, accelerometer, magnetometer, deltaTime); #undef Q } @@ -366,9 +399,7 @@ void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector * @param ahrs AHRS algorithm structure. * @return Quaternion describing the sensor relative to the Earth. */ -FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs) { - return ahrs->quaternion; -} +FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs) { return ahrs->quaternion; } /** * @brief Sets the quaternion describing the sensor relative to the Earth. @@ -376,7 +407,7 @@ FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs) { * @param quaternion Quaternion describing the sensor relative to the Earth. */ void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quaternion) { - ahrs->quaternion = quaternion; + ahrs->quaternion = quaternion; } /** @@ -386,12 +417,12 @@ void FusionAhrsSetQuaternion(FusionAhrs *const ahrs, const FusionQuaternion quat */ FusionVector FusionAhrsGetGravity(const FusionAhrs *const ahrs) { #define Q ahrs->quaternion.element - const FusionVector gravity = {.axis = { - .x = 2.0f * (Q.x * Q.z - Q.w * Q.y), - .y = 2.0f * (Q.y * Q.z + Q.w * Q.x), - .z = 2.0f * (Q.w * Q.w - 0.5f + Q.z * Q.z), - }}; // third column of transposed rotation matrix - return gravity; + const FusionVector gravity = {.axis = { + .x = 2.0f * (Q.x * Q.z - Q.w * Q.y), + .y = 2.0f * (Q.y * Q.z + Q.w * Q.x), + .z = 2.0f * (Q.w * Q.w - 0.5f + Q.z * Q.z), + }}; // third column of transposed rotation matrix + return gravity; #undef Q } @@ -402,16 +433,16 @@ FusionVector FusionAhrsGetGravity(const FusionAhrs *const ahrs) { * @return Linear acceleration measurement in g. */ FusionVector FusionAhrsGetLinearAcceleration(const FusionAhrs *const ahrs) { - switch (ahrs->settings.convention) { - case FusionConventionNwu: - case FusionConventionEnu: { - return FusionVectorSubtract(ahrs->accelerometer, FusionAhrsGetGravity(ahrs)); - } - case FusionConventionNed: { - return FusionVectorAdd(ahrs->accelerometer, FusionAhrsGetGravity(ahrs)); - } - } - return FUSION_VECTOR_ZERO; // avoid compiler warning + switch (ahrs->settings.convention) { + case FusionConventionNwu: + case FusionConventionEnu: { + return FusionVectorSubtract(ahrs->accelerometer, FusionAhrsGetGravity(ahrs)); + } + case FusionConventionNed: { + return FusionVectorAdd(ahrs->accelerometer, FusionAhrsGetGravity(ahrs)); + } + } + return FUSION_VECTOR_ZERO; // avoid compiler warning } /** @@ -424,31 +455,32 @@ FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs) { #define Q ahrs->quaternion.element #define A ahrs->accelerometer.axis - // Calculate accelerometer measurement in the Earth coordinate frame - const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations - const float qwqx = Q.w * Q.x; - const float qwqy = Q.w * Q.y; - const float qwqz = Q.w * Q.z; - const float qxqy = Q.x * Q.y; - const float qxqz = Q.x * Q.z; - const float qyqz = Q.y * Q.z; - FusionVector accelerometer = {.axis = { - .x = 2.0f * ((qwqw - 0.5f + Q.x * Q.x) * A.x + (qxqy - qwqz) * A.y + (qxqz + qwqy) * A.z), - .y = 2.0f * ((qxqy + qwqz) * A.x + (qwqw - 0.5f + Q.y * Q.y) * A.y + (qyqz - qwqx) * A.z), - .z = 2.0f * ((qxqz - qwqy) * A.x + (qyqz + qwqx) * A.y + (qwqw - 0.5f + Q.z * Q.z) * A.z), - }}; // rotation matrix multiplied with the accelerometer - - // Remove gravity from accelerometer measurement - switch (ahrs->settings.convention) { - case FusionConventionNwu: - case FusionConventionEnu: - accelerometer.axis.z -= 1.0f; - break; - case FusionConventionNed: - accelerometer.axis.z += 1.0f; - break; - } - return accelerometer; + // Calculate accelerometer measurement in the Earth coordinate frame + const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations + const float qwqx = Q.w * Q.x; + const float qwqy = Q.w * Q.y; + const float qwqz = Q.w * Q.z; + const float qxqy = Q.x * Q.y; + const float qxqz = Q.x * Q.z; + const float qyqz = Q.y * Q.z; + FusionVector accelerometer = { + .axis = { + .x = 2.0f * ((qwqw - 0.5f + Q.x * Q.x) * A.x + (qxqy - qwqz) * A.y + (qxqz + qwqy) * A.z), + .y = 2.0f * ((qxqy + qwqz) * A.x + (qwqw - 0.5f + Q.y * Q.y) * A.y + (qyqz - qwqx) * A.z), + .z = 2.0f * ((qxqz - qwqy) * A.x + (qyqz + qwqx) * A.y + (qwqw - 0.5f + Q.z * Q.z) * A.z), + }}; // rotation matrix multiplied with the accelerometer + + // Remove gravity from accelerometer measurement + switch (ahrs->settings.convention) { + case FusionConventionNwu: + case FusionConventionEnu: + accelerometer.axis.z -= 1.0f; + break; + case FusionConventionNed: + accelerometer.axis.z += 1.0f; + break; + } + return accelerometer; #undef Q #undef A } @@ -459,15 +491,23 @@ FusionVector FusionAhrsGetEarthAcceleration(const FusionAhrs *const ahrs) { * @return AHRS algorithm internal states. */ FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahrs) { - const FusionAhrsInternalStates internalStates = { - .accelerationError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfAccelerometerFeedback))), - .accelerometerIgnored = ahrs->accelerometerIgnored, - .accelerationRecoveryTrigger = ahrs->settings.recoveryTriggerPeriod == 0 ? 0.0f : (float) ahrs->accelerationRecoveryTrigger / (float) ahrs->settings.recoveryTriggerPeriod, - .magneticError = FusionRadiansToDegrees(FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfMagnetometerFeedback))), - .magnetometerIgnored = ahrs->magnetometerIgnored, - .magneticRecoveryTrigger = ahrs->settings.recoveryTriggerPeriod == 0 ? 0.0f : (float) ahrs->magneticRecoveryTrigger / (float) ahrs->settings.recoveryTriggerPeriod, - }; - return internalStates; + const FusionAhrsInternalStates internalStates = { + .accelerationError = FusionRadiansToDegrees( + FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfAccelerometerFeedback))), + .accelerometerIgnored = ahrs->accelerometerIgnored, + .accelerationRecoveryTrigger = ahrs->settings.recoveryTriggerPeriod == 0 + ? 0.0f + : (float)ahrs->accelerationRecoveryTrigger / + (float)ahrs->settings.recoveryTriggerPeriod, + .magneticError = FusionRadiansToDegrees( + FusionAsin(2.0f * FusionVectorMagnitude(ahrs->halfMagnetometerFeedback))), + .magnetometerIgnored = ahrs->magnetometerIgnored, + .magneticRecoveryTrigger = + ahrs->settings.recoveryTriggerPeriod == 0 + ? 0.0f + : (float)ahrs->magneticRecoveryTrigger / (float)ahrs->settings.recoveryTriggerPeriod, + }; + return internalStates; } /** @@ -476,13 +516,13 @@ FusionAhrsInternalStates FusionAhrsGetInternalStates(const FusionAhrs *const ahr * @return AHRS algorithm flags. */ FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs) { - const FusionAhrsFlags flags = { - .initialising = ahrs->initialising, - .angularRateRecovery = ahrs->angularRateRecovery, - .accelerationRecovery = ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout, - .magneticRecovery= ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout, - }; - return flags; + const FusionAhrsFlags flags = { + .initialising = ahrs->initialising, + .angularRateRecovery = ahrs->angularRateRecovery, + .accelerationRecovery = ahrs->accelerationRecoveryTrigger > ahrs->accelerationRecoveryTimeout, + .magneticRecovery = ahrs->magneticRecoveryTrigger > ahrs->magneticRecoveryTimeout, + }; + return flags; } /** @@ -494,15 +534,15 @@ FusionAhrsFlags FusionAhrsGetFlags(const FusionAhrs *const ahrs) { */ void FusionAhrsSetHeading(FusionAhrs *const ahrs, const float heading) { #define Q ahrs->quaternion.element - const float yaw = atan2f(Q.w * Q.z + Q.x * Q.y, 0.5f - Q.y * Q.y - Q.z * Q.z); - const float halfYawMinusHeading = 0.5f * (yaw - FusionDegreesToRadians(heading)); - const FusionQuaternion rotation = {.element = { - .w = cosf(halfYawMinusHeading), - .x = 0.0f, - .y = 0.0f, - .z = -1.0f * sinf(halfYawMinusHeading), - }}; - ahrs->quaternion = FusionQuaternionMultiply(rotation, ahrs->quaternion); + const float yaw = atan2f(Q.w * Q.z + Q.x * Q.y, 0.5f - Q.y * Q.y - Q.z * Q.z); + const float halfYawMinusHeading = 0.5f * (yaw - FusionDegreesToRadians(heading)); + const FusionQuaternion rotation = {.element = { + .w = cosf(halfYawMinusHeading), + .x = 0.0f, + .y = 0.0f, + .z = -1.0f * sinf(halfYawMinusHeading), + }}; + ahrs->quaternion = FusionQuaternionMultiply(rotation, ahrs->quaternion); #undef Q } diff --git a/Fusion/FusionAhrs.h b/Fusion/FusionAhrs.h index 4cf785b..9fbe40a 100644 --- a/Fusion/FusionAhrs.h +++ b/Fusion/FusionAhrs.h @@ -22,12 +22,12 @@ * @brief AHRS algorithm settings. */ typedef struct { - FusionConvention convention; - float gain; - float gyroscopeRange; - float accelerationRejection; - float magneticRejection; - unsigned int recoveryTriggerPeriod; + FusionConvention convention; + float gain; + float gyroscopeRange; + float accelerationRejection; + float magneticRejection; + unsigned int recoveryTriggerPeriod; } FusionAhrsSettings; /** @@ -35,43 +35,43 @@ typedef struct { * must not be accessed by the application. */ typedef struct { - FusionAhrsSettings settings; - FusionQuaternion quaternion; - FusionVector accelerometer; - bool initialising; - float rampedGain; - float rampedGainStep; - bool angularRateRecovery; - FusionVector halfAccelerometerFeedback; - FusionVector halfMagnetometerFeedback; - bool accelerometerIgnored; - int accelerationRecoveryTrigger; - int accelerationRecoveryTimeout; - bool magnetometerIgnored; - int magneticRecoveryTrigger; - int magneticRecoveryTimeout; + FusionAhrsSettings settings; + FusionQuaternion quaternion; + FusionVector accelerometer; + bool initialising; + float rampedGain; + float rampedGainStep; + bool angularRateRecovery; + FusionVector halfAccelerometerFeedback; + FusionVector halfMagnetometerFeedback; + bool accelerometerIgnored; + int accelerationRecoveryTrigger; + int accelerationRecoveryTimeout; + bool magnetometerIgnored; + int magneticRecoveryTrigger; + int magneticRecoveryTimeout; } FusionAhrs; /** * @brief AHRS algorithm internal states. */ typedef struct { - float accelerationError; - bool accelerometerIgnored; - float accelerationRecoveryTrigger; - float magneticError; - bool magnetometerIgnored; - float magneticRecoveryTrigger; + float accelerationError; + bool accelerometerIgnored; + float accelerationRecoveryTrigger; + float magneticError; + bool magnetometerIgnored; + float magneticRecoveryTrigger; } FusionAhrsInternalStates; /** * @brief AHRS algorithm flags. */ typedef struct { - bool initialising; - bool angularRateRecovery; - bool accelerationRecovery; - bool magneticRecovery; + bool initialising; + bool angularRateRecovery; + bool accelerationRecovery; + bool magneticRecovery; } FusionAhrsFlags; //------------------------------------------------------------------------------ @@ -83,11 +83,16 @@ void FusionAhrsReset(FusionAhrs *const ahrs); void FusionAhrsSetSettings(FusionAhrs *const ahrs, const FusionAhrsSettings *const settings); -void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const FusionVector magnetometer, const float deltaTime); +void FusionAhrsUpdate(FusionAhrs *const ahrs, const FusionVector gyroscope, + const FusionVector accelerometer, const FusionVector magnetometer, + const float deltaTime); -void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float deltaTime); +void FusionAhrsUpdateNoMagnetometer(FusionAhrs *const ahrs, const FusionVector gyroscope, + const FusionVector accelerometer, const float deltaTime); -void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, const FusionVector accelerometer, const float heading, const float deltaTime); +void FusionAhrsUpdateExternalHeading(FusionAhrs *const ahrs, const FusionVector gyroscope, + const FusionVector accelerometer, const float heading, + const float deltaTime); FusionQuaternion FusionAhrsGetQuaternion(const FusionAhrs *const ahrs); diff --git a/Fusion/FusionAxes.h b/Fusion/FusionAxes.h index 32c07db..836a606 100644 --- a/Fusion/FusionAxes.h +++ b/Fusion/FusionAxes.h @@ -22,30 +22,30 @@ * then alignment is +Y-X+Z. */ typedef enum { - FusionAxesAlignmentPXPYPZ, /* +X+Y+Z */ - FusionAxesAlignmentPXNZPY, /* +X-Z+Y */ - FusionAxesAlignmentPXNYNZ, /* +X-Y-Z */ - FusionAxesAlignmentPXPZNY, /* +X+Z-Y */ - FusionAxesAlignmentNXPYNZ, /* -X+Y-Z */ - FusionAxesAlignmentNXPZPY, /* -X+Z+Y */ - FusionAxesAlignmentNXNYPZ, /* -X-Y+Z */ - FusionAxesAlignmentNXNZNY, /* -X-Z-Y */ - FusionAxesAlignmentPYNXPZ, /* +Y-X+Z */ - FusionAxesAlignmentPYNZNX, /* +Y-Z-X */ - FusionAxesAlignmentPYPXNZ, /* +Y+X-Z */ - FusionAxesAlignmentPYPZPX, /* +Y+Z+X */ - FusionAxesAlignmentNYPXPZ, /* -Y+X+Z */ - FusionAxesAlignmentNYNZPX, /* -Y-Z+X */ - FusionAxesAlignmentNYNXNZ, /* -Y-X-Z */ - FusionAxesAlignmentNYPZNX, /* -Y+Z-X */ - FusionAxesAlignmentPZPYNX, /* +Z+Y-X */ - FusionAxesAlignmentPZPXPY, /* +Z+X+Y */ - FusionAxesAlignmentPZNYPX, /* +Z-Y+X */ - FusionAxesAlignmentPZNXNY, /* +Z-X-Y */ - FusionAxesAlignmentNZPYPX, /* -Z+Y+X */ - FusionAxesAlignmentNZNXPY, /* -Z-X+Y */ - FusionAxesAlignmentNZNYNX, /* -Z-Y-X */ - FusionAxesAlignmentNZPXNY, /* -Z+X-Y */ + FusionAxesAlignmentPXPYPZ, /* +X+Y+Z */ + FusionAxesAlignmentPXNZPY, /* +X-Z+Y */ + FusionAxesAlignmentPXNYNZ, /* +X-Y-Z */ + FusionAxesAlignmentPXPZNY, /* +X+Z-Y */ + FusionAxesAlignmentNXPYNZ, /* -X+Y-Z */ + FusionAxesAlignmentNXPZPY, /* -X+Z+Y */ + FusionAxesAlignmentNXNYPZ, /* -X-Y+Z */ + FusionAxesAlignmentNXNZNY, /* -X-Z-Y */ + FusionAxesAlignmentPYNXPZ, /* +Y-X+Z */ + FusionAxesAlignmentPYNZNX, /* +Y-Z-X */ + FusionAxesAlignmentPYPXNZ, /* +Y+X-Z */ + FusionAxesAlignmentPYPZPX, /* +Y+Z+X */ + FusionAxesAlignmentNYPXPZ, /* -Y+X+Z */ + FusionAxesAlignmentNYNZPX, /* -Y-Z+X */ + FusionAxesAlignmentNYNXNZ, /* -Y-X-Z */ + FusionAxesAlignmentNYPZNX, /* -Y+Z-X */ + FusionAxesAlignmentPZPYNX, /* +Z+Y-X */ + FusionAxesAlignmentPZPXPY, /* +Z+X+Y */ + FusionAxesAlignmentPZNYPX, /* +Z-Y+X */ + FusionAxesAlignmentPZNXNY, /* +Z-X-Y */ + FusionAxesAlignmentNZPYPX, /* -Z+Y+X */ + FusionAxesAlignmentNZNXPY, /* -Z-X+Y */ + FusionAxesAlignmentNZNYNX, /* -Z-Y-X */ + FusionAxesAlignmentNZPXNY, /* -Z+X-Y */ } FusionAxesAlignment; //------------------------------------------------------------------------------ @@ -57,128 +57,129 @@ typedef enum { * @param alignment Axes alignment. * @return Sensor axes aligned with the body axes. */ -static inline FusionVector FusionAxesSwap(const FusionVector sensor, const FusionAxesAlignment alignment) { - FusionVector result; - switch (alignment) { - case FusionAxesAlignmentPXPYPZ: - break; - case FusionAxesAlignmentPXNZPY: - result.axis.x = +sensor.axis.x; - result.axis.y = -sensor.axis.z; - result.axis.z = +sensor.axis.y; - return result; - case FusionAxesAlignmentPXNYNZ: - result.axis.x = +sensor.axis.x; - result.axis.y = -sensor.axis.y; - result.axis.z = -sensor.axis.z; - return result; - case FusionAxesAlignmentPXPZNY: - result.axis.x = +sensor.axis.x; - result.axis.y = +sensor.axis.z; - result.axis.z = -sensor.axis.y; - return result; - case FusionAxesAlignmentNXPYNZ: - result.axis.x = -sensor.axis.x; - result.axis.y = +sensor.axis.y; - result.axis.z = -sensor.axis.z; - return result; - case FusionAxesAlignmentNXPZPY: - result.axis.x = -sensor.axis.x; - result.axis.y = +sensor.axis.z; - result.axis.z = +sensor.axis.y; - return result; - case FusionAxesAlignmentNXNYPZ: - result.axis.x = -sensor.axis.x; - result.axis.y = -sensor.axis.y; - result.axis.z = +sensor.axis.z; - return result; - case FusionAxesAlignmentNXNZNY: - result.axis.x = -sensor.axis.x; - result.axis.y = -sensor.axis.z; - result.axis.z = -sensor.axis.y; - return result; - case FusionAxesAlignmentPYNXPZ: - result.axis.x = +sensor.axis.y; - result.axis.y = -sensor.axis.x; - result.axis.z = +sensor.axis.z; - return result; - case FusionAxesAlignmentPYNZNX: - result.axis.x = +sensor.axis.y; - result.axis.y = -sensor.axis.z; - result.axis.z = -sensor.axis.x; - return result; - case FusionAxesAlignmentPYPXNZ: - result.axis.x = +sensor.axis.y; - result.axis.y = +sensor.axis.x; - result.axis.z = -sensor.axis.z; - return result; - case FusionAxesAlignmentPYPZPX: - result.axis.x = +sensor.axis.y; - result.axis.y = +sensor.axis.z; - result.axis.z = +sensor.axis.x; - return result; - case FusionAxesAlignmentNYPXPZ: - result.axis.x = -sensor.axis.y; - result.axis.y = +sensor.axis.x; - result.axis.z = +sensor.axis.z; - return result; - case FusionAxesAlignmentNYNZPX: - result.axis.x = -sensor.axis.y; - result.axis.y = -sensor.axis.z; - result.axis.z = +sensor.axis.x; - return result; - case FusionAxesAlignmentNYNXNZ: - result.axis.x = -sensor.axis.y; - result.axis.y = -sensor.axis.x; - result.axis.z = -sensor.axis.z; - return result; - case FusionAxesAlignmentNYPZNX: - result.axis.x = -sensor.axis.y; - result.axis.y = +sensor.axis.z; - result.axis.z = -sensor.axis.x; - return result; - case FusionAxesAlignmentPZPYNX: - result.axis.x = +sensor.axis.z; - result.axis.y = +sensor.axis.y; - result.axis.z = -sensor.axis.x; - return result; - case FusionAxesAlignmentPZPXPY: - result.axis.x = +sensor.axis.z; - result.axis.y = +sensor.axis.x; - result.axis.z = +sensor.axis.y; - return result; - case FusionAxesAlignmentPZNYPX: - result.axis.x = +sensor.axis.z; - result.axis.y = -sensor.axis.y; - result.axis.z = +sensor.axis.x; - return result; - case FusionAxesAlignmentPZNXNY: - result.axis.x = +sensor.axis.z; - result.axis.y = -sensor.axis.x; - result.axis.z = -sensor.axis.y; - return result; - case FusionAxesAlignmentNZPYPX: - result.axis.x = -sensor.axis.z; - result.axis.y = +sensor.axis.y; - result.axis.z = +sensor.axis.x; - return result; - case FusionAxesAlignmentNZNXPY: - result.axis.x = -sensor.axis.z; - result.axis.y = -sensor.axis.x; - result.axis.z = +sensor.axis.y; - return result; - case FusionAxesAlignmentNZNYNX: - result.axis.x = -sensor.axis.z; - result.axis.y = -sensor.axis.y; - result.axis.z = -sensor.axis.x; - return result; - case FusionAxesAlignmentNZPXNY: - result.axis.x = -sensor.axis.z; - result.axis.y = +sensor.axis.x; - result.axis.z = -sensor.axis.y; - return result; - } - return sensor; // avoid compiler warning +static inline FusionVector FusionAxesSwap(const FusionVector sensor, + const FusionAxesAlignment alignment) { + FusionVector result; + switch (alignment) { + case FusionAxesAlignmentPXPYPZ: + break; + case FusionAxesAlignmentPXNZPY: + result.axis.x = +sensor.axis.x; + result.axis.y = -sensor.axis.z; + result.axis.z = +sensor.axis.y; + return result; + case FusionAxesAlignmentPXNYNZ: + result.axis.x = +sensor.axis.x; + result.axis.y = -sensor.axis.y; + result.axis.z = -sensor.axis.z; + return result; + case FusionAxesAlignmentPXPZNY: + result.axis.x = +sensor.axis.x; + result.axis.y = +sensor.axis.z; + result.axis.z = -sensor.axis.y; + return result; + case FusionAxesAlignmentNXPYNZ: + result.axis.x = -sensor.axis.x; + result.axis.y = +sensor.axis.y; + result.axis.z = -sensor.axis.z; + return result; + case FusionAxesAlignmentNXPZPY: + result.axis.x = -sensor.axis.x; + result.axis.y = +sensor.axis.z; + result.axis.z = +sensor.axis.y; + return result; + case FusionAxesAlignmentNXNYPZ: + result.axis.x = -sensor.axis.x; + result.axis.y = -sensor.axis.y; + result.axis.z = +sensor.axis.z; + return result; + case FusionAxesAlignmentNXNZNY: + result.axis.x = -sensor.axis.x; + result.axis.y = -sensor.axis.z; + result.axis.z = -sensor.axis.y; + return result; + case FusionAxesAlignmentPYNXPZ: + result.axis.x = +sensor.axis.y; + result.axis.y = -sensor.axis.x; + result.axis.z = +sensor.axis.z; + return result; + case FusionAxesAlignmentPYNZNX: + result.axis.x = +sensor.axis.y; + result.axis.y = -sensor.axis.z; + result.axis.z = -sensor.axis.x; + return result; + case FusionAxesAlignmentPYPXNZ: + result.axis.x = +sensor.axis.y; + result.axis.y = +sensor.axis.x; + result.axis.z = -sensor.axis.z; + return result; + case FusionAxesAlignmentPYPZPX: + result.axis.x = +sensor.axis.y; + result.axis.y = +sensor.axis.z; + result.axis.z = +sensor.axis.x; + return result; + case FusionAxesAlignmentNYPXPZ: + result.axis.x = -sensor.axis.y; + result.axis.y = +sensor.axis.x; + result.axis.z = +sensor.axis.z; + return result; + case FusionAxesAlignmentNYNZPX: + result.axis.x = -sensor.axis.y; + result.axis.y = -sensor.axis.z; + result.axis.z = +sensor.axis.x; + return result; + case FusionAxesAlignmentNYNXNZ: + result.axis.x = -sensor.axis.y; + result.axis.y = -sensor.axis.x; + result.axis.z = -sensor.axis.z; + return result; + case FusionAxesAlignmentNYPZNX: + result.axis.x = -sensor.axis.y; + result.axis.y = +sensor.axis.z; + result.axis.z = -sensor.axis.x; + return result; + case FusionAxesAlignmentPZPYNX: + result.axis.x = +sensor.axis.z; + result.axis.y = +sensor.axis.y; + result.axis.z = -sensor.axis.x; + return result; + case FusionAxesAlignmentPZPXPY: + result.axis.x = +sensor.axis.z; + result.axis.y = +sensor.axis.x; + result.axis.z = +sensor.axis.y; + return result; + case FusionAxesAlignmentPZNYPX: + result.axis.x = +sensor.axis.z; + result.axis.y = -sensor.axis.y; + result.axis.z = +sensor.axis.x; + return result; + case FusionAxesAlignmentPZNXNY: + result.axis.x = +sensor.axis.z; + result.axis.y = -sensor.axis.x; + result.axis.z = -sensor.axis.y; + return result; + case FusionAxesAlignmentNZPYPX: + result.axis.x = -sensor.axis.z; + result.axis.y = +sensor.axis.y; + result.axis.z = +sensor.axis.x; + return result; + case FusionAxesAlignmentNZNXPY: + result.axis.x = -sensor.axis.z; + result.axis.y = -sensor.axis.x; + result.axis.z = +sensor.axis.y; + return result; + case FusionAxesAlignmentNZNYNX: + result.axis.x = -sensor.axis.z; + result.axis.y = -sensor.axis.y; + result.axis.z = -sensor.axis.x; + return result; + case FusionAxesAlignmentNZPXNY: + result.axis.x = -sensor.axis.z; + result.axis.y = +sensor.axis.x; + result.axis.z = -sensor.axis.y; + return result; + } + return sensor; // avoid compiler warning } #endif diff --git a/Fusion/FusionCalibration.h b/Fusion/FusionCalibration.h index 733f572..86a4e36 100644 --- a/Fusion/FusionCalibration.h +++ b/Fusion/FusionCalibration.h @@ -23,8 +23,13 @@ * @param offset Offset. * @return Calibrated measurement. */ -static inline FusionVector FusionCalibrationInertial(const FusionVector uncalibrated, const FusionMatrix misalignment, const FusionVector sensitivity, const FusionVector offset) { - return FusionMatrixMultiplyVector(misalignment, FusionVectorHadamardProduct(FusionVectorSubtract(uncalibrated, offset), sensitivity)); +static inline FusionVector FusionCalibrationInertial(const FusionVector uncalibrated, + const FusionMatrix misalignment, + const FusionVector sensitivity, + const FusionVector offset) { + return FusionMatrixMultiplyVector( + misalignment, + FusionVectorHadamardProduct(FusionVectorSubtract(uncalibrated, offset), sensitivity)); } /** @@ -34,8 +39,11 @@ static inline FusionVector FusionCalibrationInertial(const FusionVector uncalibr * @param hardIronOffset Hard-iron offset. * @return Calibrated measurement. */ -static inline FusionVector FusionCalibrationMagnetic(const FusionVector uncalibrated, const FusionMatrix softIronMatrix, const FusionVector hardIronOffset) { - return FusionMatrixMultiplyVector(softIronMatrix, FusionVectorSubtract(uncalibrated, hardIronOffset)); +static inline FusionVector FusionCalibrationMagnetic(const FusionVector uncalibrated, + const FusionMatrix softIronMatrix, + const FusionVector hardIronOffset) { + return FusionMatrixMultiplyVector(softIronMatrix, + FusionVectorSubtract(uncalibrated, hardIronOffset)); } #endif diff --git a/Fusion/FusionCompass.c b/Fusion/FusionCompass.c index 0ec99d1..3dc56e8 100644 --- a/Fusion/FusionCompass.c +++ b/Fusion/FusionCompass.c @@ -8,8 +8,8 @@ //------------------------------------------------------------------------------ // Includes -#include "FusionAxes.h" #include "FusionCompass.h" +#include "FusionAxes.h" #include // atan2f //------------------------------------------------------------------------------ @@ -22,27 +22,31 @@ * @param magnetometer Magnetometer measurement in any calibrated units. * @return Heading angle in degrees. */ -float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer, const FusionVector magnetometer) { - switch (convention) { - case FusionConventionNwu: { - const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer)); - const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, accelerometer)); - return FusionRadiansToDegrees(atan2f(west.axis.x, north.axis.x)); - } - case FusionConventionEnu: { - const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer)); - const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, accelerometer)); - const FusionVector east = FusionVectorMultiplyScalar(west, -1.0f); - return FusionRadiansToDegrees(atan2f(north.axis.x, east.axis.x)); - } - case FusionConventionNed: { - const FusionVector up = FusionVectorMultiplyScalar(accelerometer, -1.0f); - const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(up, magnetometer)); - const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, up)); - return FusionRadiansToDegrees(atan2f(west.axis.x, north.axis.x)); - } - } - return 0; // avoid compiler warning +float FusionCompassCalculateHeading(const FusionConvention convention, + const FusionVector accelerometer, + const FusionVector magnetometer) { + switch (convention) { + case FusionConventionNwu: { + const FusionVector west = + FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer)); + const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, accelerometer)); + return FusionRadiansToDegrees(atan2f(west.axis.x, north.axis.x)); + } + case FusionConventionEnu: { + const FusionVector west = + FusionVectorNormalise(FusionVectorCrossProduct(accelerometer, magnetometer)); + const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, accelerometer)); + const FusionVector east = FusionVectorMultiplyScalar(west, -1.0f); + return FusionRadiansToDegrees(atan2f(north.axis.x, east.axis.x)); + } + case FusionConventionNed: { + const FusionVector up = FusionVectorMultiplyScalar(accelerometer, -1.0f); + const FusionVector west = FusionVectorNormalise(FusionVectorCrossProduct(up, magnetometer)); + const FusionVector north = FusionVectorNormalise(FusionVectorCrossProduct(west, up)); + return FusionRadiansToDegrees(atan2f(west.axis.x, north.axis.x)); + } + } + return 0; // avoid compiler warning } //------------------------------------------------------------------------------ diff --git a/Fusion/FusionCompass.h b/Fusion/FusionCompass.h index 78326c0..ec49651 100644 --- a/Fusion/FusionCompass.h +++ b/Fusion/FusionCompass.h @@ -17,7 +17,9 @@ //------------------------------------------------------------------------------ // Function declarations -float FusionCompassCalculateHeading(const FusionConvention convention, const FusionVector accelerometer, const FusionVector magnetometer); +float FusionCompassCalculateHeading(const FusionConvention convention, + const FusionVector accelerometer, + const FusionVector magnetometer); #endif diff --git a/Fusion/FusionConvention.h b/Fusion/FusionConvention.h index 0b0d43a..c1fb77f 100644 --- a/Fusion/FusionConvention.h +++ b/Fusion/FusionConvention.h @@ -14,9 +14,9 @@ * @brief Earth axes convention. */ typedef enum { - FusionConventionNwu, /* North-West-Up */ - FusionConventionEnu, /* East-North-Up */ - FusionConventionNed, /* North-East-Down */ + FusionConventionNwu, /* North-West-Up */ + FusionConventionEnu, /* East-North-Up */ + FusionConventionNed, /* North-East-Down */ } FusionConvention; #endif diff --git a/Fusion/FusionMath.h b/Fusion/FusionMath.h index 6765d29..9c88755 100644 --- a/Fusion/FusionMath.h +++ b/Fusion/FusionMath.h @@ -21,27 +21,27 @@ * @brief 3D vector. */ typedef union { - float array[3]; + float array[3]; - struct { - float x; - float y; - float z; - } axis; + struct { + float x; + float y; + float z; + } axis; } FusionVector; /** * @brief Quaternion. */ typedef union { - float array[4]; - - struct { - float w; - float x; - float y; - float z; - } element; + float array[4]; + + struct { + float w; + float x; + float y; + float z; + } element; } FusionQuaternion; /** @@ -49,19 +49,19 @@ typedef union { * See http://en.wikipedia.org/wiki/Row-major_order */ typedef union { - float array[3][3]; - - struct { - float xx; - float xy; - float xz; - float yx; - float yy; - float yz; - float zx; - float zy; - float zz; - } element; + float array[3][3]; + + struct { + float xx; + float xy; + float xz; + float yx; + float yy; + float yz; + float zx; + float zy; + float zz; + } element; } FusionMatrix; /** @@ -69,39 +69,40 @@ typedef union { * X, Y, and Z respectively. */ typedef union { - float array[3]; + float array[3]; - struct { - float roll; - float pitch; - float yaw; - } angle; + struct { + float roll; + float pitch; + float yaw; + } angle; } FusionEuler; /** * @brief Vector of zeros. */ -#define FUSION_VECTOR_ZERO ((FusionVector){ .array = {0.0f, 0.0f, 0.0f} }) +#define FUSION_VECTOR_ZERO ((FusionVector){.array = {0.0f, 0.0f, 0.0f}}) /** * @brief Vector of ones. */ -#define FUSION_VECTOR_ONES ((FusionVector){ .array = {1.0f, 1.0f, 1.0f} }) +#define FUSION_VECTOR_ONES ((FusionVector){.array = {1.0f, 1.0f, 1.0f}}) /** * @brief Identity quaternion. */ -#define FUSION_IDENTITY_QUATERNION ((FusionQuaternion){ .array = {1.0f, 0.0f, 0.0f, 0.0f} }) +#define FUSION_IDENTITY_QUATERNION ((FusionQuaternion){.array = {1.0f, 0.0f, 0.0f, 0.0f}}) /** * @brief Identity matrix. */ -#define FUSION_IDENTITY_MATRIX ((FusionMatrix){ .array = {{1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, {0.0f, 0.0f, 1.0f}} }) +#define FUSION_IDENTITY_MATRIX \ + ((FusionMatrix){.array = {{1.0f, 0.0f, 0.0f}, {0.0f, 1.0f, 0.0f}, {0.0f, 0.0f, 1.0f}}}) /** * @brief Euler angles of zero. */ -#define FUSION_EULER_ZERO ((FusionEuler){ .array = {0.0f, 0.0f, 0.0f} }) +#define FUSION_EULER_ZERO ((FusionEuler){.array = {0.0f, 0.0f, 0.0f}}) /** * @brief Pi. May not be defined in math.h. @@ -114,7 +115,7 @@ typedef union { * @brief Include this definition or add as a preprocessor definition to use * normal square root operations. */ -//#define FUSION_USE_NORMAL_SQRT +// #define FUSION_USE_NORMAL_SQRT //------------------------------------------------------------------------------ // Inline functions - Degrees and radians conversion @@ -125,7 +126,7 @@ typedef union { * @return Radians. */ static inline float FusionDegreesToRadians(const float degrees) { - return degrees * ((float) M_PI / 180.0f); + return degrees * ((float)M_PI / 180.0f); } /** @@ -134,7 +135,7 @@ static inline float FusionDegreesToRadians(const float degrees) { * @return Degrees. */ static inline float FusionRadiansToDegrees(const float radians) { - return radians * (180.0f / (float) M_PI); + return radians * (180.0f / (float)M_PI); } //------------------------------------------------------------------------------ @@ -146,13 +147,13 @@ static inline float FusionRadiansToDegrees(const float radians) { * @return Arc sine of the value. */ static inline float FusionAsin(const float value) { - if (value <= -1.0f) { - return (float) M_PI / -2.0f; - } - if (value >= 1.0f) { - return (float) M_PI / 2.0f; - } - return asinf(value); + if (value <= -1.0f) { + return (float)M_PI / -2.0f; + } + if (value >= 1.0f) { + return (float)M_PI / 2.0f; + } + return asinf(value); } //------------------------------------------------------------------------------ @@ -168,14 +169,14 @@ static inline float FusionAsin(const float value) { */ static inline float FusionFastInverseSqrt(const float x) { - typedef union { - float f; - int32_t i; - } Union32; + typedef union { + float f; + int32_t i; + } Union32; - Union32 union32 = {.f = x}; - union32.i = 0x5F1F1412 - (union32.i >> 1); - return union32.f * (1.69000231f - 0.714158168f * x * union32.f * union32.f); + Union32 union32 = {.f = x}; + union32.i = 0x5F1F1412 - (union32.i >> 1); + return union32.f * (1.69000231f - 0.714158168f * x * union32.f * union32.f); } #endif @@ -189,7 +190,7 @@ static inline float FusionFastInverseSqrt(const float x) { * @return True if the vector is zero. */ static inline bool FusionVectorIsZero(const FusionVector vector) { - return (vector.axis.x == 0.0f) && (vector.axis.y == 0.0f) && (vector.axis.z == 0.0f); + return (vector.axis.x == 0.0f) && (vector.axis.y == 0.0f) && (vector.axis.z == 0.0f); } /** @@ -199,12 +200,12 @@ static inline bool FusionVectorIsZero(const FusionVector vector) { * @return Sum of two vectors. */ static inline FusionVector FusionVectorAdd(const FusionVector vectorA, const FusionVector vectorB) { - const FusionVector result = {.axis = { - .x = vectorA.axis.x + vectorB.axis.x, - .y = vectorA.axis.y + vectorB.axis.y, - .z = vectorA.axis.z + vectorB.axis.z, - }}; - return result; + const FusionVector result = {.axis = { + .x = vectorA.axis.x + vectorB.axis.x, + .y = vectorA.axis.y + vectorB.axis.y, + .z = vectorA.axis.z + vectorB.axis.z, + }}; + return result; } /** @@ -213,13 +214,14 @@ static inline FusionVector FusionVectorAdd(const FusionVector vectorA, const Fus * @param vectorB Vector B. * @return Vector B subtracted from vector A. */ -static inline FusionVector FusionVectorSubtract(const FusionVector vectorA, const FusionVector vectorB) { - const FusionVector result = {.axis = { - .x = vectorA.axis.x - vectorB.axis.x, - .y = vectorA.axis.y - vectorB.axis.y, - .z = vectorA.axis.z - vectorB.axis.z, - }}; - return result; +static inline FusionVector FusionVectorSubtract(const FusionVector vectorA, + const FusionVector vectorB) { + const FusionVector result = {.axis = { + .x = vectorA.axis.x - vectorB.axis.x, + .y = vectorA.axis.y - vectorB.axis.y, + .z = vectorA.axis.z - vectorB.axis.z, + }}; + return result; } /** @@ -228,7 +230,7 @@ static inline FusionVector FusionVectorSubtract(const FusionVector vectorA, cons * @return Sum of the elements. */ static inline float FusionVectorSum(const FusionVector vector) { - return vector.axis.x + vector.axis.y + vector.axis.z; + return vector.axis.x + vector.axis.y + vector.axis.z; } /** @@ -237,13 +239,14 @@ static inline float FusionVectorSum(const FusionVector vector) { * @param scalar Scalar. * @return Multiplication of a vector by a scalar. */ -static inline FusionVector FusionVectorMultiplyScalar(const FusionVector vector, const float scalar) { - const FusionVector result = {.axis = { - .x = vector.axis.x * scalar, - .y = vector.axis.y * scalar, - .z = vector.axis.z * scalar, - }}; - return result; +static inline FusionVector FusionVectorMultiplyScalar(const FusionVector vector, + const float scalar) { + const FusionVector result = {.axis = { + .x = vector.axis.x * scalar, + .y = vector.axis.y * scalar, + .z = vector.axis.z * scalar, + }}; + return result; } /** @@ -252,13 +255,14 @@ static inline FusionVector FusionVectorMultiplyScalar(const FusionVector vector, * @param vectorB Vector B. * @return Hadamard product. */ -static inline FusionVector FusionVectorHadamardProduct(const FusionVector vectorA, const FusionVector vectorB) { - const FusionVector result = {.axis = { - .x = vectorA.axis.x * vectorB.axis.x, - .y = vectorA.axis.y * vectorB.axis.y, - .z = vectorA.axis.z * vectorB.axis.z, - }}; - return result; +static inline FusionVector FusionVectorHadamardProduct(const FusionVector vectorA, + const FusionVector vectorB) { + const FusionVector result = {.axis = { + .x = vectorA.axis.x * vectorB.axis.x, + .y = vectorA.axis.y * vectorB.axis.y, + .z = vectorA.axis.z * vectorB.axis.z, + }}; + return result; } /** @@ -267,15 +271,16 @@ static inline FusionVector FusionVectorHadamardProduct(const FusionVector vector * @param vectorB Vector B. * @return Cross product. */ -static inline FusionVector FusionVectorCrossProduct(const FusionVector vectorA, const FusionVector vectorB) { +static inline FusionVector FusionVectorCrossProduct(const FusionVector vectorA, + const FusionVector vectorB) { #define A vectorA.axis #define B vectorB.axis - const FusionVector result = {.axis = { - .x = A.y * B.z - A.z * B.y, - .y = A.z * B.x - A.x * B.z, - .z = A.x * B.y - A.y * B.x, - }}; - return result; + const FusionVector result = {.axis = { + .x = A.y * B.z - A.z * B.y, + .y = A.z * B.x - A.x * B.z, + .z = A.x * B.y - A.y * B.x, + }}; + return result; #undef A #undef B } @@ -287,7 +292,7 @@ static inline FusionVector FusionVectorCrossProduct(const FusionVector vectorA, * @return Dot product. */ static inline float FusionVectorDotProduct(const FusionVector vectorA, const FusionVector vectorB) { - return FusionVectorSum(FusionVectorHadamardProduct(vectorA, vectorB)); + return FusionVectorSum(FusionVectorHadamardProduct(vectorA, vectorB)); } /** @@ -296,7 +301,7 @@ static inline float FusionVectorDotProduct(const FusionVector vectorA, const Fus * @return Vector magnitude squared. */ static inline float FusionVectorMagnitudeSquared(const FusionVector vector) { - return FusionVectorSum(FusionVectorHadamardProduct(vector, vector)); + return FusionVectorSum(FusionVectorHadamardProduct(vector, vector)); } /** @@ -305,7 +310,7 @@ static inline float FusionVectorMagnitudeSquared(const FusionVector vector) { * @return Vector magnitude. */ static inline float FusionVectorMagnitude(const FusionVector vector) { - return sqrtf(FusionVectorMagnitudeSquared(vector)); + return sqrtf(FusionVectorMagnitudeSquared(vector)); } /** @@ -315,11 +320,11 @@ static inline float FusionVectorMagnitude(const FusionVector vector) { */ static inline FusionVector FusionVectorNormalise(const FusionVector vector) { #ifdef FUSION_USE_NORMAL_SQRT - const float magnitudeReciprocal = 1.0f / sqrtf(FusionVectorMagnitudeSquared(vector)); + const float magnitudeReciprocal = 1.0f / sqrtf(FusionVectorMagnitudeSquared(vector)); #else - const float magnitudeReciprocal = FusionFastInverseSqrt(FusionVectorMagnitudeSquared(vector)); + const float magnitudeReciprocal = FusionFastInverseSqrt(FusionVectorMagnitudeSquared(vector)); #endif - return FusionVectorMultiplyScalar(vector, magnitudeReciprocal); + return FusionVectorMultiplyScalar(vector, magnitudeReciprocal); } //------------------------------------------------------------------------------ @@ -331,14 +336,15 @@ static inline FusionVector FusionVectorNormalise(const FusionVector vector) { * @param quaternionB Quaternion B. * @return Sum of two quaternions. */ -static inline FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) { - const FusionQuaternion result = {.element = { - .w = quaternionA.element.w + quaternionB.element.w, - .x = quaternionA.element.x + quaternionB.element.x, - .y = quaternionA.element.y + quaternionB.element.y, - .z = quaternionA.element.z + quaternionB.element.z, - }}; - return result; +static inline FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quaternionA, + const FusionQuaternion quaternionB) { + const FusionQuaternion result = {.element = { + .w = quaternionA.element.w + quaternionB.element.w, + .x = quaternionA.element.x + quaternionB.element.x, + .y = quaternionA.element.y + quaternionB.element.y, + .z = quaternionA.element.z + quaternionB.element.z, + }}; + return result; } /** @@ -347,16 +353,17 @@ static inline FusionQuaternion FusionQuaternionAdd(const FusionQuaternion quater * @param quaternionB Quaternion B (to be pre-multiplied). * @return Multiplication of two quaternions. */ -static inline FusionQuaternion FusionQuaternionMultiply(const FusionQuaternion quaternionA, const FusionQuaternion quaternionB) { +static inline FusionQuaternion FusionQuaternionMultiply(const FusionQuaternion quaternionA, + const FusionQuaternion quaternionB) { #define A quaternionA.element #define B quaternionB.element - const FusionQuaternion result = {.element = { - .w = A.w * B.w - A.x * B.x - A.y * B.y - A.z * B.z, - .x = A.w * B.x + A.x * B.w + A.y * B.z - A.z * B.y, - .y = A.w * B.y - A.x * B.z + A.y * B.w + A.z * B.x, - .z = A.w * B.z + A.x * B.y - A.y * B.x + A.z * B.w, - }}; - return result; + const FusionQuaternion result = {.element = { + .w = A.w * B.w - A.x * B.x - A.y * B.y - A.z * B.z, + .x = A.w * B.x + A.x * B.w + A.y * B.z - A.z * B.y, + .y = A.w * B.y - A.x * B.z + A.y * B.w + A.z * B.x, + .z = A.w * B.z + A.x * B.y - A.y * B.x + A.z * B.w, + }}; + return result; #undef A #undef B } @@ -370,16 +377,17 @@ static inline FusionQuaternion FusionQuaternionMultiply(const FusionQuaternion q * @param vector Vector. * @return Multiplication of a quaternion with a vector. */ -static inline FusionQuaternion FusionQuaternionMultiplyVector(const FusionQuaternion quaternion, const FusionVector vector) { +static inline FusionQuaternion FusionQuaternionMultiplyVector(const FusionQuaternion quaternion, + const FusionVector vector) { #define Q quaternion.element #define V vector.axis - const FusionQuaternion result = {.element = { - .w = -Q.x * V.x - Q.y * V.y - Q.z * V.z, - .x = Q.w * V.x + Q.y * V.z - Q.z * V.y, - .y = Q.w * V.y - Q.x * V.z + Q.z * V.x, - .z = Q.w * V.z + Q.x * V.y - Q.y * V.x, - }}; - return result; + const FusionQuaternion result = {.element = { + .w = -Q.x * V.x - Q.y * V.y - Q.z * V.z, + .x = Q.w * V.x + Q.y * V.z - Q.z * V.y, + .y = Q.w * V.y - Q.x * V.z + Q.z * V.x, + .z = Q.w * V.z + Q.x * V.y - Q.y * V.x, + }}; + return result; #undef Q #undef V } @@ -392,17 +400,18 @@ static inline FusionQuaternion FusionQuaternionMultiplyVector(const FusionQuater static inline FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion quaternion) { #define Q quaternion.element #ifdef FUSION_USE_NORMAL_SQRT - const float magnitudeReciprocal = 1.0f / sqrtf(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z); + const float magnitudeReciprocal = 1.0f / sqrtf(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z); #else - const float magnitudeReciprocal = FusionFastInverseSqrt(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z); + const float magnitudeReciprocal = + FusionFastInverseSqrt(Q.w * Q.w + Q.x * Q.x + Q.y * Q.y + Q.z * Q.z); #endif - const FusionQuaternion result = {.element = { - .w = Q.w * magnitudeReciprocal, - .x = Q.x * magnitudeReciprocal, - .y = Q.y * magnitudeReciprocal, - .z = Q.z * magnitudeReciprocal, - }}; - return result; + const FusionQuaternion result = {.element = { + .w = Q.w * magnitudeReciprocal, + .x = Q.x * magnitudeReciprocal, + .y = Q.y * magnitudeReciprocal, + .z = Q.z * magnitudeReciprocal, + }}; + return result; #undef Q } @@ -415,14 +424,16 @@ static inline FusionQuaternion FusionQuaternionNormalise(const FusionQuaternion * @param vector Vector. * @return Multiplication of a matrix with a vector. */ -static inline FusionVector FusionMatrixMultiplyVector(const FusionMatrix matrix, const FusionVector vector) { +static inline FusionVector FusionMatrixMultiplyVector(const FusionMatrix matrix, + const FusionVector vector) { #define R matrix.element - const FusionVector result = {.axis = { - .x = R.xx * vector.axis.x + R.xy * vector.axis.y + R.xz * vector.axis.z, - .y = R.yx * vector.axis.x + R.yy * vector.axis.y + R.yz * vector.axis.z, - .z = R.zx * vector.axis.x + R.zy * vector.axis.y + R.zz * vector.axis.z, - }}; - return result; + const FusionVector result = { + .axis = { + .x = R.xx * vector.axis.x + R.xy * vector.axis.y + R.xz * vector.axis.z, + .y = R.yx * vector.axis.x + R.yy * vector.axis.y + R.yz * vector.axis.z, + .z = R.zx * vector.axis.x + R.zy * vector.axis.y + R.zz * vector.axis.z, + }}; + return result; #undef R } @@ -436,25 +447,25 @@ static inline FusionVector FusionMatrixMultiplyVector(const FusionMatrix matrix, */ static inline FusionMatrix FusionQuaternionToMatrix(const FusionQuaternion quaternion) { #define Q quaternion.element - const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations - const float qwqx = Q.w * Q.x; - const float qwqy = Q.w * Q.y; - const float qwqz = Q.w * Q.z; - const float qxqy = Q.x * Q.y; - const float qxqz = Q.x * Q.z; - const float qyqz = Q.y * Q.z; - const FusionMatrix matrix = {.element = { - .xx = 2.0f * (qwqw - 0.5f + Q.x * Q.x), - .xy = 2.0f * (qxqy - qwqz), - .xz = 2.0f * (qxqz + qwqy), - .yx = 2.0f * (qxqy + qwqz), - .yy = 2.0f * (qwqw - 0.5f + Q.y * Q.y), - .yz = 2.0f * (qyqz - qwqx), - .zx = 2.0f * (qxqz - qwqy), - .zy = 2.0f * (qyqz + qwqx), - .zz = 2.0f * (qwqw - 0.5f + Q.z * Q.z), - }}; - return matrix; + const float qwqw = Q.w * Q.w; // calculate common terms to avoid repeated operations + const float qwqx = Q.w * Q.x; + const float qwqy = Q.w * Q.y; + const float qwqz = Q.w * Q.z; + const float qxqy = Q.x * Q.y; + const float qxqz = Q.x * Q.z; + const float qyqz = Q.y * Q.z; + const FusionMatrix matrix = {.element = { + .xx = 2.0f * (qwqw - 0.5f + Q.x * Q.x), + .xy = 2.0f * (qxqy - qwqz), + .xz = 2.0f * (qxqz + qwqy), + .yx = 2.0f * (qxqy + qwqz), + .yy = 2.0f * (qwqw - 0.5f + Q.y * Q.y), + .yz = 2.0f * (qyqz - qwqx), + .zx = 2.0f * (qxqz - qwqy), + .zy = 2.0f * (qyqz + qwqx), + .zz = 2.0f * (qwqw - 0.5f + Q.z * Q.z), + }}; + return matrix; #undef Q } @@ -465,13 +476,17 @@ static inline FusionMatrix FusionQuaternionToMatrix(const FusionQuaternion quate */ static inline FusionEuler FusionQuaternionToEuler(const FusionQuaternion quaternion) { #define Q quaternion.element - const float halfMinusQySquared = 0.5f - Q.y * Q.y; // calculate common terms to avoid repeated operations - const FusionEuler euler = {.angle = { - .roll = FusionRadiansToDegrees(atan2f(Q.w * Q.x + Q.y * Q.z, halfMinusQySquared - Q.x * Q.x)), - .pitch = FusionRadiansToDegrees(FusionAsin(2.0f * (Q.w * Q.y - Q.z * Q.x))), - .yaw = FusionRadiansToDegrees(atan2f(Q.w * Q.z + Q.x * Q.y, halfMinusQySquared - Q.z * Q.z)), - }}; - return euler; + const float halfMinusQySquared = + 0.5f - Q.y * Q.y; // calculate common terms to avoid repeated operations + const FusionEuler euler = { + .angle = { + .roll = + FusionRadiansToDegrees(atan2f(Q.w * Q.x + Q.y * Q.z, halfMinusQySquared - Q.x * Q.x)), + .pitch = FusionRadiansToDegrees(FusionAsin(2.0f * (Q.w * Q.y - Q.z * Q.x))), + .yaw = + FusionRadiansToDegrees(atan2f(Q.w * Q.z + Q.x * Q.y, halfMinusQySquared - Q.z * Q.z)), + }}; + return euler; #undef Q } diff --git a/Fusion/FusionOffset.c b/Fusion/FusionOffset.c index a037e1e..eb72524 100644 --- a/Fusion/FusionOffset.c +++ b/Fusion/FusionOffset.c @@ -38,10 +38,10 @@ * @param sampleRate Sample rate in Hz. */ void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampleRate) { - offset->filterCoefficient = 2.0f * (float) M_PI * CUTOFF_FREQUENCY * (1.0f / (float) sampleRate); - offset->timeout = TIMEOUT * sampleRate; - offset->timer = 0; - offset->gyroscopeOffset = FUSION_VECTOR_ZERO; + offset->filterCoefficient = 2.0f * (float)M_PI * CUTOFF_FREQUENCY * (1.0f / (float)sampleRate); + offset->timeout = TIMEOUT * sampleRate; + offset->timer = 0; + offset->gyroscopeOffset = FUSION_VECTOR_ZERO; } /** @@ -53,24 +53,26 @@ void FusionOffsetInitialise(FusionOffset *const offset, const unsigned int sampl */ FusionVector FusionOffsetUpdate(FusionOffset *const offset, FusionVector gyroscope) { - // Subtract offset from gyroscope measurement - gyroscope = FusionVectorSubtract(gyroscope, offset->gyroscopeOffset); - - // Reset timer if gyroscope not stationary - if ((fabsf(gyroscope.axis.x) > THRESHOLD) || (fabsf(gyroscope.axis.y) > THRESHOLD) || (fabsf(gyroscope.axis.z) > THRESHOLD)) { - offset->timer = 0; - return gyroscope; - } + // Subtract offset from gyroscope measurement + gyroscope = FusionVectorSubtract(gyroscope, offset->gyroscopeOffset); - // Increment timer while gyroscope stationary - if (offset->timer < offset->timeout) { - offset->timer++; - return gyroscope; - } + // Reset timer if gyroscope not stationary + if ((fabsf(gyroscope.axis.x) > THRESHOLD) || (fabsf(gyroscope.axis.y) > THRESHOLD) || + (fabsf(gyroscope.axis.z) > THRESHOLD)) { + offset->timer = 0; + return gyroscope; + } - // Adjust offset if timer has elapsed - offset->gyroscopeOffset = FusionVectorAdd(offset->gyroscopeOffset, FusionVectorMultiplyScalar(gyroscope, offset->filterCoefficient)); + // Increment timer while gyroscope stationary + if (offset->timer < offset->timeout) { + offset->timer++; return gyroscope; + } + + // Adjust offset if timer has elapsed + offset->gyroscopeOffset = FusionVectorAdd( + offset->gyroscopeOffset, FusionVectorMultiplyScalar(gyroscope, offset->filterCoefficient)); + return gyroscope; } //------------------------------------------------------------------------------ diff --git a/Fusion/FusionOffset.h b/Fusion/FusionOffset.h index 51ae4a8..7ad9454 100644 --- a/Fusion/FusionOffset.h +++ b/Fusion/FusionOffset.h @@ -21,10 +21,10 @@ * internally and must not be accessed by the application. */ typedef struct { - float filterCoefficient; - unsigned int timeout; - unsigned int timer; - FusionVector gyroscopeOffset; + float filterCoefficient; + unsigned int timeout; + unsigned int timer; + FusionVector gyroscopeOffset; } FusionOffset; //------------------------------------------------------------------------------ diff --git a/Python/Python-C-API/Ahrs.h b/Python/Python-C-API/Ahrs.h index be39b8a..a0bb409 100644 --- a/Python/Python-C-API/Ahrs.h +++ b/Python/Python-C-API/Ahrs.h @@ -5,136 +5,295 @@ #include "Flags.h" #include "Helpers.h" #include "InternalStates.h" -#include #include "Quaternion.h" #include "Settings.h" +#include #include typedef struct { - PyObject_HEAD - FusionAhrs ahrs; + PyObject_HEAD FusionAhrs ahrs; } Ahrs; static PyObject *ahrs_new(PyTypeObject *subtype, PyObject *args, PyObject *keywords) { - Ahrs *const self = (Ahrs *) subtype->tp_alloc(subtype, 0); - FusionAhrsInitialise(&self->ahrs); - return (PyObject *) self; + Ahrs *const self = (Ahrs *)subtype->tp_alloc(subtype, 0); + FusionAhrsInitialise(&self->ahrs); + return (PyObject *)self; } -static void ahrs_free(Ahrs *self) { - Py_TYPE(self)->tp_free(self); -} +static void ahrs_free(Ahrs *self) { Py_TYPE(self)->tp_free(self); } static int ahrs_set_settings(Ahrs *self, PyObject *value, void *closure) { - if (PyObject_IsInstance(value, (PyObject *) &settings_object) == false) { - static char string[64]; - snprintf(string, sizeof(string), "Value type is not %s", settings_object.tp_name); - PyErr_SetString(PyExc_TypeError, string); - return -1; - } - FusionAhrsSetSettings(&self->ahrs, &((Settings *) value)->settings); - return 0; + if (PyObject_IsInstance(value, (PyObject *)&settings_object) == false) { + static char string[64]; + snprintf(string, sizeof(string), "Value type is not %s", settings_object.tp_name); + PyErr_SetString(PyExc_TypeError, string); + return -1; + } + FusionAhrsSetSettings(&self->ahrs, &((Settings *)value)->settings); + return 0; } static PyObject *ahrs_get_quaternion(Ahrs *self) { - const FusionQuaternion quaternion = FusionAhrsGetQuaternion(&self->ahrs); - return quaternion_from(&quaternion); + const FusionQuaternion quaternion = FusionAhrsGetQuaternion(&self->ahrs); + return quaternion_from(&quaternion); } static int ahrs_set_quaternion(Ahrs *self, PyObject *value, void *closure) { - if (PyObject_IsInstance(value, (PyObject *) &quaternion_object) == false) { - static char string[64]; - snprintf(string, sizeof(string), "Value type is not %s", quaternion_object.tp_name); - PyErr_SetString(PyExc_TypeError, string); - return -1; - } - FusionAhrsSetQuaternion(&self->ahrs, ((Quaternion *) value)->quaternion); - return 0; + if (PyObject_IsInstance(value, (PyObject *)&quaternion_object) == false) { + static char string[64]; + snprintf(string, sizeof(string), "Value type is not %s", quaternion_object.tp_name); + PyErr_SetString(PyExc_TypeError, string); + return -1; + } + FusionAhrsSetQuaternion(&self->ahrs, ((Quaternion *)value)->quaternion); + return 0; } static PyObject *ahrs_get_gravity(Ahrs *self) { - FusionVector *const gravity = malloc(sizeof(FusionVector)); - *gravity = FusionAhrsGetGravity(&self->ahrs); + FusionVector *const gravity = malloc(sizeof(FusionVector)); + *gravity = FusionAhrsGetGravity(&self->ahrs); - const npy_intp dims[] = {3}; - PyObject *array = PyArray_SimpleNewFromData(1, dims, NPY_FLOAT, gravity->array); - PyArray_ENABLEFLAGS((PyArrayObject *) array, NPY_ARRAY_OWNDATA); - return array; + const npy_intp dims[] = {3}; + PyObject *array = PyArray_SimpleNewFromData(1, dims, NPY_FLOAT, gravity->array); + PyArray_ENABLEFLAGS((PyArrayObject *)array, NPY_ARRAY_OWNDATA); + return array; } static PyObject *ahrs_get_linear_acceleration(Ahrs *self) { - FusionVector *const linear_acceleration = malloc(sizeof(FusionVector)); - *linear_acceleration = FusionAhrsGetLinearAcceleration(&self->ahrs); + FusionVector *const linear_acceleration = malloc(sizeof(FusionVector)); + *linear_acceleration = FusionAhrsGetLinearAcceleration(&self->ahrs); - const npy_intp dims[] = {3}; - PyObject *array = PyArray_SimpleNewFromData(1, dims, NPY_FLOAT, linear_acceleration->array); - PyArray_ENABLEFLAGS((PyArrayObject *) array, NPY_ARRAY_OWNDATA); - return array; + const npy_intp dims[] = {3}; + PyObject *array = PyArray_SimpleNewFromData(1, dims, NPY_FLOAT, linear_acceleration->array); + PyArray_ENABLEFLAGS((PyArrayObject *)array, NPY_ARRAY_OWNDATA); + return array; } static PyObject *ahrs_get_earth_acceleration(Ahrs *self) { - FusionVector *const earth_acceleration = malloc(sizeof(FusionVector)); - *earth_acceleration = FusionAhrsGetEarthAcceleration(&self->ahrs); + FusionVector *const earth_acceleration = malloc(sizeof(FusionVector)); + *earth_acceleration = FusionAhrsGetEarthAcceleration(&self->ahrs); - const npy_intp dims[] = {3}; - PyObject *array = PyArray_SimpleNewFromData(1, dims, NPY_FLOAT, earth_acceleration->array); - PyArray_ENABLEFLAGS((PyArrayObject *) array, NPY_ARRAY_OWNDATA); - return array; + const npy_intp dims[] = {3}; + PyObject *array = PyArray_SimpleNewFromData(1, dims, NPY_FLOAT, earth_acceleration->array); + PyArray_ENABLEFLAGS((PyArrayObject *)array, NPY_ARRAY_OWNDATA); + return array; } static PyObject *ahrs_get_internal_states(Ahrs *self) { - const FusionAhrsInternalStates internal_states = FusionAhrsGetInternalStates(&self->ahrs); - return internal_states_from(&internal_states); + const FusionAhrsInternalStates internal_states = FusionAhrsGetInternalStates(&self->ahrs); + return internal_states_from(&internal_states); } static PyObject *ahrs_get_flags(Ahrs *self) { - const FusionAhrsFlags flags = FusionAhrsGetFlags(&self->ahrs); - return flags_from(&flags); + const FusionAhrsFlags flags = FusionAhrsGetFlags(&self->ahrs); + return flags_from(&flags); } static PyObject *ahrs_reset(Ahrs *self, PyObject *args) { - FusionAhrsReset(&self->ahrs); - Py_INCREF(Py_None); - return Py_None; + FusionAhrsReset(&self->ahrs); + Py_INCREF(Py_None); + return Py_None; } static PyObject *ahrs_update(Ahrs *self, PyObject *args) { - PyArrayObject *gyroscope_array; - PyArrayObject *accelerometer_array; - PyArrayObject *magnetometer_array; - float delta_time; - - const char *error = PARSE_TUPLE(args, "O!O!O!f", &PyArray_Type, &gyroscope_array, &PyArray_Type, &accelerometer_array, &PyArray_Type, &magnetometer_array, &delta_time); - if (error != NULL) { - PyErr_SetString(PyExc_TypeError, error); - return NULL; - } - - FusionVector gyroscope_vector; - FusionVector accelerometer_vector; - FusionVector magnetometer_vector; + PyArrayObject *gyroscope_array; + PyArrayObject *accelerometer_array; + PyArrayObject *magnetometer_array; + float delta_time; + + const char *error = + PARSE_TUPLE(args, "O!O!O!f", &PyArray_Type, &gyroscope_array, &PyArray_Type, + &accelerometer_array, &PyArray_Type, &magnetometer_array, &delta_time); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + + FusionVector gyroscope_vector; + FusionVector accelerometer_vector; + FusionVector magnetometer_vector; + + error = parse_array(gyroscope_vector.array, gyroscope_array, 3); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + + error = parse_array(accelerometer_vector.array, accelerometer_array, 3); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + + error = parse_array(magnetometer_vector.array, magnetometer_array, 3); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + + FusionAhrsUpdate(&self->ahrs, gyroscope_vector, accelerometer_vector, magnetometer_vector, + delta_time); + Py_INCREF(Py_None); + return Py_None; +} - error = parse_array(gyroscope_vector.array, gyroscope_array, 3); - if (error != NULL) { - PyErr_SetString(PyExc_TypeError, error); - return NULL; - } +static PyObject *ahrs_update_batch(Ahrs *self, PyObject *args) { + PyArrayObject *gyroscope_array; + PyArrayObject *accelerometer_array; + PyArrayObject *magnetometer_array; + PyArrayObject *delta_time_array; + + const char *error = PARSE_TUPLE(args, "O!O!O!O!", &PyArray_Type, &gyroscope_array, &PyArray_Type, + &accelerometer_array, &PyArray_Type, &magnetometer_array, + &PyArray_Type, &delta_time_array); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + + // Convert to float32 C-contiguous arrays (handles float64, int, F-contiguous, etc.) + gyroscope_array = (PyArrayObject *)PyArray_FromAny( + (PyObject *)gyroscope_array, PyArray_DescrFromType(NPY_FLOAT), 0, 0, + NPY_ARRAY_C_CONTIGUOUS | NPY_ARRAY_FORCECAST, NULL); + if (!gyroscope_array) + return NULL; + accelerometer_array = (PyArrayObject *)PyArray_FromAny( + (PyObject *)accelerometer_array, PyArray_DescrFromType(NPY_FLOAT), 0, 0, + NPY_ARRAY_C_CONTIGUOUS | NPY_ARRAY_FORCECAST, NULL); + if (!accelerometer_array) { + Py_DECREF(gyroscope_array); + return NULL; + } + magnetometer_array = (PyArrayObject *)PyArray_FromAny( + (PyObject *)magnetometer_array, PyArray_DescrFromType(NPY_FLOAT), 0, 0, + NPY_ARRAY_C_CONTIGUOUS | NPY_ARRAY_FORCECAST, NULL); + if (!magnetometer_array) { + Py_DECREF(gyroscope_array); + Py_DECREF(accelerometer_array); + return NULL; + } + delta_time_array = (PyArrayObject *)PyArray_FromAny( + (PyObject *)delta_time_array, PyArray_DescrFromType(NPY_FLOAT), 0, 0, + NPY_ARRAY_C_CONTIGUOUS | NPY_ARRAY_FORCECAST, NULL); + if (!delta_time_array) { + Py_DECREF(gyroscope_array); + Py_DECREF(accelerometer_array); + Py_DECREF(magnetometer_array); + return NULL; + } + + npy_intp n = PyArray_SIZE(gyroscope_array) / 3; + if ((PyArray_SIZE(accelerometer_array) / 3) != n || (PyArray_SIZE(magnetometer_array) / 3) != n || + (PyArray_SIZE(delta_time_array)) != n) { + PyErr_SetString(PyExc_ValueError, "All input arrays must have the same size."); + Py_DECREF(gyroscope_array); + Py_DECREF(accelerometer_array); + Py_DECREF(magnetometer_array); + Py_DECREF(delta_time_array); + return NULL; + } + + npy_intp quaternion_dims[2] = {n, 4}; + npy_intp dims[2] = {n, 3}; + PyObject *quaternion_array = PyArray_SimpleNew(2, quaternion_dims, NPY_FLOAT); + if (!quaternion_array) { + PyErr_NoMemory(); + Py_DECREF(gyroscope_array); + Py_DECREF(accelerometer_array); + Py_DECREF(magnetometer_array); + Py_DECREF(delta_time_array); + return NULL; + } + float *quaternion_data = (float *)PyArray_DATA((PyArrayObject *)quaternion_array); + PyObject *earth_accel_array = PyArray_SimpleNew(2, dims, NPY_FLOAT); + if (!earth_accel_array) { + PyErr_NoMemory(); + Py_DECREF(gyroscope_array); + Py_DECREF(accelerometer_array); + Py_DECREF(magnetometer_array); + Py_DECREF(delta_time_array); + Py_DECREF(quaternion_array); + return NULL; + } + float *earth_accel_data = (float *)PyArray_DATA((PyArrayObject *)earth_accel_array); + PyObject *linear_accel_array = PyArray_SimpleNew(2, dims, NPY_FLOAT); + if (!linear_accel_array) { + PyErr_NoMemory(); + Py_DECREF(gyroscope_array); + Py_DECREF(accelerometer_array); + Py_DECREF(magnetometer_array); + Py_DECREF(delta_time_array); + Py_DECREF(quaternion_array); + Py_DECREF(earth_accel_array); + return NULL; + } + float *linear_accel_data = (float *)PyArray_DATA((PyArrayObject *)linear_accel_array); + + float *gyro_data = (float *)PyArray_DATA(gyroscope_array); + float *accel_data = (float *)PyArray_DATA(accelerometer_array); + float *mag_data = (float *)PyArray_DATA(magnetometer_array); + float *dt_data = (float *)PyArray_DATA(delta_time_array); + + FusionVector gyro_vec, accel_vec, mag_vec, earth_accel, linear_accel; + FusionQuaternion quaternion; + + for (npy_intp i = 0; i < n; i++) { + for (int j = 0; j < 3; j++) { + gyro_vec.array[j] = gyro_data[i * 3 + j]; + accel_vec.array[j] = accel_data[i * 3 + j]; + mag_vec.array[j] = mag_data[i * 3 + j]; + } + + float dt = dt_data[i]; + + FusionAhrsUpdate(&self->ahrs, gyro_vec, accel_vec, mag_vec, dt); + quaternion = FusionAhrsGetQuaternion(&self->ahrs); + earth_accel = FusionAhrsGetEarthAcceleration(&self->ahrs); + linear_accel = FusionAhrsGetLinearAcceleration(&self->ahrs); + + quaternion_data[i * 4 + 0] = quaternion.element.w; + quaternion_data[i * 4 + 1] = quaternion.element.x; + quaternion_data[i * 4 + 2] = quaternion.element.y; + quaternion_data[i * 4 + 3] = quaternion.element.z; + + earth_accel_data[i * 3 + 0] = earth_accel.axis.x; + earth_accel_data[i * 3 + 1] = earth_accel.axis.y; + earth_accel_data[i * 3 + 2] = earth_accel.axis.z; + + linear_accel_data[i * 3 + 0] = linear_accel.axis.x; + linear_accel_data[i * 3 + 1] = linear_accel.axis.y; + linear_accel_data[i * 3 + 2] = linear_accel.axis.z; + } + + Py_DECREF(gyroscope_array); + Py_DECREF(accelerometer_array); + Py_DECREF(magnetometer_array); + Py_DECREF(delta_time_array); + + PyObject *result = PyDict_New(); + if (!result) { + PyErr_NoMemory(); + Py_DECREF(quaternion_array); + Py_DECREF(earth_accel_array); + Py_DECREF(linear_accel_array); + return NULL; + } - error = parse_array(accelerometer_vector.array, accelerometer_array, 3); - if (error != NULL) { - PyErr_SetString(PyExc_TypeError, error); - return NULL; - } + if (PyDict_SetItemString(result, "quaternion", quaternion_array) < 0 || + PyDict_SetItemString(result, "earth_acceleration", earth_accel_array) < 0 || + PyDict_SetItemString(result, "linear_acceleration", linear_accel_array) < 0) { + Py_DECREF(quaternion_array); + Py_DECREF(earth_accel_array); + Py_DECREF(linear_accel_array); + Py_DECREF(result); + return NULL; + } - error = parse_array(magnetometer_vector.array, magnetometer_array, 3); - if (error != NULL) { - PyErr_SetString(PyExc_TypeError, error); - return NULL; - } + Py_DECREF(quaternion_array); + Py_DECREF(earth_accel_array); + Py_DECREF(linear_accel_array); - FusionAhrsUpdate(&self->ahrs, gyroscope_vector, accelerometer_vector, magnetometer_vector, delta_time); - Py_INCREF(Py_None); - return Py_None; + return result; } static PyObject *ahrs_update_batch(Ahrs *self, PyObject *args) { @@ -267,34 +426,174 @@ static PyObject *ahrs_update_batch(Ahrs *self, PyObject *args) { static PyObject *ahrs_update_no_magnetometer(Ahrs *self, PyObject *args) { - PyArrayObject *gyroscope_array; - PyArrayObject *accelerometer_array; - float delta_time; + PyArrayObject *gyroscope_array; + PyArrayObject *accelerometer_array; + float delta_time; + + const char *error = PARSE_TUPLE(args, "O!O!f", &PyArray_Type, &gyroscope_array, &PyArray_Type, + &accelerometer_array, &delta_time); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + + FusionVector gyroscope_vector; + FusionVector accelerometer_vector; + + error = parse_array(gyroscope_vector.array, gyroscope_array, 3); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + + error = parse_array(accelerometer_vector.array, accelerometer_array, 3); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + + FusionAhrsUpdateNoMagnetometer(&self->ahrs, gyroscope_vector, accelerometer_vector, delta_time); + Py_INCREF(Py_None); + return Py_None; +} - const char *error = PARSE_TUPLE(args, "O!O!f", &PyArray_Type, &gyroscope_array, &PyArray_Type, &accelerometer_array, &delta_time); - if (error != NULL) { - PyErr_SetString(PyExc_TypeError, error); - return NULL; - } +static PyObject *ahrs_update_no_magnetometer_batch(Ahrs *self, PyObject *args) { + PyArrayObject *gyroscope_array; + PyArrayObject *accelerometer_array; + PyArrayObject *delta_time_array; + + const char *error = PARSE_TUPLE(args, "O!O!O!", &PyArray_Type, &gyroscope_array, &PyArray_Type, + &accelerometer_array, &PyArray_Type, &delta_time_array); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + // Convert to float32 C-contiguous arrays (handles float64, int, F-contiguous, etc.) + gyroscope_array = (PyArrayObject *)PyArray_FromAny( + (PyObject *)gyroscope_array, PyArray_DescrFromType(NPY_FLOAT), 0, 0, + NPY_ARRAY_C_CONTIGUOUS | NPY_ARRAY_FORCECAST, NULL); + if (!gyroscope_array) + return NULL; + accelerometer_array = (PyArrayObject *)PyArray_FromAny( + (PyObject *)accelerometer_array, PyArray_DescrFromType(NPY_FLOAT), 0, 0, + NPY_ARRAY_C_CONTIGUOUS | NPY_ARRAY_FORCECAST, NULL); + if (!accelerometer_array) { + Py_DECREF(gyroscope_array); + return NULL; + } + delta_time_array = (PyArrayObject *)PyArray_FromAny( + (PyObject *)delta_time_array, PyArray_DescrFromType(NPY_FLOAT), 0, 0, + NPY_ARRAY_C_CONTIGUOUS | NPY_ARRAY_FORCECAST, NULL); + if (!delta_time_array) { + Py_DECREF(gyroscope_array); + Py_DECREF(accelerometer_array); + return NULL; + } + + npy_intp n = PyArray_SIZE(gyroscope_array) / 3; + if ((PyArray_SIZE(accelerometer_array) / 3) != n || (PyArray_SIZE(delta_time_array)) != n) { + PyErr_SetString(PyExc_ValueError, "All input arrays must have the same size."); + Py_DECREF(gyroscope_array); + Py_DECREF(accelerometer_array); + Py_DECREF(delta_time_array); + return NULL; + } + + npy_intp quaternion_dims[2] = {n, 4}; + npy_intp dims[2] = {n, 3}; + PyObject *quaternion_array = PyArray_SimpleNew(2, quaternion_dims, NPY_FLOAT); + if (!quaternion_array) { + PyErr_NoMemory(); + Py_DECREF(gyroscope_array); + Py_DECREF(accelerometer_array); + Py_DECREF(delta_time_array); + return NULL; + } + float *quaternion_data = (float *)PyArray_DATA((PyArrayObject *)quaternion_array); + PyObject *earth_accel_array = PyArray_SimpleNew(2, dims, NPY_FLOAT); + if (!earth_accel_array) { + PyErr_NoMemory(); + Py_DECREF(gyroscope_array); + Py_DECREF(accelerometer_array); + Py_DECREF(delta_time_array); + Py_DECREF(quaternion_array); + return NULL; + } + float *earth_accel_data = (float *)PyArray_DATA((PyArrayObject *)earth_accel_array); + PyObject *linear_accel_array = PyArray_SimpleNew(2, dims, NPY_FLOAT); + if (!linear_accel_array) { + PyErr_NoMemory(); + Py_DECREF(gyroscope_array); + Py_DECREF(accelerometer_array); + Py_DECREF(delta_time_array); + Py_DECREF(quaternion_array); + Py_DECREF(earth_accel_array); + return NULL; + } + float *linear_accel_data = (float *)PyArray_DATA((PyArrayObject *)linear_accel_array); - FusionVector gyroscope_vector; - FusionVector accelerometer_vector; + float *gyro_data = (float *)PyArray_DATA(gyroscope_array); + float *accel_data = (float *)PyArray_DATA(accelerometer_array); + float *dt_data = (float *)PyArray_DATA(delta_time_array); - error = parse_array(gyroscope_vector.array, gyroscope_array, 3); - if (error != NULL) { - PyErr_SetString(PyExc_TypeError, error); - return NULL; - } + FusionVector gyro_vec, accel_vec, earth_accel, linear_accel; + FusionQuaternion quaternion; - error = parse_array(accelerometer_vector.array, accelerometer_array, 3); - if (error != NULL) { - PyErr_SetString(PyExc_TypeError, error); - return NULL; + for (npy_intp i = 0; i < n; i++) { + for (int j = 0; j < 3; j++) { + gyro_vec.array[j] = gyro_data[i * 3 + j]; + accel_vec.array[j] = accel_data[i * 3 + j]; } - FusionAhrsUpdateNoMagnetometer(&self->ahrs, gyroscope_vector, accelerometer_vector, delta_time); - Py_INCREF(Py_None); - return Py_None; + float dt = dt_data[i]; + + FusionAhrsUpdateNoMagnetometer(&self->ahrs, gyro_vec, accel_vec, dt); + quaternion = FusionAhrsGetQuaternion(&self->ahrs); + earth_accel = FusionAhrsGetEarthAcceleration(&self->ahrs); + linear_accel = FusionAhrsGetLinearAcceleration(&self->ahrs); + + quaternion_data[i * 4 + 0] = quaternion.element.w; + quaternion_data[i * 4 + 1] = quaternion.element.x; + quaternion_data[i * 4 + 2] = quaternion.element.y; + quaternion_data[i * 4 + 3] = quaternion.element.z; + + earth_accel_data[i * 3 + 0] = earth_accel.axis.x; + earth_accel_data[i * 3 + 1] = earth_accel.axis.y; + earth_accel_data[i * 3 + 2] = earth_accel.axis.z; + + linear_accel_data[i * 3 + 0] = linear_accel.axis.x; + linear_accel_data[i * 3 + 1] = linear_accel.axis.y; + linear_accel_data[i * 3 + 2] = linear_accel.axis.z; + } + + Py_DECREF(gyroscope_array); + Py_DECREF(accelerometer_array); + Py_DECREF(delta_time_array); + + PyObject *result = PyDict_New(); + if (!result) { + PyErr_NoMemory(); + Py_DECREF(quaternion_array); + Py_DECREF(earth_accel_array); + Py_DECREF(linear_accel_array); + return NULL; + } + + if (PyDict_SetItemString(result, "quaternion", quaternion_array) < 0 || + PyDict_SetItemString(result, "earth_acceleration", earth_accel_array) < 0 || + PyDict_SetItemString(result, "linear_acceleration", linear_accel_array) < 0) { + Py_DECREF(quaternion_array); + Py_DECREF(earth_accel_array); + Py_DECREF(linear_accel_array); + Py_DECREF(result); + return NULL; + } + + Py_DECREF(quaternion_array); + Py_DECREF(earth_accel_array); + Py_DECREF(linear_accel_array); + + return result; } @@ -418,78 +717,81 @@ static PyObject *ahrs_update_no_magnetometer_batch(Ahrs *self, PyObject *args) { static PyObject *ahrs_update_external_heading(Ahrs *self, PyObject *args) { - PyArrayObject *gyroscope_array; - PyArrayObject *accelerometer_array; - float heading; - float delta_time; - - const char *error = PARSE_TUPLE(args, "O!O!ff", &PyArray_Type, &gyroscope_array, &PyArray_Type, &accelerometer_array, &heading, &delta_time); - if (error != NULL) { - PyErr_SetString(PyExc_TypeError, error); - return NULL; - } - - FusionVector gyroscope_vector; - FusionVector accelerometer_vector; - - error = parse_array(gyroscope_vector.array, gyroscope_array, 3); - if (error != NULL) { - PyErr_SetString(PyExc_TypeError, error); - return NULL; - } - - error = parse_array(accelerometer_vector.array, accelerometer_array, 3); - if (error != NULL) { - PyErr_SetString(PyExc_TypeError, error); - return NULL; - } - - FusionAhrsUpdateExternalHeading(&self->ahrs, gyroscope_vector, accelerometer_vector, heading, delta_time); - Py_INCREF(Py_None); - return Py_None; + PyArrayObject *gyroscope_array; + PyArrayObject *accelerometer_array; + float heading; + float delta_time; + + const char *error = PARSE_TUPLE(args, "O!O!ff", &PyArray_Type, &gyroscope_array, &PyArray_Type, + &accelerometer_array, &heading, &delta_time); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + + FusionVector gyroscope_vector; + FusionVector accelerometer_vector; + + error = parse_array(gyroscope_vector.array, gyroscope_array, 3); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + + error = parse_array(accelerometer_vector.array, accelerometer_array, 3); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + + FusionAhrsUpdateExternalHeading(&self->ahrs, gyroscope_vector, accelerometer_vector, heading, + delta_time); + Py_INCREF(Py_None); + return Py_None; } static int ahrs_set_heading(Ahrs *self, PyObject *value, void *closure) { - const float heading = (float) PyFloat_AsDouble(value); + const float heading = (float)PyFloat_AsDouble(value); - if (PyErr_Occurred()) { - return -1; - } + if (PyErr_Occurred()) { + return -1; + } - FusionAhrsSetHeading(&self->ahrs, heading);; - return 0; + FusionAhrsSetHeading(&self->ahrs, heading); + ; + return 0; } static PyGetSetDef ahrs_get_set[] = { - {"settings", NULL, (setter) ahrs_set_settings, "", NULL}, - {"quaternion", (getter) ahrs_get_quaternion, (setter) ahrs_set_quaternion, "", NULL}, - {"gravity", (getter) ahrs_get_gravity, NULL, "", NULL}, - {"linear_acceleration", (getter) ahrs_get_linear_acceleration, NULL, "", NULL}, - {"earth_acceleration", (getter) ahrs_get_earth_acceleration, NULL, "", NULL}, - {"internal_states", (getter) ahrs_get_internal_states, NULL, "", NULL}, - {"flags", (getter) ahrs_get_flags, NULL, "", NULL}, - {"heading", NULL, (setter) ahrs_set_heading, "", NULL}, - {NULL} /* sentinel */ + {"settings", NULL, (setter)ahrs_set_settings, "", NULL}, + {"quaternion", (getter)ahrs_get_quaternion, (setter)ahrs_set_quaternion, "", NULL}, + {"gravity", (getter)ahrs_get_gravity, NULL, "", NULL}, + {"linear_acceleration", (getter)ahrs_get_linear_acceleration, NULL, "", NULL}, + {"earth_acceleration", (getter)ahrs_get_earth_acceleration, NULL, "", NULL}, + {"internal_states", (getter)ahrs_get_internal_states, NULL, "", NULL}, + {"flags", (getter)ahrs_get_flags, NULL, "", NULL}, + {"heading", NULL, (setter)ahrs_set_heading, "", NULL}, + {NULL} /* sentinel */ }; static PyMethodDef ahrs_methods[] = { - {"reset", (PyCFunction) ahrs_reset, METH_NOARGS, ""}, - {"update", (PyCFunction) ahrs_update, METH_VARARGS, ""}, - {"update_no_magnetometer", (PyCFunction) ahrs_update_no_magnetometer, METH_VARARGS, ""}, - {"update_external_heading", (PyCFunction) ahrs_update_external_heading, METH_VARARGS, ""}, - {"update_batch", (PyCFunction) ahrs_update_batch, METH_VARARGS, ""}, - {"update_no_magnetometer_batch", (PyCFunction) ahrs_update_no_magnetometer_batch, METH_VARARGS, ""}, - {NULL} /* sentinel */ + {"reset", (PyCFunction)ahrs_reset, METH_NOARGS, ""}, + {"update", (PyCFunction)ahrs_update, METH_VARARGS, ""}, + {"update_no_magnetometer", (PyCFunction)ahrs_update_no_magnetometer, METH_VARARGS, ""}, + {"update_external_heading", (PyCFunction)ahrs_update_external_heading, METH_VARARGS, ""}, + {"update_batch", (PyCFunction)ahrs_update_batch, METH_VARARGS, ""}, + {"update_no_magnetometer_batch", (PyCFunction)ahrs_update_no_magnetometer_batch, METH_VARARGS, + ""}, + {NULL} /* sentinel */ }; static PyTypeObject ahrs_object = { - PyVarObject_HEAD_INIT(NULL, 0) - .tp_name = "imufusion.Ahrs", - .tp_basicsize = sizeof(Ahrs), - .tp_dealloc = (destructor) ahrs_free, - .tp_new = ahrs_new, - .tp_getset = ahrs_get_set, - .tp_methods = ahrs_methods, + PyVarObject_HEAD_INIT(NULL, 0).tp_name = "imufusion.Ahrs", + .tp_basicsize = sizeof(Ahrs), + .tp_dealloc = (destructor)ahrs_free, + .tp_new = ahrs_new, + .tp_getset = ahrs_get_set, + .tp_methods = ahrs_methods, }; #endif diff --git a/Python/Python-C-API/Axes.h b/Python/Python-C-API/Axes.h index be4ac31..eba3878 100644 --- a/Python/Python-C-API/Axes.h +++ b/Python/Python-C-API/Axes.h @@ -3,40 +3,39 @@ #include "../../Fusion/Fusion.h" #include "Helpers.h" -#include #include +#include #include static PyObject *axes_swap(PyObject *self, PyObject *args) { - PyArrayObject *input_array; - int alignment; - - const char *error = PARSE_TUPLE(args, "O!i", &PyArray_Type, &input_array, &alignment); - if (error != NULL) { - PyErr_SetString(PyExc_TypeError, error); - return NULL; - } - - FusionVector input_vector; - - error = parse_array(input_vector.array, input_array, 3); - if (error != NULL) { - PyErr_SetString(PyExc_TypeError, error); - return NULL; - } - - FusionVector *const output_vector = malloc(sizeof(FusionVector)); - *output_vector = FusionAxesSwap(input_vector, (FusionAxesAlignment) alignment); - - const npy_intp dims[] = {3}; - PyObject *output_array = PyArray_SimpleNewFromData(1, dims, NPY_FLOAT, output_vector->array); - PyArray_ENABLEFLAGS((PyArrayObject *) output_array, NPY_ARRAY_OWNDATA); - return output_array; + PyArrayObject *input_array; + int alignment; + + const char *error = PARSE_TUPLE(args, "O!i", &PyArray_Type, &input_array, &alignment); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + + FusionVector input_vector; + + error = parse_array(input_vector.array, input_array, 3); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + + FusionVector *const output_vector = malloc(sizeof(FusionVector)); + *output_vector = FusionAxesSwap(input_vector, (FusionAxesAlignment)alignment); + + const npy_intp dims[] = {3}; + PyObject *output_array = PyArray_SimpleNewFromData(1, dims, NPY_FLOAT, output_vector->array); + PyArray_ENABLEFLAGS((PyArrayObject *)output_array, NPY_ARRAY_OWNDATA); + return output_array; } static PyMethodDef axes_methods[] = { - {"axes_swap", (PyCFunction) axes_swap, METH_VARARGS, ""}, - {NULL} /* sentinel */ + {"axes_swap", (PyCFunction)axes_swap, METH_VARARGS, ""}, {NULL} /* sentinel */ }; #endif diff --git a/Python/Python-C-API/Compass.h b/Python/Python-C-API/Compass.h index 9b9ad92..f4e2f6d 100644 --- a/Python/Python-C-API/Compass.h +++ b/Python/Python-C-API/Compass.h @@ -3,42 +3,44 @@ #include "../../Fusion/Fusion.h" #include "Helpers.h" -#include #include +#include #include static PyObject *compass_calculate_heading(PyObject *self, PyObject *args) { - FusionConvention convention; - PyArrayObject *accelerometer_array; - PyArrayObject *magnetometer_array; - - const char *error = PARSE_TUPLE(args, "iO!O!", &convention, &PyArray_Type, &accelerometer_array, &PyArray_Type, &magnetometer_array); - if (error != NULL) { - PyErr_SetString(PyExc_TypeError, error); - return NULL; - } - - FusionVector accelerometer_vector; - FusionVector magnetometer_vector; - - error = parse_array(accelerometer_vector.array, accelerometer_array, 3); - if (error != NULL) { - PyErr_SetString(PyExc_TypeError, error); - return NULL; - } - - error = parse_array(magnetometer_vector.array, magnetometer_array, 3); - if (error != NULL) { - PyErr_SetString(PyExc_TypeError, error); - return NULL; - } - - return Py_BuildValue("f", FusionCompassCalculateHeading(convention, accelerometer_vector, magnetometer_vector)); + FusionConvention convention; + PyArrayObject *accelerometer_array; + PyArrayObject *magnetometer_array; + + const char *error = PARSE_TUPLE(args, "iO!O!", &convention, &PyArray_Type, &accelerometer_array, + &PyArray_Type, &magnetometer_array); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + + FusionVector accelerometer_vector; + FusionVector magnetometer_vector; + + error = parse_array(accelerometer_vector.array, accelerometer_array, 3); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + + error = parse_array(magnetometer_vector.array, magnetometer_array, 3); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + + return Py_BuildValue( + "f", FusionCompassCalculateHeading(convention, accelerometer_vector, magnetometer_vector)); } static PyMethodDef compass_methods[] = { - {"compass_calculate_heading", (PyCFunction) compass_calculate_heading, METH_VARARGS, ""}, - {NULL} /* sentinel */ + {"compass_calculate_heading", (PyCFunction)compass_calculate_heading, METH_VARARGS, ""}, + {NULL} /* sentinel */ }; #endif diff --git a/Python/Python-C-API/Flags.h b/Python/Python-C-API/Flags.h index d026202..d589f2f 100644 --- a/Python/Python-C-API/Flags.h +++ b/Python/Python-C-API/Flags.h @@ -6,50 +6,46 @@ #include typedef struct { - PyObject_HEAD - FusionAhrsFlags flags; + PyObject_HEAD FusionAhrsFlags flags; } Flags; -static void flags_free(Flags *self) { - Py_TYPE(self)->tp_free(self); -} +static void flags_free(Flags *self) { Py_TYPE(self)->tp_free(self); } static PyObject *flags_get_initialising(Flags *self) { - return build_bool(self->flags.initialising); + return build_bool(self->flags.initialising); } static PyObject *flags_get_angular_rate_recovery(Flags *self) { - return build_bool(self->flags.angularRateRecovery); + return build_bool(self->flags.angularRateRecovery); } static PyObject *flags_get_acceleration_recovery(Flags *self) { - return build_bool(self->flags.accelerationRecovery); + return build_bool(self->flags.accelerationRecovery); } static PyObject *flags_get_magnetic_recovery(Flags *self) { - return build_bool(self->flags.magneticRecovery); + return build_bool(self->flags.magneticRecovery); } static PyGetSetDef flags_get_set[] = { - {"initialising", (getter) flags_get_initialising, NULL, "", NULL}, - {"angular_rate_recovery", (getter) flags_get_angular_rate_recovery, NULL, "", NULL}, - {"acceleration_recovery", (getter) flags_get_acceleration_recovery, NULL, "", NULL}, - {"magnetic_recovery", (getter) flags_get_magnetic_recovery, NULL, "", NULL}, - {NULL} /* sentinel */ + {"initialising", (getter)flags_get_initialising, NULL, "", NULL}, + {"angular_rate_recovery", (getter)flags_get_angular_rate_recovery, NULL, "", NULL}, + {"acceleration_recovery", (getter)flags_get_acceleration_recovery, NULL, "", NULL}, + {"magnetic_recovery", (getter)flags_get_magnetic_recovery, NULL, "", NULL}, + {NULL} /* sentinel */ }; static PyTypeObject flags_object = { - PyVarObject_HEAD_INIT(NULL, 0) - .tp_name = "imufusion.Flags", - .tp_basicsize = sizeof(Flags), - .tp_dealloc = (destructor) flags_free, - .tp_getset = flags_get_set, + PyVarObject_HEAD_INIT(NULL, 0).tp_name = "imufusion.Flags", + .tp_basicsize = sizeof(Flags), + .tp_dealloc = (destructor)flags_free, + .tp_getset = flags_get_set, }; static PyObject *flags_from(const FusionAhrsFlags *const flags) { - Flags *const self = (Flags *) flags_object.tp_alloc(&flags_object, 0); - self->flags = *flags; - return (PyObject *) self; + Flags *const self = (Flags *)flags_object.tp_alloc(&flags_object, 0); + self->flags = *flags; + return (PyObject *)self; } #endif diff --git a/Python/Python-C-API/Helpers.h b/Python/Python-C-API/Helpers.h index e7249e0..179db81 100644 --- a/Python/Python-C-API/Helpers.h +++ b/Python/Python-C-API/Helpers.h @@ -1,83 +1,87 @@ #ifndef HELPERS_H #define HELPERS_H -#include #include +#include #include #include -#define PARSE_TUPLE(args, format, ...) (PyArg_ParseTuple(args, format, __VA_ARGS__) == 0 ? create_parse_tuple_error_string(format) : NULL) +#define PARSE_TUPLE(args, format, ...) \ + (PyArg_ParseTuple(args, format, __VA_ARGS__) == 0 ? create_parse_tuple_error_string(format) \ + : NULL) -static void concatenate(char *const destination, const size_t destination_size, const char *const source) { - strncat(destination, source, destination_size - strlen(destination) - 1); +static void concatenate(char *const destination, const size_t destination_size, + const char *const source) { + strncat(destination, source, destination_size - strlen(destination) - 1); } static char *const create_parse_tuple_error_string(const char *format) { - static char string[256] = "Arguments are not ("; - - while (*format != '\0') { - switch (*format) { - case 'I': - concatenate(string, sizeof(string), "unsigned int"); - break; - case 'O': - concatenate(string, sizeof(string), "numpy.array"); - break; - case 'f': - concatenate(string, sizeof(string), "float"); - break; - case 'l': - concatenate(string, sizeof(string), "long int"); - break; - default: - concatenate(string, sizeof(string), "unknown type"); - break; - } - - do { - format++; - } while (*format == '!'); - - if (*format != '\0') { - concatenate(string, sizeof(string), ", "); - } else { - concatenate(string, sizeof(string), ")"); - } + static char string[256] = "Arguments are not ("; + + while (*format != '\0') { + switch (*format) { + case 'I': + concatenate(string, sizeof(string), "unsigned int"); + break; + case 'O': + concatenate(string, sizeof(string), "numpy.array"); + break; + case 'f': + concatenate(string, sizeof(string), "float"); + break; + case 'l': + concatenate(string, sizeof(string), "long int"); + break; + default: + concatenate(string, sizeof(string), "unknown type"); + break; } - return string; -} -static char *const parse_array(float *const destination, const PyArrayObject *const array, const int size) { - if (PyArray_NDIM(array) != 1) { - return "Array dimensions is not 1"; - } + do { + format++; + } while (*format == '!'); - if (PyArray_SIZE(array) != size) { - static char string[32]; - snprintf(string, sizeof(string), "Array size is not %u", size); - return string; + if (*format != '\0') { + concatenate(string, sizeof(string), ", "); + } else { + concatenate(string, sizeof(string), ")"); } + } + return string; +} + +static char *const parse_array(float *const destination, const PyArrayObject *const array, + const int size) { + if (PyArray_NDIM(array) != 1) { + return "Array dimensions is not 1"; + } - int offset = 0; + if (PyArray_SIZE(array) != size) { + static char string[32]; + snprintf(string, sizeof(string), "Array size is not %u", size); + return string; + } - for (int index = 0; index < size; index++) { - PyObject *object = PyArray_GETITEM(array, PyArray_BYTES(array) + offset); + int offset = 0; - destination[index] = (float) PyFloat_AsDouble(object); - Py_DECREF(object); + for (int index = 0; index < size; index++) { + PyObject *object = PyArray_GETITEM(array, PyArray_BYTES(array) + offset); - if (PyErr_Occurred()) { - return "Invalid array element type"; - } + destination[index] = (float)PyFloat_AsDouble(object); + Py_DECREF(object); - offset += (int) PyArray_STRIDE(array, 0); + if (PyErr_Occurred()) { + return "Invalid array element type"; } - return NULL; + offset += (int)PyArray_STRIDE(array, 0); + } + + return NULL; } static PyObject *const build_bool(const bool value) { - return Py_BuildValue("O", value ? Py_True : Py_False); + return Py_BuildValue("O", value ? Py_True : Py_False); } #endif diff --git a/Python/Python-C-API/InternalStates.h b/Python/Python-C-API/InternalStates.h index a028323..7b21122 100644 --- a/Python/Python-C-API/InternalStates.h +++ b/Python/Python-C-API/InternalStates.h @@ -6,60 +6,59 @@ #include typedef struct { - PyObject_HEAD - FusionAhrsInternalStates internal_states; + PyObject_HEAD FusionAhrsInternalStates internal_states; } InternalStates; -static void internal_states_free(InternalStates *self) { - Py_TYPE(self)->tp_free(self); -} +static void internal_states_free(InternalStates *self) { Py_TYPE(self)->tp_free(self); } static PyObject *internal_states_get_acceleration_error(InternalStates *self) { - return Py_BuildValue("f", self->internal_states.accelerationError); + return Py_BuildValue("f", self->internal_states.accelerationError); } static PyObject *internal_states_get_accelerometer_ignored(InternalStates *self) { - return build_bool(self->internal_states.accelerometerIgnored); + return build_bool(self->internal_states.accelerometerIgnored); } static PyObject *internal_states_get_acceleration_recovery_trigger(InternalStates *self) { - return Py_BuildValue("f", self->internal_states.accelerationRecoveryTrigger); + return Py_BuildValue("f", self->internal_states.accelerationRecoveryTrigger); } static PyObject *internal_states_get_magnetic_error(InternalStates *self) { - return Py_BuildValue("f", self->internal_states.magneticError); + return Py_BuildValue("f", self->internal_states.magneticError); } static PyObject *internal_states_get_magnetometer_ignored(InternalStates *self) { - return build_bool(self->internal_states.magnetometerIgnored); + return build_bool(self->internal_states.magnetometerIgnored); } static PyObject *internal_states_get_magnetic_recovery_trigger(InternalStates *self) { - return Py_BuildValue("f", self->internal_states.magneticRecoveryTrigger); + return Py_BuildValue("f", self->internal_states.magneticRecoveryTrigger); } static PyGetSetDef internal_states_get_set[] = { - {"acceleration_error", (getter) internal_states_get_acceleration_error, NULL, "", NULL}, - {"accelerometer_ignored", (getter) internal_states_get_accelerometer_ignored, NULL, "", NULL}, - {"acceleration_recovery_trigger", (getter) internal_states_get_acceleration_recovery_trigger, NULL, "", NULL}, - {"magnetic_error", (getter) internal_states_get_magnetic_error, NULL, "", NULL}, - {"magnetometer_ignored", (getter) internal_states_get_magnetometer_ignored, NULL, "", NULL}, - {"magnetic_recovery_trigger", (getter) internal_states_get_magnetic_recovery_trigger, NULL, "", NULL}, - {NULL} /* sentinel */ + {"acceleration_error", (getter)internal_states_get_acceleration_error, NULL, "", NULL}, + {"accelerometer_ignored", (getter)internal_states_get_accelerometer_ignored, NULL, "", NULL}, + {"acceleration_recovery_trigger", (getter)internal_states_get_acceleration_recovery_trigger, + NULL, "", NULL}, + {"magnetic_error", (getter)internal_states_get_magnetic_error, NULL, "", NULL}, + {"magnetometer_ignored", (getter)internal_states_get_magnetometer_ignored, NULL, "", NULL}, + {"magnetic_recovery_trigger", (getter)internal_states_get_magnetic_recovery_trigger, NULL, "", + NULL}, + {NULL} /* sentinel */ }; static PyTypeObject internal_states_object = { - PyVarObject_HEAD_INIT(NULL, 0) - .tp_name = "imufusion.InternalStates", - .tp_basicsize = sizeof(InternalStates), - .tp_dealloc = (destructor) internal_states_free, - .tp_getset = internal_states_get_set, + PyVarObject_HEAD_INIT(NULL, 0).tp_name = "imufusion.InternalStates", + .tp_basicsize = sizeof(InternalStates), + .tp_dealloc = (destructor)internal_states_free, + .tp_getset = internal_states_get_set, }; static PyObject *internal_states_from(const FusionAhrsInternalStates *const internal_states) { - InternalStates *const self = (InternalStates *) internal_states_object.tp_alloc(&internal_states_object, 0); - self->internal_states = *internal_states; - return (PyObject *) self; + InternalStates *const self = + (InternalStates *)internal_states_object.tp_alloc(&internal_states_object, 0); + self->internal_states = *internal_states; + return (PyObject *)self; } #endif diff --git a/Python/Python-C-API/Offset.h b/Python/Python-C-API/Offset.h index 37fde03..7d03d20 100644 --- a/Python/Python-C-API/Offset.h +++ b/Python/Python-C-API/Offset.h @@ -3,57 +3,113 @@ #include "../../Fusion/Fusion.h" #include "Helpers.h" -#include #include +#include #include typedef struct { - PyObject_HEAD - FusionOffset offset; + PyObject_HEAD FusionOffset offset; } Offset; static PyObject *offset_new(PyTypeObject *subtype, PyObject *args, PyObject *keywords) { - unsigned int sample_rate; + unsigned int sample_rate; - const char *const error = PARSE_TUPLE(args, "I", &sample_rate); - if (error != NULL) { - PyErr_SetString(PyExc_TypeError, error); - return NULL; - } + const char *const error = PARSE_TUPLE(args, "I", &sample_rate); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } - Offset *const self = (Offset *) subtype->tp_alloc(subtype, 0); - FusionOffsetInitialise(&self->offset, sample_rate); - return (PyObject *) self; + Offset *const self = (Offset *)subtype->tp_alloc(subtype, 0); + FusionOffsetInitialise(&self->offset, sample_rate); + return (PyObject *)self; } -static void offset_free(Offset *self) { - Py_TYPE(self)->tp_free(self); -} +static void offset_free(Offset *self) { Py_TYPE(self)->tp_free(self); } static PyObject *offset_update(Offset *self, PyObject *args) { - PyArrayObject *input_array; + PyArrayObject *input_array; - const char *error = PARSE_TUPLE(args, "O!", &PyArray_Type, &input_array); - if (error != NULL) { - PyErr_SetString(PyExc_TypeError, error); - return NULL; + const char *error = PARSE_TUPLE(args, "O!", &PyArray_Type, &input_array); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + + FusionVector input_vector; + + error = parse_array(input_vector.array, input_array, 3); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + + FusionVector *const output_vector = malloc(sizeof(FusionVector)); + *output_vector = FusionOffsetUpdate(&self->offset, input_vector); + + const npy_intp dims[] = {3}; + PyObject *output_array = PyArray_SimpleNewFromData(1, dims, NPY_FLOAT, output_vector->array); + PyArray_ENABLEFLAGS((PyArrayObject *)output_array, NPY_ARRAY_OWNDATA); + return output_array; +} + +static PyObject *offset_update_batch(Offset *self, PyObject *args) { + PyArrayObject *input_array; + + const char *error = PARSE_TUPLE(args, "O!", &PyArray_Type, &input_array); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + + // Convert to float32 C-contiguous array (handles float64, int, F-contiguous, etc.) + input_array = + (PyArrayObject *)PyArray_FromAny((PyObject *)input_array, PyArray_DescrFromType(NPY_FLOAT), 2, + 2, NPY_ARRAY_C_CONTIGUOUS | NPY_ARRAY_FORCECAST, NULL); + if (!input_array) + return NULL; + + npy_intp *dims = PyArray_DIMS(input_array); + if (dims[1] != 3) { + PyErr_SetString(PyExc_ValueError, "Input array's second dimension must be 3."); + Py_DECREF(input_array); + return NULL; + } + npy_intp n = dims[0]; + + FusionVector input_vector, output_vector; + + const npy_intp out_dims[2] = {n, 3}; + PyObject *output_array = PyArray_SimpleNew(2, out_dims, NPY_FLOAT); + if (!output_array) { + PyErr_NoMemory(); + Py_DECREF(input_array); + return NULL; + } + + float *output_data = (float *)PyArray_DATA((PyArrayObject *)output_array); + + float *input_data = (float *)PyArray_DATA(input_array); + + for (npy_intp i = 0; i < n; i++) { + for (int j = 0; j < 3; j++) { + input_vector.array[j] = input_data[i * 3 + j]; } - FusionVector input_vector; + // FusionVector *const output_vector = malloc(sizeof(FusionVector)); + // *output_vector = FusionOffsetUpdate(&self->offset, input_vector); + output_vector = FusionOffsetUpdate(&self->offset, input_vector); - error = parse_array(input_vector.array, input_array, 3); - if (error != NULL) { - PyErr_SetString(PyExc_TypeError, error); - return NULL; + for (int j = 0; j < 3; j++) { + output_data[i * 3 + j] = output_vector.array[j]; } + // free(output_vector); + } - FusionVector *const output_vector = malloc(sizeof(FusionVector)); - *output_vector = FusionOffsetUpdate(&self->offset, input_vector); + Py_DECREF(input_array); - const npy_intp dims[] = {3}; - PyObject *output_array = PyArray_SimpleNewFromData(1, dims, NPY_FLOAT, output_vector->array); - PyArray_ENABLEFLAGS((PyArrayObject *) output_array, NPY_ARRAY_OWNDATA); - return output_array; + PyArray_ENABLEFLAGS((PyArrayObject *)output_array, NPY_ARRAY_OWNDATA); + return output_array; } @@ -111,18 +167,17 @@ static PyObject *offset_update_batch(Offset *self, PyObject *args) { } static PyMethodDef offset_methods[] = { - {"update", (PyCFunction) offset_update, METH_VARARGS, ""}, - {"update_batch", (PyCFunction) offset_update_batch, METH_VARARGS, ""}, - {NULL} /* sentinel */ + {"update", (PyCFunction)offset_update, METH_VARARGS, ""}, + {"update_batch", (PyCFunction)offset_update_batch, METH_VARARGS, ""}, + {NULL} /* sentinel */ }; static PyTypeObject offset_object = { - PyVarObject_HEAD_INIT(NULL, 0) - .tp_name = "imufusion.Offset", - .tp_basicsize = sizeof(Offset), - .tp_dealloc = (destructor) offset_free, - .tp_new = offset_new, - .tp_methods = offset_methods, + PyVarObject_HEAD_INIT(NULL, 0).tp_name = "imufusion.Offset", + .tp_basicsize = sizeof(Offset), + .tp_dealloc = (destructor)offset_free, + .tp_new = offset_new, + .tp_methods = offset_methods, }; #endif diff --git a/Python/Python-C-API/Quaternion.h b/Python/Python-C-API/Quaternion.h index 78bfea2..b97af32 100644 --- a/Python/Python-C-API/Quaternion.h +++ b/Python/Python-C-API/Quaternion.h @@ -3,114 +3,110 @@ #include "../../Fusion/Fusion.h" #include "Helpers.h" -#include #include +#include #include typedef struct { - PyObject_HEAD - FusionQuaternion quaternion; + PyObject_HEAD FusionQuaternion quaternion; } Quaternion; static PyObject *quaternion_new(PyTypeObject *subtype, PyObject *args, PyObject *keywords) { - PyArrayObject *array; + PyArrayObject *array; - const char *error = PARSE_TUPLE(args, "O!", &PyArray_Type, &array); - if (error != NULL) { - PyErr_SetString(PyExc_TypeError, error); - return NULL; - } + const char *error = PARSE_TUPLE(args, "O!", &PyArray_Type, &array); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } - FusionQuaternion quaternion; + FusionQuaternion quaternion; - error = parse_array(quaternion.array, array, 4); - if (error != NULL) { - PyErr_SetString(PyExc_TypeError, error); - return NULL; - } + error = parse_array(quaternion.array, array, 4); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } - Quaternion *const self = (Quaternion *) subtype->tp_alloc(subtype, 0); - self->quaternion = quaternion; - return (PyObject *) self; + Quaternion *const self = (Quaternion *)subtype->tp_alloc(subtype, 0); + self->quaternion = quaternion; + return (PyObject *)self; } -static void quaternion_free(Quaternion *self) { - Py_TYPE(self)->tp_free(self); -} +static void quaternion_free(Quaternion *self) { Py_TYPE(self)->tp_free(self); } static PyObject *quaternion_get_wxyz(Quaternion *self) { - const npy_intp dims[] = {4}; - PyObject* array = PyArray_SimpleNewFromData(1, dims, NPY_FLOAT, self->quaternion.array); - Py_INCREF(self); - PyArray_SetBaseObject((PyArrayObject *) array, (PyObject *) self); - return array; + const npy_intp dims[] = {4}; + PyObject *array = PyArray_SimpleNewFromData(1, dims, NPY_FLOAT, self->quaternion.array); + Py_INCREF(self); + PyArray_SetBaseObject((PyArrayObject *)array, (PyObject *)self); + return array; } static PyObject *quaternion_get_w(Quaternion *self) { - return Py_BuildValue("f", self->quaternion.element.w); + return Py_BuildValue("f", self->quaternion.element.w); } static PyObject *quaternion_get_x(Quaternion *self) { - return Py_BuildValue("f", self->quaternion.element.x); + return Py_BuildValue("f", self->quaternion.element.x); } static PyObject *quaternion_get_y(Quaternion *self) { - return Py_BuildValue("f", self->quaternion.element.y); + return Py_BuildValue("f", self->quaternion.element.y); } static PyObject *quaternion_get_z(Quaternion *self) { - return Py_BuildValue("f", self->quaternion.element.z); + return Py_BuildValue("f", self->quaternion.element.z); } static PyObject *quaternion_to_matrix(Quaternion *self, PyObject *args) { - FusionMatrix *const matrix = malloc(sizeof(FusionMatrix)); - *matrix = FusionQuaternionToMatrix(self->quaternion); + FusionMatrix *const matrix = malloc(sizeof(FusionMatrix)); + *matrix = FusionQuaternionToMatrix(self->quaternion); - const npy_intp dims[] = {3, 3}; - PyObject *array = PyArray_SimpleNewFromData(2, dims, NPY_FLOAT, matrix->array); - PyArray_ENABLEFLAGS((PyArrayObject *) array, NPY_ARRAY_OWNDATA); - return array; + const npy_intp dims[] = {3, 3}; + PyObject *array = PyArray_SimpleNewFromData(2, dims, NPY_FLOAT, matrix->array); + PyArray_ENABLEFLAGS((PyArrayObject *)array, NPY_ARRAY_OWNDATA); + return array; } static PyObject *quaternion_to_euler(Quaternion *self, PyObject *args) { - FusionEuler *const euler = malloc(sizeof(FusionEuler)); - *euler = FusionQuaternionToEuler(self->quaternion); + FusionEuler *const euler = malloc(sizeof(FusionEuler)); + *euler = FusionQuaternionToEuler(self->quaternion); - const npy_intp dims[] = {3}; - PyObject *array = PyArray_SimpleNewFromData(1, dims, NPY_FLOAT, euler->array); - PyArray_ENABLEFLAGS((PyArrayObject *) array, NPY_ARRAY_OWNDATA); - return array; + const npy_intp dims[] = {3}; + PyObject *array = PyArray_SimpleNewFromData(1, dims, NPY_FLOAT, euler->array); + PyArray_ENABLEFLAGS((PyArrayObject *)array, NPY_ARRAY_OWNDATA); + return array; } static PyGetSetDef quaternion_get_set[] = { - {"wxyz", (getter) quaternion_get_wxyz, NULL, "", NULL}, - {"w", (getter) quaternion_get_w, NULL, "", NULL}, - {"x", (getter) quaternion_get_x, NULL, "", NULL}, - {"y", (getter) quaternion_get_y, NULL, "", NULL}, - {"z", (getter) quaternion_get_z, NULL, "", NULL}, - {NULL} /* sentinel */ + {"wxyz", (getter)quaternion_get_wxyz, NULL, "", NULL}, + {"w", (getter)quaternion_get_w, NULL, "", NULL}, + {"x", (getter)quaternion_get_x, NULL, "", NULL}, + {"y", (getter)quaternion_get_y, NULL, "", NULL}, + {"z", (getter)quaternion_get_z, NULL, "", NULL}, + {NULL} /* sentinel */ }; static PyMethodDef quaternion_methods[] = { - {"to_matrix", (PyCFunction) quaternion_to_matrix, METH_NOARGS, ""}, - {"to_euler", (PyCFunction) quaternion_to_euler, METH_NOARGS, ""}, - {NULL} /* sentinel */ + {"to_matrix", (PyCFunction)quaternion_to_matrix, METH_NOARGS, ""}, + {"to_euler", (PyCFunction)quaternion_to_euler, METH_NOARGS, ""}, + {NULL} /* sentinel */ }; static PyTypeObject quaternion_object = { - PyVarObject_HEAD_INIT(NULL, 0) - .tp_name = "imufusion.Quaternion", - .tp_basicsize = sizeof(Quaternion), - .tp_dealloc = (destructor) quaternion_free, - .tp_new = quaternion_new, - .tp_getset = quaternion_get_set, - .tp_methods = quaternion_methods, + PyVarObject_HEAD_INIT(NULL, 0).tp_name = "imufusion.Quaternion", + .tp_basicsize = sizeof(Quaternion), + .tp_dealloc = (destructor)quaternion_free, + .tp_new = quaternion_new, + .tp_getset = quaternion_get_set, + .tp_methods = quaternion_methods, }; static PyObject *quaternion_from(const FusionQuaternion *const quaternion) { - Quaternion *const self = (Quaternion *) quaternion_object.tp_alloc(&quaternion_object, 0); - self->quaternion = *quaternion; - return (PyObject *) self; + Quaternion *const self = (Quaternion *)quaternion_object.tp_alloc(&quaternion_object, 0); + self->quaternion = *quaternion; + return (PyObject *)self; } #endif diff --git a/Python/Python-C-API/Settings.h b/Python/Python-C-API/Settings.h index 17a5d3b..3b560be 100644 --- a/Python/Python-C-API/Settings.h +++ b/Python/Python-C-API/Settings.h @@ -6,132 +6,135 @@ #include typedef struct { - PyObject_HEAD - FusionAhrsSettings settings; + PyObject_HEAD FusionAhrsSettings settings; } Settings; static PyObject *settings_new(PyTypeObject *subtype, PyObject *args, PyObject *keywords) { - Settings *const self = (Settings *) subtype->tp_alloc(subtype, 0); - - const char *const error = PARSE_TUPLE(args, "iffffI", &self->settings.convention, &self->settings.gain, &self->settings.gyroscopeRange, &self->settings.accelerationRejection, &self->settings.magneticRejection, &self->settings.recoveryTriggerPeriod); - if (error != NULL) { - PyErr_SetString(PyExc_TypeError, error); - return NULL; - } - return (PyObject *) self; + Settings *const self = (Settings *)subtype->tp_alloc(subtype, 0); + + const char *const error = + PARSE_TUPLE(args, "iffffI", &self->settings.convention, &self->settings.gain, + &self->settings.gyroscopeRange, &self->settings.accelerationRejection, + &self->settings.magneticRejection, &self->settings.recoveryTriggerPeriod); + if (error != NULL) { + PyErr_SetString(PyExc_TypeError, error); + return NULL; + } + return (PyObject *)self; } -static void settings_free(Settings *self) { - Py_TYPE(self)->tp_free(self); -} +static void settings_free(Settings *self) { Py_TYPE(self)->tp_free(self); } static PyObject *settings_get_convention(Settings *self) { - return Py_BuildValue("l", self->settings.convention); + return Py_BuildValue("l", self->settings.convention); } static int settings_set_convention(Settings *self, PyObject *value, void *closure) { - const FusionConvention convention = (FusionConvention) PyFloat_AsDouble(value); + const FusionConvention convention = (FusionConvention)PyFloat_AsDouble(value); - if (PyErr_Occurred()) { - return -1; - } + if (PyErr_Occurred()) { + return -1; + } - self->settings.convention = convention; - return 0; + self->settings.convention = convention; + return 0; } static PyObject *settings_get_gain(Settings *self) { - return Py_BuildValue("f", self->settings.gain); + return Py_BuildValue("f", self->settings.gain); } static int settings_set_gain(Settings *self, PyObject *value, void *closure) { - const float gain = (float) PyFloat_AsDouble(value); + const float gain = (float)PyFloat_AsDouble(value); - if (PyErr_Occurred()) { - return -1; - } + if (PyErr_Occurred()) { + return -1; + } - self->settings.gain = gain; - return 0; + self->settings.gain = gain; + return 0; } static PyObject *settings_get_gyroscope_range(Settings *self) { - return Py_BuildValue("f", self->settings.gyroscopeRange); + return Py_BuildValue("f", self->settings.gyroscopeRange); } static int settings_set_gyroscope_range(Settings *self, PyObject *value, void *closure) { - const float gyroscope_range = (float) PyFloat_AsDouble(value); + const float gyroscope_range = (float)PyFloat_AsDouble(value); - if (PyErr_Occurred()) { - return -1; - } + if (PyErr_Occurred()) { + return -1; + } - self->settings.gyroscopeRange = gyroscope_range; - return 0; + self->settings.gyroscopeRange = gyroscope_range; + return 0; } static PyObject *settings_get_acceleration_rejection(Settings *self) { - return Py_BuildValue("f", self->settings.accelerationRejection); + return Py_BuildValue("f", self->settings.accelerationRejection); } static int settings_set_acceleration_rejection(Settings *self, PyObject *value, void *closure) { - const float acceleration_rejection = (float) PyFloat_AsDouble(value); + const float acceleration_rejection = (float)PyFloat_AsDouble(value); - if (PyErr_Occurred()) { - return -1; - } + if (PyErr_Occurred()) { + return -1; + } - self->settings.accelerationRejection = acceleration_rejection; - return 0; + self->settings.accelerationRejection = acceleration_rejection; + return 0; } static PyObject *settings_get_magnetic_rejection(Settings *self) { - return Py_BuildValue("f", self->settings.magneticRejection); + return Py_BuildValue("f", self->settings.magneticRejection); } static int settings_set_magnetic_rejection(Settings *self, PyObject *value, void *closure) { - const float magnetic_rejection = (float) PyFloat_AsDouble(value); + const float magnetic_rejection = (float)PyFloat_AsDouble(value); - if (PyErr_Occurred()) { - return -1; - } + if (PyErr_Occurred()) { + return -1; + } - self->settings.magneticRejection = magnetic_rejection; - return 0; + self->settings.magneticRejection = magnetic_rejection; + return 0; } static PyObject *settings_get_recovery_trigger_period(Settings *self) { - return Py_BuildValue("I", self->settings.recoveryTriggerPeriod); + return Py_BuildValue("I", self->settings.recoveryTriggerPeriod); } static int settings_set_recovery_trigger_period(Settings *self, PyObject *value, void *closure) { - const unsigned int recovery_trigger_period = (unsigned int) PyFloat_AsDouble(value); + const unsigned int recovery_trigger_period = (unsigned int)PyFloat_AsDouble(value); - if (PyErr_Occurred()) { - return -1; - } + if (PyErr_Occurred()) { + return -1; + } - self->settings.recoveryTriggerPeriod = recovery_trigger_period; - return 0; + self->settings.recoveryTriggerPeriod = recovery_trigger_period; + return 0; } static PyGetSetDef settings_get_set[] = { - {"convention", (getter) settings_get_convention, (setter) settings_set_convention, "", NULL}, - {"gain", (getter) settings_get_gain, (setter) settings_set_gain, "", NULL}, - {"gyroscope_range", (getter) settings_get_gyroscope_range, (setter) settings_set_gyroscope_range, "", NULL}, - {"acceleration_rejection", (getter) settings_get_acceleration_rejection, (setter) settings_set_acceleration_rejection, "", NULL}, - {"magnetic_rejection", (getter) settings_get_magnetic_rejection, (setter) settings_set_magnetic_rejection, "", NULL}, - {"recovery_trigger_period", (getter) settings_get_recovery_trigger_period, (setter) settings_set_recovery_trigger_period, "", NULL}, - {NULL} /* sentinel */ + {"convention", (getter)settings_get_convention, (setter)settings_set_convention, "", NULL}, + {"gain", (getter)settings_get_gain, (setter)settings_set_gain, "", NULL}, + {"gyroscope_range", (getter)settings_get_gyroscope_range, (setter)settings_set_gyroscope_range, + "", NULL}, + {"acceleration_rejection", (getter)settings_get_acceleration_rejection, + (setter)settings_set_acceleration_rejection, "", NULL}, + {"magnetic_rejection", (getter)settings_get_magnetic_rejection, + (setter)settings_set_magnetic_rejection, "", NULL}, + {"recovery_trigger_period", (getter)settings_get_recovery_trigger_period, + (setter)settings_set_recovery_trigger_period, "", NULL}, + {NULL} /* sentinel */ }; static PyTypeObject settings_object = { - PyVarObject_HEAD_INIT(NULL, 0) - .tp_name = "imufusion.Settings", - .tp_basicsize = sizeof(Settings), - .tp_dealloc = (destructor) settings_free, - .tp_new = settings_new, - .tp_getset = settings_get_set, + PyVarObject_HEAD_INIT(NULL, 0).tp_name = "imufusion.Settings", + .tp_basicsize = sizeof(Settings), + .tp_dealloc = (destructor)settings_free, + .tp_new = settings_new, + .tp_getset = settings_get_set, }; #endif diff --git a/Python/Python-C-API/imufusion.c b/Python/Python-C-API/imufusion.c index 1802433..82670df 100644 --- a/Python/Python-C-API/imufusion.c +++ b/Python/Python-C-API/imufusion.c @@ -3,74 +3,73 @@ #include "Compass.h" #include "Flags.h" #include "InternalStates.h" -#include #include "Offset.h" -#include #include "Quaternion.h" #include "Settings.h" +#include +#include static struct PyModuleDef config = { - PyModuleDef_HEAD_INIT, - "imufusion", - "", - -1, + PyModuleDef_HEAD_INIT, + "imufusion", + "", + -1, }; bool add_object(PyObject *const module, PyTypeObject *const type_object, const char *const name) { - if (PyType_Ready(type_object) == 0) { - Py_INCREF(type_object); - if (PyModule_AddObject(module, name, (PyObject *) type_object) == 0) { - return true; - } - Py_DECREF(type_object); - return false; + if (PyType_Ready(type_object) == 0) { + Py_INCREF(type_object); + if (PyModule_AddObject(module, name, (PyObject *)type_object) == 0) { + return true; } + Py_DECREF(type_object); return false; + } + return false; } PyMODINIT_FUNC PyInit_imufusion() { - import_array(); + import_array(); - PyObject *const module = PyModule_Create(&config); + PyObject *const module = PyModule_Create(&config); - if (module != NULL && - (PyModule_AddIntConstant(module, "ALIGNMENT_PXPYPZ", FusionAxesAlignmentPXPYPZ) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_PXNZPY", FusionAxesAlignmentPXNZPY) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_PXNYNZ", FusionAxesAlignmentPXNYNZ) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_PXPZNY", FusionAxesAlignmentPXPZNY) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_NXPYNZ", FusionAxesAlignmentNXPYNZ) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_NXPZPY", FusionAxesAlignmentNXPZPY) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_NXNYPZ", FusionAxesAlignmentNXNYPZ) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_NXNZNY", FusionAxesAlignmentNXNZNY) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_PYNXPZ", FusionAxesAlignmentPYNXPZ) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_PYNZNX", FusionAxesAlignmentPYNZNX) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_PYPXNZ", FusionAxesAlignmentPYPXNZ) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_PYPZPX", FusionAxesAlignmentPYPZPX) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_NYPXPZ", FusionAxesAlignmentNYPXPZ) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_NYNZPX", FusionAxesAlignmentNYNZPX) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_NYNXNZ", FusionAxesAlignmentNYNXNZ) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_NYPZNX", FusionAxesAlignmentNYPZNX) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_PZPYNX", FusionAxesAlignmentPZPYNX) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_PZPXPY", FusionAxesAlignmentPZPXPY) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_PZNYPX", FusionAxesAlignmentPZNYPX) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_PZNXNY", FusionAxesAlignmentPZNXNY) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_NZPYPX", FusionAxesAlignmentNZPYPX) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_NZNXPY", FusionAxesAlignmentNZNXPY) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_NZNYNX", FusionAxesAlignmentNZNYNX) == 0) && - (PyModule_AddIntConstant(module, "ALIGNMENT_NZPXNY", FusionAxesAlignmentNZPXNY) == 0) && - (PyModule_AddIntConstant(module, "CONVENTION_NWU", FusionConventionNwu) == 0) && - (PyModule_AddIntConstant(module, "CONVENTION_ENU", FusionConventionEnu) == 0) && - (PyModule_AddIntConstant(module, "CONVENTION_NED", FusionConventionNed) == 0) && - (PyModule_AddFunctions(module, axes_methods) == 0) && - (PyModule_AddFunctions(module, compass_methods) == 0) && - add_object(module, &ahrs_object, "Ahrs") && - add_object(module, &flags_object, "Flags") && - add_object(module, &internal_states_object, "InternalStates") && - add_object(module, &offset_object, "Offset") && - add_object(module, &settings_object, "Settings") && - add_object(module, &quaternion_object, "Quaternion")) { - return module; - } - Py_DECREF(module); - return NULL; + if (module != NULL && + (PyModule_AddIntConstant(module, "ALIGNMENT_PXPYPZ", FusionAxesAlignmentPXPYPZ) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_PXNZPY", FusionAxesAlignmentPXNZPY) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_PXNYNZ", FusionAxesAlignmentPXNYNZ) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_PXPZNY", FusionAxesAlignmentPXPZNY) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_NXPYNZ", FusionAxesAlignmentNXPYNZ) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_NXPZPY", FusionAxesAlignmentNXPZPY) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_NXNYPZ", FusionAxesAlignmentNXNYPZ) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_NXNZNY", FusionAxesAlignmentNXNZNY) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_PYNXPZ", FusionAxesAlignmentPYNXPZ) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_PYNZNX", FusionAxesAlignmentPYNZNX) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_PYPXNZ", FusionAxesAlignmentPYPXNZ) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_PYPZPX", FusionAxesAlignmentPYPZPX) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_NYPXPZ", FusionAxesAlignmentNYPXPZ) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_NYNZPX", FusionAxesAlignmentNYNZPX) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_NYNXNZ", FusionAxesAlignmentNYNXNZ) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_NYPZNX", FusionAxesAlignmentNYPZNX) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_PZPYNX", FusionAxesAlignmentPZPYNX) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_PZPXPY", FusionAxesAlignmentPZPXPY) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_PZNYPX", FusionAxesAlignmentPZNYPX) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_PZNXNY", FusionAxesAlignmentPZNXNY) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_NZPYPX", FusionAxesAlignmentNZPYPX) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_NZNXPY", FusionAxesAlignmentNZNXPY) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_NZNYNX", FusionAxesAlignmentNZNYNX) == 0) && + (PyModule_AddIntConstant(module, "ALIGNMENT_NZPXNY", FusionAxesAlignmentNZPXNY) == 0) && + (PyModule_AddIntConstant(module, "CONVENTION_NWU", FusionConventionNwu) == 0) && + (PyModule_AddIntConstant(module, "CONVENTION_ENU", FusionConventionEnu) == 0) && + (PyModule_AddIntConstant(module, "CONVENTION_NED", FusionConventionNed) == 0) && + (PyModule_AddFunctions(module, axes_methods) == 0) && + (PyModule_AddFunctions(module, compass_methods) == 0) && + add_object(module, &ahrs_object, "Ahrs") && add_object(module, &flags_object, "Flags") && + add_object(module, &internal_states_object, "InternalStates") && + add_object(module, &offset_object, "Offset") && + add_object(module, &settings_object, "Settings") && + add_object(module, &quaternion_object, "Quaternion")) { + return module; + } + Py_DECREF(module); + return NULL; }