Skip to content

Support batch update #206

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 7 commits into
base: main
Choose a base branch
from
+312 −6
Open

Support batch update #206

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
b6f09c2
Add batch update methods for Ahrs and Offset classes
changjo Mar 21, 2025
950a611
Bump version to 1.2.8+1 in pyproject.toml
changjo Mar 21, 2025
f90d575
Update `ahrs_update_batch` and `ahrs_update_no_magnetometer_batch` to…
changjo May 7, 2025
bdcd04d
Bump version to 1.2.8+2 in pyproject.toml
changjo May 7, 2025
e243669
Add memory allocation checks in batch update methods for quaternion a…
changjo May 8, 2025
e6f78c8
Update error message in ahrs_update_no_magnetometer_batch for input a…
changjo May 9, 2025
7662fd8
Add memory checks and error handling in ahrs_update_batch and ahrs_up…
changjo May 15, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
259 changes: 255 additions & 4 deletions Python/Python-C-API/Ahrs.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -137,6 +137,135 @@ 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 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);
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;
FusionQuaternion quaternion;
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);
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;

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 = PyDict_New();
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;
}


static PyObject *ahrs_update_no_magnetometer(Ahrs *self, PyObject *args) {
PyArrayObject *gyroscope_array;
PyArrayObject *accelerometer_array;
Expand Down Expand Up @@ -168,6 +297,126 @@ 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, "All input arrays must have the same size.");
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();
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);
float *accel_data = (float *) PyArray_DATA(accelerometer_array);
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++) {
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);
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;

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 = PyDict_New();
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;
}


static PyObject *ahrs_update_external_heading(Ahrs *self, PyObject *args) {
PyArrayObject *gyroscope_array;
PyArrayObject *accelerometer_array;
Expand Down Expand Up @@ -224,10 +473,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 */
};

Expand Down
57 changes: 56 additions & 1 deletion Python/Python-C-API/Offset.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 */
};

Expand Down
2 changes: 1 addition & 1 deletion pyproject.toml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,7 +4,7 @@ build-backend = "setuptools.build_meta"

[project]
name = "imufusion"
version = "1.2.8"
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 = "[email protected]" }]
Expand Down