From f1827bfd99fa43a85f5e4c6c022e01ea3df7d097 Mon Sep 17 00:00:00 2001 From: dlktdr Date: Sun, 13 Apr 2025 06:39:44 -0700 Subject: [PATCH 1/5] Replace Mad with Aqua Filter --- firmware/src/CMakeLists.txt | 1 + firmware/src/headtracker.code-workspace | 11 +- firmware/src/src/aqua_ahrs/aqua.cpp | 187 ++ firmware/src/src/aqua_ahrs/matrices.cpp | 1952 +++++++++++++++++ firmware/src/src/aqua_ahrs/quaternions.cpp | 720 ++++++ firmware/src/src/aqua_ahrs/vectors.cpp | 523 +++++ firmware/src/src/include/fusion/aqua.h | 121 + firmware/src/src/include/fusion/euler.h | 83 + firmware/src/src/include/fusion/matrices.h | 1113 ++++++++++ firmware/src/src/include/fusion/quaternions.h | 513 +++++ firmware/src/src/include/fusion/vectors.h | 438 ++++ firmware/src/src/include/fusion/zsl.h | 32 + firmware/src/src/sense.cpp | 59 +- 13 files changed, 5741 insertions(+), 12 deletions(-) create mode 100644 firmware/src/src/aqua_ahrs/aqua.cpp create mode 100644 firmware/src/src/aqua_ahrs/matrices.cpp create mode 100644 firmware/src/src/aqua_ahrs/quaternions.cpp create mode 100644 firmware/src/src/aqua_ahrs/vectors.cpp create mode 100644 firmware/src/src/include/fusion/aqua.h create mode 100644 firmware/src/src/include/fusion/euler.h create mode 100644 firmware/src/src/include/fusion/matrices.h create mode 100644 firmware/src/src/include/fusion/quaternions.h create mode 100644 firmware/src/src/include/fusion/vectors.h create mode 100644 firmware/src/src/include/fusion/zsl.h diff --git a/firmware/src/CMakeLists.txt b/firmware/src/CMakeLists.txt index 005aee43..6e16bcb4 100644 --- a/firmware/src/CMakeLists.txt +++ b/firmware/src/CMakeLists.txt @@ -120,6 +120,7 @@ endif() target_compile_definitions(app PUBLIC FW_GIT_REV=${GIT_REV}) target_compile_definitions(app PUBLIC FW_VER_TAG=${GIT_TAG}) target_include_directories(app PRIVATE src/include) +target_include_directories(app PRIVATE src/include/fusion) target_include_directories(app PRIVATE src) target_include_directories(app PRIVATE third-party/ArduinoJson/src) target_include_directories(app PRIVATE third-party/BMI270_SensorAPI) diff --git a/firmware/src/headtracker.code-workspace b/firmware/src/headtracker.code-workspace index 441ef655..3d4d37d4 100644 --- a/firmware/src/headtracker.code-workspace +++ b/firmware/src/headtracker.code-workspace @@ -28,9 +28,18 @@ "ZEPHYR_BASE": "${workspaceRoot:src}\\..\\zephyr\\zephyr" }, "files.associations": { + "*.module": "php", "random": "cpp", "system_error": "cpp", - "stdio.h": "c" + "stdio.h": "c", + "chrono": "c", + "xlocale": "c", + "array": "cpp", + "ranges": "cpp", + "span": "cpp", + "vector": "cpp", + "xstring": "cpp", + "xutility": "cpp" }, }, diff --git a/firmware/src/src/aqua_ahrs/aqua.cpp b/firmware/src/src/aqua_ahrs/aqua.cpp new file mode 100644 index 00000000..ca1a7ed8 --- /dev/null +++ b/firmware/src/src/aqua_ahrs/aqua.cpp @@ -0,0 +1,187 @@ +/* + * Copyright (c) 2021 Marti Riba Pons + * + * SPDX-License-Identifier: Apache-2.0 + */ + + #include + #include + #include + #include + #include "aqua.h" + + static uint32_t zsl_fus_aqua_freq; + static uint32_t zsl_fus_aqua_initialised; + + static int zsl_fus_aqua(struct zsl_vec *a, struct zsl_vec *m, + struct zsl_vec *g, zsl_real_t *e_a, zsl_real_t *e_m, + zsl_real_t *alpha, zsl_real_t *beta, struct zsl_quat *q) + { + int rc = 0; + + /* Convert the input quaternion to a unit quaternion. */ + zsl_quat_to_unit_d(q); + + /* Calculate an estimation of the orientation using only the data of the + * gyroscope and quaternion integration. */ + zsl_vec_scalar_mult(g, -1.0f); + zsl_quat_from_ang_vel(g, q, 1.0f / zsl_fus_aqua_freq, q); + + /* Continue with the calculations only if the data from the accelerometer + * is valid (non zero). */ + if ((a != NULL) && ZSL_ABS(zsl_vec_norm(a)) > 1E-6f) { + + /* Normalize the acceleration vector. */ + zsl_vec_to_unit(a); + + /* Turn the data of the accelerometer into a pure quaterion. */ + struct zsl_quat qa = { + .r = 0.0f, + .i = a->data[0], + .j = a->data[1], + .k = a->data[2] + }; + + /* Invert q. */ + struct zsl_quat q_inv; + zsl_quat_inv(q, &q_inv); + + /* Rotate the accelerometer data quaternion (qa) using the inverse of + * previous estimation of the orientation (q_inv). */ + struct zsl_quat qg; + zsl_quat_rot(&q_inv, &qa, &qg); + + /* Compute the delta q_acc quaternion. */ + struct zsl_quat Dq_acc = { + .r = ZSL_SQRT((qg.k + 1.0f) / 2.0f), + .i = -qg.j / ZSL_SQRT(2.0f * (qg.k + 1.0f)), + .j = qg.i / ZSL_SQRT(2.0f * (qg.k + 1.0f)), + .k = 0.0 + }; + + /* Scale down the delta q_acc by using spherical linear interpolation + * or simply linear interpolation to minimize the effects of high + * frequency noise in the accelerometer. */ + struct zsl_quat q_acc; + struct zsl_quat qi; + zsl_quat_init(&qi, ZSL_QUAT_TYPE_IDENTITY); + if (Dq_acc.r > *e_a) { + zsl_quat_lerp(&qi, &Dq_acc, *alpha, &q_acc); + } else { + zsl_quat_slerp(&qi, &Dq_acc, *alpha, &q_acc); + } + + /* New orientation estimation with accelerometer correction. */ + zsl_quat_mult(q, &q_acc, q); + + /* Continue with the calculations only if the data from the + * magnetometer is valid (non zero). */ + if ((m != NULL) && ZSL_ABS(zsl_vec_norm(m)) > 1E-6f) { + + /* Normalize the magnetic field vector. */ + zsl_vec_to_unit(m); + + /* Turn the data of the magnetometer into a pure quaterion. */ + struct zsl_quat qm = { + .r = 0.0f, + .i = m->data[0], + .j = m->data[1], + .k = m->data[2] + }; + + /* Invert q again. */ + zsl_quat_inv(q, &q_inv); + + /* Rotate the magnetometer data quaternion (qm) using the inverse + * of the previous estimation of the orientation (q_inv). */ + struct zsl_quat ql; + zsl_quat_rot(&q_inv, &qm, &ql); + + /* Compute the delta q_mag quaternion. */ + zsl_real_t y = ZSL_SQRT(ql.i * ql.i + ql.j * ql.j); + struct zsl_quat Dq_mag = { + .r = ZSL_SQRT((y * y + ql.i * y) / (2.0f * y * y)), + .i = 0.0f, + .j = 0.0f, + .k = ql.j / ZSL_SQRT(2.0f * (y * y + ql.i * y)) + }; + + /* Scale down the delta q_mag quaternion by using spherical linear + * interpolation or simply linear interpolation to minimize the + * effects of high frequency noise in the magnetometer. */ + struct zsl_quat q_mag; + if (Dq_mag.r > *e_m) { + zsl_quat_lerp(&qi, &Dq_mag, *beta, &q_mag); + } else { + zsl_quat_slerp(&qi, &Dq_mag, *beta, &q_mag); + } + + /* New orientation estimation with magnetometer correction. */ + zsl_quat_mult(q, &q_mag, q); + } + } + + /* Normalize the output quaternion. */ + zsl_quat_to_unit_d(q); + + return rc; + } + + static int zsl_fus_aqua_alpha_init(struct zsl_vec *a, zsl_real_t *alpha) + { + int rc = 0; + + /* Calculate the value of alpha, which depends on the magnitude error + * (m_e) and the value of alpha in static conditions. */ + zsl_real_t n = zsl_vec_norm(a); + zsl_real_t gr = 9.81; /* Earth's gravity. */ + zsl_real_t m_e = ZSL_ABS(n - gr) / gr; /* Magnitude error. */ + if (m_e >= 0.2f) { + *alpha = 0.0f; + } else if (m_e > 0.1f && m_e < 0.2f) { + *alpha *= (0.2f - m_e) / 0.1f; + } + + /* Inidicate that we have already called this function. */ + zsl_fus_aqua_initialised++; + + return rc; + } + + int zsl_fus_aqua_init(uint32_t freq, void *cfg) + { + int rc = 0; + + struct zsl_fus_aqua_cfg *mcfg = (struct zsl_fus_aqua_cfg *)cfg; + + (void)mcfg; + + zsl_fus_aqua_freq = freq; + + return rc; + } + + int zsl_fus_aqua_feed(struct zsl_vec *a, struct zsl_vec *m, + struct zsl_vec *g, zsl_real_t *incl, struct zsl_quat *q, + void *cfg) + { + struct zsl_fus_aqua_cfg *mcfg = (struct zsl_fus_aqua_cfg *)cfg; + + if (mcfg->alpha < 0.0f || mcfg->alpha > 1.0f || mcfg->beta < 0.0f || + mcfg->beta > 1.0f) { + return -EINVAL; + } + + /* This functions should only be called once. */ + if (!zsl_fus_aqua_initialised) { + zsl_fus_aqua_alpha_init(a, &(mcfg->alpha)); + } + + return zsl_fus_aqua(a, m, g, &(mcfg->e_a), &(mcfg->e_m), &(mcfg->alpha), + &(mcfg->beta), q); + } + + void zsl_fus_aqua_error(int error) + { + /* ToDo: Log error in default handler. */ + } \ No newline at end of file diff --git a/firmware/src/src/aqua_ahrs/matrices.cpp b/firmware/src/src/aqua_ahrs/matrices.cpp new file mode 100644 index 00000000..66d00bd1 --- /dev/null +++ b/firmware/src/src/aqua_ahrs/matrices.cpp @@ -0,0 +1,1952 @@ +/* + * Copyright (c) 2019 Kevin Townsend (KTOWN) + * + * SPDX-License-Identifier: Apache-2.0 + */ + + #include + #include + #include + #include + #include "zsl.h" + #include "matrices.h" + + /* + * WARNING: Work in progress! + * + * The code in this module is very 'naive' in the sense that no attempt + * has been made at efficiency. It is written from the perspective + * that code should be written to be 'reliable, elegant, efficient' in that + * order. + * + * Clarity and reliability have been absolutely prioritized in this + * early stage, with the key goal being good unit test coverage before + * moving on to any form of general-purpose or architecture-specific + * optimisation. + */ + + // TODO: Introduce local macros for bounds/shape checks to avoid duplication! + + int + zsl_mtx_entry_fn_empty(struct zsl_mtx *m, size_t i, size_t j) + { + return zsl_mtx_set(m, i, j, 0); + } + + int + zsl_mtx_entry_fn_identity(struct zsl_mtx *m, size_t i, size_t j) + { + return zsl_mtx_set(m, i, j, i == j ? 1.0 : 0); + } + + int + zsl_mtx_entry_fn_random(struct zsl_mtx *m, size_t i, size_t j) + { + /* TODO: Determine an appropriate random number generator. */ + return zsl_mtx_set(m, i, j, 0); + } + + int + zsl_mtx_init(struct zsl_mtx *m, zsl_mtx_init_entry_fn_t entry_fn) + { + int rc; + + for (size_t i = 0; i < m->sz_rows; i++) { + for (size_t j = 0; j < m->sz_cols; j++) { + /* If entry_fn is NULL, assign 0.0 values. */ + if (entry_fn == NULL) { + rc = zsl_mtx_entry_fn_empty(m, i, j); + } else { + rc = entry_fn(m, i, j); + } + /* Abort if entry_fn returned an error code. */ + if (rc) { + return rc; + } + } + } + + return 0; + } + + int + zsl_mtx_from_arr(struct zsl_mtx *m, zsl_real_t *a) + { + memcpy(m->data, a, (m->sz_rows * m->sz_cols) * sizeof(zsl_real_t)); + + return 0; + } + + int + zsl_mtx_copy(struct zsl_mtx *mdest, struct zsl_mtx *msrc) + { + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Ensure that msrc and mdest have the same shape. */ + if ((mdest->sz_rows != msrc->sz_rows) || + (mdest->sz_cols != msrc->sz_cols)) { + return -EINVAL; + } + #endif + + /* Make a copy of matrix 'msrc'. */ + memcpy(mdest->data, msrc->data, sizeof(zsl_real_t) * + msrc->sz_rows * msrc->sz_cols); + + return 0; + } + + int + zsl_mtx_get(struct zsl_mtx *m, size_t i, size_t j, zsl_real_t *x) + { + #if CONFIG_ZSL_BOUNDS_CHECKS + if ((i >= m->sz_rows) || (j >= m->sz_cols)) { + return -EINVAL; + } + #endif + + *x = m->data[(i * m->sz_cols) + j]; + + return 0; + } + + int + zsl_mtx_set(struct zsl_mtx *m, size_t i, size_t j, zsl_real_t x) + { + #if CONFIG_ZSL_BOUNDS_CHECKS + if ((i >= m->sz_rows) || (j >= m->sz_cols)) { + return -EINVAL; + } + #endif + + m->data[(i * m->sz_cols) + j] = x; + + return 0; + } + + int + zsl_mtx_get_row(struct zsl_mtx *m, size_t i, zsl_real_t *v) + { + int rc; + zsl_real_t x; + + for (size_t j = 0; j < m->sz_cols; j++) { + rc = zsl_mtx_get(m, i, j, &x); + if (rc) { + return rc; + } + v[j] = x; + } + + return 0; + } + + int + zsl_mtx_set_row(struct zsl_mtx *m, size_t i, zsl_real_t *v) + { + int rc; + + for (size_t j = 0; j < m->sz_cols; j++) { + rc = zsl_mtx_set(m, i, j, v[j]); + if (rc) { + return rc; + } + } + + return 0; + } + + int + zsl_mtx_get_col(struct zsl_mtx *m, size_t j, zsl_real_t *v) + { + int rc; + zsl_real_t x; + + for (size_t i = 0; i < m->sz_rows; i++) { + rc = zsl_mtx_get(m, i, j, &x); + if (rc) { + return rc; + } + v[i] = x; + } + + return 0; + } + + int + zsl_mtx_set_col(struct zsl_mtx *m, size_t j, zsl_real_t *v) + { + int rc; + + for (size_t i = 0; i < m->sz_rows; i++) { + rc = zsl_mtx_set(m, i, j, v[i]); + if (rc) { + return rc; + } + } + + return 0; + } + + int + zsl_mtx_unary_op(struct zsl_mtx *m, zsl_mtx_unary_op_t op) + { + /* Execute the unary operation component by component. */ + for (size_t i = 0; i < m->sz_cols * m->sz_rows; i++) { + switch (op) { + case ZSL_MTX_UNARY_OP_INCREMENT: + m->data[i] += 1.0; + break; + case ZSL_MTX_UNARY_OP_DECREMENT: + m->data[i] -= 1.0; + break; + case ZSL_MTX_UNARY_OP_NEGATIVE: + m->data[i] = -m->data[i]; + break; + case ZSL_MTX_UNARY_OP_ROUND: + m->data[i] = ZSL_ROUND(m->data[i]); + break; + case ZSL_MTX_UNARY_OP_ABS: + m->data[i] = ZSL_ABS(m->data[i]); + break; + case ZSL_MTX_UNARY_OP_FLOOR: + m->data[i] = ZSL_FLOOR(m->data[i]); + break; + case ZSL_MTX_UNARY_OP_CEIL: + m->data[i] = ZSL_CEIL(m->data[i]); + break; + case ZSL_MTX_UNARY_OP_EXP: + m->data[i] = ZSL_EXP(m->data[i]); + break; + case ZSL_MTX_UNARY_OP_LOG: + m->data[i] = ZSL_LOG(m->data[i]); + break; + case ZSL_MTX_UNARY_OP_LOG10: + m->data[i] = ZSL_LOG10(m->data[i]); + break; + case ZSL_MTX_UNARY_OP_SQRT: + m->data[i] = ZSL_SQRT(m->data[i]); + break; + case ZSL_MTX_UNARY_OP_SIN: + m->data[i] = ZSL_SIN(m->data[i]); + break; + case ZSL_MTX_UNARY_OP_COS: + m->data[i] = ZSL_COS(m->data[i]); + break; + case ZSL_MTX_UNARY_OP_TAN: + m->data[i] = ZSL_TAN(m->data[i]); + break; + case ZSL_MTX_UNARY_OP_ASIN: + m->data[i] = ZSL_ASIN(m->data[i]); + break; + case ZSL_MTX_UNARY_OP_ACOS: + m->data[i] = ZSL_ACOS(m->data[i]); + break; + case ZSL_MTX_UNARY_OP_ATAN: + m->data[i] = ZSL_ATAN(m->data[i]); + break; + case ZSL_MTX_UNARY_OP_SINH: + m->data[i] = ZSL_SINH(m->data[i]); + break; + case ZSL_MTX_UNARY_OP_COSH: + m->data[i] = ZSL_COSH(m->data[i]); + break; + case ZSL_MTX_UNARY_OP_TANH: + m->data[i] = ZSL_TANH(m->data[i]); + break; + default: + /* Not yet implemented! */ + return -ENOSYS; + } + } + + return 0; + } + + int + zsl_mtx_unary_func(struct zsl_mtx *m, zsl_mtx_unary_fn_t fn) + { + int rc; + + for (size_t i = 0; i < m->sz_rows; i++) { + for (size_t j = 0; j < m->sz_cols; j++) { + /* If fn is NULL, do nothing. */ + if (fn != NULL) { + rc = fn(m, i, j); + if (rc) { + return rc; + } + } + } + } + + return 0; + } + + int + zsl_mtx_binary_op(struct zsl_mtx *ma, struct zsl_mtx *mb, struct zsl_mtx *mc, + zsl_mtx_binary_op_t op) + { + #if CONFIG_ZSL_BOUNDS_CHECKS + if ((ma->sz_rows != mb->sz_rows) || (mb->sz_rows != mc->sz_rows) || + (ma->sz_cols != mb->sz_cols) || (mb->sz_cols != mc->sz_cols)) { + return -EINVAL; + } + #endif + + /* Execute the binary operation component by component. */ + for (size_t i = 0; i < ma->sz_cols * ma->sz_rows; i++) { + switch (op) { + case ZSL_MTX_BINARY_OP_ADD: + mc->data[i] = ma->data[i] + mb->data[i]; + break; + case ZSL_MTX_BINARY_OP_SUB: + mc->data[i] = ma->data[i] - mb->data[i]; + break; + case ZSL_MTX_BINARY_OP_MULT: + mc->data[i] = ma->data[i] * mb->data[i]; + break; + case ZSL_MTX_BINARY_OP_DIV: + if (mb->data[i] == 0.0) { + mc->data[i] = 0.0; + } else { + mc->data[i] = ma->data[i] / mb->data[i]; + } + break; + case ZSL_MTX_BINARY_OP_MEAN: + mc->data[i] = (ma->data[i] + mb->data[i]) / 2.0; + case ZSL_MTX_BINARY_OP_EXPON: + mc->data[i] = ZSL_POW(ma->data[i], mb->data[i]); + case ZSL_MTX_BINARY_OP_MIN: + mc->data[i] = ma->data[i] < mb->data[i] ? + ma->data[i] : mb->data[i]; + case ZSL_MTX_BINARY_OP_MAX: + mc->data[i] = ma->data[i] > mb->data[i] ? + ma->data[i] : mb->data[i]; + case ZSL_MTX_BINARY_OP_EQUAL: + mc->data[i] = ma->data[i] == mb->data[i] ? 1.0 : 0.0; + case ZSL_MTX_BINARY_OP_NEQUAL: + mc->data[i] = ma->data[i] != mb->data[i] ? 1.0 : 0.0; + case ZSL_MTX_BINARY_OP_LESS: + mc->data[i] = ma->data[i] < mb->data[i] ? 1.0 : 0.0; + case ZSL_MTX_BINARY_OP_GREAT: + mc->data[i] = ma->data[i] > mb->data[i] ? 1.0 : 0.0; + case ZSL_MTX_BINARY_OP_LEQ: + mc->data[i] = ma->data[i] <= mb->data[i] ? 1.0 : 0.0; + case ZSL_MTX_BINARY_OP_GEQ: + mc->data[i] = ma->data[i] >= mb->data[i] ? 1.0 : 0.0; + default: + /* Not yet implemented! */ + return -ENOSYS; + } + } + + return 0; + } + + int + zsl_mtx_binary_func(struct zsl_mtx *ma, struct zsl_mtx *mb, + struct zsl_mtx *mc, zsl_mtx_binary_fn_t fn) + { + int rc; + + #if CONFIG_ZSL_BOUNDS_CHECKS + if ((ma->sz_rows != mb->sz_rows) || (mb->sz_rows != mc->sz_rows) || + (ma->sz_cols != mb->sz_cols) || (mb->sz_cols != mc->sz_cols)) { + return -EINVAL; + } + #endif + + for (size_t i = 0; i < ma->sz_rows; i++) { + for (size_t j = 0; j < ma->sz_cols; j++) { + /* If fn is NULL, do nothing. */ + if (fn != NULL) { + rc = fn(ma, mb, mc, i, j); + if (rc) { + return rc; + } + } + } + } + + return 0; + } + + int + zsl_mtx_add(struct zsl_mtx *ma, struct zsl_mtx *mb, struct zsl_mtx *mc) + { + return zsl_mtx_binary_op(ma, mb, mc, ZSL_MTX_BINARY_OP_ADD); + } + + int + zsl_mtx_add_d(struct zsl_mtx *ma, struct zsl_mtx *mb) + { + return zsl_mtx_binary_op(ma, mb, ma, ZSL_MTX_BINARY_OP_ADD); + } + + int + zsl_mtx_sum_rows_d(struct zsl_mtx *m, size_t i, size_t j) + { + #if CONFIG_ZSL_BOUNDS_CHECKS + if ((i >= m->sz_rows) || (j >= m->sz_rows)) { + return -EINVAL; + } + #endif + + /* Add row j to row i, element by element. */ + for (size_t x = 0; x < m->sz_cols; x++) { + m->data[(i * m->sz_cols) + x] += m->data[(j * m->sz_cols) + x]; + } + + return 0; + } + + int zsl_mtx_sum_rows_scaled_d(struct zsl_mtx *m, + size_t i, size_t j, zsl_real_t s) + { + #if CONFIG_ZSL_BOUNDS_CHECKS + if ((i >= m->sz_rows) || (j >= m->sz_cols)) { + return -EINVAL; + } + #endif + + /* Set the values in row 'i' to 'i[n] += j[n] * s' . */ + for (size_t x = 0; x < m->sz_cols; x++) { + m->data[(i * m->sz_cols) + x] += + (m->data[(j * m->sz_cols) + x] * s); + } + + return 0; + } + + int + zsl_mtx_sub(struct zsl_mtx *ma, struct zsl_mtx *mb, struct zsl_mtx *mc) + { + return zsl_mtx_binary_op(ma, mb, mc, ZSL_MTX_BINARY_OP_SUB); + } + + int + zsl_mtx_sub_d(struct zsl_mtx *ma, struct zsl_mtx *mb) + { + return zsl_mtx_binary_op(ma, mb, ma, ZSL_MTX_BINARY_OP_SUB); + } + + int + zsl_mtx_mult(struct zsl_mtx *ma, struct zsl_mtx *mb, struct zsl_mtx *mc) + { + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Ensure that ma has the same number as columns as mb has rows. */ + if (ma->sz_cols != mb->sz_rows) { + return -EINVAL; + } + + /* Ensure that mc has ma rows and mb cols */ + if ((mc->sz_rows != ma->sz_rows) || (mc->sz_cols != mb->sz_cols)) { + return -EINVAL; + } + #endif + + ZSL_MATRIX_DEF(ma_copy, ma->sz_rows, ma->sz_cols); + ZSL_MATRIX_DEF(mb_copy, mb->sz_rows, mb->sz_cols); + zsl_mtx_copy(&ma_copy, ma); + zsl_mtx_copy(&mb_copy, mb); + + for (size_t i = 0; i < ma_copy.sz_rows; i++) { + for (size_t j = 0; j < mb_copy.sz_cols; j++) { + mc->data[j + i * mb_copy.sz_cols] = 0; + for (size_t k = 0; k < ma_copy.sz_cols; k++) { + mc->data[j + i * mb_copy.sz_cols] += + ma_copy.data[k + i * ma_copy.sz_cols] * + mb_copy.data[j + k * mb_copy.sz_cols]; + } + } + } + + return 0; + } + + int + zsl_mtx_mult_d(struct zsl_mtx *ma, struct zsl_mtx *mb) + { + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Ensure that ma has the same number as columns as mb has rows. */ + if (ma->sz_cols != mb->sz_rows) { + return -EINVAL; + } + + /* Ensure that mb is a square matrix. */ + if (mb->sz_rows != mb->sz_cols) { + return -EINVAL; + } + #endif + + zsl_mtx_mult(ma, mb, ma); + + return 0; + } + + int + zsl_mtx_scalar_mult_d(struct zsl_mtx *m, zsl_real_t s) + { + for (size_t i = 0; i < m->sz_rows * m->sz_cols; i++) { + m->data[i] *= s; + } + + return 0; + } + + int + zsl_mtx_scalar_mult_row_d(struct zsl_mtx *m, size_t i, zsl_real_t s) + { + #if CONFIG_ZSL_BOUNDS_CHECKS + if (i >= m->sz_rows) { + return -EINVAL; + } + #endif + + for (size_t k = 0; k < m->sz_cols; k++) { + m->data[(i * m->sz_cols) + k] *= s; + } + + return 0; + } + + int + zsl_mtx_trans(struct zsl_mtx *ma, struct zsl_mtx *mb) + { + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Ensure that ma and mb have the same shape. */ + if ((ma->sz_rows != mb->sz_cols) || (ma->sz_cols != mb->sz_rows)) { + return -EINVAL; + } + #endif + + zsl_real_t d[ma->sz_cols]; + + for (size_t i = 0; i < ma->sz_rows; i++) { + zsl_mtx_get_row(ma, i, d); + zsl_mtx_set_col(mb, i, d); + } + + return 0; + } + + int + zsl_mtx_adjoint_3x3(struct zsl_mtx *m, struct zsl_mtx *ma) + { + /* Make sure this is a square matrix. */ + if ((m->sz_rows != m->sz_cols) || (ma->sz_rows != ma->sz_cols)) { + return -EINVAL; + } + + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure this is a 3x3 matrix. */ + if ((m->sz_rows != 3) || (ma->sz_rows != 3)) { + return -EINVAL; + } + #endif + + /* + * 3x3 matrix element to array table: + * + * 1,1 = 0 1,2 = 1 1,3 = 2 + * 2,1 = 3 2,2 = 4 2,3 = 5 + * 3,1 = 6 3,2 = 7 3,3 = 8 + */ + + ma->data[0] = m->data[4] * m->data[8] - m->data[7] * m->data[5]; + ma->data[1] = m->data[7] * m->data[2] - m->data[1] * m->data[8]; + ma->data[2] = m->data[1] * m->data[5] - m->data[4] * m->data[2]; + + ma->data[3] = m->data[6] * m->data[5] - m->data[3] * m->data[8]; + ma->data[4] = m->data[0] * m->data[8] - m->data[6] * m->data[2]; + ma->data[5] = m->data[3] * m->data[2] - m->data[0] * m->data[5]; + + ma->data[6] = m->data[3] * m->data[7] - m->data[6] * m->data[4]; + ma->data[7] = m->data[6] * m->data[1] - m->data[0] * m->data[7]; + ma->data[8] = m->data[0] * m->data[4] - m->data[3] * m->data[1]; + + return 0; + } + + int + zsl_mtx_adjoint(struct zsl_mtx *m, struct zsl_mtx *ma) + { + /* Shortcut for 3x3 matrices. */ + if (m->sz_rows == 3) { + return zsl_mtx_adjoint_3x3(m, ma); + } + + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure this is a square matrix. */ + if (m->sz_rows != m->sz_cols) { + return -EINVAL; + } + #endif + + zsl_real_t sign; + zsl_real_t d; + ZSL_MATRIX_DEF(mr, (m->sz_cols - 1), (m->sz_cols - 1)); + + for (size_t i = 0; i < m->sz_cols; i++) { + for (size_t j = 0; j < m->sz_cols; j++) { + sign = 1.0; + if ((i + j) % 2 != 0) { + sign = -1.0; + } + zsl_mtx_reduce(m, &mr, i, j); + zsl_mtx_deter(&mr, &d); + d *= sign; + zsl_mtx_set(ma, i, j, d); + } + } + + return 0; + } + + #ifndef CONFIG_ZSL_SINGLE_PRECISION + int zsl_mtx_vec_wedge(struct zsl_mtx *m, struct zsl_vec *v) + { + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure the dimensions of 'm' and 'v' match. */ + if (v->sz != m->sz_cols || v->sz < 4 || m->sz_rows != (m->sz_cols - 1)) { + return -EINVAL; + } + #endif + + zsl_real_t d; + + ZSL_MATRIX_DEF(A, m->sz_cols, m->sz_cols); + ZSL_MATRIX_DEF(Ai, m->sz_cols, m->sz_cols); + ZSL_VECTOR_DEF(Av, m->sz_cols); + ZSL_MATRIX_DEF(b, m->sz_cols, 1); + + zsl_mtx_init(&A, NULL); + A.data[(m->sz_cols * m->sz_cols - 1)] = 1.0; + + for (size_t i = 0; i < m->sz_rows; i++) { + zsl_mtx_get_row(m, i, Av.data); + zsl_mtx_set_row(&A, i, Av.data); + } + + zsl_mtx_deter(&A, &d); + zsl_mtx_inv(&A, &Ai); + zsl_mtx_init(&b, NULL); + b.data[(m->sz_cols - 1)] = d; + + zsl_mtx_mult(&Ai, &b, &b); + + zsl_vec_from_arr(v, b.data); + + return 0; + } + #endif + + int + zsl_mtx_reduce(struct zsl_mtx *m, struct zsl_mtx *mr, size_t i, size_t j) + { + size_t u = 0; + zsl_real_t x; + zsl_real_t v[mr->sz_rows * mr->sz_rows]; + + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure mr is 1 less than m. */ + if (mr->sz_rows != m->sz_rows - 1) { + return -EINVAL; + } + if (mr->sz_cols != m->sz_cols - 1) { + return -EINVAL; + } + if ((i >= m->sz_rows) || (j >= m->sz_cols)) { + return -EINVAL; + } + #endif + + for (size_t k = 0; k < m->sz_rows; k++) { + for (size_t g = 0; g < m->sz_rows; g++) { + if (k != i && g != j) { + zsl_mtx_get(m, k, g, &x); + v[u] = x; + u++; + } + } + } + + zsl_mtx_from_arr(mr, v); + + return 0; + } + + int + zsl_mtx_reduce_iter(struct zsl_mtx *m, struct zsl_mtx *mred, + struct zsl_mtx *place1, struct zsl_mtx *place2) + { + /* TODO: Properly check if matrix is square. */ + if (m->sz_rows == place1->sz_rows) { + zsl_mtx_copy(place1, m); + } + + if (place1->sz_rows == mred->sz_rows) { + zsl_mtx_copy(mred, place1); + + /* restore the original placeholder size */ + place1->sz_rows = m->sz_rows; + place1->sz_cols = m->sz_cols; + place2->sz_rows = m->sz_rows; + place2->sz_cols = m->sz_cols; + return 0; + } + + /* trick the iterative method by generating the inner + * call intermediate matrix, adjusting its size + */ + place2->sz_rows = place1->sz_rows - 1; + place2->sz_cols = place1->sz_cols - 1; + zsl_mtx_reduce(place1, place2, 0, 0); + + /* Do the same with the second placeholder matrix */ + place1->sz_rows--; + place1->sz_cols--; + zsl_mtx_copy(place1, place2); + + return -EAGAIN; + } + + int + zsl_mtx_augm_diag(struct zsl_mtx *m, struct zsl_mtx *maug) + { + zsl_real_t x; + /* TODO: Properly check if matrix is square, and diff > 0. */ + size_t diff = (maug->sz_rows) - (m->sz_rows); + + zsl_mtx_init(maug, zsl_mtx_entry_fn_identity); + for (size_t i = 0; i < m->sz_rows; i++) { + for (size_t j = 0; j < m->sz_rows; j++) { + zsl_mtx_get(m, i, j, &x); + zsl_mtx_set(maug, i + diff, j + diff, x); + } + } + + return 0; + } + + int + zsl_mtx_deter_3x3(struct zsl_mtx *m, zsl_real_t *d) + { + /* Make sure this is a square matrix. */ + if (m->sz_rows != m->sz_cols) { + return -EINVAL; + } + + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure this is a 3x3 matrix. */ + if (m->sz_rows != 3) { + return -EINVAL; + } + #endif + + /* + * 3x3 matrix element to array table: + * + * 1,1 = 0 1,2 = 1 1,3 = 2 + * 2,1 = 3 2,2 = 4 2,3 = 5 + * 3,1 = 6 3,2 = 7 3,3 = 8 + */ + + *d = m->data[0] * (m->data[4] * m->data[8] - m->data[7] * m->data[5]); + *d -= m->data[3] * (m->data[1] * m->data[8] - m->data[7] * m->data[2]); + *d += m->data[6] * (m->data[1] * m->data[5] - m->data[4] * m->data[2]); + + return 0; + } + + int + zsl_mtx_deter(struct zsl_mtx *m, zsl_real_t *d) + { + /* Shortcut for 1x1 matrices. */ + if (m->sz_rows == 1) { + *d = m->data[0]; + return 0; + } + + /* Shortcut for 2x2 matrices. */ + if (m->sz_rows == 2) { + *d = m->data[0] * m->data[3] - m->data[2] * m->data[1]; + return 0; + } + + /* Shortcut for 3x3 matrices. */ + if (m->sz_rows == 3) { + return zsl_mtx_deter_3x3(m, d); + } + + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure this is a square matrix. */ + if (m->sz_rows != m->sz_cols) { + return -EINVAL; + } + #endif + + /* Full calculation required for non 3x3 matrices. */ + int rc; + zsl_real_t dtmp; + zsl_real_t cur; + zsl_real_t sign; + ZSL_MATRIX_DEF(mr, (m->sz_rows - 1), (m->sz_rows - 1)); + + /* Clear determinant output before starting. */ + *d = 0.0; + + /* + * Iterate across row 0, removing columns one by one. + * Note that these calls are recursive until we reach a 3x3 matrix, + * which will be calculated using the shortcut at the top of this + * function. + */ + for (size_t g = 0; g < m->sz_cols; g++) { + zsl_mtx_get(m, 0, g, &cur); /* Get value at (0, g). */ + zsl_mtx_init(&mr, NULL); /* Clear mr. */ + zsl_mtx_reduce(m, &mr, 0, g); /* Remove row 0, column g. */ + rc = zsl_mtx_deter(&mr, &dtmp); /* Calc. determinant of mr. */ + sign = 1.0; + if (rc) { + return -EINVAL; + } + + /* Uneven elements are negative. */ + if (g % 2 != 0) { + sign = -1.0; + } + + /* Add current determinant to final output value. */ + *d += dtmp * cur * sign; + } + + return 0; + } + + int + zsl_mtx_gauss_elim(struct zsl_mtx *m, struct zsl_mtx *mg, struct zsl_mtx *mi, + size_t i, size_t j) + { + int rc; + zsl_real_t x, y; + zsl_real_t epsilon = 1E-6; + + /* Make a copy of matrix m. */ + rc = zsl_mtx_copy(mg, m); + if (rc) { + return -EINVAL; + } + + /* Get the value of the element at position (i, j). */ + rc = zsl_mtx_get(mg, i, j, &y); + if (rc) { + return rc; + } + + /* If this is a zero value, don't do anything. */ + if ((y >= 0 && y < epsilon) || (y <= 0 && y > -epsilon)) { + return 0; + } + + /* Cycle through the matrix row by row. */ + for (size_t p = 0; p < mg->sz_rows; p++) { + /* Skip row 'i'. */ + if (p == i) { + p++; + } + if (p == mg->sz_rows) { + break; + } + /* Get the value of (p, j), aborting if value is zero. */ + zsl_mtx_get(mg, p, j, &x); + if ((x >= 1E-6) || (x <= -1E-6)) { + rc = zsl_mtx_sum_rows_scaled_d(mg, p, i, -(x / y)); + + if (rc) { + return -EINVAL; + } + rc = zsl_mtx_sum_rows_scaled_d(mi, p, i, -(x / y)); + if (rc) { + return -EINVAL; + } + } + } + + return 0; + } + + int + zsl_mtx_gauss_elim_d(struct zsl_mtx *m, struct zsl_mtx *mi, size_t i, size_t j) + { + return zsl_mtx_gauss_elim(m, m, mi, i, j); + } + + int + zsl_mtx_gauss_reduc(struct zsl_mtx *m, struct zsl_mtx *mi, + struct zsl_mtx *mg) + { + zsl_real_t v[m->sz_rows]; + zsl_real_t epsilon = 1E-6; + zsl_real_t x; + zsl_real_t y; + + /* Copy the input matrix into 'mg' so all the changes will be done to + * 'mg' and the input matrix will not be destroyed. */ + zsl_mtx_copy(mg, m); + + for (size_t k = 0; k < m->sz_rows; k++) { + + /* Get every element in the diagonal. */ + zsl_mtx_get(mg, k, k, &x); + + /* If the diagonal element is zero, find another value in the + * same column that isn't zero and add the row containing + * the non-zero element to the diagonal element's row. */ + if ((x >= 0 && x < epsilon) || (x <= 0 && x > -epsilon)) { + zsl_mtx_get_col(mg, k, v); + for (size_t q = 0; q < m->sz_rows; q++) { + zsl_mtx_get(mg, q, q, &y); + if ((v[q] >= epsilon) || (v[q] <= -epsilon)) { + + /* If the non-zero element found is + * above the diagonal, only add its row + * if the diagonal element in this row + * is zero, to avoid undoing previous + * steps. */ + if (q < k && ((y >= epsilon) + || (y <= -epsilon))) { + } else { + zsl_mtx_sum_rows_d(mg, k, q); + zsl_mtx_sum_rows_d(mi, k, q); + break; + } + } + } + } + + /* Perform the gaussian elimination in the column of the + * diagonal element to get rid of all the values in the column + * except for the diagonal one. */ + zsl_mtx_gauss_elim_d(mg, mi, k, k); + + /* Divide the diagonal element's row by the diagonal element. */ + zsl_mtx_norm_elem_d(mg, mi, k, k); + } + + return 0; + } + + int + zsl_mtx_gram_schmidt(struct zsl_mtx *m, struct zsl_mtx *mort) + { + ZSL_VECTOR_DEF(v, m->sz_rows); + ZSL_VECTOR_DEF(w, m->sz_rows); + ZSL_VECTOR_DEF(q, m->sz_rows); + + for (size_t t = 0; t < m->sz_cols; t++) { + zsl_vec_init(&q); + zsl_mtx_get_col(m, t, v.data); + for (size_t g = 0; g < t; g++) { + zsl_mtx_get_col(mort, g, w.data); + + /* Calculate the projection of every column vector + * before 'g' on the 't'th column. */ + zsl_vec_project(&w, &v, &w); + zsl_vec_add(&q, &w, &q); + } + + /* Substract the sum of the projections on the 't'th column from + * the 't'th column and set this vector as the 't'th column of + * the output matrix. */ + zsl_vec_sub(&v, &q, &v); + zsl_mtx_set_col(mort, t, v.data); + } + + return 0; + } + + int + zsl_mtx_cols_norm(struct zsl_mtx *m, struct zsl_mtx *mnorm) + { + ZSL_VECTOR_DEF(v, m->sz_rows); + + for (size_t g = 0; g < m->sz_cols; g++) { + zsl_mtx_get_col(m, g, v.data); + zsl_vec_to_unit(&v); + zsl_mtx_set_col(mnorm, g, v.data); + } + + return 0; + } + + int + zsl_mtx_norm_elem(struct zsl_mtx *m, struct zsl_mtx *mn, struct zsl_mtx *mi, + size_t i, size_t j) + { + int rc; + zsl_real_t x; + zsl_real_t epsilon = 1E-6; + + /* Make a copy of matrix m. */ + rc = zsl_mtx_copy(mn, m); + if (rc) { + return -EINVAL; + } + + /* Get the value to normalise. */ + rc = zsl_mtx_get(mn, i, j, &x); + if (rc) { + return rc; + } + + /* If the value is 0.0, abort. */ + if ((x >= 0 && x < epsilon) || (x <= 0 && x > -epsilon)) { + return 0; + } + + rc = zsl_mtx_scalar_mult_row_d(mn, i, (1.0 / x)); + if (rc) { + return -EINVAL; + } + + rc = zsl_mtx_scalar_mult_row_d(mi, i, (1.0 / x)); + if (rc) { + return -EINVAL; + } + + return 0; + } + + int + zsl_mtx_norm_elem_d(struct zsl_mtx *m, struct zsl_mtx *mi, size_t i, size_t j) + { + return zsl_mtx_norm_elem(m, m, mi, i, j); + } + + int + zsl_mtx_inv_3x3(struct zsl_mtx *m, struct zsl_mtx *mi) + { + int rc; + zsl_real_t d; /* Determinant. */ + zsl_real_t s; /* Scale factor. */ + + /* Make sure these are square matrices. */ + if ((m->sz_rows != m->sz_cols) || (mi->sz_rows != mi->sz_cols)) { + return -EINVAL; + } + + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure 'm' and 'mi' have the same shape. */ + if (m->sz_rows != mi->sz_rows) { + return -EINVAL; + } + if (m->sz_cols != mi->sz_cols) { + return -EINVAL; + } + /* Make sure these are 3x3 matrices. */ + if ((m->sz_cols != 3) || (mi->sz_cols != 3)) { + return -EINVAL; + } + #endif + + /* Calculate the determinant. */ + rc = zsl_mtx_deter_3x3(m, &d); + if (rc) { + goto err; + } + + /* Calculate the adjoint matrix. */ + rc = zsl_mtx_adjoint_3x3(m, mi); + if (rc) { + goto err; + } + + /* Scale the output using the determinant. */ + if (d != 0) { + s = 1.0 / d; + rc = zsl_mtx_scalar_mult_d(mi, s); + } else { + /* Provide an identity matrix if the determinant is zero. */ + rc = zsl_mtx_init(mi, zsl_mtx_entry_fn_identity); + if (rc) { + return -EINVAL; + } + } + + return 0; + err: + return rc; + } + + int + zsl_mtx_inv(struct zsl_mtx *m, struct zsl_mtx *mi) + { + int rc; + zsl_real_t d = 0.0; + + /* Shortcut for 3x3 matrices. */ + if (m->sz_rows == 3) { + return zsl_mtx_inv_3x3(m, mi); + } + + /* Make sure we have square matrices. */ + if ((m->sz_rows != m->sz_cols) || (mi->sz_rows != mi->sz_cols)) { + return -EINVAL; + } + + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure 'm' and 'mi' have the same shape. */ + if (m->sz_rows != mi->sz_rows) { + return -EINVAL; + } + if (m->sz_cols != mi->sz_cols) { + return -EINVAL; + } + #endif + + /* Make a copy of matrix m on the stack to avoid modifying it. */ + ZSL_MATRIX_DEF(m_tmp, mi->sz_rows, mi->sz_cols); + rc = zsl_mtx_copy(&m_tmp, m); + if (rc) { + return -EINVAL; + } + + /* Initialise 'mi' as an identity matrix. */ + rc = zsl_mtx_init(mi, zsl_mtx_entry_fn_identity); + if (rc) { + return -EINVAL; + } + + /* Make sure the determinant of 'm' is not zero. */ + zsl_mtx_deter(m, &d); + + if (d == 0) { + return 0; + } + + /* Use Gauss-Jordan elimination for nxn matrices. */ + zsl_mtx_gauss_reduc(m, mi, &m_tmp); + + return 0; + } + + int + zsl_mtx_cholesky(struct zsl_mtx *m, struct zsl_mtx *l) + { + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure 'm' is square. */ + if (m->sz_rows != m->sz_cols) { + return -EINVAL; + } + + /* Make sure 'm' is symmetric. */ + zsl_real_t a, b; + for (size_t i = 0; i < m->sz_rows; i++) { + for (size_t j = 0; j < m->sz_rows; j++) { + zsl_mtx_get(m, i, j, &a); + zsl_mtx_get(m, j, i, &b); + if (a != b) { + return -EINVAL; + } + } + } + + /* Make sure 'm' and 'l' have the same shape. */ + if (m->sz_rows != l->sz_rows) { + return -EINVAL; + } + if (m->sz_cols != l->sz_cols) { + return -EINVAL; + } + #endif + + zsl_real_t sum, x, y; + zsl_mtx_init(l, zsl_mtx_entry_fn_empty); + for (size_t j = 0; j < m->sz_cols; j++) { + sum = 0.0; + for (size_t k = 0; k < j; k++) { + zsl_mtx_get(l, j, k, &x); + sum += x * x; + } + zsl_mtx_get(m, j, j, &x); + zsl_mtx_set(l, j, j, ZSL_SQRT(x - sum)); + + for (size_t i = j + 1; i < m->sz_cols; i++) { + sum = 0.0; + for (size_t k = 0; k < j; k++) { + zsl_mtx_get(l, j, k, &x); + zsl_mtx_get(l, i, k, &y); + sum += y * x; + } + zsl_mtx_get(l, j, j, &x); + zsl_mtx_get(m, i, j, &y); + zsl_mtx_set(l, i, j, (y - sum) / x); + } + } + + return 0; + } + + int + zsl_mtx_balance(struct zsl_mtx *m, struct zsl_mtx *mout) + { + int rc; + bool done = false; + zsl_real_t sum; + zsl_real_t row, row2; + zsl_real_t col, col2; + + /* Make sure we have square matrices. */ + if ((m->sz_rows != m->sz_cols) || (mout->sz_rows != mout->sz_cols)) { + return -EINVAL; + } + + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure 'm' and 'mout' have the same shape. */ + if (m->sz_rows != mout->sz_rows) { + return -EINVAL; + } + if (m->sz_cols != mout->sz_cols) { + return -EINVAL; + } + #endif + + rc = zsl_mtx_copy(mout, m); + if (rc) { + goto err; + } + + while (!done) { + done = true; + + for (size_t i = 0; i < m->sz_rows; i++) { + /* Calculate sum of components of each row, column. */ + for (size_t j = 0; j < m->sz_cols; j++) { + row += ZSL_ABS(mout->data[(i * m->sz_rows) + + j]); + col += ZSL_ABS(mout->data[(j * m->sz_rows) + + i]); + } + + /* TODO: Extend with a check against epsilon? */ + if (col != 0.0 && row != 0.0) { + row2 = row / 2.0; + col2 = 1.0; + sum = col + row; + + while (col < row2) { + col2 *= 2.0; + col *= 4.0; + } + + row2 = row * 2.0; + + while (col > row2) { + col2 /= 2.0; + col /= 4.0; + } + + if ((col + row) / col2 < 0.95 * sum) { + done = false; + row2 = 1.0 / col2; + + for (int k = 0; k < m->sz_rows; k++) { + mout->data[(i * m->sz_rows) + k] + *= row2; + mout->data[(k * m->sz_rows) + i] + *= col2; + } + } + } + + row = 0.0; + col = 0.0; + } + } + + err: + return rc; + } + + int + zsl_mtx_householder(struct zsl_mtx *m, struct zsl_mtx *h, bool hessenberg) + { + size_t size = m->sz_rows; + + if (hessenberg == true) { + size--; + } + + ZSL_VECTOR_DEF(v, size); + ZSL_VECTOR_DEF(v2, m->sz_rows); + ZSL_VECTOR_DEF(e1, size); + + ZSL_MATRIX_DEF(mv, size, 1); + ZSL_MATRIX_DEF(mvt, 1, size); + ZSL_MATRIX_DEF(id, size, size); + ZSL_MATRIX_DEF(vvt, size, size); + ZSL_MATRIX_DEF(h2, size, size); + + /* Create the e1 vector, i.e. the vector (1, 0, 0, ...). */ + zsl_vec_init(&e1); + e1.data[0] = 1.0; + + /* Get the first column of the input matrix. */ + zsl_mtx_get_col(m, 0, v2.data); + if (hessenberg == true) { + zsl_vec_get_subset(&v2, 1, size, &v); + } else { + zsl_vec_copy(&v, &v2); + } + + /* Change the 'sign' value according to the sign of the first + * coefficient of the matrix. */ + zsl_real_t sign = 1.0; + + if (v.data[0] < 0) { + sign = -1.0; + } + + /* Calculate the vector 'v' that will later be used to calculate the + * Householder matrix. */ + zsl_vec_scalar_mult(&e1, -sign * zsl_vec_norm(&v)); + + zsl_vec_add(&v, &e1, &v); + + zsl_vec_scalar_div(&v, zsl_vec_norm(&v)); + + /* Calculate the H householder matrix by doing: + * H = IDENTITY - 2 * v * v^t. */ + zsl_mtx_from_arr(&mv, v.data); + zsl_mtx_trans(&mv, &mvt); + zsl_mtx_mult(&mv, &mvt, &vvt); + zsl_mtx_init(&id, zsl_mtx_entry_fn_identity); + zsl_mtx_scalar_mult_d(&vvt, -2); + zsl_mtx_add(&id, &vvt, &h2); + + /* If Hessenberg set to true, augment the output to the size of 'm'. + * If Hessenberg set to false, this line of code will do nothing but + * copy the matrix 'h2' into the output matrix 'h', */ + zsl_mtx_augm_diag(&h2, h); + + return 0; + } + + int + zsl_mtx_qrd(struct zsl_mtx *m, struct zsl_mtx *q, struct zsl_mtx *r, + bool hessenberg) + { + ZSL_MATRIX_DEF(r2, m->sz_rows, m->sz_cols); + ZSL_MATRIX_DEF(hess, m->sz_rows, m->sz_cols); + ZSL_MATRIX_DEF(h, m->sz_rows, m->sz_rows); + ZSL_MATRIX_DEF(h2, m->sz_rows, m->sz_rows); + ZSL_MATRIX_DEF(qt, m->sz_rows, m->sz_rows); + + zsl_mtx_init(&h, NULL); + zsl_mtx_init(&qt, zsl_mtx_entry_fn_identity); + zsl_mtx_copy(r, m); + + for (size_t g = 0; g < (m->sz_rows - 1); g++) { + + /* Reduce the matrix by 'g' rows and columns each time. */ + ZSL_MATRIX_DEF(mred, (m->sz_rows - g), (m->sz_cols - g)); + ZSL_MATRIX_DEF(hred, (m->sz_rows - g), (m->sz_rows - g)); + + /* allocate the placeholder matrices for the reduction loop */ + ZSL_MATRIX_DEF(place1, r->sz_rows, r->sz_cols); + ZSL_MATRIX_DEF(place2, r->sz_rows, r->sz_cols); + + while(zsl_mtx_reduce_iter(r, &mred, &place1, &place2) != 0); + + /* Calculate the reduced Householder matrix 'hred'. */ + if (hessenberg == true) { + zsl_mtx_householder(&mred, &hred, true); + } else { + zsl_mtx_householder(&mred, &hred, false); + } + + /* Augment the Householder matrix to the input matrix size. */ + zsl_mtx_augm_diag(&hred, &h); + zsl_mtx_mult(&h, r, &r2); + + /* Multiply this Householder matrix by the previous ones, + * stacked in 'qt'. */ + zsl_mtx_mult(&h, &qt, &h2); + zsl_mtx_copy(&qt, &h2); + if (hessenberg == true) { + zsl_mtx_mult(&r2, &h, &hess); + zsl_mtx_copy(r, &hess); + } else { + zsl_mtx_copy(r, &r2); + } + } + + /* Calculate the 'q' matrix by transposing 'qt'. */ + zsl_mtx_trans(&qt, q); + + return 0; + } + + #ifndef CONFIG_ZSL_SINGLE_PRECISION + int + zsl_mtx_qrd_iter(struct zsl_mtx *m, struct zsl_mtx *mout, size_t iter) + { + int rc; + + ZSL_MATRIX_DEF(q, m->sz_rows, m->sz_rows); + ZSL_MATRIX_DEF(r, m->sz_rows, m->sz_rows); + + + /* Make a copy of 'm'. */ + rc = zsl_mtx_copy(mout, m); + if (rc) { + return -EINVAL; + } + + for (size_t g = 1; g <= iter; g++) { + /* Perform the QR decomposition. */ + zsl_mtx_qrd(mout, &q, &r, false); + + /* Multiply the results of the QR decomposition together but + * changing its order. */ + zsl_mtx_mult(&r, &q, mout); + } + + return 0; + } + #endif + + #ifndef CONFIG_ZSL_SINGLE_PRECISION + int + zsl_mtx_eigenvalues(struct zsl_mtx *m, struct zsl_vec *v, size_t iter) + { + zsl_real_t diag; + zsl_real_t sdiag; + size_t real = 0; + + /* Epsilon is used to check 0 values in the subdiagonal, to determine + * if any coimplekx values were found. Increasing the number of + * iterations will move these values closer to 0, but when using + * single-precision floats the numbers can still be quite large, so + * we need to set a delta of +/- 0.001 in this case. */ + + zsl_real_t epsilon = 1E-6; + + ZSL_MATRIX_DEF(mout, m->sz_rows, m->sz_rows); + ZSL_MATRIX_DEF(mtemp, m->sz_rows, m->sz_rows); + ZSL_MATRIX_DEF(mtemp2, m->sz_rows, m->sz_rows); + + /* Balance the matrix. */ + zsl_mtx_balance(m, &mtemp); + + /* Put the balanced matrix into hessenberg form. */ + zsl_mtx_qrd(&mtemp, &mout, &mtemp2, true); + + /* Calculate the upper triangular matrix by using the recursive QR + * decomposition method. */ + zsl_mtx_qrd_iter(&mtemp2, &mout, iter); + + zsl_vec_init(v); + + /* If the matrix is symmetric, then it will always have real + * eigenvalues, so treat this case appart. */ + if (zsl_mtx_is_sym(m) == true) { + for (size_t g = 0; g < m->sz_rows; g++) { + zsl_mtx_get(&mout, g, g, &diag); + v->data[g] = diag; + } + + return 0; + } + + /* + * If any value just below the diagonal is non-zero, it means that the + * numbers above and to the right of the non-zero value are a pair of + * complex values, a complex number and its conjugate. + * + * SVD will always return real numbers so this can be ignored, but if + * you are calculating eigenvalues outside the SVD method, you may + * get complex numbers, which will be indicated with the return error + * code '-ECOMPLEXVAL'. + * + * If the imput matrix has complex eigenvalues, then these will be + * ignored and the output vector will not include them. + * + * NOTE: The real and imaginary parts of the complex numbers are not + * available. This only checks if there are any complex eigenvalues and + * returns an appropriate error code to alert the user that there are + * non-real eigenvalues present. + */ + + for (size_t g = 0; g < (m->sz_rows - 1); g++) { + /* Check if any element just below the diagonal isn't zero. */ + zsl_mtx_get(&mout, g + 1, g, &sdiag); + if ((sdiag >= epsilon) || (sdiag <= -epsilon)) { + /* Skip two elements if the element below + * is not zero. */ + g++; + } else { + /* Get the diagonal element if the element below + * is zero. */ + zsl_mtx_get(&mout, g, g, &diag); + v->data[real] = diag; + real++; + } + } + + + /* Since it's not possible to check the coefficient below the last + * diagonal element, then check the element to its left. */ + zsl_mtx_get(&mout, (m->sz_rows - 1), (m->sz_rows - 2), &sdiag); + if ((sdiag >= epsilon) || (sdiag <= -epsilon)) { + /* Do nothing if the element to its left is not zero. */ + } else { + /* Get the last diagonal element if the element to its left + * is zero. */ + zsl_mtx_get(&mout, (m->sz_rows - 1), (m->sz_rows - 1), &diag); + v->data[real] = diag; + real++; + } + + /* If the number of real eigenvalues ('real' coefficient) is less than + * the matrix dimensions, then there must be complex eigenvalues. */ + v->sz = real; + if (real != m->sz_rows) { + return -ECOMPLEXVAL; + } + + /* Put the zeros to the end. */ + zsl_vec_zte(v); + + return 0; + } + #endif + + #ifndef CONFIG_ZSL_SINGLE_PRECISION + int + zsl_mtx_eigenvectors(struct zsl_mtx *m, struct zsl_mtx *mev, size_t iter, + bool orthonormal) + { + size_t b = 0; /* Total number of eigenvectors. */ + size_t e_vals = 0; /* Number of unique eigenvalues. */ + size_t count = 0; /* Number of eigenvectors for an eigenvalue. */ + size_t ga = 0; + + zsl_real_t epsilon = 1E-6; + zsl_real_t x; + + /* The vector where all eigenvalues will be stored. */ + ZSL_VECTOR_DEF(k, m->sz_rows); + /* Temp vector to store column data. */ + ZSL_VECTOR_DEF(f, m->sz_rows); + /* The vector where all UNIQUE eigenvalues will be stored. */ + ZSL_VECTOR_DEF(o, m->sz_rows); + /* Temporary mxm identity matrix placeholder. */ + ZSL_MATRIX_DEF(id, m->sz_rows, m->sz_rows); + /* 'm' minus the eigenvalues * the identity matrix (id). */ + ZSL_MATRIX_DEF(mi, m->sz_rows, m->sz_rows); + /* Placeholder for zsl_mtx_gauss_reduc calls (required param). */ + ZSL_MATRIX_DEF(mid, m->sz_rows, m->sz_rows); + /* Matrix containing all column eigenvectors for an eigenvalue. */ + ZSL_MATRIX_DEF(evec, m->sz_rows, m->sz_rows); + /* Matrix containing all column eigenvectors for an eigenvalue. + * Two matrices are required for the Gramm-Schmidt operation. */ + ZSL_MATRIX_DEF(evec2, m->sz_rows, m->sz_rows); + /* Matrix containing all column eigenvectors. */ + ZSL_MATRIX_DEF(mev2, m->sz_rows, m->sz_rows); + + /* TODO: Check that we have a SQUARE matrix, etc. */ + zsl_mtx_init(&mev2, NULL); + zsl_vec_init(&o); + zsl_mtx_eigenvalues(m, &k, iter); + + /* Copy every non-zero eigenvalue ONCE in the 'o' vector to get rid of + * repeated values. */ + for (size_t q = 0; q < m->sz_rows; q++) { + if ((k.data[q] >= epsilon) || (k.data[q] <= -epsilon)) { + if (zsl_vec_contains(&o, k.data[q], epsilon) == 0) { + o.data[e_vals] = k.data[q]; + /* Increment the unique eigenvalue counter. */ + e_vals++; + } + } + } + + /* If zero is also an eigenvalue, copy it once in 'o'. */ + if (zsl_vec_contains(&k, 0.0, epsilon) > 0) { + e_vals++; + } + + /* Calculates the null space of 'm' minus each eigenvalue times + * the identity matrix by performing the gaussian reduction. */ + for (size_t g = 0; g < e_vals; g++) { + count = 0; + ga = 0; + + zsl_mtx_init(&id, zsl_mtx_entry_fn_identity); + zsl_mtx_scalar_mult_d(&id, -o.data[g]); + zsl_mtx_add_d(&id, m); + zsl_mtx_gauss_reduc(&id, &mid, &mi); + + /* If 'orthonormal' is true, perform the following process. */ + if (orthonormal == true) { + /* Count how many eigenvectors ('count' coefficient) + * there are for each eigenvalue. */ + for (size_t h = 0; h < m->sz_rows; h++) { + zsl_mtx_get(&mi, h, h, &x); + if ((x >= 0.0 && x < epsilon) || + (x <= 0.0 && x > -epsilon)) { + count++; + } + } + + /* Resize evec* placeholders to have 'count' cols. */ + evec.sz_cols = count; + evec2.sz_cols = count; + + /* Get all the eigenvectors for each eigenvalue and set + * them as the columns of 'evec'. */ + for (size_t h = 0; h < m->sz_rows; h++) { + zsl_mtx_get(&mi, h, h, &x); + if ((x >= 0.0 && x < epsilon) || + (x <= 0.0 && x > -epsilon)) { + zsl_mtx_set(&mi, h, h, -1); + zsl_mtx_get_col(&mi, h, f.data); + zsl_vec_neg(&f); + zsl_mtx_set_col(&evec, ga, f.data); + ga++; + } + } + /* Orthonormalize the set of eigenvectors for each + * eigenvalue using the Gram-Schmidt process. */ + zsl_mtx_gram_schmidt(&evec, &evec2); + zsl_mtx_cols_norm(&evec2, &evec); + + /* Place these eigenvectors in the 'mev2' matrix, + * that will hold all the eigenvectors for different + * eigenvalues. */ + for (size_t gi = 0; gi < count; gi++) { + zsl_mtx_get_col(&evec, gi, f.data); + zsl_mtx_set_col(&mev2, b, f.data); + b++; + } + + } else { + /* Orthonormal is false. */ + /* Get the eigenvectors for every eigenvalue and place + * them in 'mev2'. */ + for (size_t h = 0; h < m->sz_rows; h++) { + zsl_mtx_get(&mi, h, h, &x); + if ((x >= 0.0 && x < epsilon) || + (x <= 0.0 && x > -epsilon)) { + zsl_mtx_set(&mi, h, h, -1); + zsl_mtx_get_col(&mi, h, f.data); + zsl_vec_neg(&f); + zsl_mtx_set_col(&mev2, b, f.data); + b++; + } + } + } + } + + /* Since 'b' is the number of eigenvectors, reduce 'mev' (of size + * m->sz_rows times b) to erase columns of zeros. */ + mev->sz_cols = b; + + for (size_t s = 0; s < b; s++) { + zsl_mtx_get_col(&mev2, s, f.data); + zsl_mtx_set_col(mev, s, f.data); + } + + /* Checks if the number of eigenvectors is the same as the shape of + * the input matrix. If the number of eigenvectors is less than + * the number of columns in the input matrix 'm', this will be + * indicated by EEIGENSIZE as a return code. */ + if (b != m->sz_cols) { + return -EEIGENSIZE; + } + + return 0; + } + #endif + + #ifndef CONFIG_ZSL_SINGLE_PRECISION + int + zsl_mtx_svd(struct zsl_mtx *m, struct zsl_mtx *u, struct zsl_mtx *e, + struct zsl_mtx *v, size_t iter) + { + ZSL_MATRIX_DEF(aat, m->sz_rows, m->sz_rows); + ZSL_MATRIX_DEF(upri, m->sz_rows, m->sz_rows); + ZSL_MATRIX_DEF(ata, m->sz_cols, m->sz_cols); + ZSL_MATRIX_DEF(at, m->sz_cols, m->sz_rows); + ZSL_VECTOR_DEF(ui, m->sz_rows); + ZSL_MATRIX_DEF(ui2, m->sz_cols, 1); + ZSL_MATRIX_DEF(ui3, m->sz_rows, 1); + ZSL_VECTOR_DEF(hu, m->sz_rows); + + zsl_real_t d; + size_t pu = 0; + size_t min = m->sz_cols; + zsl_real_t epsilon = 1E-6; + + zsl_mtx_trans(m, &at); + + /* Calculate 'm' times 'm' transposed and viceversa. */ + zsl_mtx_mult(m, &at, &aat); + zsl_mtx_mult(&at, m, &ata); + + /* Set the value 'min' as the minimum of number of columns and number + * of rows. */ + if (m->sz_rows <= m->sz_cols) { + min = m->sz_rows; + } + + /* Calculate the eigenvalues of the square matrix 'm' times 'm' + * transposed or the square matrix 'm' transposed times 'm', whichever + * is smaller in dimensions. */ + ZSL_VECTOR_DEF(ev, min); + if (min < m->sz_cols) { + zsl_mtx_eigenvalues(&aat, &ev, iter); + } else { + zsl_mtx_eigenvalues(&ata, &ev, iter); + } + + /* Place the square root of these eigenvalues in the diagonal entries + * of 'e', the sigma matrix. */ + zsl_mtx_init(e, NULL); + for (size_t g = 0; g < min; g++) { + zsl_mtx_set(e, g, g, ZSL_SQRT(ev.data[g])); + } + + /* Calculate the eigenvectors of 'm' times 'm' transposed and set them + * as the columns of the 'v' matrix. */ + zsl_mtx_eigenvectors(&ata, v, iter, true); + for (size_t i = 0; i < min; i++) { + zsl_mtx_get_col(v, i, ui.data); + zsl_mtx_from_arr(&ui2, ui.data); + zsl_mtx_get(e, i, i, &d); + + /* Calculate the column vectors of 'u' by dividing these + * eniegnvectors by the square root its eigenvalue and + * multiplying them by the input matrix. */ + zsl_mtx_mult(m, &ui2, &ui3); + if ((d >= 0.0 && d < epsilon) || (d <= 0.0 && d > -epsilon)) { + pu++; + } else { + zsl_mtx_scalar_mult_d(&ui3, (1 / d)); + zsl_vec_from_arr(&ui, ui3.data); + zsl_mtx_set_col(u, i, ui.data); + } + } + + /* Expand the columns of 'u' into an orthonormal basis if there are + * zero eigenvalues or if the number of columns in 'm' is less than the + * number of rows. */ + zsl_mtx_eigenvectors(&aat, &upri, iter, true); + for (size_t f = min - pu; f < m->sz_rows; f++) { + zsl_mtx_get_col(&upri, f, hu.data); + zsl_mtx_set_col(u, f, hu.data); + } + + return 0; + } + #endif + + #ifndef CONFIG_ZSL_SINGLE_PRECISION + int + zsl_mtx_pinv(struct zsl_mtx *m, struct zsl_mtx *pinv, size_t iter) + { + zsl_real_t x; + size_t min = m->sz_cols; + zsl_real_t epsilon = 1E-6; + + ZSL_MATRIX_DEF(u, m->sz_rows, m->sz_rows); + ZSL_MATRIX_DEF(e, m->sz_rows, m->sz_cols); + ZSL_MATRIX_DEF(v, m->sz_cols, m->sz_cols); + ZSL_MATRIX_DEF(et, m->sz_cols, m->sz_rows); + ZSL_MATRIX_DEF(ut, m->sz_rows, m->sz_rows); + ZSL_MATRIX_DEF(pas, m->sz_cols, m->sz_rows); + + /* Determine the SVD decomposition of 'm'. */ + zsl_mtx_svd(m, &u, &e, &v, iter); + + /* Transpose the 'u' matrix. */ + zsl_mtx_trans(&u, &ut); + + /* Set the value 'min' as the minimum of number of columns and number + * of rows. */ + if (m->sz_rows <= m->sz_cols) { + min = m->sz_rows; + } + + for (size_t g = 0; g < min; g++) { + + /* Invert the diagonal values in 'e'. If a value is zero, do + * nothing to it. */ + zsl_mtx_get(&e, g, g, &x); + if ((x < epsilon) || (x > -epsilon)) { + x = 1 / x; + zsl_mtx_set(&e, g, g, x); + } + } + + /* Transpose the sigma matrix. */ + zsl_mtx_trans(&e, &et); + + /* Multiply 'u' (transposed) times sigma (transposed and with inverted + * eigenvalues) times 'v'. */ + zsl_mtx_mult(&v, &et, &pas); + zsl_mtx_mult(&pas, &ut, pinv); + + return 0; + } + #endif + + int + zsl_mtx_min(struct zsl_mtx *m, zsl_real_t *x) + { + zsl_real_t min = m->data[0]; + + for (size_t i = 0; i < m->sz_cols * m->sz_rows; i++) { + if (m->data[i] < min) { + min = m->data[i]; + } + } + + *x = min; + + return 0; + } + + int + zsl_mtx_max(struct zsl_mtx *m, zsl_real_t *x) + { + zsl_real_t max = m->data[0]; + + for (size_t i = 0; i < m->sz_cols * m->sz_rows; i++) { + if (m->data[i] > max) { + max = m->data[i]; + } + } + + *x = max; + + return 0; + } + + int + zsl_mtx_min_idx(struct zsl_mtx *m, size_t *i, size_t *j) + { + zsl_real_t min = m->data[0]; + + *i = 0; + *j = 0; + + for (size_t _i = 0; _i < m->sz_rows; _i++) { + for (size_t _j = 0; _j < m->sz_cols; _j++) { + if (m->data[_i * m->sz_cols + _j] < min) { + min = m->data[_i * m->sz_cols + _j]; + *i = _i; + *j = _j; + } + } + } + + return 0; + } + + int + zsl_mtx_max_idx(struct zsl_mtx *m, size_t *i, size_t *j) + { + zsl_real_t max = m->data[0]; + + *i = 0; + *j = 0; + + for (size_t _i = 0; _i < m->sz_rows; _i++) { + for (size_t _j = 0; _j < m->sz_cols; _j++) { + if (m->data[_i * m->sz_cols + _j] > max) { + max = m->data[_i * m->sz_cols + _j]; + *i = _i; + *j = _j; + } + } + } + + return 0; + } + + bool + zsl_mtx_is_equal(struct zsl_mtx *ma, struct zsl_mtx *mb) + { + int res; + + /* Make sure shape is the same. */ + if ((ma->sz_rows != mb->sz_rows) || (ma->sz_cols != mb->sz_cols)) { + return false; + } + + res = memcmp(ma->data, mb->data, + sizeof(zsl_real_t) * (ma->sz_rows + ma->sz_cols)); + + return res == 0 ? true : false; + } + + bool + zsl_mtx_is_notneg(struct zsl_mtx *m) + { + for (size_t i = 0; i < m->sz_rows * m->sz_cols; i++) { + if (m->data[i] < 0.0) { + return false; + } + } + + return true; + } + + bool + zsl_mtx_is_sym(struct zsl_mtx *m) + { + zsl_real_t x; + zsl_real_t y; + zsl_real_t diff; + zsl_real_t epsilon = 1E-6; + + for (size_t i = 0; i < m->sz_rows; i++) { + for (size_t j = 0; j < m->sz_cols; j++) { + zsl_mtx_get(m, i, j, &x); + zsl_mtx_get(m, j, i, &y); + diff = x - y; + if (diff >= epsilon || diff <= -epsilon) { + return false; + } + } + } + + return true; + } + + int + zsl_mtx_print(struct zsl_mtx *m) + { + int rc; + zsl_real_t x; + + for (size_t i = 0; i < m->sz_rows; i++) { + for (size_t j = 0; j < m->sz_cols; j++) { + rc = zsl_mtx_get(m, i, j, &x); + if (rc) { + printf("Error reading (%zu,%zu)!\n", i, j); + return -EINVAL; + } + /* Print the current floating-point value. */ + printf("%f ", (double)x); + } + printf("\n"); + } + + printf("\n"); + + return 0; + } \ No newline at end of file diff --git a/firmware/src/src/aqua_ahrs/quaternions.cpp b/firmware/src/src/aqua_ahrs/quaternions.cpp new file mode 100644 index 00000000..660dd800 --- /dev/null +++ b/firmware/src/src/aqua_ahrs/quaternions.cpp @@ -0,0 +1,720 @@ +/* + * Copyright (c) 2021 Kevin Townsend + * + * SPDX-License-Identifier: Apache-2.0 + */ + + #include + #include + #include + #include "zsl.h" + #include "vectors.h" + #include "quaternions.h" + + /** + * @brief Helper function to compare float values. + * + * @param a First float too compare. + * @param b Second float to compare. + * @param epsilon Allowed deviatin between first and second values. + + * @return true If values are the same within the limits of epsilon. + * @return false If values are different to an extent greater than epsilon. + */ + static bool + zsl_quat_val_is_equal(zsl_real_t a, zsl_real_t b, zsl_real_t epsilon) + { + zsl_real_t c; + + c = a - b; + + if (ZSL_ABS(c) < epsilon) { + return 1; + } else { + return 0; + } + } + + void zsl_quat_init(struct zsl_quat *q, enum zsl_quat_type type) + { + switch (type) { + case ZSL_QUAT_TYPE_IDENTITY: + q->r = 1.0; + q->i = 0.0; + q->j = 0.0; + q->k = 0.0; + break; + case ZSL_QUAT_TYPE_EMPTY: + default: + q->r = 0.0; + q->i = 0.0; + q->j = 0.0; + q->k = 0.0; + break; + } + } + + zsl_real_t zsl_quat_magn(struct zsl_quat *q) + { + return ZSL_SQRT(q->r * q->r + q->i * q->i + q->j * q->j + q->k * q->k); + } + + int zsl_quat_to_unit(struct zsl_quat *q, struct zsl_quat *qn) + { + int rc = 0; + zsl_real_t m = zsl_quat_magn(q); + + if (ZSL_ABS(m) < 1E-6) { + qn->r = 0.0; + qn->i = 0.0; + qn->j = 0.0; + qn->k = 0.0; + } else { + qn->r = q->r / m; + qn->i = q->i / m; + qn->j = q->j / m; + qn->k = q->k / m; + } + + return rc; + } + + int zsl_quat_to_unit_d(struct zsl_quat *q) + { + return zsl_quat_to_unit(q, q); + } + + bool zsl_quat_is_unit(struct zsl_quat *q) + { + zsl_real_t unit_len; + + /* Verify that sqrt(r^2+i^2+j^2+k^2) = 1.0. */ + unit_len = ZSL_SQRT( + q->r * q->r + + q->i * q->i + + q->j * q->j + + q->k * q->k); + + return zsl_quat_val_is_equal(unit_len, 1.0, 1E-6); + } + + int zsl_quat_scale(struct zsl_quat *q, zsl_real_t s, struct zsl_quat *qs) + { + int rc = 0; + + qs->r = q->r * s; + qs->i = q->i * s; + qs->j = q->j * s; + qs->k = q->k * s; + + return rc; + } + + int zsl_quat_scale_d(struct zsl_quat *q, zsl_real_t s) + { + return zsl_quat_scale(q, s, q); + } + + int zsl_quat_mult(struct zsl_quat *qa, struct zsl_quat *qb, + struct zsl_quat *qm) + { + int rc = 0; + + /* Make copies so this function can be used as a destructive one. */ + struct zsl_quat qac; + struct zsl_quat qbc; + + qac.r = qa->r; + qac.i = qa->i; + qac.j = qa->j; + qac.k = qa->k; + + qbc.r = qb->r; + qbc.i = qb->i; + qbc.j = qb->j; + qbc.k = qb->k; + + qm->i = qac.r * qbc.i + qac.i * qbc.r + qac.j * qbc.k - qac.k * qbc.j; + qm->j = qac.r * qbc.j - qac.i * qbc.k + qac.j * qbc.r + qac.k * qbc.i; + qm->k = qac.r * qbc.k + qac.i * qbc.j - qac.j * qbc.i + qac.k * qbc.r; + qm->r = qac.r * qbc.r - qac.i * qbc.i - qac.j * qbc.j - qac.k * qbc.k; + + return rc; + } + + int zsl_quat_exp(struct zsl_quat *q, struct zsl_quat *qe) + { + int rc = 0; + + ZSL_VECTOR_DEF(v, 3); + zsl_real_t vmag; /* Magnitude of v. */ + zsl_real_t vsin; /* Sine of vm. */ + zsl_real_t rexp; /* Exponent of q->r. */ + + /* Populate the XYZ vector using ijk from q. */ + v.data[0] = q->i; + v.data[1] = q->j; + v.data[2] = q->k; + + /* Calculate magnitude of v. */ + vmag = zsl_vec_norm(&v); + + /* Normalise v to unit vector. */ + zsl_vec_to_unit(&v); + + vsin = ZSL_SIN(vmag); + rexp = ZSL_EXP(q->r); + + qe->r = ZSL_COS(vmag) * rexp; + qe->i = v.data[0] * vsin * rexp; + qe->j = v.data[1] * vsin * rexp; + qe->k = v.data[2] * vsin * rexp; + + return rc; + } + + int zsl_quat_log(struct zsl_quat *q, struct zsl_quat *ql) + { + int rc = 0; + + ZSL_VECTOR_DEF(v, 3); /* Vector part of unit quat q. */ + zsl_real_t qmag; /* Magnitude of q. */ + zsl_real_t racos; /* Acos of q->r/qmag. */ + + /* Populate the XYZ vector using ijk from q. */ + v.data[0] = q->i; + v.data[1] = q->j; + v.data[2] = q->k; + + /* Normalise v to unit vector. */ + zsl_vec_to_unit(&v); + + /* Calculate magnitude of input quat. */ + qmag = zsl_quat_magn(q); + + racos = ZSL_ACOS(q->r / qmag); + + ql->r = ZSL_LOG(qmag); + ql->i = v.data[0] * racos; + ql->j = v.data[1] * racos; + ql->k = v.data[2] * racos; + + return rc; + } + + int zsl_quat_pow(struct zsl_quat *q, zsl_real_t exp, + struct zsl_quat *qout) + { + int rc = 0; + + struct zsl_quat qlog; + struct zsl_quat qsc; + + rc = zsl_quat_log(q, &qlog); + if (rc) { + goto err; + } + + rc = zsl_quat_scale(&qlog, exp, &qsc); + if (rc) { + goto err; + } + + rc = zsl_quat_exp(&qsc, qout); + if (rc) { + zsl_quat_init(qout, ZSL_QUAT_TYPE_EMPTY); + goto err; + } + + err: + return rc; + } + + int zsl_quat_conj(struct zsl_quat *q, struct zsl_quat *qc) + { + int rc = 0; + + /* TODO: Check for div by zero before running this! */ + qc->r = q->r; + qc->i = q->i * -1.0; + qc->j = q->j * -1.0; + qc->k = q->k * -1.0; + + return rc; + } + + int zsl_quat_inv(struct zsl_quat *q, struct zsl_quat *qi) + { + int rc = 0; + zsl_real_t m = zsl_quat_magn(q); + + if (ZSL_ABS(m) < 1E-6) { + /* Set to all 0's. */ + zsl_quat_init(qi, ZSL_QUAT_TYPE_EMPTY); + } else { + /* TODO: Check for div by zero before running this! */ + m *= m; + qi->r = q->r / m; + qi->i = q->i / -m; + qi->j = q->j / -m; + qi->k = q->k / -m; + } + + return rc; + } + + int zsl_quat_inv_d(struct zsl_quat *q) + { + return zsl_quat_inv(q, q); + } + + int zsl_quat_diff(struct zsl_quat *qa, struct zsl_quat *qb, + struct zsl_quat *qd) + { + int rc; + struct zsl_quat qi; + + rc = zsl_quat_inv(qa, &qi); + if (rc) { + goto err; + } + + /* Note: order matters here!*/ + rc = zsl_quat_mult(&qi, qb, qd); + + err: + return rc; + } + + int zsl_quat_rot(struct zsl_quat *qa, struct zsl_quat *qb, struct zsl_quat *qr) + { + int rc = 0; + + #if CONFIG_ZSL_BOUNDS_CHECKS + if (ZSL_ABS(qb->r) > 1E-6) { + rc = -EINVAL; + goto err; + } + #endif + + struct zsl_quat qm; + struct zsl_quat qn; + struct zsl_quat qi; + + zsl_quat_to_unit(qa, &qn); + zsl_quat_mult(&qn, qb, &qm); + zsl_quat_inv(&qn, &qi); + zsl_quat_mult(&qm, &qi, qr); + + #if CONFIG_ZSL_BOUNDS_CHECKS + err: + #endif + return rc; + } + + int zsl_quat_lerp(struct zsl_quat *qa, struct zsl_quat *qb, + zsl_real_t t, struct zsl_quat *qi) + { + int rc = 0; + + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure t is between 0 and 1 (included). */ + if (t < 0.0 || t > 1.0) { + rc = -EINVAL; + goto err; + } + #endif + + struct zsl_quat q1, q2; + + /* Turn input quaternions into unit quaternions. */ + struct zsl_quat qa_u; + struct zsl_quat qb_u; + zsl_quat_to_unit(qa, &qa_u); + zsl_quat_to_unit(qb, &qb_u); + + /* Calculate intermediate quats. */ + zsl_quat_scale(&qa_u, 1.0 - t, &q1); + zsl_quat_scale(&qb_u, t, &q2); + + /* Final result = q1 + q2. */ + qi->r = q1.r + q2.r; + qi->i = q1.i + q2.i; + qi->j = q1.j + q2.j; + qi->k = q1.k + q2.k; + + /* Normalize output quaternion. */ + zsl_quat_to_unit_d(qi); + + #if CONFIG_ZSL_BOUNDS_CHECKS + err: + #endif + return rc; + } + + int zsl_quat_slerp(struct zsl_quat *qa, struct zsl_quat *qb, + zsl_real_t t, struct zsl_quat *qi) + { + int rc = 0; + + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure t is between 0 and 1 (included). */ + if (t < 0.0 || t > 1.0) { + rc = -EINVAL; + goto err; + } + #endif + + struct zsl_quat q1, q2; /* Interim quats. */ + zsl_real_t dot; /* Dot product bewteen qa and qb. */ + zsl_real_t phi; /* arccos(dot). */ + zsl_real_t phi_s; /* sin(phi). */ + zsl_real_t phi_st; /* sin(phi * (t)). */ + zsl_real_t phi_smt; /* sin(phi * (1.0 - t)). */ + + /* + * Unit quaternion slerp = qa * (qa^-1 * qb)^t + * + * We get there in a round-about way in this code, but we avoid pushing + * and popping values on the stack with trivial calls to helper functions. + */ + + /* Turn input quaternions into unit quaternions. */ + struct zsl_quat qa_u; + struct zsl_quat qb_u; + zsl_quat_to_unit(qa, &qa_u); + zsl_quat_to_unit(qb, &qb_u); + + /* When t = 0.0 or t = 1.0, just memcpy qa or qb. */ + if (t == 0.0) { + qi->r = qa_u.r; + qi->i = qa_u.i; + qi->j = qa_u.j; + qi->k = qa_u.k; + goto err; + } else if (t == 1.0) { + qi->r = qb_u.r; + qi->i = qb_u.i; + qi->j = qb_u.j; + qi->k = qb_u.k; + goto err; + } + + /* Compute the dot product of the two normalized input quaternions. */ + dot = qa_u.r * qb_u.r + qa_u.i * qb_u.i + qa_u.j * qb_u.j + qa_u.k * qb_u.k; + + /* The value dot is always between -1 and 1. If dot = 1.0, qa = qb and there + * is no interpolation. */ + if (ZSL_ABS(dot - 1.0) < 1E-6) { + qi->r = qa_u.r; + qi->i = qa_u.i; + qi->j = qa_u.j; + qi->k = qa_u.k; + goto err; + } + + /* If dot = -1, then qa = - qb and the interpolation is invald. */ + if (ZSL_ABS(dot + 1.0) < 1E-6) { + rc = -EINVAL; + goto err; + } + + /* + * Slerp often has problems with angles close to zero. Consider handling + * those edge cases slightly differently? + */ + + /* Calculate these once before-hand. */ + phi = ZSL_ACOS(dot); + phi_s = ZSL_SIN(phi); + phi_st = ZSL_SIN(phi * t); + phi_smt = ZSL_SIN(phi * (1.0 - t)); + + /* Calculate intermediate quats. */ + zsl_quat_scale(&qa_u, phi_smt / phi_s, &q1); + zsl_quat_scale(&qb_u, phi_st / phi_s, &q2); + + /* Final result = q1 + q2. */ + qi->r = q1.r + q2.r; + qi->i = q1.i + q2.i; + qi->j = q1.j + q2.j; + qi->k = q1.k + q2.k; + + err: + return rc; + } + + int zsl_quat_from_ang_vel(struct zsl_vec *w, struct zsl_quat *qin, + zsl_real_t t, struct zsl_quat *qout) + { + int rc = 0; + + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure time is positive or zero and the angular velocity is a + * tridimensional vector. */ + if (w->sz != 3 || t < 0.0) { + rc = -EINVAL; + goto err; + } + #endif + + struct zsl_quat qin2; + struct zsl_quat qout2; + struct zsl_quat wq; + struct zsl_quat wquat = { + .r = 0.0, + .i = w->data[0], + .j = w->data[1], + .k = w->data[2] + }; + + zsl_quat_to_unit(qin, &qin2); + zsl_quat_mult(&wquat, &qin2, &wq); + zsl_quat_scale_d(&wq, 0.5 * t); + qout2.r = qin2.r + wq.r; + qout2.i = qin2.i + wq.i; + qout2.j = qin2.j + wq.j; + qout2.k = qin2.k + wq.k; + + zsl_quat_to_unit(&qout2, qout); + + #if CONFIG_ZSL_BOUNDS_CHECKS + err: + #endif + return rc; + } + + int zsl_quat_from_ang_mom(struct zsl_vec *l, struct zsl_quat *qin, + zsl_real_t *i, zsl_real_t t, struct zsl_quat *qout) + { + int rc = 0; + + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure time is positive or zero and the angular velocity is a + * tridimensional vector. Inertia can't be negative or zero. */ + if (l->sz != 3 || t < 0.0 || *i <= 0.0) { + rc = -EINVAL; + goto err; + } + #endif + + ZSL_VECTOR_DEF(w, 3); + zsl_vec_copy(&w, l); + zsl_vec_scalar_div(&w, *i); + zsl_quat_from_ang_vel(&w, qin, t, qout); + + #if CONFIG_ZSL_BOUNDS_CHECKS + err: + #endif + return rc; + } + + int zsl_quat_to_euler(struct zsl_quat *q, struct zsl_euler *e) + { + int rc = 0; + struct zsl_quat qn; + + zsl_quat_to_unit(q, &qn); + zsl_real_t gl = qn.i * qn.k + qn.j * qn.r; + zsl_real_t v = 2. * gl; + + if (v > 1.0) { + v = 1.0; + } else if (v < -1.0) { + v = -1.0; + } + + e->y = ZSL_ASIN(v); + + /* Gimbal lock case. */ + if (ZSL_ABS(gl - 0.5) < 1E-6 || ZSL_ABS(gl + 0.5) < 1E-6) { + e->x = ZSL_ATAN2(2.0 * (qn.j * qn.k + qn.i * qn.r), + 1.0 - 2.0 * (qn.i * qn.i + qn.k * qn.k)); + e->z = 0.0; + return rc; + } + + e->x = ZSL_ATAN2(2.0 * (qn.i * qn.r - qn.j * qn.k), + 1.0 - 2.0 * (qn.i * qn.i + qn.j * qn.j)); + e->z = ZSL_ATAN2(2.0 * (qn.k * qn.r - qn.i * qn.j), + 1.0 - 2.0 * (qn.j * qn.j + qn.k * qn.k)); + + return rc; + } + + int zsl_quat_from_euler(struct zsl_euler *e, struct zsl_quat *q) + { + int rc = 0; + + zsl_real_t roll_c = ZSL_COS(e->x * 0.5); + zsl_real_t roll_s = ZSL_SIN(e->x * 0.5); + zsl_real_t pitch_c = ZSL_COS(e->y * 0.5); + zsl_real_t pitch_s = ZSL_SIN(e->y * 0.5); + zsl_real_t yaw_c = ZSL_COS(e->z * 0.5); + zsl_real_t yaw_s = ZSL_SIN(e->z * 0.5); + + q->r = roll_c * pitch_c * yaw_c - roll_s * pitch_s * yaw_s; + q->i = roll_s * pitch_c * yaw_c + roll_c * pitch_s * yaw_s; + q->j = roll_c * pitch_s * yaw_c - roll_s * pitch_c * yaw_s; + q->k = roll_c * pitch_c * yaw_s + roll_s * pitch_s * yaw_c; + + return rc; + } + + int zsl_quat_to_rot_mtx(struct zsl_quat *q, struct zsl_mtx *m) + { + int rc = 0; + + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure that the rotation matrix has an appropriate shape and size. */ + if ((m->sz_cols != 3) || (m->sz_rows != 3)) { + rc = -EINVAL; + goto err; + } + #endif + + /* Note: This can be optimised by pre-calculating shared values. */ + + /* Row 0. */ + zsl_mtx_set(m, 0, 0, 1.0 - 2.0 * (q->j * q->j + q->k * q->k)); + zsl_mtx_set(m, 0, 1, 2.0 * (q->i * q->j - q->k * q->r)); + zsl_mtx_set(m, 0, 2, 2.0 * (q->i * q->k + q->j * q->r)); + + /* Row 1. */ + zsl_mtx_set(m, 1, 0, 2.0 * (q->i * q->j + q->k * q->r)); + zsl_mtx_set(m, 1, 1, 1.0 - 2.0 * (q->i * q->i + q->k * q->k)); + zsl_mtx_set(m, 1, 2, 2.0 * (q->j * q->k - q->i * q->r)); + + /* Row 2. */ + zsl_mtx_set(m, 2, 0, 2.0 * (q->i * q->k - q->j * q->r)); + zsl_mtx_set(m, 2, 1, 2.0 * (q->j * q->k + q->i * q->r)); + zsl_mtx_set(m, 2, 2, 1.0 - 2.0 * (q->i * q->i + q->j * q->j)); + + #if CONFIG_ZSL_BOUNDS_CHECKS + err: + #endif + return rc; + } + + int zsl_quat_from_rot_mtx(struct zsl_mtx *m, struct zsl_quat *q) + { + int rc = 0; + + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure that the rotation matrix has an appropriate shape and size. */ + if ((m->sz_cols != 3) || (m->sz_rows != 3)) { + rc = -EINVAL; + goto err; + } + #endif + + /* Convert rotation matrix to unit quaternion. */ + q->r = 0.5 * ZSL_SQRT(m->data[0] + m->data[4] + m->data[8] + 1.0); + q->i = 0.5 * ZSL_SQRT(m->data[0] - m->data[4] - m->data[8] + 1.0); + q->j = 0.5 * ZSL_SQRT(-m->data[0] + m->data[4] - m->data[8] + 1.0); + q->k = 0.5 * ZSL_SQRT(-m->data[0] - m->data[4] + m->data[8] + 1.0); + + if (ZSL_ABS(m->data[7] - m->data[5]) > 1E-6) { + /* Multiply by the sign of m21 - m12. */ + q->i *= (m->data[7] - m->data[5]) / ZSL_ABS(m->data[7] - m->data[5]); + } + + if (ZSL_ABS(m->data[2] - m->data[6]) > 1E-6) { + /* Multiply by the sign of m02 - m20. */ + q->j *= (m->data[2] - m->data[6]) / ZSL_ABS(m->data[2] - m->data[6]); + } + + if (ZSL_ABS(m->data[3] - m->data[1]) > 1E-6) { + /* Multiply by the sign of m10 - m01. */ + q->k *= (m->data[3] - m->data[1]) / ZSL_ABS(m->data[3] - m->data[1]); + } + + #if CONFIG_ZSL_BOUNDS_CHECKS + err: + #endif + return rc; + } + + int zsl_quat_to_axis_angle(struct zsl_quat *q, struct zsl_vec *a, + zsl_real_t *b) + { + int rc = 0; + + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure that the axis vector is size 3. */ + if (a->sz != 3) { + rc = -EINVAL; + goto err; + } + #endif + + struct zsl_quat qn; + zsl_quat_to_unit(q, &qn); + + if (ZSL_ABS(qn.r - 1.0) < 1E-6) { + a->data[0] = 0.0; + a->data[1] = 0.0; + a->data[2] = 0.0; + *b = 0.0; + return 0; + } + + zsl_real_t s = ZSL_SQRT(1.0 - (qn.r * qn.r)); + *b = 2.0 * ZSL_ACOS(qn.r); + a->data[0] = qn.i / s; + a->data[1] = qn.j / s; + a->data[2] = qn.k / s; + + #if CONFIG_ZSL_BOUNDS_CHECKS + err: + #endif + return rc; + } + + int zsl_quat_from_axis_angle(struct zsl_vec *a, zsl_real_t *b, + struct zsl_quat *q) + { + int rc = 0; + + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure that the axis vector is size 3. */ + if (a->sz != 3) { + rc = -EINVAL; + goto err; + } + #endif + + zsl_real_t norm = ZSL_SQRT(a->data[0] * a->data[0] + + a->data[1] * a->data[1] + + a->data[2] * a->data[2]); + + if (norm < 1E-6) { + q->r = 0.0; + q->i = 0.0; + q->j = 0.0; + q->k = 0.0; + return 0; + } + + ZSL_VECTOR_DEF(an, 3); + zsl_vec_copy(&an, a); + zsl_vec_scalar_div(&an, norm); + + q->r = ZSL_COS(*b / 2.0); + q->i = an.data[0] * ZSL_SIN(*b / 2.0); + q->j = an.data[1] * ZSL_SIN(*b / 2.0); + q->k = an.data[2] * ZSL_SIN(*b / 2.0); + + #if CONFIG_ZSL_BOUNDS_CHECKS + err: + #endif + return rc; + } + + int zsl_quat_print(struct zsl_quat *q) + { + printf("%.16f + %.16f i + %.16f j + %.16f k\n", + (double)q->r, (double)q->i, (double)q->j, (double)q->k); + + return 0; + } \ No newline at end of file diff --git a/firmware/src/src/aqua_ahrs/vectors.cpp b/firmware/src/src/aqua_ahrs/vectors.cpp new file mode 100644 index 00000000..b8430a61 --- /dev/null +++ b/firmware/src/src/aqua_ahrs/vectors.cpp @@ -0,0 +1,523 @@ +/* + * Copyright (c) 2019 Kevin Townsend (KTOWN) + * + * SPDX-License-Identifier: Apache-2.0 + */ + + #include + #include + #include + #include + #include "vectors.h" + #include "zsl.h" + + /* Enable optimised ARM Thumb/Thumb2 functions if available. */ + #if (CONFIG_ZSL_PLATFORM_OPT == 1 || CONFIG_ZSL_PLATFORM_OPT == 2) + #include + #endif + + int zsl_vec_init(struct zsl_vec *v) + { + memset(v->data, 0, v->sz * sizeof(zsl_real_t)); + + return 0; + } + + int zsl_vec_from_arr(struct zsl_vec *v, zsl_real_t *a) + { + memcpy(v->data, a, v->sz * sizeof(zsl_real_t)); + + return 0; + } + + int zsl_vec_copy(struct zsl_vec *vdest, struct zsl_vec *vsrc) + { + vdest->sz = vsrc->sz; + memcpy(vdest->data, vsrc->data, sizeof(zsl_real_t) * + vdest->sz); + + return 0; + } + + int zsl_vec_get_subset(struct zsl_vec *v, size_t offset, size_t len, + struct zsl_vec *vsub) + { + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure offset doesn't exceed v->sz. */ + if (offset >= v->sz) { + return -EINVAL; + } + /* If offset+len exceeds v->sz, truncate len. */ + if (offset + len >= v->sz) { + len = v->sz - offset; + } + /* Make sure vsub->sz is at least len, otherwise buffer overrun. */ + if (vsub->sz < len) { + return -EINVAL; + } + #endif + + /* Truncate vsub->sz if there is an underrun. */ + if (vsub->sz > len) { + vsub->sz = len; + } + + memcpy(vsub->data, &v->data[offset], len * sizeof(zsl_real_t)); + + return 0; + } + + #if !asm_vec_add + int zsl_vec_add(struct zsl_vec *v, struct zsl_vec *w, struct zsl_vec *x) + { + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure v and w are equal length. */ + if ((v->sz != w->sz) || (v->sz != x->sz)) { + return -EINVAL; + } + #endif + + for (size_t i = 0; i < v->sz; i++) { + x->data[i] = v->data[i] + w->data[i]; + } + + return 0; + } + #endif + + int zsl_vec_sub(struct zsl_vec *v, struct zsl_vec *w, struct zsl_vec *x) + { + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure v and w are equal length. */ + if ((v->sz != w->sz) || (v->sz != x->sz)) { + return -EINVAL; + } + #endif + + for (size_t i = 0; i < v->sz; i++) { + x->data[i] = v->data[i] - w->data[i]; + } + + return 0; + } + + int zsl_vec_neg(struct zsl_vec *v) + { + for (size_t i = 0; i < v->sz; i++) { + v->data[i] = -v->data[i]; + } + + return 0; + } + + int zsl_vec_sum(struct zsl_vec **v, size_t n, struct zsl_vec *w) + { + size_t sz_last; + + if (!n) { + return -EINVAL; + } + + sz_last = v[0]->sz; + + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure all vectors have the same size. */ + for (size_t i = 0; i < n; i++) { + if (sz_last != v[i]->sz) { + return -EINVAL; + } + } + #endif + + /* Sum all vectors. */ + w->sz = sz_last; + for (size_t i = 0; i < n; i++) { + for (size_t j = 0; j < w->sz; j++) { + w->data[j] += v[i]->data[j]; + } + } + + return 0; + } + + #if !asm_vec_scalar_add + int zsl_vec_scalar_add(struct zsl_vec *v, zsl_real_t s) + { + for (size_t i = 0; i < v->sz; i++) { + v->data[i] += s; + } + + return 0; + } + #endif + + #if !asm_vec_scalar_mult + int zsl_vec_scalar_mult(struct zsl_vec *v, zsl_real_t s) + { + for (size_t i = 0; i < v->sz; i++) { + v->data[i] *= s; + } + + return 0; + } + #endif + + #if !asm_vec_scalar_div + int zsl_vec_scalar_div(struct zsl_vec *v, zsl_real_t s) + { + /* Avoid divide by zero errors. */ + if (s == 0) { + return -EINVAL; + } + + for (size_t i = 0; i < v->sz; i++) { + v->data[i] /= s; + } + + return 0; + } + #endif + + zsl_real_t zsl_vec_dist(struct zsl_vec *v, struct zsl_vec *w) + { + int rc = 0; + + zsl_real_t xdat[v->sz]; + + struct zsl_vec x = { .sz = v->sz, .data = xdat }; + + memset(xdat, 0, x.sz * sizeof(zsl_real_t)); + + rc = zsl_vec_sub(v, w, &x); + if (rc) { + return NAN; + } + + return zsl_vec_norm(&x); + } + + int zsl_vec_dot(struct zsl_vec *v, struct zsl_vec *w, zsl_real_t *d) + { + zsl_real_t res = 0.0; + + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure v and w are equal length. */ + if (v->sz != w->sz) { + return -EINVAL; + } + #endif + + for (size_t i = 0; i < v->sz; i++) { + res += v->data[i] * w->data[i]; + } + + *d = res; + + return 0; + } + + zsl_real_t zsl_vec_norm(struct zsl_vec *v) + { + /* + * |v| = sqrt( v[0]^2 + v[1]^2 + V[...]^2 ) + */ + if (v == NULL) { + return 0; + } + return ZSL_SQRT(zsl_vec_sum_of_sqrs(v)); + } + + int zsl_vec_project(struct zsl_vec *u, struct zsl_vec *v, struct zsl_vec *w) + { + zsl_real_t p; + zsl_real_t t; + + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure v, w and u are equal length. */ + if ((v->sz != w->sz) || (v->sz != u->sz)) { + return -EINVAL; + } + #endif + + zsl_vec_copy(w, u); + zsl_vec_dot(v, u, &p); + zsl_vec_dot(u, u, &t); + zsl_vec_scalar_mult(w, p / t); + + return 0; + } + + int zsl_vec_to_unit(struct zsl_vec *v) + { + zsl_real_t norm = zsl_vec_norm(v); + + /* + * v + * unit(v) = --- + * |v| + */ + + /* Avoid divide by zero errors. */ + if (norm != 0.0f) { + zsl_vec_scalar_mult(v, 1.0f / norm); + } else { + /* TODO: What is the best approach here? */ + /* On div by zero clear vector and return v[0] = 1.0. */ + zsl_vec_init(v); + v->data[0] = 1.0f; + } + + return 0; + } + + int zsl_vec_cross(struct zsl_vec *v, struct zsl_vec *w, struct zsl_vec *c) + { + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure this is a 3-vector. */ + if ((v->sz != 3) || (w->sz != 3) || (c->sz != 3)) { + return -EINVAL; + } + #endif + + /* + * Given: + * + * |Cx| |Vx| |Wx| + * C = |Cy|, V = |Vy|, W = |Wy| + * |Cz| |Vz| |Wz| + * + * The cross product can be represented as: + * + * Cx = VyWz - VzWy + * Cy = VzWx - VxWz + * Cz = VxWy - VyWx + */ + + c->data[0] = v->data[1] * w->data[2] - v->data[2] * w->data[1]; + c->data[1] = v->data[2] * w->data[0] - v->data[0] * w->data[2]; + c->data[2] = v->data[0] * w->data[1] - v->data[1] * w->data[0]; + + return 0; + } + + zsl_real_t zsl_vec_sum_of_sqrs(struct zsl_vec *v) + { + zsl_real_t dot = 0.0f; + + zsl_vec_dot(v, v, &dot); + + return dot; + } + + int zsl_vec_mean(struct zsl_vec **v, size_t n, struct zsl_vec *m) + { + int rc; + + #if CONFIG_ZSL_BOUNDS_CHECKS + /* Make sure the mean vector has an approproate size. */ + if (m->sz != v[0]->sz) { + return -EINVAL; + } + #endif + + /* sum also checks that all vectors have the same length. */ + rc = zsl_vec_sum(v, n, m); + if (rc) { + return rc; + } + + rc = zsl_vec_scalar_mult(m, 1.0f / (zsl_real_t) n); + + return 0; + } + + int zsl_vec_ar_mean(struct zsl_vec *v, zsl_real_t *m) + { + /* Avoid divide by zero errors. */ + if (v->sz < 1) { + return -EINVAL; + } + + *m = 0.0; + for (size_t i = 0; i < v->sz; i++) { + *m += v->data[i]; + } + + *m /= v->sz; + + return 0; + } + + int zsl_vec_rev(struct zsl_vec *v) + { + zsl_real_t t; + size_t start = 0; + size_t end = v->sz - 1; + + while (start < end) { + t = v->data[start]; + v->data[start] = v->data[end]; + v->data[end] = t; + start++; + end--; + } + + return 0; + } + + int zsl_vec_zte(struct zsl_vec *v) + { + size_t x = 0; + zsl_real_t epsilon = 1E-6f; + + for (size_t g = 0; g < v->sz; g++) { + if ((v->data[g - x] >= 0.0f && v->data[g - x] < epsilon) || + (v->data[g - x] <= 0.0f && v->data[g - x] > epsilon)) { + for (size_t p = g - x; p < (v->sz - 1); p++) { + v->data[p] = v->data[p + 1]; + } + v->data[v->sz - 1] = 0.0; + x++; + } + } + + return 0; + } + + bool zsl_vec_is_equal(struct zsl_vec *v, struct zsl_vec *w, zsl_real_t eps) + { + zsl_real_t c; + + if (v->sz != w->sz) { + return false; + } + + for (size_t i = 0; i < v->sz; i++) { + c = v->data[i] - w->data[i]; + if (c >= eps || -c >= eps) { + return false; + } + } + + return true; + } + + bool zsl_vec_is_nonneg(struct zsl_vec *v) + { + for (size_t i = 0; i < v->sz; i++) { + if (v->data[i] < 0.0f) { + return false; + } + } + + return true; + } + + int zsl_vec_contains(struct zsl_vec *v, zsl_real_t val, zsl_real_t eps) + { + zsl_real_t c; + int count = 0; + + for (size_t i = 0; i < v->sz; i++) { + c = v->data[i] - val; + if (c < eps && -c < eps) { + count++; + } + } + + return count; + } + + /** + * @brief Quicksort implementation used by zsl_vec_sort. + * + * @param v Unsorted input vector. + * @param low Low index value. + * @param high High index value. + * + * @return 0 if everything executed properly, otherwise a negative error code. + */ + static int zsl_vec_quicksort(struct zsl_vec *v, size_t low, size_t high) + { + size_t i, j, p; + zsl_real_t t; + + if (low < high) { + p = low; + i = low; + j = high; + + while (i < j) { + while (v->data[i] <= v->data[p] && i <= high) { + i++; + } + while (v->data[j] > v->data[p] && j >= low) { + j--; + } + if (i < j) { + t = v->data[i]; + v->data[i] = v->data[j]; + v->data[j] = t; + } + } + + t = v->data[j]; + v->data[j] = v->data[p]; + v->data[p] = t; + + zsl_vec_quicksort(v, low, i - 1); + zsl_vec_quicksort(v, j + 1, high); + } + + return 0; + } + + int zsl_vec_sort(struct zsl_vec *v, struct zsl_vec *w) + { + /* Set counters to zero. */ + size_t count = 0; + size_t count2 = 0; + size_t i, j, k; + + ZSL_VECTOR_DEF(u, v->sz); + zsl_vec_init(&u); + + /* Copy the vector 'v' into the vector 'u' with no repeated values. */ + for (j = 0; j < v->sz; j++) { + if (v->data[j] >= 1E-5f || v->data[j] <= 1E-5f) { + if (zsl_vec_contains(&u, v->data[j], 1E-5f) == 0) { + u.data[count] = v->data[j]; + count++; + } + } + } + + if (zsl_vec_contains(v, 0.0f, 1E-5f) > 0) { + count++; + } + + u.sz = count; + + /* Sort the vector with no repeated values 'u'. */ + zsl_vec_quicksort(&u, 0, count - 1); + + /* Add back the repeated values in the correct order into the vector 'w'. */ + for (i = 0; i < count; i++) { + for (k = 0; k < zsl_vec_contains(v, u.data[i], 1E-5f); k++) { + w->data[count2] = u.data[i]; + count2++; + } + } + + return 0; + } + + int zsl_vec_print(struct zsl_vec *v) + { + for (size_t g = 0; g < v->sz; g++) { + printf("%f ", (double)v->data[g]); + } + printf("\n\n"); + + return 0; + } \ No newline at end of file diff --git a/firmware/src/src/include/fusion/aqua.h b/firmware/src/src/include/fusion/aqua.h new file mode 100644 index 00000000..7fda1266 --- /dev/null +++ b/firmware/src/src/include/fusion/aqua.h @@ -0,0 +1,121 @@ +/* + * Copyright (c) 2021 Marti Riba Pons + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/** + * @defgroup FUSION_ALG_AQUA AQUA + * + * @brief AQUA sensor fuion algorithm. + * + * @ingroup FUSION_ALGORITHMS + * @{ + */ + +/** + * @file + * @brief AQUA sensor fusion algorithm. + * + * This file implements the AQUA sensor fusion algorithm. + */ + + #ifndef ZEPHYR_INCLUDE_ZSL_FUSION_AQUA_H_ + #define ZEPHYR_INCLUDE_ZSL_FUSION_AQUA_H_ + + #include "vectors.h" + #include "matrices.h" + #include "quaternions.h" + + #ifdef __cplusplus + extern "C" { + #endif + + /* Source: + * https://res.mdpi.com/sensors/sensors-15-19302/article_deploy/sensors-15-19302.pdf + */ + + /** + * @brief Config settings for the AQUA sensor fusion algorithm. + */ + struct zsl_fus_aqua_cfg { + /** + * @brief A value between 0.0 and 1.0 which sets the gain for the + * cut-off frequency of the filter. Adjust this value to control + * LERP or SLERP, which reduces the amount of high frequency + * noise in the accelerometer. Alpha must be calculated in + * static conditions. + */ + zsl_real_t alpha; + + /** + * @brief Similar to alpha, this variable is set between 0.0 and 1.0 + * and is used to control the LERP or SLERP, which reduces the + * amount of high frequency noise in the magnetometer. Beta + * must be calculated in static conditions. + */ + zsl_real_t beta; + + /** + * @brief Threshold value to determine whether to compute LERP or + * SLERP to reduce the amount of high frequency noise of the + * accelerometer in the Dq_acc vector. Typically set to 0.9. + */ + zsl_real_t e_a; + + /** + * @brief Threshold value to determine whether to compute LERP or + * SLERP to reduce the amount of high frequency noise of the + * magnetometer in the Dq_mag vector. Typically set to 0.9. + */ + zsl_real_t e_m; + }; + + /** + * @brief Sets the sample frequency (in Hz) for the algorithm. + * + * @param freq Sampling frequency in Hz. + * @param cfg Config struct for this filter. + * + * @return int 0 if everything executed correctly, otherwise an appropriate + * negative error code. + */ + int zsl_fus_aqua_init(uint32_t freq, void *cfg); + + /** + * @brief Algebraic Quaterion Algorithm (AQUA) sensor fusion algorithm + * implementation. + * + * Source: + * https://res.mdpi.com/sensors/sensors-15-19302/article_deploy/sensors-15-19302.pdf + * + * @param a Input accelerometer vector (3 samples required). NULL if none. + * @param m Input magnetometer vector (3 samples required). NULL if none. + * @param g Input gyroscope vector (3 samples required). NULL if none. + * @param incl Input magnetic inclination, in degrees. NULL for none. + * @param q Pointer to the output @ref zsl_quat. + * @param cfg Pointer to the config struct for this algorithm. + * + * @return int 0 if everything executed correctly, otherwise an appropriate + * negative error code. + */ + int zsl_fus_aqua_feed(struct zsl_vec *a, struct zsl_vec *m, + struct zsl_vec *g, zsl_real_t *incl, struct zsl_quat *q, + void *cfg); + + /** + * @brief Default error handler for the AQUA sensor fusion driver. + * + * @note Normally you will want to implement your own version of this function. + * + * @param error The error code genereated. + */ + void zsl_fus_aqua_error(int error); + + #ifdef __cplusplus + } + #endif + + #endif /* ZEPHYR_INCLUDE_ZSL_FUSION_AQUA_H_ */ + + /** @} */ /* End of algorithms group */ \ No newline at end of file diff --git a/firmware/src/src/include/fusion/euler.h b/firmware/src/src/include/fusion/euler.h new file mode 100644 index 00000000..7c1fe7db --- /dev/null +++ b/firmware/src/src/include/fusion/euler.h @@ -0,0 +1,83 @@ +/* + * Copyright (c) 2019-2021 Kevin Townsend + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/** + * @defgroup EULER Euler Angles + * + * @brief Functions and structs for dealing with Euler (specifically, + * Cardanian) angles. + * + * @ingroup ORIENTATION + * @{ */ + +/** + * @file + * @brief API header file for euler angles in zscilib. + * + * This file contains the zscilib euler angle APIs + */ + + #ifndef ZEPHYR_INCLUDE_ZSL_EULER_H_ + #define ZEPHYR_INCLUDE_ZSL_EULER_H_ + + #include "vectors.h" + #include "matrices.h" + + #ifdef __cplusplus + extern "C" { + #endif + + /** + * @brief An ordered triplet of real numbers describing the orientation of a + * rigid body in three-dimensional (Euclidean) space, with respect to a + * fixed coordinate system. Each element represents the rotation about + * the specified axis, expressed in radians. + * + * @note Technically a Cardanian (AKA Tait–Bryan) angle, being limited to a + * single instance each of X, Y and Z. + */ + struct zsl_euler { + union { + struct { + zsl_real_t x; /**< @brief X axis, in radians. */ + zsl_real_t y; /**< @brief Y axis, in radians. */ + zsl_real_t z; /**< @brief Z axis, in radians. */ + }; + /** @brief Array-based access. */ + zsl_real_t idx[3]; + }; + }; + + /** + * @brief Takes the values in the supplied @ref zsl_euler, and assigns the + * same memory address to a @ref zsl_vec, allowing for vector functions + * to be used on the zsl_euler XYZ values. + * + * @param e Pointer to the zsl_euler struct to use. + * @param v Pointer to the zsl_vec struct to use. + * + * @return 0 if everything executed correctly, otherwise a negative error code. + */ + int zsl_eul_to_vec(struct zsl_euler *e, struct zsl_vec *v); + + /** + * @brief Print the supplied euler angles vector using printf in a + * human-readable manner. + * + * @param e Pointer to the vector containing the euler angles to print. + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_eul_print(struct zsl_euler *e); + + #ifdef __cplusplus + } + #endif + + #endif /* ZEPHYR_INCLUDE_ZSL_EULER_H_ */ + + /** @} */ /* End of euler group */ \ No newline at end of file diff --git a/firmware/src/src/include/fusion/matrices.h b/firmware/src/src/include/fusion/matrices.h new file mode 100644 index 00000000..a090e6bd --- /dev/null +++ b/firmware/src/src/include/fusion/matrices.h @@ -0,0 +1,1113 @@ +/* + * Copyright (c) 2019-2020 Kevin Townsend (KTOWN) + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/** + * @defgroup MATRICES Matrices + * + * @brief Various mxn matrices functions. + * + * TODO: Expand with examples, better high-level documentation, etc. + */ + +/** + * @file + * @brief API header file for matrices in zscilib. + * + * This file contains the zscilib matrix APIs + */ + + #ifndef ZEPHYR_INCLUDE_ZSL_MATRICES_H_ + #define ZEPHYR_INCLUDE_ZSL_MATRICES_H_ + + #include "vectors.h" + + #ifdef __cplusplus + extern "C" { + #endif + + /** + * @addtogroup MTX_STRUCTS Structs and Macros + * + * @brief Common structs and macros for working with matrices. + * + * @ingroup MATRICES + * @{ */ + + /** Error: The number of eigenvectors is less than the matrice's dimension. */ + #define EEIGENSIZE (100) + /** Error: Occurs when the input matrix has complex eigenvalues. */ + #define ECOMPLEXVAL (101) + + /** @brief Represents a m x n matrix, with data stored in row-major order. */ + struct zsl_mtx { + /** The number of rows in the matrix (typically denoted as 'm'). */ + size_t sz_rows; + /** The number of columns in the matrix (typically denoted as 'n'). */ + size_t sz_cols; + /** Data assigned to the matrix, in row-major order (left to right). */ + zsl_real_t *data; + }; + + /** + * Macro to declare a matrix of shape m*n. + * + * Be sure to also call 'zsl_mtx_init' on the matrix after this macro, since + * matrices declared on the stack may have non-zero values by default! + */ + #define ZSL_MATRIX_DEF(name, m, n); \ + zsl_real_t name ## _mtx[m * n]; \ + struct zsl_mtx name = { \ + .sz_rows = m, \ + .sz_cols = n, \ + .data = name ## _mtx \ + } + + /** @} */ /* End of MTX_STRUCTS group */ + + /** + * @addtogroup MTX_OPERANDS Operands + * + * @brief Unary and binary operands that can be performed on matrix + * coefficients. + * + * @ingroup MATRICES + * @{ */ + + /** @brief Component-wise unary operations. */ + typedef enum zsl_mtx_unary_op { + ZSL_MTX_UNARY_OP_INCREMENT, /**< ++ */ + ZSL_MTX_UNARY_OP_DECREMENT, /**< -- */ + ZSL_MTX_UNARY_OP_NEGATIVE, + ZSL_MTX_UNARY_OP_ROUND, + ZSL_MTX_UNARY_OP_ABS, + ZSL_MTX_UNARY_OP_FLOOR, + ZSL_MTX_UNARY_OP_CEIL, + ZSL_MTX_UNARY_OP_EXP, + ZSL_MTX_UNARY_OP_LOG, + ZSL_MTX_UNARY_OP_LOG10, + ZSL_MTX_UNARY_OP_SQRT, + ZSL_MTX_UNARY_OP_SIN, + ZSL_MTX_UNARY_OP_COS, + ZSL_MTX_UNARY_OP_TAN, + ZSL_MTX_UNARY_OP_ASIN, + ZSL_MTX_UNARY_OP_ACOS, + ZSL_MTX_UNARY_OP_ATAN, + ZSL_MTX_UNARY_OP_SINH, + ZSL_MTX_UNARY_OP_COSH, + ZSL_MTX_UNARY_OP_TANH, + } zsl_mtx_unary_op_t; + + /** @brief Component-wise binary operations. */ + typedef enum zsl_mtx_binary_op { + ZSL_MTX_BINARY_OP_ADD, /**< a + b */ + ZSL_MTX_BINARY_OP_SUB, /**< a - b */ + ZSL_MTX_BINARY_OP_MULT, /**< a * b */ + ZSL_MTX_BINARY_OP_DIV, /**< a / b */ + ZSL_MTX_BINARY_OP_MEAN, /**< mean(a, b) */ + ZSL_MTX_BINARY_OP_EXPON, /**< a ^ b */ + ZSL_MTX_BINARY_OP_MIN, /**< min(a, b) */ + ZSL_MTX_BINARY_OP_MAX, /**< max(a, b) */ + ZSL_MTX_BINARY_OP_EQUAL, /**< a == b */ + ZSL_MTX_BINARY_OP_NEQUAL, /**< a != b */ + ZSL_MTX_BINARY_OP_LESS, /**< a < b */ + ZSL_MTX_BINARY_OP_GREAT, /**< a > b */ + ZSL_MTX_BINARY_OP_LEQ, /**< a <= b */ + ZSL_MTX_BINARY_OP_GEQ, /**< a >= b */ + } zsl_mtx_binary_op_t; + + /** @} */ /* End of MTX_OPERANDS group */ + + /** + * @addtogroup MTX_INIT Initialisation + * + * @brief Functions used to initialise matrices. + * + * @ingroup MATRICES + * @{ */ + + /** + * @brief Function prototype called when applying a set of component-wise unary + * operations to a matrix via `zsl_mtx_unary_func`. + * + * @param m Pointer to the zsl_mtx to use. + * @param i The row number to write (0-based). + * @param j The column number to write (0-based). + * + * @return 0 on success, and non-zero error code on failure + */ + typedef int (*zsl_mtx_unary_fn_t)(struct zsl_mtx *m, size_t i, size_t j); + + /** + * @brief Function prototype called when applying a set of component-wise binary + * operations using a pair of symmetrical matrices via `zsl_mtx_binary_func`. + * + * @param ma Pointer to first zsl_mtx to use in the binary operation. + * @param mb Pointer to second zsl_mtx to use in the binary operation. + * @param mc Pointer to output zsl_mtx used to store results. + * @param i The row number to write (0-based). + * @param j The column number to write (0-based). + * + * @return 0 on success, and non-zero error code on failure + */ + typedef int (*zsl_mtx_binary_fn_t)(struct zsl_mtx *ma, struct zsl_mtx *mb, + struct zsl_mtx *mc, size_t i, size_t j); + + /** + * @brief Function prototype called when populating a matrix via `zsl_mtx_init`. + * + * @param m Pointer to the zsl_mtx to use. + * @param i The row number to write (0-based). + * @param j The column number to write (0-based). + * + * @return 0 on success, and non-zero error code on failure + */ + typedef int (*zsl_mtx_init_entry_fn_t)(struct zsl_mtx *m, size_t i, size_t j); + + /** + * @brief Assigns a zero-value to all entries in the matrix. + * + * @param m Pointer to the zsl_mtx to use. + * @param i The row number to write (0-based). + * @param j The column number to write (0-based). + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_mtx_entry_fn_empty(struct zsl_mtx *m, size_t i, size_t j); + + /** + * @brief Sets the value to '1.0' if the entry is on the diagonal (row=col), + * otherwise '0.0'. + * + * @param m Pointer to the zsl_mtx to use. + * @param i The row number to write (0-based). + * @param j The column number to write (0-based). + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_mtx_entry_fn_identity(struct zsl_mtx *m, size_t i, size_t j); + + /** + * @brief Sets the value to a random number between -1.0 and 1.0. + * + * @param m Pointer to the zsl_mtx to use. + * @param i The row number to write (0-based). + * @param j The column number to write (0-based). + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_mtx_entry_fn_random(struct zsl_mtx *m, size_t i, size_t j); + + /** + * @brief Initialises matrix 'm' using the specified entry function to + * assign values. + * + * @param m Pointer to the zsl_mtx to use. + * @param entry_fn The zsl_mtx_init_entry_fn_t instance to call. If this + * is set to NULL 'zsl_mtx_entry_fn_empty' will be called. + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_mtx_init(struct zsl_mtx *m, zsl_mtx_init_entry_fn_t entry_fn); + + /** + * @brief Converts an array of values into a matrix. + * + * The number of elements in array 'a' must match the number of elements in + * matrix 'm' (m.sz_rows * m.sz_cols). As such, 'm' should be a previously + * initialised matrix with appropriate values assigned to m.sz_rows and + * m.sz_cols. Assumes array values are in row-major order. + * + * @param m The matrix that the contents of array 'a' should be assigned to. + * The m.sz_rows and m.sz_cols dimensions must match the number of + * elements in 'a', meaning that the matrix should be initialised + * before being passed in to this function. + * @param a Pointer to the array containing the values to assign to 'm' in + * row-major order (left-to-right, top-to-bottom). The array will be + * read m.sz_rows * m.sz_cols elements deep. + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_mtx_from_arr(struct zsl_mtx *m, zsl_real_t *a); + + /** + * @brief Copies the contents of matrix 'msrc' into matrix 'mdest'. + * + * @param mdest Pointer to the destination matrix data will be copied to. + * @param msrc Pointer to the source matrix data will be copied from. + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_mtx_copy(struct zsl_mtx *mdest, struct zsl_mtx *msrc); + + /** @} */ /* End of MTX_INIT group */ + + /** + * @addtogroup MTX_DATACCESS Data Access + * + * @brief Functions used to access or modify matrix rows, columns or + * coefficients. + * + * @ingroup MATRICES + * @{ */ + + /** + * @brief Gets a single value from the specified row (i) and column (j). + * + * @param m Pointer to the zsl_mtx to use. + * @param i The row number to read (0-based). + * @param j The column number to read (0-based). + * @param x Pointer to where the value should be stored. + * + * @return 0 if everything executed correctly, or -EINVAL on an out of + * bounds error. + */ + int zsl_mtx_get(struct zsl_mtx *m, size_t i, size_t j, zsl_real_t *x); + + /** + * @brief Sets a single value at the specified row (i) and column (j). + * + * @param m Pointer to the zsl_mtx to use. + * @param i The row number to update (0-based). + * @param j The column number to update (0-based). + * @param x The value to assign. + * + * @return 0 if everything executed correctly, or -EINVAL on an out of + * bounds error. + */ + int zsl_mtx_set(struct zsl_mtx *m, size_t i, size_t j, zsl_real_t x); + + /** + * @brief Gets the contents of row 'i' from matrix 'm', assigning the array + * of data to 'v'. + * + * @param m Pointer to the zsl_mtx to use. + * @param i The row number to read (0-based). + * @param v Pointer to the array where the row vector should be written. + * Must be at least m->sz_cols elements long! + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_mtx_get_row(struct zsl_mtx *m, size_t i, zsl_real_t *v); + + /** + * @brief Sets the contents of row 'i' in matrix 'm', assigning the values + * found in array 'v'. + * + * @param m Pointer to the zsl_mtx to use. + * @param i The row number to write to (0-based). + * @param v Pointer to the array where the row vector data is stored. + * Must be at least m->sz_cols elements long! + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_mtx_set_row(struct zsl_mtx *m, size_t i, zsl_real_t *v); + + /** + * @brief Gets the contents of column 'j' from matrix 'm', assigning the array + * of data to 'v'. + * + * @param m Pointer to the zsl_mtx to use. + * @param j The column number to read (0-based). + * @param v Pointer to the array where the column vector should be written. + * Must be at least m->sz_rows elements long! + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_mtx_get_col(struct zsl_mtx *m, size_t j, zsl_real_t *v); + + /** + * @brief Sets the contents of column 'j' in matrix 'm', assigning the values + * found in array 'v'. + * + * @param m Pointer to the zsl_mtx to use. + * @param j The column number to write to (0-based). + * @param v Pointer to the array where the column vector data is stored. + * Must be at least m->sz_rows elements long! + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_mtx_set_col(struct zsl_mtx *m, size_t j, zsl_real_t *v); + + /** @} */ /* End of MTX_DATAACCESS group */ + + /** + * @ingroup MTX_OPERANDS + * @{ */ + + /** + * @brief Applies a unary operand on every coefficient in matrix 'm'. + * + * @param m Pointer to the zsl_mtx to use. + * @param op The unary operation to apply to each coefficient. + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_mtx_unary_op(struct zsl_mtx *m, zsl_mtx_unary_op_t op); + + /** + * @brief Applies a unary function on every coefficient in matrix 'm', using the + * specified 'zsl_mtx_apply_unary_fn_t' instance. + * + * @param m Pointer to the zsl_mtx to use. + * @param fn The zsl_mtx_unary_fn_t instance to call. + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_mtx_unary_func(struct zsl_mtx *m, zsl_mtx_unary_fn_t fn); + + /** + * @brief Applies a component-wise binary operation on every coefficient in + * symmetrical matrices 'ma' and 'mb', with the results being stored in the + * identically shaped `mc` matrix. + * + * @param ma Pointer to first zsl_mtx to use in the binary operation. + * @param mb Pointer to second zsl_mtx to use in the binary operation. + * @param mc Pointer to output zsl_mtx used to store results. + * @param op The binary operation to apply to each coefficient. + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_mtx_binary_op(struct zsl_mtx *ma, struct zsl_mtx *mb, + struct zsl_mtx *mc, zsl_mtx_binary_op_t op); + + /** + * @brief Applies a component-wise binary operztion on every coefficient in + * symmetrical matrices 'ma' and 'mb', with the results being stored in the + * identically shaped 'mc' matrix. The actual binary operation is executed + * using the specified 'zsl_mtx_binary_fn_t' callback. + * + * @param ma Pointer to first zsl_mtx to use in the binary operation. + * @param mb Pointer to second zsl_mtx to use in the binary operation. + * @param mc Pointer to output zsl_mtx used to store results. + * @param fn The zsl_mtx_binary_fn_t instance to call. + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_mtx_binary_func(struct zsl_mtx *ma, struct zsl_mtx *mb, + struct zsl_mtx *mc, zsl_mtx_binary_fn_t fn); + + /** @} */ /* End of MTX_OPERANDS group */ + + /** + * @addtogroup MTX_BASICMATH Basic Math + * + * @brief Basic mathematical operations for matrices (add, substract, etc.). + * + * @ingroup MATRICES + * @{ */ + + /** + * @brief Adds matrices 'ma' and 'mb', assigning the output to 'mc'. + * Matrices 'ma', 'mb' and 'mc' must all be identically shaped. + * + * @param ma Pointer to the first input zsl_mtx. + * @param mb Pointer to the second input zsl_mtx. + * @param mc Pointer to the output zsl_mtx. + * + * @return 0 if everything executed correctly, or -EINVAL if the three + * matrices are not all identically shaped. + */ + int zsl_mtx_add(struct zsl_mtx *ma, struct zsl_mtx *mb, struct zsl_mtx *mc); + + /** + * @brief Adds matrices 'ma' and 'mb', assigning the output to 'ma'. + * Matrices 'ma', and 'mb' must be identically shaped. + * + * @param ma Pointer to the first input zsl_mtx. This matrix will be + * overwritten with the results of the addition operations! + * @param mb Pointer to the second input zsl_mtx. + * + * @warning This function is destructive to 'ma'! + * + * @return 0 if everything executed correctly, or -EINVAL if the two input + * matrices are not identically shaped. + */ + int zsl_mtx_add_d(struct zsl_mtx *ma, struct zsl_mtx *mb); + + /** + * @brief Adds the values of row 'j' to row 'i' in matrix 'm'. This operation + * is destructive for row 'i'. + * + * @param m Pointer to the zsl_mtx to use. + * @param i The first row number to add (0-based). + * @param j The second row number to add (0-based). + * + * @return 0 if everything executed correctly, or -EINVAL on an out of + * bounds error. + */ + int zsl_mtx_sum_rows_d(struct zsl_mtx *m, size_t i, size_t j); + + /** + * @brief This function takes the coefficients of row 'j' and multiplies them + * by scalar 's', then adds the resulting coefficient to the parallel + * element in row 'i'. Row 'i' will be modified in this operation. + * + * This function implements a key mechanism of Gauss–Jordan elimination, as can + * be seen in @ref zsl_mtx_gauss_elim. + * + * @param m Pointer to the zsl_mtx to use. + * @param i The row number to update (0-based). + * @param j The row number to scale and then add to 'i' (0-based). + * @param s The scalar value to apply to the values in row 'j'. + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_mtx_sum_rows_scaled_d(struct zsl_mtx *m, size_t i, size_t j, + zsl_real_t s); + + /** + * @brief Subtracts matrices 'mb' from 'ma', assigning the output to 'mc'. + * Matrices 'ma', 'mb' and 'mc' must all be identically shaped. + * + * @param ma Pointer to the first input zsl_mtx. + * @param mb Pointer to the second input zsl_mtx. + * @param mc Pointer to the output zsl_mtx. + * + * @return 0 if everything executed correctly, or -EINVAL if the three + * matrices are not all identically shaped. + */ + int zsl_mtx_sub(struct zsl_mtx *ma, struct zsl_mtx *mb, struct zsl_mtx *mc); + + /** + * @brief Subtracts matrix 'mb' from 'ma', assigning the output to 'ma'. + * Matrices 'ma', and 'mb' must be identically shaped. + * + * @param ma Pointer to the first input zsl_mtx. This matrix will be + * overwritten with the results of the subtraction operations! + * @param mb Pointer to the second input zsl_mtx. + * + * @warning This function is destructive to 'ma'! + * + * @return 0 if everything executed correctly, or -EINVAL if the two input + * matrices are not identically shaped. + */ + int zsl_mtx_sub_d(struct zsl_mtx *ma, struct zsl_mtx *mb); + + /** + * @brief Multiplies matrix 'ma' by 'mb', assigning the output to 'mc'. + * Matrices 'ma' and 'mb' must be compatibly shaped, meaning that + * 'ma' must have the same numbers of columns as there are rows + * in 'mb'. + * + * @param ma Pointer to the first input zsl_mtx. + * @param mb Pointer to the second input zsl_mtx. + * @param mc Pointer to the output zsl_mtx. + * + * @return 0 if everything executed correctly, or -EINVAL if the input + * matrices are not compatibly shaped. + */ + int zsl_mtx_mult(struct zsl_mtx *ma, struct zsl_mtx *mb, struct zsl_mtx *mc); + + /** + * @brief Multiplies matrix 'ma' by 'mb', assigning the output to 'ma'. + * Matrices 'ma' and 'mb' must be compatibly shaped, meaning that + * 'ma' must have the same numbers of columns as there are rows + * in 'mb'. To use this function, 'mb' must be a square matrix. + * This function is destructive. + * + * @param ma Pointer to the first input zsl_mtx. + * @param mb Pointer to the second input zsl_mtx. + * + * @return 0 if everything executed correctly, or -EINVAL if the input + * matrices are not compatibly shaped. + */ + int zsl_mtx_mult_d(struct zsl_mtx *ma, struct zsl_mtx *mb); + + /** + * @brief Multiplies all elements in matrix 'm' by scalar value 's'. + * + * @param m Pointer to the zsl_mtz to adjust. + * @param s The scalar value to multiply elements in matrix 'm' with. + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_mtx_scalar_mult_d(struct zsl_mtx *m, zsl_real_t s); + + /** + * @brief Multiplies the elements of row 'i' in matrix 'm' by scalar 's'. + * + * @param m Pointer to the zsl_mtx to use. + * @param i The row number to multiply by scalar 's' (0-based). + * @param s The scalar to use when multiplying elements of row 'i'. + * + * @return 0 if everything executed correctly, or -EINVAL on an out of + * bounds error. + */ + int zsl_mtx_scalar_mult_row_d(struct zsl_mtx *m, size_t i, zsl_real_t s); + + /** @} */ /* End of MTX_BASICMATH group */ + + /** + * @addtogroup MTX_TRANSFORMATIONS Transformation + * + * @brief Transformation functions for matrices (transpose, inverse, guassian + * elimination, etc.). + * + * @ingroup MATRICES + * @{ */ + + /** + * @brief Transposes the matrix 'ma' into matrix 'mb'. Note that output + * matrix 'mb' must have 'ma->sz_rows' columns, and 'ma->sz_cols' rows. + * + * @param ma Pointer to the input matrix to transpose. + * @param mb Pointer to the output zsl_mtz. + * + * @return 0 if everything executed correctly, or -EINVAL if ma and mb are + * not compatibly shaped. + */ + int zsl_mtx_trans(struct zsl_mtx *ma, struct zsl_mtx *mb); + + /** + * @brief Calculates the ajoint matrix, based on the input 3x3 matrix 'm'. + * + * @param m The input 3x3 square matrix to use. + * @param ma The output 3x3 square matrix the adjoint values will be + * assigned to. + * + * @return 0 if everything executed correctly, or -EINVAL if this isn't a + * 3x3 square matrix. + */ + int zsl_mtx_adjoint_3x3(struct zsl_mtx *m, struct zsl_mtx *ma); + + /** + * @brief Calculates the ajoint matrix, based on the input square matrix 'm'. + * + * @param m The input square matrix to use. + * @param ma The output square matrix the adjoint values will be assigned to. + * + * @return 0 if everything executed correctly, or -EINVAL if this isn't a + * square matrix. + */ + int zsl_mtx_adjoint(struct zsl_mtx *m, struct zsl_mtx *ma); + + #ifndef CONFIG_ZSL_SINGLE_PRECISION + /** + * @brief Calculates the wedge product of n-1 vectors of size n, which are the + * rows of the matrix 'm'. This n-1 vectors must be linearly independent. + * + * The wedge product is an extension of the cross product for dimension greater + * than 3. Given a set of n-1 vectors of size n, the wedge product calculates + * a n-dimensional vector that is perpendicular to all the others. + * + * @param m The input (n-1) x n matrix, with n > 3, whose rows are the + * linearly independent vectors to use in the wedge product. + * @param v The output vector of dimension n, perpendicular to all the + * input vectors. + * + * @return 0 if everything executed correctly, or -EINVAL if n > 3, or if + * the input matrix isn't of the form (n-1) x n. + */ + int zsl_mtx_vec_wedge(struct zsl_mtx *m, struct zsl_vec *v); + #endif + + /** + * @brief Removes row 'i' and column 'j' from square matrix 'm', assigning the + * remaining elements in the matrix to 'mr'. + * + * @param m The input nxn square matrix to use. + * @param mr The output (n-1)x(n-1) square matrix. + * @param i The row number to remove (0-based). + * @param j The column number to remove (0-based). + * + * @return 0 if everything executed correctly, or -EINVAL if this isn't a + * square matrix. + */ + int zsl_mtx_reduce(struct zsl_mtx *m, struct zsl_mtx *mr, size_t i, size_t j); + + /* NOTE: This is used for household method/QR. Should it be in the main lib? */ + /** + * @brief Reduces the number of rows/columns in the input square matrix 'm' + * to match the shape of 'mred', where mred < m. Rows/cols will be + * removed starting on the left and upper vectors. + * + * @param m Pointer to the input square matrix. + * @param mred Pointer the the reduced output square matrix. + * @param place1 Pointer to first placeholder matrix, it must have same size of m. + * @param place2 Pointer to second placeholder matrix, it must have same size of m. + * + * @return 0 if everything executed correctly, otherwise -EAGAIN to instruct + * caller to call this method again because operation is not done yet + * + * @note this method should be called iteratively until it returns 0 + */ + int zsl_mtx_reduce_iter(struct zsl_mtx *m, struct zsl_mtx *mred, + struct zsl_mtx *place1, struct zsl_mtx *place2); + + /* NOTE: This is used for household method/QR. Should it be in the main lib? */ + /** + * @brief Augments the input square matrix with additional rows and columns, + * based on the size 'diff' between m and maug (where maug > m). New rows + * and columns are assigned values based on an identity matrix, meaning + * 1.0 on the new diagonal values and 0.0 above and below the diagonal. + * + * @param m Pointer to the input square matrix. + * @param maug Pointer the the augmented output square matrix. + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_mtx_augm_diag(struct zsl_mtx *m, struct zsl_mtx *maug); + + /** + * @brief Calculates the determinant of the input 3x3 matrix 'm'. + * + * @param m The input 3x3 square matrix to use. + * @param d The determinant of 3x3 square matrix m. + * + * @return 0 on success, or -EINVAL if this isn't a 3x3 square matrix. + */ + int zsl_mtx_deter_3x3(struct zsl_mtx *m, zsl_real_t *d); + + /** + * @brief Calculates the determinant of the input square matrix 'm'. + * + * @param m The input square matrix to use. + * @param d The determinant of square matrix m. + * + * @return 0 if everything executed correctly, or -EINVAL if this isn't a + * square matrix. + */ + int zsl_mtx_deter(struct zsl_mtx *m, zsl_real_t *d); + + /** + * @brief Given the element (i,j) in matrix 'm', this function performs + * gaussian elimination by adding row 'i' to the other rows until + * all of the elements in column 'j' are equal to 0.0, aside from the + * element at position (i, j). The result of this process will be + * assigned to matrix 'mg'. + * + * @param m Pointer to input zsl_mtx to use. + * @param mg Pointer to gaussian form output matrix. + * @param mi Pointer to the identity output matrix. + * @param i The row number of the element to use (0-based). + * @param j The column number of the element to use (0-based). + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_mtx_gauss_elim(struct zsl_mtx *m, struct zsl_mtx *mg, + struct zsl_mtx *mi, size_t i, size_t j); + + /** + * @brief Given the element (i,j) in matrix 'm', this function performs + * gaussian elimination by adding row 'i' to the other rows until + * all of the elements in column 'j' are equal to 0.0, aside from the + * element at position (i, j). This function is destructive and will + * modify the contents of m. + * + * @param m Pointer to zsl_mtx to use. + * @param mi Pointer to the identity output matrix. + * @param i The row number of the element to use (0-based). + * @param j The column number of the element to use (0-based). + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_mtx_gauss_elim_d(struct zsl_mtx *m, struct zsl_mtx *mi, + size_t i, size_t j); + + /** + * @brief Given matrix 'm', puts the matrix into echelon form using + * Gauss-Jordan reduction. + * + * @param m Pointer to the input square matrix to use. + * @param mi Pointer to the identity output matrix. + * @param mg Pointer to the output square matrix in echelon form. + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_mtx_gauss_reduc(struct zsl_mtx *m, struct zsl_mtx *mi, + struct zsl_mtx *mg); + + /** + * @brief Updates the values of every column vector in input matrix 'm' to have + * unitary values. + * + * @param m Pointer to the input matrix. + * @param mnorm Pointer to the output matrix where the columns are unitary + * vectors. + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_mtx_cols_norm(struct zsl_mtx *m, struct zsl_mtx *mnorm); + + /** + * @brief Performs the Gram-Schmidt algorithm on the set of column vectors in + * matrix 'm'. This algorithm calculates a set of orthogonal vectors in + * the same vectorial space as the original vectors. + * + * @param m Pointer to the input matrix containing the vector data. + * @param mort Pointer to the output matrix containing the orthogonal vector + * data. + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_mtx_gram_schmidt(struct zsl_mtx *m, struct zsl_mtx *mort); + + /** + * @brief Normalises elements in matrix m such that the element at position + * (i, j) is equal to 1.0. + * + * @param m Pointer to input zsl_mtx to use. + * @param mn Pointer to normalised output zsl_mtx. + * @param mi Pointer to the output identity zsl_mtx. + * @param i The row number of the element to use (0-based). + * @param j The column number of the element to use (0-based). + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_mtx_norm_elem(struct zsl_mtx *m, struct zsl_mtx *mn, struct zsl_mtx *mi, + size_t i, size_t j); + + /** + * @brief Normalises elements in matrix m such that the element at position + * (i, j) is equal to 1.0. This function is destructive and will + * modify the contents of m. + * + * @param m Pointer to zsl_mtx to use. + * @param mi Pointer to the output identity zsl_mtx. + * @param i The row number of the element to use (0-based). + * @param j The column number of the element to use (0-based). + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_mtx_norm_elem_d(struct zsl_mtx *m, struct zsl_mtx *mi, + size_t i, size_t j); + + /** + * @brief Calculates the inverse of 3x3 matrix 'm'. If the determinant of + * 'm' is zero, an identity matrix will be returned via 'mi'. + * + * @param m The input 3x3 matrix to use. + * @param mi The output inverse 3x3 matrix. + * + * @return 0 if everything executed correctly, or -EINVAL if this isn't a + * 3x3 matrix. + */ + int zsl_mtx_inv_3x3(struct zsl_mtx *m, struct zsl_mtx *mi); + + /** + * @brief Calculates the inverse of square matrix 'm'. + * + * @param m The input square matrix to use. + * @param mi The output inverse square matrix. + * + * @return 0 if everything executed correctly, or -EINVAL if this isn't a + * square matrix. + */ + int zsl_mtx_inv(struct zsl_mtx *m, struct zsl_mtx *mi); + + /** + * @brief Calculates the Cholesky decomposition of a symmetric square matrix + * using the Cholesky–Crout algorithm. + * + * Computing the Cholesky decomposition of a symmetric square matrix M consists + * in finding a matrix L such that M = L * Lt (L multiplied by its transpose). + * + * @param m The input symmetric square matrix to use. + * @param l The output lower triangular matrix. + * + * @return 0 if everything executed correctly, or -EINVAL if 'm' isn't a + * symmetric square matrix. + */ + int zsl_mtx_cholesky(struct zsl_mtx *m, struct zsl_mtx *l); + + /** + * @brief Balances the square matrix 'm', a process in which the eigenvalues of + * the output matrix are the same as the eigenvalues of the input matrix. + * + * @param m The input square matrix to use. + * @param mout The output balanced matrix. + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_mtx_balance(struct zsl_mtx *m, struct zsl_mtx *mout); + + /** + * @brief Calculates the householder reflection of 'm'. Used as part of QR + * decomposition. When 'hessenberg' is active, it calculates the + * householder reflection but without using the first line of 'm'. + * + * @param m Pointer to the input matrix to use. + * @param hessenberg If set to true, the first line in 'm' is ignored. + * @param h Pointer to the output square matrix where the + * values of the householder reflection should be assigned. + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_mtx_householder(struct zsl_mtx *m, struct zsl_mtx *h, bool hessenberg); + + /** + * @brief If 'hessenberg' is set to false, this function performs the QR + * decomposition, which is a factorisation of matrix 'm' into an + * orthogonal matrix (q) and an upper triangular matrix (r). If + * 'hessenberg' is set to true, this function puts the matrix 'm' into + * hessenberg form. This function uses the householder reflections. + * + * One of the uses of this function is to make use of the householder method to + * perform the QR decomposition, which introduces the zeros below the diagonal + * of matrix 'r'. Other algorithms exist, such as the Gram-Schmidt process, + * but they tend to be less stable than the householder method for a similar + * computational cost. + * + * @param m Pointer to the input square matrix. + * @param q Pointer to the output orthoogonal square matrix. + * @param r Pointer to the output upper triangular square matrix or + * hessenberg matrix if set to true. + * @param hessenberg Sets the matrix to hessenberg format if 'true'. + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_mtx_qrd(struct zsl_mtx *m, struct zsl_mtx *q, struct zsl_mtx *r, + bool hessenberg); + + #ifndef CONFIG_ZSL_SINGLE_PRECISION + /** + * @brief Computes recursively the QR decompisition method to put the input + * square matrix into upper triangular form. + * + * @param m The input square matrix to use when performing the QR + * decomposition. + * @param mout The output upper triangular square matrix where the results + * should be stored. + * @param iter The number of times that 'zsl_mtx_qrd' should be called. + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_mtx_qrd_iter(struct zsl_mtx *m, struct zsl_mtx *mout, size_t iter); + #endif + + #ifndef CONFIG_ZSL_SINGLE_PRECISION + /** + * @brief Calculates the eigenvalues for input matrix 'm' using QR + * decomposition recursively. The output vector will only contain real + * eigenvalues, even if the input matrix has complex eigenvalues. + * + * @param m The input square matrix to use. + * @param v The placeholder for the output vector where the real eigenvalues + * should be stored. + * @param iter The number of times that 'zsl_mtx_qrd' should be called + * during the QR decomposition phase. + * + * NOTE: Large values required more iterations to get precise results, and + * some trial and error may be required to find the right balance of + * iterations versus processing time. + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. If -ECOMPLEXVAL is returned, it means that complex + * numbers were detected in the output eigenvalues. + */ + int zsl_mtx_eigenvalues(struct zsl_mtx *m, struct zsl_vec *v, size_t iter); + #endif + + #ifndef CONFIG_ZSL_SINGLE_PRECISION + /** + * @brief Calcualtes the set of eigenvectors for input matrix 'm', using the + * specified number of iterations to find a balance between precision and + * processing effort. Optionally, the output eigenvectors can be + * orthonormalised. + * + * @param m The input square matrix to use. + * @param mev The placeholder for the output square matrix where the + * eigenvectors should be stored as column vectors. + * @param iter The number of times that 'zsl_mtx_qrd' should be called. + * during the QR decomposition phase. + * @param orthonormal If set to true, the output matrix 'mev' will be + * orthonormalised. + * + * NOTE: Large values required more iterations to get precise results, and + * some trial and error may be required to find the right balance of + * iterations versus processing time. + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. If the number of calcualted eigenvectors is less + * than the columns in 'm', EEIGENSIZE will be returned. + */ + int zsl_mtx_eigenvectors(struct zsl_mtx *m, struct zsl_mtx *mev, size_t iter, + bool orthonormal); + #endif + + #ifndef CONFIG_ZSL_SINGLE_PRECISION + /** + * @brief Performs singular value decomposition, converting input matrix 'm' + * into matrices 'u', 'e', and 'v'. + * + * @param m The input mxn matrix to use. + * @param u The placeholder for the output mxm matrix u. + * @param e The placeholder for the output mxn matrix sigma. + * @param v The placeholder for the output nxn matrix v. + * @param iter The number of times that 'zsl_mtx_qrd' should be called. + * during the QR decomposition phase. + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_mtx_svd(struct zsl_mtx *m, struct zsl_mtx *u, struct zsl_mtx *e, + struct zsl_mtx *v, size_t iter); + #endif + + #ifndef CONFIG_ZSL_SINGLE_PRECISION + /** + * @brief Performs the pseudo-inverse (aka pinv or Moore-Penrose inverse) + * on input matrix 'm'. + * + * @param m The input mxn matrix to use. + * @param pinv The placeholder for the output pseudo inverse nxm matrix. + * @param iter The number of times that 'zsl_mtx_qrd' should be called. + * during the QR decomposition phase. + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_mtx_pinv(struct zsl_mtx *m, struct zsl_mtx *pinv, size_t iter); + #endif + + /** @} */ /* End of MTX_TRANSFORMATIONS group */ + + /** + * @addtogroup MTX_LIMITS Limits + * + * @brief Min/max helpers to determine the range of the matrice's coefficients. + * + * @ingroup MATRICES + * @{ */ + + /** + * @brief Traverses the matrix elements to find the minimum element value. + * + * @param m Pointer to the zsl_mtz to traverse. + * @param x The minimum element value found in matrix 'm'. + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_mtx_min(struct zsl_mtx *m, zsl_real_t *x); + + /** + * @brief Traverses the matrix elements to find the maximum element value. + * + * @param m Pointer to the zsl_mtz to traverse. + * @param x The maximum element value found in matrix 'm'. + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_mtx_max(struct zsl_mtx *m, zsl_real_t *x); + + /** + * @brief Traverses the matrix elements to find the (i,j) index of the minimum + * element value. If multiple identical minimum values are founds, + * the (i, j) index values returned will refer to the first element. + * + * @param m Pointer to the zsl_mtz to traverse. + * @param i Pointer to the row index of the minimum element value found + * in matrix 'm'. + * @param j Pointer to the column index of the minimum element value + * found in matrix 'm'. + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_mtx_min_idx(struct zsl_mtx *m, size_t *i, size_t *j); + + /** + * @brief Traverses the matrix elements to find the (i,j) index of the maximum + * element value. If multiple identical maximum values are founds, + * the (i, j) index values returned will refer to the first element. + * + * @param m Pointer to the zsl_mtz to traverse. + * @param i Pointer to the row index of the maximum element value found + * in matrix 'm'. + * @param j Pointer to the column index of the maximum element value + * found in matrix 'm'. + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_mtx_max_idx(struct zsl_mtx *m, size_t *i, size_t *j); + + /** @} */ /* End of MTX_LIMITS group */ + + /** + * @addtogroup MTX_COMPARISON Comparison + * + * @brief Functions used to compare or verify matrices. + * + * @ingroup MATRICES + * @{ */ + + /** + * @brief Checks if two matrices are identical in shape and content. + * + * @param ma The first matrix. + * @param mb The second matrix. + * + * @return true if the two matrices have the same shape and values, + * otherwise false. + */ + bool zsl_mtx_is_equal(struct zsl_mtx *ma, struct zsl_mtx *mb); + + /** + * @brief Checks if all elements in matrix m are >= zero. + * + * @param m The matrix to check. + * + * @return true if the all matrix elements are zero or positive, + * otherwise false. + */ + bool zsl_mtx_is_notneg(struct zsl_mtx *m); + + /** + * @brief Checks if the square input matrix is symmetric. + * + * @param m The matrix to check. + * + * @return true if the matrix is symmetric, otherwise false. + */ + bool zsl_mtx_is_sym(struct zsl_mtx *m); + + /** @} */ /* End of MTX_COMPARISON group */ + + /** + * @addtogroup MTX_DISPLAY Display + * + * @brief Functions used to present matrices in a user-friendly format. + * + * @ingroup MATRICES + * @{ */ + + /** + * @brief Printf the supplied matrix using printf in a human-readable manner. + * + * @param m Pointer to the matrix to print. + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_mtx_print(struct zsl_mtx *m); + + // int zsl_mtx_fprint(FILE *stream, zsl_mtx *m); + + /** @} */ /* End of MTX_DISPLAY group */ + + #ifdef __cplusplus + } + #endif + + #endif /* ZEPHYR_INCLUDE_ZSL_MATRICES_H_ */ + + /** @} */ /* End of matrices group */ \ No newline at end of file diff --git a/firmware/src/src/include/fusion/quaternions.h b/firmware/src/src/include/fusion/quaternions.h new file mode 100644 index 00000000..612860db --- /dev/null +++ b/firmware/src/src/include/fusion/quaternions.h @@ -0,0 +1,513 @@ +/* + * Copyright (c) 2019-2021 Kevin Townsend + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/** + * @defgroup QUATERNIONS Quaternions + * + * @brief Functions and structs for dealing with unit quaternions and + * orientation in 3D space. + * + * These functions deal largely with unit quaternions. Further work is + * required to support arbitrary quaternion math. + * + * Quaternions can be thought of in one of three ways: + * + * 1. As a vector with four components + * 2. As a scalar (r) with a three component vector in right-hand ijk + * space (i, j, k) + * 3. As a hypercomplex number consisting of a scalar real number (r), + * and three imaginary numbers (i, j, k) + * + * When working with unit quaternions and orientation or rotation, we'll + * generally look at quaternions from the second point-of-view, as a scalar + * and three component vector in 3D space. + * + * @note The component parts of quaternions are sometimes referred to + * with a "w, x, y, z" notation instead of the "r, i, j, k" notation used in + * zscilib. The real number (r or w) may also be positioned after the + * imaginary numbers (xyzw or ijkr). The values are the same, but it's + * important to maintain consistent ordering. An optional 'alias' field is + * included in the zsl_quat struct if you prefer to use wxyz notation + * (q.alias.w == q.r, etc.). + * + * With an extra degree of freedom compared to three-value Euler angles, + * quaternions provide a compact and efficient means of rotating or orienting + * vectors (x, y, z) in 3D space, similar to how vectors translate points + * (x, y) in 2D space. + * + * They allow us to avoid a common problem with (otherwise more intuitive!) + * Euler angles called "gimbal lock". + * + * Gimbal lock occurs when two axes in the Eular angle coincide after a + * rotation by a multiple of 90 degrees about the third axis, resulting in the + * loss of a degree of freedom. This leaves us with an unclear value for the + * logitude of the north or south poles, i.e. "which way is up?!?". + * + * Quaternions also enable not just LINEAR interpolation (lerp) between two + * points (a and b) in normal space, but spherical linear interpolation (slerp) + * between two points in a sphere, travelling across the surface of the + * sphere rather than in a straight line. This enables us to interpolate + * between arbitrary orientations (x, y, z), not just arbitrary points (x, y). + * + * @ingroup ORIENTATION + * @{ + */ + +/** + * @file + * @brief API header file for quaternions in zscilib. + * + * This file contains the zscilib quaternion APIs + */ + + #ifndef ZEPHYR_INCLUDE_ZSL_QUATERNIONS_H_ + #define ZEPHYR_INCLUDE_ZSL_QUATERNIONS_H_ + + #include + #include "zsl.h" + #include "matrices.h" + #include "euler.h" + + #ifdef __cplusplus + extern "C" { + #endif + + /** + * @addtogroup QUAT_STRUCTS Structs, Enums and Macros + * + * @brief Various structs, enums and macros related to quaternions. + * + * @ingroup QUATERNIONS + * @{ */ + + /** + * @brief Represents a quaternion, a 4D structure capable of representing 3D + * rigid body orientations. + */ + struct zsl_quat { + union { + struct { + zsl_real_t r; /**< @brief The real component. */ + zsl_real_t i; /**< @brief The first imaginary component. */ + zsl_real_t j; /**< @brief The second imaginary component */ + zsl_real_t k; /**< @brief The third imaginary component. */ + }; + struct { + zsl_real_t w; /**< @brief The scalar component. */ + zsl_real_t x; /**< @brief The first vector component. */ + zsl_real_t y; /**< @brief The second vector component. */ + zsl_real_t z; /**< @brief The third vector component. */ + } alias; + zsl_real_t idx[4]; /**< @brief Allows access as an array. */ + }; + }; + + /** + * @brief Represents various 'types' of quaternions that can be initialised + * with the zsl_quat_init function. + */ + enum zsl_quat_type { + /** + * @brief + */ + ZSL_QUAT_TYPE_EMPTY = 0, + /** + * @brief Indicates no rotation, with the scalar value set to 1 and + * imaginary values set to 0. + */ + ZSL_QUAT_TYPE_IDENTITY = 1, + }; + + /** @} */ /* End of QUAT_STRUCTS group */ + + /** + * @addtogroup QUAT_INIT Initialisation + * + * @brief Quaternion initisialisation. + * + * @ingroup QUATERNIONS + * @{ */ + + /** + * @brief Convenience function to set the supplied quaternion into a known + * state. + * + * @param q The quaterion to initialise. + * @param type The memory layout to use when initialising the quaternion. + */ + void zsl_quat_init(struct zsl_quat *q, enum zsl_quat_type type); + + /** @} */ /* End of QUAT_INIT group */ + + /** + * @addtogroup QUAT_FUNCTIONS Functions + * + * @brief Quaternion APIU functionsn. + * + * @ingroup QUATERNIONS + * @{ */ + + /** + * @brief Calculates the magnitude (or length) of the quaternion, a scalar + * value equal to the square root of the sum of the squares of the four + * components. + * + * @note A quaternion with a magnitude of 1 is referred to as a unit + * quaternion, and represents a rotation in 3D space. + * + * @param q The source quaternion. + * + * @return zsl_real_t The magnitude of q. + */ + zsl_real_t zsl_quat_magn(struct zsl_quat *q); + + /** + * @brief Normalises the input to be a "unit" or "rotation" quaternion, AKA + * a versor, such that the square root of the sum of the squares of the + * four componeents, sqrt(r^2+i^2+j^2+k^2), equals 1.0. This + * normalisation process is what allows a quaternion to represent an + * rotation. + * + * Quaternion normalisation is required to represent rotation of objects + * in 3D space, and for transforming orientation coordinates in said space. + * You must normalise before using a quaternion to generate a rotation or + * transformation matrix, or to rotate or transform a vector. + * + * Normalisation is accomplished by dividing each of the components by the + * square root of the sum of the squares of the four quaternion components + * (AKA the Euclidean norm). + * + * To check if a quaternion is a unit quaternion, see @ref zsl_quat_is_unit. + * + * @param q The source quaternion. + * @param qn The normalised output quaternion. + * + * @return 0 if everything executed normally, or a negative error code. + */ + int zsl_quat_to_unit(struct zsl_quat *q, struct zsl_quat *qn); + + /** + * @brief Normalises to unit quaternion (destructive). + * + * @param q The quaternion to normalise. + * + * @return 0 if everything executed normally, or a negative error code. + */ + int zsl_quat_to_unit_d(struct zsl_quat *q); + + /** + * @brief Verifies that this is a "unit" quaternion, meaning that it has a + * magnitude of 1, where sqrt(r^2+i^2+j^2+k^2) = 1.0. + * + * @param q The quaternion to verify. + * + * @return true If this is a unit quaternion. + * @return false If this is not a unit quaternion. + */ + bool zsl_quat_is_unit(struct zsl_quat *q); + + /** + * @brief Multiples each element in the quaternion by a scalar value. + * + * @param q The quaternion to scale. + * @param s The scalar value to use. + * @param qs The scaled output quaternion. + * + * @return 0 if everything executed normally, or a negative error code. + */ + int zsl_quat_scale(struct zsl_quat *q, zsl_real_t s, struct zsl_quat *qs); + + /** + * @brief Multiples each element in the quaternion by a scalar value + * (destructive). + * + * @param q The quaternion to scale. + * @param s The scalar value to use. + * + * @return 0 if everything executed normally, or a negative error code. + */ + int zsl_quat_scale_d(struct zsl_quat *q, zsl_real_t s); + + /** + * @brief Multiples the two supplied unit quaternions (a * b) together, and + * stores the results in qm. + * + * @note Quaternion multiplication is not commutative, meaning that + * A*B != B*A. The order in which the quaternions appear when + * multiplying is important. To apply rotation q1, then rotation + * q2, we need to execute q3 = q2 * q1, in that order. + * + * @param qa The first input unit quaternion. + * @param qb The second input unit quaternion. + * @param qm The output placeholder. + * + * @return 0 if everything executed normally, or a negative error code. + */ + int zsl_quat_mult(struct zsl_quat *qa, struct zsl_quat *qb, + struct zsl_quat *qm); + + /** + * @brief Calculates the exponential of a unit quaternion. + * + * @param q The input unit quaternion. + * @param qe The output unit quaternion. + * + * @return 0 if everything executed normally, or a negative error code. + */ + int zsl_quat_exp(struct zsl_quat *q, struct zsl_quat *qe); + + /** + * @brief Calculates the natural logarithm of a unit quaternion. + * + * @param q The input unit quaternion. + * @param ql The output unit quaternion. + * + * @return zsl_real_t The logarithm of q. + */ + int zsl_quat_log(struct zsl_quat *q, struct zsl_quat *ql); + + /** + * @brief Multiples the supplied unit quaternions to the specified exponent, + * and stores the results in qout (where qout = qa^pow). + * + * @param q The input unit quaternion. + * @param pow The exponent to use, where qa^exp. + * @param qout The output placeholder. + * + * @return 0 if everything executed normally, or a negative error code. + */ + int zsl_quat_pow(struct zsl_quat *q, zsl_real_t pow, + struct zsl_quat *qout); + + /** + * @brief Calculates the complex conjugate of 'q'. + * + * @param q The input quaternion. + * @param qc The conjugate output; + * + * @return 0 if everything executed normally, or a negative error code. + */ + int zsl_quat_conj(struct zsl_quat *q, struct zsl_quat *qc); + + /** + * @brief Calculates the multiplicative inverse of unit quaternion 'q'. + * + * @note This function only works for unit quaternions, which have the unique + * property that their inverse is equal to the quaternion's conjugate + * (since qq' = 1), so we can simply negate the three imaginary values. + * + * @param q The input unit quaternion. + * @param qi The inverted output; + * + * @return 0 if everything executed normally, or a negative error code. + */ + int zsl_quat_inv(struct zsl_quat *q, struct zsl_quat *qi); + + /** + * @brief Inverts unit quaternion 'q' (destructive). + * + * @param q The input unit quaternion. + * + * @return 0 if everything executed normally, or a negative error code. + */ + int zsl_quat_inv_d(struct zsl_quat *q); + + /** + * @brief Calculates the rotation (qd) from qa to qb using the multiplicative + * inverse, such that qb = qa * qd. + * + * @param qa The initial unit quaternion value. + * @param qb The final unit quaternion value. + * @param qd The difference as a quaternion. + * + * @return 0 if everything executed normally, or a negative error code. + */ + int zsl_quat_diff(struct zsl_quat *qa, struct zsl_quat *qb, + struct zsl_quat *qd); + + /** + * @brief Rotates the pure quaternion qb using the quaternion qa. + * + * Quaternion rotarion follows as: qr = qa* * qb * qa, where qa* is the inverse + * of the quaternion qa. + * + * @param qa The input unit quaternion value, i.e., the rotation quaternion. + * @param qb The pure quaternion to rotate (zero real part). + * @param qr The pure rotated quaternion. + * + * @return 0 if everything executed normally, or a negative error code. + */ + int zsl_quat_rot(struct zsl_quat *qa, struct zsl_quat *qb, + struct zsl_quat *qr); + + /** + * @brief Linear interpolation (LERP). + * + * Calculates an intermediate quaternion qa and qb, based on the + * provided interpolation factor, t (0.0..1.0). Output assigned to qi. + * + * @param qa The starting quaternion value. + * @param qb The target quaternion value. + * @param t The interpolation factor (0.0..1.0) + * @param qi The interpolated unit quaternion. + * + * @return 0 if everything executed normally, or a negative error code if the + * interpolation factor is not between 0 and 1. + */ + int zsl_quat_lerp(struct zsl_quat *qa, struct zsl_quat *qb, + zsl_real_t t, struct zsl_quat *qi); + + /** + * @brief Spherical linear interpolation (SLERP). + * + * Calculates an intermediate rotation between qa and qb, based on the + * provided interpolation factor, t (0.0..1.0). Output assigned to qi. + * + * @param qa The starting quaternion value. + * @param qb The target quaternion value. + * @param t The interpolation factor (0.0..1.0) + * @param qi The interpolated quaternion. + * + * @return 0 if everything executed normally, or a negative error code if the + * interpolation factor is not between 0 and 1. If qa = -qb, the + * spherical interpolation is impossible and a negative error is + * returned. + */ + int zsl_quat_slerp(struct zsl_quat *qa, struct zsl_quat *qb, + zsl_real_t t, struct zsl_quat *qi); + + /** + * @brief Updates an orientation quaternion with new information in form of + * angular velocity and time elapsed. + * + * Given a quaterion that describes the current orientation (qin) of a body, a + * time value (t) and a tridimensional angular velocity vector (w), this + * function returns an estimation of the orientation after a time 't', in the + * form of a unit quaternion (qout). + * + * @param w Tridimensional angular velocity vector, in radians per second. + * @param qin The starting orientation quaternion of the body. + * @param t Time between qin and qout orientations, in seconds. + * @param qout The estimated orientation after a time t elapses. + * + * @return 0 if everything executed normally, or a negative error code if the + * time is negative or the angular velocity vector dimension is not 3. + */ + int zsl_quat_from_ang_vel(struct zsl_vec *w, struct zsl_quat *qin, + zsl_real_t t, struct zsl_quat *qout); + + /** + * @brief Updates an orientation quaternion with new information in form of + * angular momentum, moment of inertia and time elapsed. + * + * Given a quaterion that describes the current orientation (qin) of a body, a + * time value (t), a tridimensional angular momentum vector (l), and the moment + * of inertia of the body (i), this function returns an estimation of the + * orientation of the body after a time 't', in the form of a unit + * quaternion (qout). + * + * @param l Tridimensional angular momentum vector, in kilogram and metres + * squared per second. + * @param qin The starting orientation quaternion of the body. + * @param i The moment of inertia of the body, in kilogram and metres + * squared. + * @param t Time between qin and qout orientations, in seconds. + * @param qout The estimated orientation after a time t elapses. + * + * @return 0 if everything executed normally, or a negative error code if the + * time or the moment of inertia are negative or if the angular + * momentum vector dimension is not 3. + */ + int zsl_quat_from_ang_mom(struct zsl_vec *l, struct zsl_quat *qin, + zsl_real_t *i, zsl_real_t t, struct zsl_quat *qout); + + /** + * @brief Converts a unit quaternion to it's equivalent Euler angle. Euler + * values expressed in radians. + * + * @param q Pointer to the unit quaternion to convert. + * @param e Pointer to the Euler angle placeholder. Expressed in radians. + * + * @return 0 if everything executed normally, or a negative error code. + */ + int zsl_quat_to_euler(struct zsl_quat *q, struct zsl_euler *e); + + /** + * @brief Converts a Euler angle to a unit quaternion. Euler values expressed + * in radians. + * + * @param e Pointer to the Euler angle to convert. Expressed in radians. + * @param q Pointer to the unit quaternion placeholder. + * + * @return 0 if everything executed normally, or a negative error code. + */ + int zsl_quat_from_euler(struct zsl_euler *e, struct zsl_quat *q); + + /** + * @brief Converts a unit quaternion to it's equivalent rotation matrix. + * + * @param q Pointer to the unit quaternion to convert. + * @param m Pointer to the 4x4 rotation matrix. + * + * @return 0 if everything executed normally, or a negative error code. + */ + int zsl_quat_to_rot_mtx(struct zsl_quat *q, struct zsl_mtx *m); + + /** + * @brief Converts a rotation matrix to it's equivalent unit quaternion. + * + * @param m Pointer to the 4x4 rotation matrix. + * @param q Pointer to the unit quaternion to convert. + * + * @return 0 if everything executed normally, or a negative error code. + */ + int zsl_quat_from_rot_mtx(struct zsl_mtx *m, struct zsl_quat *q); + + /** + * @brief Converts a unit quaternion to it's axis-angle rotation equivalent. + * + * @param q Pointer to the unit quaternion to convert. + * @param a Pointer to the tridimensional axis of rotation. + * @param b Pointer to the angle of the rotation in radians. + * + * @return 0 if everything executed normally, or a negative error code if the + * dimension of the vector defining the axis is not three. + */ + int zsl_quat_to_axis_angle(struct zsl_quat *q, struct zsl_vec *a, + zsl_real_t *b); + + /** + * @brief Converts axis-angle rotation to its unit quaternion equivalent. + * + * @param a Pointer to the tridimensional axis of rotation. + * @param b Pointer to the angle of the rotation in radians. + * @param q Pointer to the converted unit quaternion. + * + * @return 0 if everything executed normally, or a negative error code if the + * dimension of the vector defining the axis is not three. + */ + int zsl_quat_from_axis_angle(struct zsl_vec *a, zsl_real_t *b, + struct zsl_quat *q); + + /** + * @brief Print the supplied quaternion using printf in a human-readable manner. + * + * @param q Pointer to the quaternion to print. + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_quat_print(struct zsl_quat *q); + + /** @} */ /* End of QUAT_FUNCTIONS group */ + + /** @} */ /* End of QUATERNIONS group */ + + #ifdef __cplusplus + } + #endif + + #endif /* ZEPHYR_INCLUDE_ZSL_QUATERNIONS_H_ */ \ No newline at end of file diff --git a/firmware/src/src/include/fusion/vectors.h b/firmware/src/src/include/fusion/vectors.h new file mode 100644 index 00000000..d50c54f0 --- /dev/null +++ b/firmware/src/src/include/fusion/vectors.h @@ -0,0 +1,438 @@ +/* + * Copyright (c) 2019-2020 Kevin Townsend (KTOWN) + * + * SPDX-License-Identifier: Apache-2.0 + */ + +/** + * @defgroup VECTORS Vectors + * + * @brief Vectors functions + * + * TODO: Expand with examples, better high-level documentation, etc. + */ + +/** + * @file + * @brief API header file for vectors in zscilib. + * + * This file contains the zscilib vector APIs + */ + + #ifndef ZEPHYR_INCLUDE_ZSL_VECTORS_H_ + #define ZEPHYR_INCLUDE_ZSL_VECTORS_H_ + + #include "zsl.h" + + #ifdef __cplusplus + extern "C" { + #endif + + /** + * @addtogroup VEC_STRUCTS Structs, Enums and Macros + * + * @brief Various struct, enums and macros related to vectors. + * + * @ingroup VECTORS + * @{ */ + + /** @brief Represents a vector. */ + struct zsl_vec { + /** The number of elements in the vector. */ + size_t sz; + /** The array of real number values assigned to the vector. */ + zsl_real_t *data; + }; + + /** Macro to declare a vector of size `n`. + * + * Be sure to also call 'zsl_vec_init' on the vector after this macro, since + * matrices declared on the stack may have non-zero values by default! + */ + #define ZSL_VECTOR_DEF(name, n) \ + zsl_real_t name ## _vec[n]; \ + struct zsl_vec name = { \ + .sz = n, \ + .data = name ## _vec \ + } + + /** @} */ /* End of VEC_STRUCTS group */ + + /** + * @addtogroup VEC_INIT Initialisation + * + * @brief Vector initisialisation. + * + * @ingroup VECTORS + * @{ */ + + /** + * Initialises vector 'v' with 0.0 values. + * + * @param v Pointer to the zsl_vec to initialise/clear. + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_vec_init(struct zsl_vec *v); + + /** + * @brief Converts an array of values into a vector. The number of elements in + * array 'a' must match the number of elements in vector 'v'. + * As such, 'v' should be previously initialised with an appropriate + * value assigned to v.sz. + * + * @param v The vector that the contents of array 'a' should be assigned to. + * The v.sz value must match the number of elements in 'a', meaning + * that the vector should be initialised before being passed in to + * this function. + * @param a Pointer to the array containing the values to assign to 'v'. + * The array will be read v.sz elements deep. + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_vec_from_arr(struct zsl_vec *v, zsl_real_t *a); + + /** + * @brief Copies the contents of vector 'vsrc' into vector 'vdest'. + * + * @param vdest Pointer to the destination vector data will be copied to. + * @param vsrc Pointer to the source vector data will be copied from. + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_vec_copy(struct zsl_vec *vdest, struct zsl_vec *vsrc); + + /** @} */ /* End of VEC_INIT group */ + + /** + * @addtogroup VEC_SELECTION Data Selection + * + * @brief Functions used to access subsets of the vector. + * + * @ingroup VECTORS + * @{ */ + + /** + * @brief Returns a subset of source vector 'v' in 'vsub'.. + * + * @param v The parent vector to read a subset of. + * @param offset The starting index (zero-based) foor the data subset. + * @param len The number of values to read, starting at offset. + * @param vsub The subset vector, which must have a buffer of at least + * size 'len'. If the data buffer isn't sufficiently large, + * this function will return -EINVAL. + * + * @return 0 on success, -EINVAL on a size of index error. + */ + int zsl_vec_get_subset(struct zsl_vec *v, size_t offset, size_t len, + struct zsl_vec *vsub); + + /** @} */ /* End of VEC_SELECTION group */ + + /** + * @addtogroup VEC_BASICMATH Basic Math + * + * @brief Basic mathematical operations (add, sum, norm, etc.). + * + * @ingroup VECTORS + * @{ */ + + /** + * @brief Adds corresponding vector elements in 'v' and 'w', saving to 'x'. + * + * @param v The first vector. + * @param w The second vector. + * @param x The output vector. + * + * @return 0 on success, -EINVAL if v and w are not equal length. + */ + int zsl_vec_add(struct zsl_vec *v, struct zsl_vec *w, struct zsl_vec *x); + + /** + * @brief Subtracts corresponding vector elements in 'v' and 'w', saving to 'x'. + * + * @param v The first vector. + * @param w The second vector. + * @param x The output vector. + * + * @return 0 on success, -EINVAL if v and w are not equal length. + */ + int zsl_vec_sub(struct zsl_vec *v, struct zsl_vec *w, struct zsl_vec *x); + + /** + * @brief Negates the elements in vector 'v'. + * + * @param v The vector to negate. + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_vec_neg(struct zsl_vec *v); + + /** + * @brief Component-wise sum of a set of equal-length vectors. + * + * @param v Pointer to the array of vectors. + * @param n The number of vectors in 'v'. + * @param w Pointer to the output vector containing the component-wise sum. + * + * @return 0 on success, -EINVAL if vectors in 'v' are no equal length, or + * -EINVAL if 'n' = 0. + */ + int zsl_vec_sum(struct zsl_vec **v, size_t n, struct zsl_vec *w); + + /** + * @brief Adds a scalar to each element in a vector. + * + * @param v The vector to scale. + * @param s The scalar to add to each element. + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_vec_scalar_add(struct zsl_vec *v, zsl_real_t s); + + /** + * @brief Multiply a vector by a scalar. + * + * @param v The vector to scale. + * @param s The scalar to multiply by. + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_vec_scalar_mult(struct zsl_vec *v, zsl_real_t s); + + /** + * @brief Divide a vector by a scalar. + * + * @param v The vector to scale. + * @param s The scalar to divide all elements by. + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_vec_scalar_div(struct zsl_vec *v, zsl_real_t s); + + /** + * @brief Calculates the distance between two vectors, which is equal to the + * norm of vector v - vector w. + * + * @param v The first vector. + * @param w The second vector. + * + * @return The norm of vector v - vector w, or NAN is there was a + * size mismatch between vectors v and w. + */ + zsl_real_t zsl_vec_dist(struct zsl_vec *v, struct zsl_vec *w); + + /** + * @brief Computes the dot (aka scalar) product of two equal-length vectors + * (the sum of their component-wise products). The dot product is an + * indicator of the "agreement" between two vectors, or can be used + * to determine how far vector w deviates from vector v. + * + * @param v The first vector. + * @param w The second vector. + * @param d The dot product. + * + * @return 0 on success, or -EINVAL if vectors v and w aren't equal-length. + */ + int zsl_vec_dot(struct zsl_vec *v, struct zsl_vec *w, zsl_real_t *d); + + /** + * @brief Calculates the norm or absolute value of vector 'v' (the + * square root of the vector's dot product). + * + * @param v The vector to calculate the norm of. + * + * @return The norm of vector 'v'. + */ + zsl_real_t zsl_vec_norm(struct zsl_vec *v); + + /** + * @brief Calculates the projection of vector 'u' over vector 'v', placing + * the results in vector 'w'. + * + * @param u Pointer to the first input vector. + * @param v Pointer to the second input vector. + * @param w Pointer to the output vector where the proj. results are stored. + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_vec_project(struct zsl_vec *u, struct zsl_vec *v, struct zsl_vec *w); + + /** + * @brief Converts (normalises) vector 'v' to a unit vector (a vector of + * magnitude or length 1). This is accomploished by dividing each + * element by the it's norm. + * + * @note Unit vectors are important since they have the ability to provide + * 'directional' information, without requiring a specific magnitude. + * + * @param v The vector to convert to a unit vector. + * + * @return 0 on success, and non-zero error code on failure + */ + int zsl_vec_to_unit(struct zsl_vec *v); + + /** + * @brief Computes the cross (or vector) product of two 3-vectors. + * + * @param v The first 3-vector. + * @param w The second 3-vector. + * @param c The cross product 3-vector output. + * + * @return 0 on success, or -EINVAL if a non 3-vector is provided. + * + * Given two linearly independent 3-vectors (v and w), the cross product, + * (v X w), is a vector that is perpendicular to both v and w and thus 'normal' + * to the plane containing them. + * + * If two vectors have the same direction (or have the exact opposite direction + * from one another, i.e. are not linearly independent) or if either one has + * zero length, then their cross product is zero. + * + * The norm of the cross product equals the area of a parallelogram + * with vectors v and w for sides. + * + * For a discusson of geometric and algebraic applications, see: + * https://en.wikipedia.org/wiki/Cross_product + */ + int zsl_vec_cross(struct zsl_vec *v, struct zsl_vec *w, struct zsl_vec *c); + + /** + * @brief Computes the vector's sum of squares. + * + * @param v The vector to use. + * + * @return The sum of the squares of vector 'v'. + */ + zsl_real_t zsl_vec_sum_of_sqrs(struct zsl_vec *v); + + /** + * @brief Computes the component-wise mean of a set of identically-sized + * vectors. + * + * @param v Pointer to the array of vectors. + * @param n The number of vectors in 'v'. + * @param m Pointer to the output vector whose i'th element is the mean of + * the i'th elements of the input vectors. + * + * @return 0 on success, and -EINVAL if all vectors aren't identically sized. + */ + int zsl_vec_mean(struct zsl_vec **v, size_t n, struct zsl_vec *m); + + /** + * @brief Computes the arithmetic mean of a vector. + * + * @param v The vector to use. + * @param m The arithmetic mean of vector v. + * + * @return 0 on success, otherwise an appropriate error code. + */ + int zsl_vec_ar_mean(struct zsl_vec *v, zsl_real_t *m); + + /** + * @brief Reverses the order of the entries in vector 'v'. + * + * @param v The vector to use. + * + * @return 0 on success, otherwise an appropriate error code. + */ + int zsl_vec_rev(struct zsl_vec *v); + + /** + * @brief Rearranges the input vector to place any zero-values at the end. + * + * @param v The input vector to rearrange. + * + * @warning This function is destructive to 'v'! + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_vec_zte(struct zsl_vec *v); + + /** @} */ /* End of VEC_BASICMATH group */ + + /** + * @addtogroup VEC_COMPARE Comparison + * + * @brief Functions used to compare or verify vectors. + * + * @ingroup VECTORS + * @{ */ + + /** + * @brief Checks if two vectors are identical in size and content. + * + * @param v The first vector. + * @param w The second vector. + * @param eps The margin for floating-point equality (ex. '1E-5'). + * + * @return true if the two vectors have the same size and the same values, + * otherwise false. + */ + bool zsl_vec_is_equal(struct zsl_vec *v, struct zsl_vec *w, zsl_real_t eps); + + /** + * @brief Checks if all elements in vector v are >= zero. + * + * @param v The vector to check. + * + * @return true if all elements in 'v' are zero or positive, otherwise false. + */ + bool zsl_vec_is_nonneg(struct zsl_vec *v); + + /** + * @brief Checks if vector v contains val, returning the number of occurences. + * + * @param v The vector to check. + * @param val The value to check all coefficients for. + * @param eps The margin for floating-point equality (ex. '1E-5'). + * + * @return The number of occurences of val withing range eps, 0 if no + * matching occurences were found, or a negative error code. + */ + int zsl_vec_contains(struct zsl_vec *v, zsl_real_t val, zsl_real_t eps); + + /** + * @brief Sorts the values in vector v from smallest to largest using quicksort, + * and assigns the sorted output to vector w. + * + * @param v The unsorted, input vector. + * @param w The sorted, output vector. + * + * @return 0 if everything executed properly, otherwise a negative error code. + */ + int zsl_vec_sort(struct zsl_vec *v, struct zsl_vec *w); + + /** @} */ /* End of VEC_COMPARE group */ + + /** + * @addtogroup VEC_DISPLAY Display + * + * @brief Functions used to present vectors in a user-friendly format. + * + * @ingroup VECTORS + * @{ */ + + /** + * @brief Print the supplied vector using printf in a human-readable manner. + * + * @param v Pointer to the vector to print. + * + * @return 0 if everything executed correctly, otherwise an appropriate + * error code. + */ + int zsl_vec_print(struct zsl_vec *v); + + // int zsl_vec_fprint(FILE *stream, zsl_vec *v); + + /** @} */ /* End of VEC_DISPLAY group */ + + #ifdef __cplusplus + } + #endif + + #endif /* ZEPHYR_INCLUDE_ZSL_VECTORS_H_ */ + + /** @} */ /* End of vectors group */ \ No newline at end of file diff --git a/firmware/src/src/include/fusion/zsl.h b/firmware/src/src/include/fusion/zsl.h new file mode 100644 index 00000000..aa691221 --- /dev/null +++ b/firmware/src/src/include/fusion/zsl.h @@ -0,0 +1,32 @@ +#pragma once + +#include +#include +#include +#include + +typedef float zsl_real_t; +#define ZSL_CEIL ceilf +#define ZSL_FLOOR floorf +#define ZSL_ROUND roundf +#define ZSL_ABS fabsf +#define ZSL_MIN fminf +#define ZSL_MAX fmaxf +#define ZSL_POW powf +#define ZSL_EXP expf +#define ZSL_LOG logf +#define ZSL_LOG10 log10f +#define ZSL_SQRT sqrtf +#define ZSL_SIN sinf +#define ZSL_COS cosf +#define ZSL_TAN tanf +#define ZSL_ASIN asinf +#define ZSL_ACOS acosf +#define ZSL_ATAN atanf +#define ZSL_ATAN2 atan2f +#define ZSL_SINH sinhf +#define ZSL_COSH coshf +#define ZSL_TANH tanhf +#define ZSL_ERF erff +#define ZSL_FMA fmaf +#define ZSL_EXPM1 expm1f diff --git a/firmware/src/src/sense.cpp b/firmware/src/src/sense.cpp index ce02b2eb..592dad14 100644 --- a/firmware/src/src/sense.cpp +++ b/firmware/src/src/sense.cpp @@ -23,7 +23,7 @@ #include #include -#include "MadgwickAHRS/MadgwickAHRS.h" +#include "aqua.h" #include "analog.h" #include "ble.h" #include "defines.h" @@ -95,7 +95,14 @@ static float bt_chansf[TrackerSettings::BT_CHANNELS]; // Output channel data static uint16_t channel_data[16]; -Madgwick madgwick; +// Aqua config; +struct zsl_fus_aqua_cfg aqua_cfg = { + .alpha = 0.1, + .beta = 0.1, + .e_a = 0.1, + .e_m = 0.2, +}; +struct zsl_quat q = {0.0f, 0.0f, 0.0f, 1.0f}; int64_t usduration = 0; //TODO unsinged int64_t senseUsDuration = 0; @@ -791,8 +798,8 @@ void calculate_Thread() trkset.setDataGyroCal(gyroCalibrated); // Qauterion Data - float *qd = madgwick.getQuat(); - trkset.setDataQuat(qd); + // float *qd = madgwick.getQuat(); + // trkset.setDataQuat(qd); // Bluetooth connected trkset.setDataBtCon(bleconnected); @@ -829,6 +836,9 @@ void calculate_Thread() void sensor_Thread() { LOG_INF("Sensor Thread Loaded"); + + zsl_fus_aqua_init(150 ,&aqua_cfg); + while (1) { // Do not execute below until after initialization has happened k_poll(senseRunEvents, 1, K_FOREVER); @@ -1131,20 +1141,47 @@ void sensor_Thread() } else if (madgreads == MADGSTART_SAMPLES - 1) { LOG_INF("Initial Orientation Set"); // Pass it averaged values - madgwick.begin(aacc[0], aacc[1], aacc[2], amag[0], amag[1], amag[2]); + madgreads = MADGSTART_SAMPLES; } // Do the AHRS calculations if (madgreads == MADGSTART_SAMPLES) { // Period Between Samples - float delttime = madgwick.deltatUpdate(); + ZSL_VECTOR_DEF(a, 3); + ZSL_VECTOR_DEF(m, 3); + ZSL_VECTOR_DEF(g, 3); + a.data[0] = accx * GRAVITY_EARTH; + a.data[1] = accy * GRAVITY_EARTH; + a.data[2] = accz * GRAVITY_EARTH; + m.data[0] = magx; + m.data[1] = magy; + m.data[2] = magz; + g.data[0] = gyrx * DEG_TO_RAD; + g.data[1] = gyry * DEG_TO_RAD; + g.data[2] = gyrz * DEG_TO_RAD; + + struct zsl_vec *ap = NULL; + struct zsl_vec *mp = NULL; + struct zsl_vec *gp = NULL; + + if(accValid) { + ap = &a; + } + if(magValid && !trkset.getDisMag()) { + mp = &m; + } + if(gyrValid) { + gp = &g; + } + + zsl_fus_aqua_feed(ap, mp, gp, NULL, &q, &aqua_cfg); + struct zsl_euler euler; + zsl_quat_to_euler(&q, &euler); - madgwick.update(gyrx * DEG_TO_RAD, gyry * DEG_TO_RAD, gyrz * DEG_TO_RAD, accx, accy, accz, - magx, magy, magz, delttime); - roll = madgwick.getPitch(); - tilt = madgwick.getRoll(); - pan = madgwick.getYaw(); + roll = euler.y * RAD_TO_DEG; + tilt = euler.x * RAD_TO_DEG; + pan = euler.z * RAD_TO_DEG; if (firstrun && pan != 0) { panoffset = pan; From ade893edb73c3868bb178d08c91cfea05f7741cc Mon Sep 17 00:00:00 2001 From: dlktdr Date: Fri, 2 May 2025 22:11:02 -0700 Subject: [PATCH 2/5] Remove compile warnings --- firmware/src/src/aqua_ahrs/matrices.cpp | 122 +++++++-------- firmware/src/src/aqua_ahrs/quaternions.cpp | 172 ++++++++++----------- firmware/src/src/aqua_ahrs/vectors.cpp | 10 +- 3 files changed, 152 insertions(+), 152 deletions(-) diff --git a/firmware/src/src/aqua_ahrs/matrices.cpp b/firmware/src/src/aqua_ahrs/matrices.cpp index 66d00bd1..d7a941b7 100644 --- a/firmware/src/src/aqua_ahrs/matrices.cpp +++ b/firmware/src/src/aqua_ahrs/matrices.cpp @@ -36,7 +36,7 @@ int zsl_mtx_entry_fn_identity(struct zsl_mtx *m, size_t i, size_t j) { - return zsl_mtx_set(m, i, j, i == j ? 1.0 : 0); + return zsl_mtx_set(m, i, j, i == j ? 1.0f : 0); } int @@ -53,7 +53,7 @@ for (size_t i = 0; i < m->sz_rows; i++) { for (size_t j = 0; j < m->sz_cols; j++) { - /* If entry_fn is NULL, assign 0.0 values. */ + /* If entry_fn is NULL, assign 0.0f values. */ if (entry_fn == NULL) { rc = zsl_mtx_entry_fn_empty(m, i, j); } else { @@ -194,10 +194,10 @@ for (size_t i = 0; i < m->sz_cols * m->sz_rows; i++) { switch (op) { case ZSL_MTX_UNARY_OP_INCREMENT: - m->data[i] += 1.0; + m->data[i] += 1.0f; break; case ZSL_MTX_UNARY_OP_DECREMENT: - m->data[i] -= 1.0; + m->data[i] -= 1.0f; break; case ZSL_MTX_UNARY_OP_NEGATIVE: m->data[i] = -m->data[i]; @@ -306,14 +306,14 @@ mc->data[i] = ma->data[i] * mb->data[i]; break; case ZSL_MTX_BINARY_OP_DIV: - if (mb->data[i] == 0.0) { - mc->data[i] = 0.0; + if (mb->data[i] == 0.0f) { + mc->data[i] = 0.0f; } else { mc->data[i] = ma->data[i] / mb->data[i]; } break; case ZSL_MTX_BINARY_OP_MEAN: - mc->data[i] = (ma->data[i] + mb->data[i]) / 2.0; + mc->data[i] = (ma->data[i] + mb->data[i]) / 2.0f; case ZSL_MTX_BINARY_OP_EXPON: mc->data[i] = ZSL_POW(ma->data[i], mb->data[i]); case ZSL_MTX_BINARY_OP_MIN: @@ -323,17 +323,17 @@ mc->data[i] = ma->data[i] > mb->data[i] ? ma->data[i] : mb->data[i]; case ZSL_MTX_BINARY_OP_EQUAL: - mc->data[i] = ma->data[i] == mb->data[i] ? 1.0 : 0.0; + mc->data[i] = ma->data[i] == mb->data[i] ? 1.0f : 0.0f; case ZSL_MTX_BINARY_OP_NEQUAL: - mc->data[i] = ma->data[i] != mb->data[i] ? 1.0 : 0.0; + mc->data[i] = ma->data[i] != mb->data[i] ? 1.0f : 0.0f; case ZSL_MTX_BINARY_OP_LESS: - mc->data[i] = ma->data[i] < mb->data[i] ? 1.0 : 0.0; + mc->data[i] = ma->data[i] < mb->data[i] ? 1.0f : 0.0f; case ZSL_MTX_BINARY_OP_GREAT: - mc->data[i] = ma->data[i] > mb->data[i] ? 1.0 : 0.0; + mc->data[i] = ma->data[i] > mb->data[i] ? 1.0f : 0.0f; case ZSL_MTX_BINARY_OP_LEQ: - mc->data[i] = ma->data[i] <= mb->data[i] ? 1.0 : 0.0; + mc->data[i] = ma->data[i] <= mb->data[i] ? 1.0f : 0.0f; case ZSL_MTX_BINARY_OP_GEQ: - mc->data[i] = ma->data[i] >= mb->data[i] ? 1.0 : 0.0; + mc->data[i] = ma->data[i] >= mb->data[i] ? 1.0f : 0.0f; default: /* Not yet implemented! */ return -ENOSYS; @@ -589,9 +589,9 @@ for (size_t i = 0; i < m->sz_cols; i++) { for (size_t j = 0; j < m->sz_cols; j++) { - sign = 1.0; + sign = 1.0f; if ((i + j) % 2 != 0) { - sign = -1.0; + sign = -1.0f; } zsl_mtx_reduce(m, &mr, i, j); zsl_mtx_deter(&mr, &d); @@ -621,7 +621,7 @@ ZSL_MATRIX_DEF(b, m->sz_cols, 1); zsl_mtx_init(&A, NULL); - A.data[(m->sz_cols * m->sz_cols - 1)] = 1.0; + A.data[(m->sz_cols * m->sz_cols - 1)] = 1.0f; for (size_t i = 0; i < m->sz_rows; i++) { zsl_mtx_get_row(m, i, Av.data); @@ -794,7 +794,7 @@ ZSL_MATRIX_DEF(mr, (m->sz_rows - 1), (m->sz_rows - 1)); /* Clear determinant output before starting. */ - *d = 0.0; + *d = 0.0f; /* * Iterate across row 0, removing columns one by one. @@ -807,14 +807,14 @@ zsl_mtx_init(&mr, NULL); /* Clear mr. */ zsl_mtx_reduce(m, &mr, 0, g); /* Remove row 0, column g. */ rc = zsl_mtx_deter(&mr, &dtmp); /* Calc. determinant of mr. */ - sign = 1.0; + sign = 1.0f; if (rc) { return -EINVAL; } /* Uneven elements are negative. */ if (g % 2 != 0) { - sign = -1.0; + sign = -1.0f; } /* Add current determinant to final output value. */ @@ -830,7 +830,7 @@ { int rc; zsl_real_t x, y; - zsl_real_t epsilon = 1E-6; + zsl_real_t epsilon = 1E-6f; /* Make a copy of matrix m. */ rc = zsl_mtx_copy(mg, m); @@ -860,7 +860,7 @@ } /* Get the value of (p, j), aborting if value is zero. */ zsl_mtx_get(mg, p, j, &x); - if ((x >= 1E-6) || (x <= -1E-6)) { + if ((x >= 1E-6f) || (x <= -1E-6f)) { rc = zsl_mtx_sum_rows_scaled_d(mg, p, i, -(x / y)); if (rc) { @@ -887,7 +887,7 @@ struct zsl_mtx *mg) { zsl_real_t v[m->sz_rows]; - zsl_real_t epsilon = 1E-6; + zsl_real_t epsilon = 1E-6f; zsl_real_t x; zsl_real_t y; @@ -986,7 +986,7 @@ { int rc; zsl_real_t x; - zsl_real_t epsilon = 1E-6; + zsl_real_t epsilon = 1E-6f; /* Make a copy of matrix m. */ rc = zsl_mtx_copy(mn, m); @@ -1000,17 +1000,17 @@ return rc; } - /* If the value is 0.0, abort. */ + /* If the value is 0.0f, abort. */ if ((x >= 0 && x < epsilon) || (x <= 0 && x > -epsilon)) { return 0; } - rc = zsl_mtx_scalar_mult_row_d(mn, i, (1.0 / x)); + rc = zsl_mtx_scalar_mult_row_d(mn, i, (1.0f / x)); if (rc) { return -EINVAL; } - rc = zsl_mtx_scalar_mult_row_d(mi, i, (1.0 / x)); + rc = zsl_mtx_scalar_mult_row_d(mi, i, (1.0f / x)); if (rc) { return -EINVAL; } @@ -1064,7 +1064,7 @@ /* Scale the output using the determinant. */ if (d != 0) { - s = 1.0 / d; + s = 1.0f / d; rc = zsl_mtx_scalar_mult_d(mi, s); } else { /* Provide an identity matrix if the determinant is zero. */ @@ -1083,7 +1083,7 @@ zsl_mtx_inv(struct zsl_mtx *m, struct zsl_mtx *mi) { int rc; - zsl_real_t d = 0.0; + zsl_real_t d = 0.0f; /* Shortcut for 3x3 matrices. */ if (m->sz_rows == 3) { @@ -1164,7 +1164,7 @@ zsl_real_t sum, x, y; zsl_mtx_init(l, zsl_mtx_entry_fn_empty); for (size_t j = 0; j < m->sz_cols; j++) { - sum = 0.0; + sum = 0.0f; for (size_t k = 0; k < j; k++) { zsl_mtx_get(l, j, k, &x); sum += x * x; @@ -1173,7 +1173,7 @@ zsl_mtx_set(l, j, j, ZSL_SQRT(x - sum)); for (size_t i = j + 1; i < m->sz_cols; i++) { - sum = 0.0; + sum = 0.0f; for (size_t k = 0; k < j; k++) { zsl_mtx_get(l, j, k, &x); zsl_mtx_get(l, i, k, &y); @@ -1230,28 +1230,28 @@ } /* TODO: Extend with a check against epsilon? */ - if (col != 0.0 && row != 0.0) { - row2 = row / 2.0; - col2 = 1.0; + if (col != 0.0f && row != 0.0f) { + row2 = row / 2.0f; + col2 = 1.0f; sum = col + row; while (col < row2) { - col2 *= 2.0; - col *= 4.0; + col2 *= 2.0f; + col *= 4.0f; } - row2 = row * 2.0; + row2 = row * 2.0f; while (col > row2) { - col2 /= 2.0; - col /= 4.0; + col2 /= 2.0f; + col /= 4.0f; } - if ((col + row) / col2 < 0.95 * sum) { + if ((col + row) / col2 < 0.95f * sum) { done = false; - row2 = 1.0 / col2; + row2 = 1.0f / col2; - for (int k = 0; k < m->sz_rows; k++) { + for (size_t k = 0; k < m->sz_rows; k++) { mout->data[(i * m->sz_rows) + k] *= row2; mout->data[(k * m->sz_rows) + i] @@ -1260,8 +1260,8 @@ } } - row = 0.0; - col = 0.0; + row = 0.0f; + col = 0.0f; } } @@ -1290,7 +1290,7 @@ /* Create the e1 vector, i.e. the vector (1, 0, 0, ...). */ zsl_vec_init(&e1); - e1.data[0] = 1.0; + e1.data[0] = 1.0f; /* Get the first column of the input matrix. */ zsl_mtx_get_col(m, 0, v2.data); @@ -1302,10 +1302,10 @@ /* Change the 'sign' value according to the sign of the first * coefficient of the matrix. */ - zsl_real_t sign = 1.0; + zsl_real_t sign = 1.0f; if (v.data[0] < 0) { - sign = -1.0; + sign = -1.0f; } /* Calculate the vector 'v' that will later be used to calculate the @@ -1429,9 +1429,9 @@ * if any coimplekx values were found. Increasing the number of * iterations will move these values closer to 0, but when using * single-precision floats the numbers can still be quite large, so - * we need to set a delta of +/- 0.001 in this case. */ + * we need to set a delta of +/- 0.0f01 in this case. */ - zsl_real_t epsilon = 1E-6; + zsl_real_t epsilon = 1E-6f; ZSL_MATRIX_DEF(mout, m->sz_rows, m->sz_rows); ZSL_MATRIX_DEF(mtemp, m->sz_rows, m->sz_rows); @@ -1533,7 +1533,7 @@ size_t count = 0; /* Number of eigenvectors for an eigenvalue. */ size_t ga = 0; - zsl_real_t epsilon = 1E-6; + zsl_real_t epsilon = 1E-6f; zsl_real_t x; /* The vector where all eigenvalues will be stored. */ @@ -1574,7 +1574,7 @@ } /* If zero is also an eigenvalue, copy it once in 'o'. */ - if (zsl_vec_contains(&k, 0.0, epsilon) > 0) { + if (zsl_vec_contains(&k, 0.0f, epsilon) > 0) { e_vals++; } @@ -1595,8 +1595,8 @@ * there are for each eigenvalue. */ for (size_t h = 0; h < m->sz_rows; h++) { zsl_mtx_get(&mi, h, h, &x); - if ((x >= 0.0 && x < epsilon) || - (x <= 0.0 && x > -epsilon)) { + if ((x >= 0.0f && x < epsilon) || + (x <= 0.0f && x > -epsilon)) { count++; } } @@ -1609,8 +1609,8 @@ * them as the columns of 'evec'. */ for (size_t h = 0; h < m->sz_rows; h++) { zsl_mtx_get(&mi, h, h, &x); - if ((x >= 0.0 && x < epsilon) || - (x <= 0.0 && x > -epsilon)) { + if ((x >= 0.0f && x < epsilon) || + (x <= 0.0f && x > -epsilon)) { zsl_mtx_set(&mi, h, h, -1); zsl_mtx_get_col(&mi, h, f.data); zsl_vec_neg(&f); @@ -1638,8 +1638,8 @@ * them in 'mev2'. */ for (size_t h = 0; h < m->sz_rows; h++) { zsl_mtx_get(&mi, h, h, &x); - if ((x >= 0.0 && x < epsilon) || - (x <= 0.0 && x > -epsilon)) { + if ((x >= 0.0f && x < epsilon) || + (x <= 0.0f && x > -epsilon)) { zsl_mtx_set(&mi, h, h, -1); zsl_mtx_get_col(&mi, h, f.data); zsl_vec_neg(&f); @@ -1688,7 +1688,7 @@ zsl_real_t d; size_t pu = 0; size_t min = m->sz_cols; - zsl_real_t epsilon = 1E-6; + zsl_real_t epsilon = 1E-6f; zsl_mtx_trans(m, &at); @@ -1731,7 +1731,7 @@ * eniegnvectors by the square root its eigenvalue and * multiplying them by the input matrix. */ zsl_mtx_mult(m, &ui2, &ui3); - if ((d >= 0.0 && d < epsilon) || (d <= 0.0 && d > -epsilon)) { + if ((d >= 0.0f && d < epsilon) || (d <= 0.0f && d > -epsilon)) { pu++; } else { zsl_mtx_scalar_mult_d(&ui3, (1 / d)); @@ -1759,7 +1759,7 @@ { zsl_real_t x; size_t min = m->sz_cols; - zsl_real_t epsilon = 1E-6; + zsl_real_t epsilon = 1E-6f; ZSL_MATRIX_DEF(u, m->sz_rows, m->sz_rows); ZSL_MATRIX_DEF(e, m->sz_rows, m->sz_cols); @@ -1897,7 +1897,7 @@ zsl_mtx_is_notneg(struct zsl_mtx *m) { for (size_t i = 0; i < m->sz_rows * m->sz_cols; i++) { - if (m->data[i] < 0.0) { + if (m->data[i] < 0.0f) { return false; } } @@ -1911,7 +1911,7 @@ zsl_real_t x; zsl_real_t y; zsl_real_t diff; - zsl_real_t epsilon = 1E-6; + zsl_real_t epsilon = 1E-6f; for (size_t i = 0; i < m->sz_rows; i++) { for (size_t j = 0; j < m->sz_cols; j++) { diff --git a/firmware/src/src/aqua_ahrs/quaternions.cpp b/firmware/src/src/aqua_ahrs/quaternions.cpp index 660dd800..350e9f18 100644 --- a/firmware/src/src/aqua_ahrs/quaternions.cpp +++ b/firmware/src/src/aqua_ahrs/quaternions.cpp @@ -39,17 +39,17 @@ { switch (type) { case ZSL_QUAT_TYPE_IDENTITY: - q->r = 1.0; - q->i = 0.0; - q->j = 0.0; - q->k = 0.0; + q->r = 1.0f; + q->i = 0.0f; + q->j = 0.0f; + q->k = 0.0f; break; case ZSL_QUAT_TYPE_EMPTY: default: - q->r = 0.0; - q->i = 0.0; - q->j = 0.0; - q->k = 0.0; + q->r = 0.0f; + q->i = 0.0f; + q->j = 0.0f; + q->k = 0.0f; break; } } @@ -64,11 +64,11 @@ int rc = 0; zsl_real_t m = zsl_quat_magn(q); - if (ZSL_ABS(m) < 1E-6) { - qn->r = 0.0; - qn->i = 0.0; - qn->j = 0.0; - qn->k = 0.0; + if (ZSL_ABS(m) < 1E-6f) { + qn->r = 0.0f; + qn->i = 0.0f; + qn->j = 0.0f; + qn->k = 0.0f; } else { qn->r = q->r / m; qn->i = q->i / m; @@ -88,14 +88,14 @@ { zsl_real_t unit_len; - /* Verify that sqrt(r^2+i^2+j^2+k^2) = 1.0. */ + /* Verify that sqrt(r^2+i^2+j^2+k^2) = 1.0f. */ unit_len = ZSL_SQRT( q->r * q->r + q->i * q->i + q->j * q->j + q->k * q->k); - return zsl_quat_val_is_equal(unit_len, 1.0, 1E-6); + return zsl_quat_val_is_equal(unit_len, 1.0f, 1E-6f); } int zsl_quat_scale(struct zsl_quat *q, zsl_real_t s, struct zsl_quat *qs) @@ -236,9 +236,9 @@ /* TODO: Check for div by zero before running this! */ qc->r = q->r; - qc->i = q->i * -1.0; - qc->j = q->j * -1.0; - qc->k = q->k * -1.0; + qc->i = q->i * -1.0f; + qc->j = q->j * -1.0f; + qc->k = q->k * -1.0f; return rc; } @@ -248,7 +248,7 @@ int rc = 0; zsl_real_t m = zsl_quat_magn(q); - if (ZSL_ABS(m) < 1E-6) { + if (ZSL_ABS(m) < 1E-6f) { /* Set to all 0's. */ zsl_quat_init(qi, ZSL_QUAT_TYPE_EMPTY); } else { @@ -291,7 +291,7 @@ int rc = 0; #if CONFIG_ZSL_BOUNDS_CHECKS - if (ZSL_ABS(qb->r) > 1E-6) { + if (ZSL_ABS(qb->r) > 1E-6f) { rc = -EINVAL; goto err; } @@ -319,7 +319,7 @@ #if CONFIG_ZSL_BOUNDS_CHECKS /* Make sure t is between 0 and 1 (included). */ - if (t < 0.0 || t > 1.0) { + if (t < 0.0f || t > 1.0f) { rc = -EINVAL; goto err; } @@ -334,7 +334,7 @@ zsl_quat_to_unit(qb, &qb_u); /* Calculate intermediate quats. */ - zsl_quat_scale(&qa_u, 1.0 - t, &q1); + zsl_quat_scale(&qa_u, 1.0f - t, &q1); zsl_quat_scale(&qb_u, t, &q2); /* Final result = q1 + q2. */ @@ -359,7 +359,7 @@ #if CONFIG_ZSL_BOUNDS_CHECKS /* Make sure t is between 0 and 1 (included). */ - if (t < 0.0 || t > 1.0) { + if (t < 0.0f || t > 1.0f) { rc = -EINVAL; goto err; } @@ -370,7 +370,7 @@ zsl_real_t phi; /* arccos(dot). */ zsl_real_t phi_s; /* sin(phi). */ zsl_real_t phi_st; /* sin(phi * (t)). */ - zsl_real_t phi_smt; /* sin(phi * (1.0 - t)). */ + zsl_real_t phi_smt; /* sin(phi * (1.0f - t)). */ /* * Unit quaternion slerp = qa * (qa^-1 * qb)^t @@ -385,14 +385,14 @@ zsl_quat_to_unit(qa, &qa_u); zsl_quat_to_unit(qb, &qb_u); - /* When t = 0.0 or t = 1.0, just memcpy qa or qb. */ - if (t == 0.0) { + /* When t = 0.0f or t = 1.0f, just memcpy qa or qb. */ + if (t == 0.0f) { qi->r = qa_u.r; qi->i = qa_u.i; qi->j = qa_u.j; qi->k = qa_u.k; goto err; - } else if (t == 1.0) { + } else if (t == 1.0f) { qi->r = qb_u.r; qi->i = qb_u.i; qi->j = qb_u.j; @@ -403,9 +403,9 @@ /* Compute the dot product of the two normalized input quaternions. */ dot = qa_u.r * qb_u.r + qa_u.i * qb_u.i + qa_u.j * qb_u.j + qa_u.k * qb_u.k; - /* The value dot is always between -1 and 1. If dot = 1.0, qa = qb and there + /* The value dot is always between -1 and 1. If dot = 1.0f, qa = qb and there * is no interpolation. */ - if (ZSL_ABS(dot - 1.0) < 1E-6) { + if (ZSL_ABS(dot - 1.0f) < 1E-6f) { qi->r = qa_u.r; qi->i = qa_u.i; qi->j = qa_u.j; @@ -414,7 +414,7 @@ } /* If dot = -1, then qa = - qb and the interpolation is invald. */ - if (ZSL_ABS(dot + 1.0) < 1E-6) { + if (ZSL_ABS(dot + 1.0f) < 1E-6f) { rc = -EINVAL; goto err; } @@ -428,7 +428,7 @@ phi = ZSL_ACOS(dot); phi_s = ZSL_SIN(phi); phi_st = ZSL_SIN(phi * t); - phi_smt = ZSL_SIN(phi * (1.0 - t)); + phi_smt = ZSL_SIN(phi * (1.0f - t)); /* Calculate intermediate quats. */ zsl_quat_scale(&qa_u, phi_smt / phi_s, &q1); @@ -452,7 +452,7 @@ #if CONFIG_ZSL_BOUNDS_CHECKS /* Make sure time is positive or zero and the angular velocity is a * tridimensional vector. */ - if (w->sz != 3 || t < 0.0) { + if (w->sz != 3 || t < 0.0f) { rc = -EINVAL; goto err; } @@ -462,7 +462,7 @@ struct zsl_quat qout2; struct zsl_quat wq; struct zsl_quat wquat = { - .r = 0.0, + .r = 0.0f, .i = w->data[0], .j = w->data[1], .k = w->data[2] @@ -470,7 +470,7 @@ zsl_quat_to_unit(qin, &qin2); zsl_quat_mult(&wquat, &qin2, &wq); - zsl_quat_scale_d(&wq, 0.5 * t); + zsl_quat_scale_d(&wq, 0.5f * t); qout2.r = qin2.r + wq.r; qout2.i = qin2.i + wq.i; qout2.j = qin2.j + wq.j; @@ -492,7 +492,7 @@ #if CONFIG_ZSL_BOUNDS_CHECKS /* Make sure time is positive or zero and the angular velocity is a * tridimensional vector. Inertia can't be negative or zero. */ - if (l->sz != 3 || t < 0.0 || *i <= 0.0) { + if (l->sz != 3 || t < 0.0f || *i <= 0.0f) { rc = -EINVAL; goto err; } @@ -516,28 +516,28 @@ zsl_quat_to_unit(q, &qn); zsl_real_t gl = qn.i * qn.k + qn.j * qn.r; - zsl_real_t v = 2. * gl; + zsl_real_t v = 2.0f * gl; - if (v > 1.0) { - v = 1.0; - } else if (v < -1.0) { - v = -1.0; + if (v > 1.0f) { + v = 1.0f; + } else if (v < -1.0f) { + v = -1.0f; } e->y = ZSL_ASIN(v); /* Gimbal lock case. */ - if (ZSL_ABS(gl - 0.5) < 1E-6 || ZSL_ABS(gl + 0.5) < 1E-6) { - e->x = ZSL_ATAN2(2.0 * (qn.j * qn.k + qn.i * qn.r), - 1.0 - 2.0 * (qn.i * qn.i + qn.k * qn.k)); - e->z = 0.0; + if (ZSL_ABS(gl - 0.5f) < 1E-6f || ZSL_ABS(gl + 0.5f) < 1E-6f) { + e->x = ZSL_ATAN2(2.0f * (qn.j * qn.k + qn.i * qn.r), + 1.0f - 2.0f * (qn.i * qn.i + qn.k * qn.k)); + e->z = 0.0f; return rc; } - e->x = ZSL_ATAN2(2.0 * (qn.i * qn.r - qn.j * qn.k), - 1.0 - 2.0 * (qn.i * qn.i + qn.j * qn.j)); - e->z = ZSL_ATAN2(2.0 * (qn.k * qn.r - qn.i * qn.j), - 1.0 - 2.0 * (qn.j * qn.j + qn.k * qn.k)); + e->x = ZSL_ATAN2(2.0f * (qn.i * qn.r - qn.j * qn.k), + 1.0f - 2.0f * (qn.i * qn.i + qn.j * qn.j)); + e->z = ZSL_ATAN2(2.0f * (qn.k * qn.r - qn.i * qn.j), + 1.0f - 2.0f * (qn.j * qn.j + qn.k * qn.k)); return rc; } @@ -546,12 +546,12 @@ { int rc = 0; - zsl_real_t roll_c = ZSL_COS(e->x * 0.5); - zsl_real_t roll_s = ZSL_SIN(e->x * 0.5); - zsl_real_t pitch_c = ZSL_COS(e->y * 0.5); - zsl_real_t pitch_s = ZSL_SIN(e->y * 0.5); - zsl_real_t yaw_c = ZSL_COS(e->z * 0.5); - zsl_real_t yaw_s = ZSL_SIN(e->z * 0.5); + zsl_real_t roll_c = ZSL_COS(e->x * 0.5f); + zsl_real_t roll_s = ZSL_SIN(e->x * 0.5f); + zsl_real_t pitch_c = ZSL_COS(e->y * 0.5f); + zsl_real_t pitch_s = ZSL_SIN(e->y * 0.5f); + zsl_real_t yaw_c = ZSL_COS(e->z * 0.5f); + zsl_real_t yaw_s = ZSL_SIN(e->z * 0.5f); q->r = roll_c * pitch_c * yaw_c - roll_s * pitch_s * yaw_s; q->i = roll_s * pitch_c * yaw_c + roll_c * pitch_s * yaw_s; @@ -576,19 +576,19 @@ /* Note: This can be optimised by pre-calculating shared values. */ /* Row 0. */ - zsl_mtx_set(m, 0, 0, 1.0 - 2.0 * (q->j * q->j + q->k * q->k)); - zsl_mtx_set(m, 0, 1, 2.0 * (q->i * q->j - q->k * q->r)); - zsl_mtx_set(m, 0, 2, 2.0 * (q->i * q->k + q->j * q->r)); + zsl_mtx_set(m, 0, 0, 1.0f - 2.0f * (q->j * q->j + q->k * q->k)); + zsl_mtx_set(m, 0, 1, 2.0f * (q->i * q->j - q->k * q->r)); + zsl_mtx_set(m, 0, 2, 2.0f * (q->i * q->k + q->j * q->r)); /* Row 1. */ - zsl_mtx_set(m, 1, 0, 2.0 * (q->i * q->j + q->k * q->r)); - zsl_mtx_set(m, 1, 1, 1.0 - 2.0 * (q->i * q->i + q->k * q->k)); - zsl_mtx_set(m, 1, 2, 2.0 * (q->j * q->k - q->i * q->r)); + zsl_mtx_set(m, 1, 0, 2.0f * (q->i * q->j + q->k * q->r)); + zsl_mtx_set(m, 1, 1, 1.0f - 2.0f * (q->i * q->i + q->k * q->k)); + zsl_mtx_set(m, 1, 2, 2.0f * (q->j * q->k - q->i * q->r)); /* Row 2. */ - zsl_mtx_set(m, 2, 0, 2.0 * (q->i * q->k - q->j * q->r)); - zsl_mtx_set(m, 2, 1, 2.0 * (q->j * q->k + q->i * q->r)); - zsl_mtx_set(m, 2, 2, 1.0 - 2.0 * (q->i * q->i + q->j * q->j)); + zsl_mtx_set(m, 2, 0, 2.0f * (q->i * q->k - q->j * q->r)); + zsl_mtx_set(m, 2, 1, 2.0f * (q->j * q->k + q->i * q->r)); + zsl_mtx_set(m, 2, 2, 1.0f - 2.0f * (q->i * q->i + q->j * q->j)); #if CONFIG_ZSL_BOUNDS_CHECKS err: @@ -609,22 +609,22 @@ #endif /* Convert rotation matrix to unit quaternion. */ - q->r = 0.5 * ZSL_SQRT(m->data[0] + m->data[4] + m->data[8] + 1.0); - q->i = 0.5 * ZSL_SQRT(m->data[0] - m->data[4] - m->data[8] + 1.0); - q->j = 0.5 * ZSL_SQRT(-m->data[0] + m->data[4] - m->data[8] + 1.0); - q->k = 0.5 * ZSL_SQRT(-m->data[0] - m->data[4] + m->data[8] + 1.0); + q->r = 0.5f * ZSL_SQRT(m->data[0] + m->data[4] + m->data[8] + 1.0f); + q->i = 0.5f * ZSL_SQRT(m->data[0] - m->data[4] - m->data[8] + 1.0f); + q->j = 0.5f * ZSL_SQRT(-m->data[0] + m->data[4] - m->data[8] + 1.0f); + q->k = 0.5f * ZSL_SQRT(-m->data[0] - m->data[4] + m->data[8] + 1.0f); - if (ZSL_ABS(m->data[7] - m->data[5]) > 1E-6) { + if (ZSL_ABS(m->data[7] - m->data[5]) > 1E-6f) { /* Multiply by the sign of m21 - m12. */ q->i *= (m->data[7] - m->data[5]) / ZSL_ABS(m->data[7] - m->data[5]); } - if (ZSL_ABS(m->data[2] - m->data[6]) > 1E-6) { + if (ZSL_ABS(m->data[2] - m->data[6]) > 1E-6f) { /* Multiply by the sign of m02 - m20. */ q->j *= (m->data[2] - m->data[6]) / ZSL_ABS(m->data[2] - m->data[6]); } - if (ZSL_ABS(m->data[3] - m->data[1]) > 1E-6) { + if (ZSL_ABS(m->data[3] - m->data[1]) > 1E-6f) { /* Multiply by the sign of m10 - m01. */ q->k *= (m->data[3] - m->data[1]) / ZSL_ABS(m->data[3] - m->data[1]); } @@ -651,16 +651,16 @@ struct zsl_quat qn; zsl_quat_to_unit(q, &qn); - if (ZSL_ABS(qn.r - 1.0) < 1E-6) { - a->data[0] = 0.0; - a->data[1] = 0.0; - a->data[2] = 0.0; - *b = 0.0; + if (ZSL_ABS(qn.r - 1.0f) < 1E-6f) { + a->data[0] = 0.0f; + a->data[1] = 0.0f; + a->data[2] = 0.0f; + *b = 0.0f; return 0; } - zsl_real_t s = ZSL_SQRT(1.0 - (qn.r * qn.r)); - *b = 2.0 * ZSL_ACOS(qn.r); + zsl_real_t s = ZSL_SQRT(1.0f - (qn.r * qn.r)); + *b = 2.0f * ZSL_ACOS(qn.r); a->data[0] = qn.i / s; a->data[1] = qn.j / s; a->data[2] = qn.k / s; @@ -688,11 +688,11 @@ a->data[1] * a->data[1] + a->data[2] * a->data[2]); - if (norm < 1E-6) { - q->r = 0.0; - q->i = 0.0; - q->j = 0.0; - q->k = 0.0; + if (norm < 1E-6f) { + q->r = 0.0f; + q->i = 0.0f; + q->j = 0.0f; + q->k = 0.0f; return 0; } @@ -700,10 +700,10 @@ zsl_vec_copy(&an, a); zsl_vec_scalar_div(&an, norm); - q->r = ZSL_COS(*b / 2.0); - q->i = an.data[0] * ZSL_SIN(*b / 2.0); - q->j = an.data[1] * ZSL_SIN(*b / 2.0); - q->k = an.data[2] * ZSL_SIN(*b / 2.0); + q->r = ZSL_COS(*b / 2.0f); + q->i = an.data[0] * ZSL_SIN(*b / 2.0f); + q->j = an.data[1] * ZSL_SIN(*b / 2.0f); + q->k = an.data[2] * ZSL_SIN(*b / 2.0f); #if CONFIG_ZSL_BOUNDS_CHECKS err: diff --git a/firmware/src/src/aqua_ahrs/vectors.cpp b/firmware/src/src/aqua_ahrs/vectors.cpp index b8430a61..02c15ffc 100644 --- a/firmware/src/src/aqua_ahrs/vectors.cpp +++ b/firmware/src/src/aqua_ahrs/vectors.cpp @@ -198,7 +198,7 @@ int zsl_vec_dot(struct zsl_vec *v, struct zsl_vec *w, zsl_real_t *d) { - zsl_real_t res = 0.0; + zsl_real_t res = 0.0f; #if CONFIG_ZSL_BOUNDS_CHECKS /* Make sure v and w are equal length. */ @@ -262,7 +262,7 @@ zsl_vec_scalar_mult(v, 1.0f / norm); } else { /* TODO: What is the best approach here? */ - /* On div by zero clear vector and return v[0] = 1.0. */ + /* On div by zero clear vector and return v[0] = 1.0f. */ zsl_vec_init(v); v->data[0] = 1.0f; } @@ -338,7 +338,7 @@ return -EINVAL; } - *m = 0.0; + *m = 0.0f; for (size_t i = 0; i < v->sz; i++) { *m += v->data[i]; } @@ -376,7 +376,7 @@ for (size_t p = g - x; p < (v->sz - 1); p++) { v->data[p] = v->data[p + 1]; } - v->data[v->sz - 1] = 0.0; + v->data[v->sz - 1] = 0.0f; x++; } } @@ -503,7 +503,7 @@ /* Add back the repeated values in the correct order into the vector 'w'. */ for (i = 0; i < count; i++) { - for (k = 0; k < zsl_vec_contains(v, u.data[i], 1E-5f); k++) { + for (k = 0; k < static_cast(zsl_vec_contains(v, u.data[i], 1E-5f)); k++) { w->data[count2] = u.data[i]; count2++; } From 3d82b56c5f2366a581c07fc409f58d1a2f2685b9 Mon Sep 17 00:00:00 2001 From: dlktdr Date: Sat, 3 May 2025 02:35:30 -0700 Subject: [PATCH 3/5] Use delta time for more accuracy --- firmware/src/src/aqua_ahrs/aqua.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/firmware/src/src/aqua_ahrs/aqua.cpp b/firmware/src/src/aqua_ahrs/aqua.cpp index ca1a7ed8..5398646a 100644 --- a/firmware/src/src/aqua_ahrs/aqua.cpp +++ b/firmware/src/src/aqua_ahrs/aqua.cpp @@ -9,9 +9,12 @@ #include #include #include "aqua.h" + #include "defines.h" static uint32_t zsl_fus_aqua_freq; static uint32_t zsl_fus_aqua_initialised; + static uint32_t lastUpdate; + float deltaT = 1.0f / 150.0f; static int zsl_fus_aqua(struct zsl_vec *a, struct zsl_vec *m, struct zsl_vec *g, zsl_real_t *e_a, zsl_real_t *e_m, @@ -25,7 +28,7 @@ /* Calculate an estimation of the orientation using only the data of the * gyroscope and quaternion integration. */ zsl_vec_scalar_mult(g, -1.0f); - zsl_quat_from_ang_vel(g, q, 1.0f / zsl_fus_aqua_freq, q); + zsl_quat_from_ang_vel(g, q, deltaT, q); /* Continue with the calculations only if the data from the accelerometer * is valid (non zero). */ @@ -172,8 +175,14 @@ return -EINVAL; } + uint32_t now = micros(); + uint32_t dt = now - lastUpdate; + lastUpdate = now; + deltaT = ((float)(dt) / 1000000.0f); + /* This functions should only be called once. */ if (!zsl_fus_aqua_initialised) { + deltaT = 1.0f / zsl_fus_aqua_freq; zsl_fus_aqua_alpha_init(a, &(mcfg->alpha)); } From b8664ca6ad788c0ff248bc46dac8703ece02e262 Mon Sep 17 00:00:00 2001 From: dlktdr Date: Sat, 3 May 2025 02:36:28 -0700 Subject: [PATCH 4/5] Add GUI Configuration --- firmware/src/src/BMM150/bmm150_defs.h | 2 + firmware/src/src/basetrackersettings.h | 52 +++++++++++ firmware/src/src/sense.cpp | 48 +++++++--- gui/src/basetrackersettings.h | 60 +++++++++++++ gui/src/mainwindow.cpp | 14 +++ gui/src/mainwindow.ui | 117 ++++++++++++++++++++++++- settings/settings.csv | 6 ++ 7 files changed, 285 insertions(+), 14 deletions(-) diff --git a/firmware/src/src/BMM150/bmm150_defs.h b/firmware/src/src/BMM150/bmm150_defs.h index 7d8b2c17..198f89ef 100644 --- a/firmware/src/src/BMM150/bmm150_defs.h +++ b/firmware/src/src/BMM150/bmm150_defs.h @@ -41,6 +41,8 @@ #ifndef _BMM150_DEFS_H #define _BMM150_DEFS_H +#define BMM150_USE_FLOATING_POINT + /******************************************************************************/ /*! @name Header includes */ /******************************************************************************/ diff --git a/firmware/src/src/basetrackersettings.h b/firmware/src/src/basetrackersettings.h index f950b553..883a8423 100644 --- a/firmware/src/src/basetrackersettings.h +++ b/firmware/src/src/basetrackersettings.h @@ -705,6 +705,46 @@ class BaseTrackerSettings { return false; } + // Aqua Filter Alpha (Accelerometer Gain) + inline const float& getAqA() {return aqa;} + bool setAqA(float val=0.03) { + if(val >= 0 && val <= 1) { + aqa = val; + return true; + } + return false; + } + + // Aqua Filter Beta (Magnetometer Gain) + inline const float& getAqB() {return aqb;} + bool setAqB(float val=0.03) { + if(val >= 0 && val <= 1) { + aqb = val; + return true; + } + return false; + } + + // Aqua Filter Ea (Accelerometer Weight) + inline const float& getAqEa() {return aqea;} + bool setAqEa(float val=0.9) { + if(val >= 0 && val <= 1) { + aqea = val; + return true; + } + return false; + } + + // Aqua Filter Em (Magnetometer Weight) + inline const float& getAqEm() {return aqem;} + bool setAqEm(float val=0.9) { + if(val >= 0 && val <= 1) { + aqem = val; + return true; + } + return false; + } + // Uart Mode (0- Off, 1-SBUS, 2-CRSFIN, 3-CRSFOUT) inline const uint8_t& getUartMode() {return uartmode;} bool setUartMode(uint8_t val=0) { @@ -1045,6 +1085,10 @@ class BaseTrackerSettings { json["rotx"] = rotx; json["roty"] = roty; json["rotz"] = rotz; + json["aqa"] = aqa; + json["aqb"] = aqb; + json["aqea"] = aqea; + json["aqem"] = aqem; json["uartmode"] = uartmode; json["crsftxrate"] = crsftxrate; json["sbustxrate"] = sbustxrate; @@ -1133,6 +1177,10 @@ class BaseTrackerSettings { v = json["rotx"]; if(!v.isNull()) {setRotX(v); chresetfusion = true;} v = json["roty"]; if(!v.isNull()) {setRotY(v); chresetfusion = true;} v = json["rotz"]; if(!v.isNull()) {setRotZ(v); chresetfusion = true;} + v = json["aqa"]; if(!v.isNull()) {setAqA(v); chresetfusion = true;} + v = json["aqb"]; if(!v.isNull()) {setAqB(v); chresetfusion = true;} + v = json["aqea"]; if(!v.isNull()) {setAqEa(v); chresetfusion = true;} + v = json["aqem"]; if(!v.isNull()) {setAqEm(v); chresetfusion = true;} v = json["uartmode"]; if(!v.isNull()) {setUartMode(v);} v = json["crsftxrate"]; if(!v.isNull()) {setCrsfTxRate(v);} v = json["sbustxrate"]; if(!v.isNull()) {setSbusTxRate(v);} @@ -1545,6 +1593,10 @@ class BaseTrackerSettings { float rotx = 0; // Board Rotation X float roty = 0; // Board Rotation Y float rotz = 0; // Board Rotation Z + float aqa = 0.03; // Aqua Filter Alpha (Accelerometer Gain) + float aqb = 0.03; // Aqua Filter Beta (Magnetometer Gain) + float aqea = 0.9; // Aqua Filter Ea (Accelerometer Weight) + float aqem = 0.9; // Aqua Filter Em (Magnetometer Weight) uint8_t uartmode = 0; // Uart Mode (0- Off, 1-SBUS, 2-CRSFIN, 3-CRSFOUT) uint8_t crsftxrate = 140; // CRSF Transmit Frequncy uint8_t sbustxrate = 80; // SBUS Transmit Freqency diff --git a/firmware/src/src/sense.cpp b/firmware/src/src/sense.cpp index 592dad14..f9fa644f 100644 --- a/firmware/src/src/sense.cpp +++ b/firmware/src/src/sense.cpp @@ -59,7 +59,6 @@ #if defined(HAS_BMI270) #include "bmi270.h" #include "BMI270/bmi270_common.h" - #endif #if defined(HAS_BMM150) #include "BMM150/bmm150.h" @@ -264,6 +263,7 @@ int sense_Init() #if defined(HAS_BMM150) /* Status of api are returned to this variable */ int8_t rbslt; + struct bmm150_settings settings; rbslt = bmm150_interface_selection(&bmm1_dev); bmm150_error_codes_print_result("bmm150_interface_selection", rbslt); @@ -272,10 +272,21 @@ int sense_Init() bmm150_error_codes_print_result("bmm150_init", rbslt); if (rbslt == BMM150_OK) { - rbslt = set_config(&bmm1_dev); - bmm150_error_codes_print_result("set_config", rbslt); + settings.pwr_mode = BMM150_POWERMODE_NORMAL; + rslt = bmm150_set_op_mode(&settings, &bmm1_dev); + bmm150_error_codes_print_result("set_op_mode", rbslt); + if (rslt == BMM150_OK) { + settings.data_rate = BMM150_DATA_RATE_30HZ; + settings.xy_rep = 15; // Repetitions for X/Y-axis (from datasheet) + settings.z_rep = 15; // Repetitions for Z-axis + rslt = bmm150_set_sensor_settings(BMM150_SEL_XY_REP | BMM150_SEL_Z_REP | BMM150_SEL_DATA_RATE, &settings, &bmm1_dev); + bmm150_error_codes_print_result("bmm150_set_sensor_settings", rslt); + if (rslt == BMM150_OK) { + LOG_INF("BMM150 Magnetometer Initialized"); + hasMag = true; + } + } } - hasMag = true; } else { LOG_ERR("Unable to init BMM150 - Continuing with no magnetomer\n"); } @@ -940,13 +951,24 @@ void sensor_Thread() #if defined(HAS_BMM150) if(hasMag) { int8_t rbslt; - struct bmm150_mag_data mag_data; - rbslt = bmm150_read_mag_data(&mag_data, &bmm1_dev); - bmm150_error_codes_print_result("bmm150_read_mag_data", rbslt); - tmag[0] = mag_data.y; - tmag[1] = mag_data.x; - tmag[2] = mag_data.z; - magValid = true; + uint8_t data_ready = 0; + rslt = bmm150_get_regs(BMM150_REG_DATA_READY_STATUS, &data_ready, 1, &bmm1_dev); + if(rslt == BMM150_OK) { + if(data_ready & 0x01) { + struct bmm150_mag_data mag_data; + rbslt = bmm150_read_mag_data(&mag_data, &bmm1_dev); + if (rbslt != BMM150_OK) { + bmm150_error_codes_print_result("bmm150_read_mag_data", rbslt); + } else { + tmag[0] = mag_data.y; + tmag[1] = mag_data.x; + tmag[2] = mag_data.z; + magValid = true; + } + } + } else { + bmm150_error_codes_print_result("bmm150_get_regs", rslt); + } } #endif @@ -1383,6 +1405,10 @@ void reset_fusion() amag[0] = 0; amag[1] = 0; amag[2] = 0; + aqua_cfg.alpha = trkset.getAqA(); + aqua_cfg.beta = trkset.getAqB(); + aqua_cfg.e_a = trkset.getAqEa(); + aqua_cfg.e_m = trkset.getAqEm(); LOG_INF("Resetting fusion algorithm"); } diff --git a/gui/src/basetrackersettings.h b/gui/src/basetrackersettings.h index d5d128f0..9448f4c4 100644 --- a/gui/src/basetrackersettings.h +++ b/gui/src/basetrackersettings.h @@ -137,6 +137,10 @@ class BaseTrackerSettings : public QObject _setting["rotx"] = 0; _setting["roty"] = 0; _setting["rotz"] = 0; + _setting["aqa"] = 0.03; + _setting["aqb"] = 0.03; + _setting["aqea"] = 0.9; + _setting["aqem"] = 0.9; _setting["uartmode"] = 0; _setting["crsftxrate"] = 140; _setting["sbustxrate"] = 80; @@ -251,6 +255,10 @@ class BaseTrackerSettings : public QObject descriptions["rotx"] = tr("Board Rotation X"); descriptions["roty"] = tr("Board Rotation Y"); descriptions["rotz"] = tr("Board Rotation Z"); + descriptions["aqa"] = tr("Aqua Filter Alpha (Accelerometer Gain)"); + descriptions["aqb"] = tr("Aqua Filter Beta (Magnetometer Gain)"); + descriptions["aqea"] = tr("Aqua Filter Ea (Accelerometer Weight)"); + descriptions["aqem"] = tr("Aqua Filter Em (Magnetometer Weight)"); descriptions["uartmode"] = tr("Uart Mode (0- Off, 1-SBUS, 2-CRSFIN, 3-CRSFOUT)"); descriptions["crsftxrate"] = tr("CRSF Transmit Frequncy"); descriptions["sbustxrate"] = tr("SBUS Transmit Freqency"); @@ -1088,6 +1096,58 @@ class BaseTrackerSettings : public QObject } + // Aqua Filter Alpha (Accelerometer Gain) + float getAqA() { + return _setting["aqa"].toFloat(); + } + bool setAqA(float val=0.03) { + if(val >= 0 && val <= 1) { + _setting["aqa"] = QString::number(val,'g',4); + return true; + } + return false; + } + + + // Aqua Filter Beta (Magnetometer Gain) + float getAqB() { + return _setting["aqb"].toFloat(); + } + bool setAqB(float val=0.03) { + if(val >= 0 && val <= 1) { + _setting["aqb"] = QString::number(val,'g',4); + return true; + } + return false; + } + + + // Aqua Filter Ea (Accelerometer Weight) + float getAqEa() { + return _setting["aqea"].toFloat(); + } + bool setAqEa(float val=0.9) { + if(val >= 0 && val <= 1) { + _setting["aqea"] = QString::number(val,'g',4); + return true; + } + return false; + } + + + // Aqua Filter Em (Magnetometer Weight) + float getAqEm() { + return _setting["aqem"].toFloat(); + } + bool setAqEm(float val=0.9) { + if(val >= 0 && val <= 1) { + _setting["aqem"] = QString::number(val,'g',4); + return true; + } + return false; + } + + // Uart Mode (0- Off, 1-SBUS, 2-CRSFIN, 3-CRSFOUT) uint8_t getUartMode() { return _setting["uartmode"].toUInt(); diff --git a/gui/src/mainwindow.cpp b/gui/src/mainwindow.cpp index 85bb24a4..137c1c0c 100644 --- a/gui/src/mainwindow.cpp +++ b/gui/src/mainwindow.cpp @@ -137,6 +137,10 @@ MainWindow::MainWindow(QWidget *parent) connect(ui->spnRstDblTapMax, &QSpinBox::valueChanged, this, &MainWindow::updateFromUI); connect(ui->spnRstDblTapMin, &QSpinBox::valueChanged, this, &MainWindow::updateFromUI); connect(ui->spnRstDblTapThres, &QSpinBox::valueChanged, this, &MainWindow::updateFromUI); + connect(ui->spnAqA, &QDoubleSpinBox::valueChanged, this, &MainWindow::updateFromUI); + connect(ui->spnAqB, &QDoubleSpinBox::valueChanged, this, &MainWindow::updateFromUI); + connect(ui->spnAqEa, &QDoubleSpinBox::valueChanged, this, &MainWindow::updateFromUI); + connect(ui->spnAqEm, &QDoubleSpinBox::valueChanged, this, &MainWindow::updateFromUI); // Gain Sliders connect(ui->til_gain, &GainSlider::valueChanged, this, &MainWindow::updateFromUI); @@ -593,6 +597,11 @@ void MainWindow::updateToUI() ui->spnRstDblTapMin->setValue(trkset.getRstOnDblTapMin()); ui->spnRstDblTapThres->setValue(trkset.getRstOnDblTapThres()); + ui->spnAqA->setValue(trkset.getAqA()); + ui->spnAqB->setValue(trkset.getAqB()); + ui->spnAqEa->setValue(trkset.getAqEa()); + ui->spnAqEm->setValue(trkset.getAqEm()); + int panCh = trkset.getPanCh(); int rllCh = trkset.getRllCh(); int tltCh = trkset.getTltCh(); @@ -747,6 +756,11 @@ void MainWindow::updateFromUI() trkset.setAn2Ch(an2Ch==0?-1:an2Ch); trkset.setAn3Ch(an3Ch==0?-1:an3Ch); + trkset.setAqA(ui->spnAqA->value()); + trkset.setAqB(ui->spnAqB->value()); + trkset.setAqEa(ui->spnAqEa->value()); + trkset.setAqEm(ui->spnAqEm->value()); + // Aux int auxF0Ch = ui->cmbAuxFn0Ch->currentIndex(); int auxF1Ch = ui->cmbAuxFn1Ch->currentIndex(); diff --git a/gui/src/mainwindow.ui b/gui/src/mainwindow.ui index 1d344dfd..c152e548 100644 --- a/gui/src/mainwindow.ui +++ b/gui/src/mainwindow.ui @@ -6,7 +6,7 @@ 0 0 - 601 + 617 751 @@ -336,7 +336,7 @@ false - 3 + 6 @@ -3006,6 +3006,117 @@ + + + IMU + + + + + + <html><head/><body><p><span style=" font-size:11pt; font-weight:700;">Aqua AHRS filter parameters</span></p></body></html> + + + + + + + + + Alpha + + + + + + + 3 + + + 1.000000000000000 + + + 0.010000000000000 + + + + + + + Beta + + + + + + + 3 + + + 1.000000000000000 + + + 0.010000000000000 + + + + + + + E_a + + + + + + + 3 + + + 1.000000000000000 + + + 0.010000000000000 + + + + + + + E_m + + + + + + + 3 + + + 1.000000000000000 + + + 0.010000000000000 + + + + + + + + + Qt::Vertical + + + + 20 + 222 + + + + + + @@ -3873,7 +3984,7 @@ 0 0 - 601 + 617 22 diff --git a/settings/settings.csv b/settings/settings.csv index 07587880..381ef631 100644 --- a/settings/settings.csv +++ b/settings/settings.csv @@ -157,6 +157,12 @@ float,Setting,RotX,0,-360,360,Board Rotation X,resetFusion,,4, float,Setting,RotY,0,-360,360,Board Rotation Y,resetFusion,,4, float,Setting,RotZ,0,-360,360,Board Rotation Z,resetFusion,,4, ,,,,,,,,,, +Aqua Filter Settings,,,,,,,,,, +float,Setting,AqA,0.03,0,1,Aqua Filter Alpha (Accelerometer Gain),resetFusion,,4, +float,Setting,AqB,0.03,0,1,Aqua Filter Beta (Magnetometer Gain),resetFusion,,4, +float,Setting,AqEa,0.9,0,1,Aqua Filter Ea (Accelerometer Weight),resetFusion,,4, +float,Setting,AqEm,0.9,0,1,Aqua Filter Em (Magnetometer Weight),resetFusion,,4, +,,,,,,,,,, Serial Settings,,,,,,,,,, u8,Setting,UartMode,0,0,3,"Uart Mode (0- Off, 1-SBUS, 2-CRSFIN, 3-CRSFOUT)",,,, u8,Setting,CrsfTxRate,140,30,140,CRSF Transmit Frequncy,,,, From 32538ce51790fbeb0b026a3582c716e7693b8398 Mon Sep 17 00:00:00 2001 From: dlktdr Date: Sat, 3 May 2025 03:03:06 -0700 Subject: [PATCH 5/5] move Gravity define --- firmware/src/src/BMI270/bmi270_common.h | 3 +-- firmware/src/src/include/defines.h | 1 + 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/firmware/src/src/BMI270/bmi270_common.h b/firmware/src/src/BMI270/bmi270_common.h index 4bc4e7d9..af008ee4 100644 --- a/firmware/src/src/BMI270/bmi270_common.h +++ b/firmware/src/src/BMI270/bmi270_common.h @@ -15,8 +15,7 @@ extern "C" { #include #include #include "bmi2.h" - -#define GRAVITY_EARTH (9.80665f) +#include "defines.h" #define ACCEL UINT8_C(0x00) #define GYRO UINT8_C(0x01) diff --git a/firmware/src/src/include/defines.h b/firmware/src/src/include/defines.h index 4e1f487e..bbe39b71 100644 --- a/firmware/src/src/include/defines.h +++ b/firmware/src/src/include/defines.h @@ -163,6 +163,7 @@ #define GYRO_STABLE_SAMPLES 400 #define GYRO_SAMPLE_WEIGHT 0.05f #define GYRO_FLASH_IF_OFFSET 0.5f // Save to flash if gyro is off more than 0.5 degrees/sec from flash value +#define GRAVITY_EARTH 9.80665f // Time macros #include "zephyr/kernel.h"