From 547be7ce2a5417f16bd5fb0a83f818755ccc70a5 Mon Sep 17 00:00:00 2001 From: Tellicious Date: Sun, 3 Mar 2024 20:49:09 +0100 Subject: [PATCH] Added possibility of selecting between static and dynamic memory management --- .github/releaseBody.md | 10 +- CMakeLists.txt | 18 +- Changelog.md | 9 + Readme.md | 17 ++ inc/LKHashTable.h | 2 + inc/LPHashTable.h | 2 + inc/list.h | 2 + inc/matrix.h | 65 +++++- inc/movingAvg.h | 17 +- inc/numMethods.h | 116 ++++++++++- inc/queue.h | 20 +- src/LKHashTable.c | 50 ++++- src/LPHashTable.c | 62 ++++-- src/list.c | 55 +++-- src/matrix.c | 143 ++++++++++++- src/movingAvg.c | 49 ++++- src/numMethods.c | 445 +++++++++++++++++++++++++++++++++++++++-- src/queue.c | 53 ++++- 18 files changed, 1056 insertions(+), 79 deletions(-) diff --git a/.github/releaseBody.md b/.github/releaseBody.md index 5f543fc..b0e1675 100644 --- a/.github/releaseBody.md +++ b/.github/releaseBody.md @@ -1,6 +1,10 @@ -## Removed warning in `GaussNewton_Sens_Cal` +## Added possibility of using static and/or dynamic memory management -**Bugfix:** -- Initialized some variables in `GaussNewton_Sens_Cal` to avoid a `-Wmaybe-uninitialized` warning +### Contains breaking change on memory management. `ADVUTILS_USE_STATIC_ALLOCATION` and/or `ADVUTILS_USE_DYNAMIC_ALLOCATION` must be defined by user + +**New features:** +- Added `*Static` variant of multiple functions with static memory management +- Added possibility of setting static and/or dynamic memory management for ADVUtils library via definition of `ADVUTILS_USE_STATIC_ALLOCATION` and `ADVUTILS_USE_DYNAMIC_ALLOCATION`. At least one of the two must be defined by the user +- Added possibility of using FreeRTOS-specific dynamic memory management functions with `set(ADVUtils_DYN_MEM_MGMT "USE_FREERTOS" CACHE STRING "" FORCE)` in CMake See [Changelog](Changelog.md) \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index 3374e71..0297bd7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,10 +6,11 @@ # # libName_COMPILE_OPTS: If defined, it provide compiler options for generated library. # libName_COMPILE_DEFS: If defined, it provides "-D" definitions to the library build +# add set(${libName}_DYN_MEM_MGMT "USE_FREERTOS" CACHE STRING "" FORCE) before add_subdirectory() to automatically configure the library to use FreeRTOS memory management functions (malloc, calloc, free) # -cmake_minimum_required(VERSION 3.20) +cmake_minimum_required(VERSION 3.22) -set (libName ADVutils) +set (libName ADVUtils) set(${libName}_src ${CMAKE_CURRENT_LIST_DIR}/src/basicMath.c @@ -33,9 +34,22 @@ set(${libName}_inc ${CMAKE_CURRENT_LIST_DIR}/inc/ ) +# Append definitions needed to automatically configure build with FreeRTOS +if ("${${libName}_DYN_MEM_MGMT}" STREQUAL "USE_FREERTOS") + list(APPEND ${libName}_COMPILE_DEFS + ADVUTILS_MEMORY_MGMT_HEADER="FreeRTOS.h" + ADVUTILS_MALLOC=pvPortMalloc + ADVUTILS_CALLOC=pvPortCalloc + ADVUTILS_FREE=vPortFree + ) +endif() + # Register library to the system add_library(${libName} STATIC) target_sources(${libName} PRIVATE ${${libName}_src}) target_include_directories(${libName} PUBLIC ${${libName}_inc}) target_compile_options(${libName} PRIVATE ${${libName}_COMPILE_OPTS}) target_compile_definitions(${libName} PRIVATE ${${libName}_COMPILE_DEFS}) +if ("${${libName}_DYN_MEM_MGMT}" STREQUAL "USE_FREERTOS") + target_link_libraries(${libName} freertos_kernel) +endif() \ No newline at end of file diff --git a/Changelog.md b/Changelog.md index 67d0567..f7189ec 100644 --- a/Changelog.md +++ b/Changelog.md @@ -1,5 +1,14 @@ # Changelog +## v1.9.0 + +### Contains breaking change on memory management. `ADVUTILS_USE_STATIC_ALLOCATION` and/or `ADVUTILS_USE_DYNAMIC_ALLOCATION` must be defined by user + +**New features:** +- Added `*Static` variant of multiple functions with static memory management +- Added possibility of setting static and/or dynamic memory management for ADVUtils library via definition of `ADVUTILS_USE_STATIC_ALLOCATION` and `ADVUTILS_USE_DYNAMIC_ALLOCATION`. At least one of the two must be defined by the user +- Added possibility of using FreeRTOS-specific dynamic memory management functions with `set(ADVUtils_DYN_MEM_MGMT "USE_FREERTOS" CACHE STRING "" FORCE)` in CMake + ## v1.8.1 **Bugfix:** diff --git a/Readme.md b/Readme.md index 78b4c48..e6b330a 100644 --- a/Readme.md +++ b/Readme.md @@ -17,5 +17,22 @@ - ***IIRFilters:*** simple discrete-time IIR filters, with on-the-fly conversion continuous -> discrete of derivative and integrator - ***commonTypes:*** collection of common type definitions +## Configuration: +- User must define `ADVUTILS_USE_STATIC_ALLOCATION` and/or `ADVUTILS_USE_DYNAMIC_ALLOCATION` to select wheter to use static and/or dynamic memory management. `list`, `LPHashTable` and `LKHashTable` are available only with `ADVUTILS_USE_DYNAMIC_ALLOCATION` +- Functions that use static allocation are defined by `Static` suffix +- To automatically use thread-safe FreeRTOS-specific implementation of dynamic memory management functions (`malloc`, `calloc` and `free`) user can add `set(ADVUtils_DYN_MEM_MGMT "USE_FREERTOS" CACHE STRING "" FORCE)` before `add_subdirectory()` in project CMakeLists. This will automatically add the following defines to ADVUtils compilation: + - `ADVUTILS_MEMORY_MGMT_HEADER="FreeRTOS.h"` + - `ADVUTILS_MALLOC=pvPortMalloc` + - `ADVUTILS_CALLOC=pvPortCalloc` + - `ADVUTILS_FREE=vPortFree` + - `target_link_libraries(ADVUtils freertos_kernel)` +- If `ADVUTILS_MEMORY_MGMT_HEADER` is not defined, the library will use `stdlib` version of `malloc`, `calloc` and `free` +- User can use another different implementation of `malloc`, `calloc` and `free` by defining the following four macros: + - `ADVUTILS_MEMORY_MGMT_HEADER` + - `ADVUTILS_MALLOC` + - `ADVUTILS_CALLOC` + - `ADVUTILS_FREE` + + diff --git a/inc/LKHashTable.h b/inc/LKHashTable.h index 0e145ad..e335692 100644 --- a/inc/LKHashTable.h +++ b/inc/LKHashTable.h @@ -71,6 +71,7 @@ typedef enum { LKHT_REMOVE_ITEM = 0, LKHT_DO_NOT_REMOVE_ITEM = 1 } lkHashTableRe /* Function prototypes -------------------------------------------------------*/ +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION /** * \brief Init linked hash-table structure * @@ -125,6 +126,7 @@ static inline void lkHashTableInfo(lkHashTable_t* lkht, uint32_t* size, uint32_t * \return UTILS_STATUS_SUCCESS if list is flushed correctly, UTILS_STATUS_ERROR if data cannot be flushed */ utilsStatus_t lkHashTableFlush(lkHashTable_t* lkht); +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ #ifdef __cplusplus } diff --git a/inc/LPHashTable.h b/inc/LPHashTable.h index 078f0e8..8d28a49 100644 --- a/inc/LPHashTable.h +++ b/inc/LPHashTable.h @@ -92,6 +92,7 @@ typedef enum { LPHT_REMOVE_ITEM = 0, LPHT_DO_NOT_REMOVE_ITEM = 1 } lpHashTableRe /* Function prototypes -------------------------------------------------------*/ +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION /** * \brief Init linear-probing hash-table structure * @@ -165,6 +166,7 @@ void lpHashTableIt(lpHashTableIterator_t* it, lpHashTable_t* lpht); * \return UTILS_STATUS_SUCCESS if iterator is moved to next value, UTILS_STATUS_ERROR if there are no more items */ utilsStatus_t lpHashTableItNext(lpHashTableIterator_t* it); +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ #ifdef __cplusplus } diff --git a/inc/list.h b/inc/list.h index dccef42..984fd24 100644 --- a/inc/list.h +++ b/inc/list.h @@ -78,6 +78,7 @@ typedef struct { /* Function prototypes -------------------------------------------------------*/ +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION /** * \brief Init list structure * @@ -228,6 +229,7 @@ void listIt(listIterator_t* it, list_t* list); * \return UTILS_STATUS_SUCCESS if iterator is moved to next value, UTILS_STATUS_ERROR if there are no more items */ utilsStatus_t listItNext(listIterator_t* it); +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ #ifdef __cplusplus } diff --git a/inc/matrix.h b/inc/matrix.h index 58f5134..1da0714 100644 --- a/inc/matrix.h +++ b/inc/matrix.h @@ -62,8 +62,9 @@ typedef struct { /* Function prototypes -------------------------------------------------------*/ +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION /** - * \brief Create a new matrix containing a pointer to an external array + * \brief Create a new matrix with dynamic memory allocation * * \param[in] matrix: pointer to matrix object * \param[in] rows: number of rows @@ -72,6 +73,19 @@ typedef struct { * \return UTILS_STATUS_SUCCESS if matrix was initialized, UTILS_STATUS_ERROR if data was not allocated correctly */ utilsStatus_t matrixInit(matrix_t* matrix, uint8_t rows, uint8_t cols); +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ + +#ifdef ADVUTILS_USE_STATIC_ALLOCATION +/** + * \brief Create a new matrix with static data + * + * \param[in] matrix: pointer to matrix object + * \param[in] data: pointer to data array of size rows * cols + * \param[in] rows: number of rows + * \param[in] cols: number of columns + */ +void matrixInitStatic(matrix_t* matrix, float* data, uint8_t rows, uint8_t cols); +#endif /* ADVUTILS_USE_STATIC_ALLOCATION */ /** * \brief Set the matrix as an identity matrix @@ -168,6 +182,7 @@ utilsStatus_t matrixMult_rhsT(matrix_t* lhs, matrix_t* rhs, matrix_t* result); */ void matrixMultScalar(matrix_t* lhs, float sc, matrix_t* result); +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION /** * \brief Matrix inversion * @@ -183,6 +198,25 @@ void matrixInversed(matrix_t* lhs, matrix_t* result); * \param[out] result: pointer to result matrix object */ void matrixInversed_rob(matrix_t* lhs, matrix_t* result); +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ + +#ifdef ADVUTILS_USE_STATIC_ALLOCATION +/** + * \brief Matrix inversion with static allocation + * + * \param[in] lhs: pointer to left-hand side matrix object + * \param[out] result: pointer to result matrix object + */ +void matrixInversedStatic(matrix_t* lhs, matrix_t* result); + +/** + * \brief Robust matrix inversion performed with LUP decomposition and static allocation + * + * \param[in] lhs: pointer to left-hand side matrix object + * \param[out] result: pointer to result matrix object + */ +void matrixInversedStatic_rob(matrix_t* lhs, matrix_t* result); +#endif /* ADVUTILS_USE_STATIC_ALLOCATION */ /** * \brief Matrix transposition @@ -200,13 +234,25 @@ void matrixTrans(matrix_t* lhs, matrix_t* result); */ void matrixNormalized(matrix_t* lhs, matrix_t* result); +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION /** * \brief Matrix Moore-Penrose pseudo-inverse * * \param[in] lhs: pointer to left-hand side matrix object * \param[out] result: pointer to result matrix object */ -void matrixPseudo_inv(matrix_t* lhs, matrix_t* result); +void matrixPseudoInv(matrix_t* lhs, matrix_t* result); +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ + +#ifdef ADVUTILS_USE_STATIC_ALLOCATION +/** + * \brief Matrix Moore-Penrose pseudo-inverse with static allocation + * + * \param[in] lhs: pointer to left-hand side matrix object + * \param[out] result: pointer to result matrix object + */ +void matrixPseudoInvStatic(matrix_t* lhs, matrix_t* result); +#endif /* ADVUTILS_USE_STATIC_ALLOCATION */ /** * \brief Set a single element inside matrix @@ -229,6 +275,7 @@ static inline void matrixSet(matrix_t* matrix, uint8_t i, uint8_t j, float value */ static inline float matrixGet(matrix_t* matrix, uint8_t i, uint8_t j) { return ELEMP(matrix, i, j); } +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION /** * \brief Calculate matrix determinant * @@ -237,6 +284,18 @@ static inline float matrixGet(matrix_t* matrix, uint8_t i, uint8_t j) { return E * \return determinant */ float matrixDet(matrix_t* matrix); +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ + +#ifdef ADVUTILS_USE_STATIC_ALLOCATION +/** + * \brief Calculate matrix determinant + * + * \param[in] matrix: pointer to matrix object + * + * \return determinant + */ +float matrixDetStatic(matrix_t* matrix); +#endif /* ADVUTILS_USE_STATIC_ALLOCATION */ /** * \brief Calculate matrix norm @@ -247,6 +306,7 @@ float matrixDet(matrix_t* matrix); */ float matrixNorm(matrix_t* matrix); +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION /** * \brief Delete matrix * @@ -255,6 +315,7 @@ float matrixNorm(matrix_t* matrix); * \return UTILS_STATUS_SUCCESS if matrix data is deleted, UTILS_STATUS_ERROR if data was not deleted correctly */ utilsStatus_t matrixDelete(matrix_t* matrix); +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ #ifdef __cplusplus } diff --git a/inc/movingAvg.h b/inc/movingAvg.h index e2b75f8..fc2fb9a 100644 --- a/inc/movingAvg.h +++ b/inc/movingAvg.h @@ -61,13 +61,26 @@ typedef struct { /* Function prototypes -------------------------------------------------------*/ +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION /** - * \brief Init moving average structure + * \brief Init moving average structure with dynamic memory allocation * * \param[in] movingAvg: pointer to moving average object * \param[in] size: required queue size */ utilsStatus_t movingAvgInit(movingAvg_t* movingAvg, MOVAVG_IND_TYPE size); +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ + +#ifdef ADVUTILS_USE_STATIC_ALLOCATION +/** + * \brief Init moving average structure with static data + * + * \param[in] movingAvg: pointer to moving average object + * \param[in] data: pointer to data array + * \param[in] size: required queue size + */ +void movingAvgInitStatic(movingAvg_t* movingAvg, MOVAVG_TYPE* data, MOVAVG_IND_TYPE size); +#endif /* ADVUTILS_USE_STATIC_ALLOCATION */ /** * \brief Calculate moving average @@ -95,12 +108,14 @@ static inline MOVAVG_TYPE movingAvgGetLatest(movingAvg_t* movingAvg) { return (m */ utilsStatus_t movingAvgFlush(movingAvg_t* movingAvg); +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION /** * \brief Delete moving average * * \param[in] movingAvg: pointer to moving average object */ utilsStatus_t movingAvgDelete(movingAvg_t* movingAvg); +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ #ifdef __cplusplus } diff --git a/inc/numMethods.h b/inc/numMethods.h index 0e34fdb..ca4165a 100644 --- a/inc/numMethods.h +++ b/inc/numMethods.h @@ -90,6 +90,15 @@ void bksub(matrix_t* A, matrix_t* B, matrix_t* result); */ void bksubPerm(matrix_t* A, matrix_t* B, matrix_t* P, matrix_t* result); +/** + * \brief Calculate sort of quadratic form result=A*B*(~A) + * + * \param[in] A: pointer to A matrix object + * \param[in] B: pointer to B matrix object + * \param[out] result: pointer to result matrix object + */ +void QuadProd(matrix_t* A, matrix_t* B, matrix_t* result); + /** * \brief Calculate L (lower triangular) and U (upper triangular) matrices such that A = LU with Crout's Method * @@ -101,6 +110,7 @@ void bksubPerm(matrix_t* A, matrix_t* B, matrix_t* P, matrix_t* result); */ utilsStatus_t LU_Crout(matrix_t* A, matrix_t* L, matrix_t* U); +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION /** * \brief Calculate L (lower triangular) and U (upper triangular) matrices such that A = LU with Cormen's Method * @@ -152,16 +162,106 @@ void LinSolveLUP(matrix_t* A, matrix_t* B, matrix_t* result); void LinSolveGauss(matrix_t* A, matrix_t* B, matrix_t* result); /** - * \brief Calculate sort of quadratic form result=A*B*(~A) + * \brief Gauss-Newton sensor calibration with 9 parameters + * \attention Approximates Data to a sphere of radius k by calculating 6 gains (s) and 3 biases (b), useful to calibrate some sensors (meas_sphere=S*(meas-B) with S symmetric) + * \attention Data has n>=9 rows corresponding to the number of measures and 3 columns corresponding to the 3 axes + * \attention b1=out(0,0); + * b2=out(1,0); + * b3=out(2,0); + * s11=out(3,0); + * s12=out(4,0); + * s13=out(5,0); + * s22=out(6,0); + * s23=out(7,0); + * s33=out(8,0); + * + * \param[in] Data: pointer to raw data matrix object Data + * \param[in] k: radius of sphere to be approximated. If 0, it is calculated automatically + * \param[in] X0: pointer to starting vector X0 (usually [0 0 0 1 0 0 1 0 1]). If NULL it is calculated automatically + * \param[in] nmax: maximum number of iterations (200 is generally fine, even if it usually converges within 10 iterations) + * \param[in] tol: stopping tolerance (1e-6 is generally fine) + * \param[out] result: pointer to result matrix object S + * + * \return UTILS_STATUS_SUCCESS if success, UTILS_STATUS_TIMEOUT if nmax is reached, UTILS_STATUS_ERROR if errors + */ +utilsStatus_t GaussNewton_Sens_Cal_9(matrix_t* Data, float k, matrix_t* X0, uint16_t nmax, float tol, matrix_t* result); + +/** + * \brief Gauss-Newton sensor calibration with 6 parameters + * \attention Approximates Data to a sphere of radius k by calculating 3 gains (s) and 3 biases (b), useful to calibrate some sensors (meas_sphere=S*(meas-B) with S symmetric) + * \attention Data has n>=6 rows corresponding to the number of measures and 3 columns corresponding to the 3 axes + * \attention b1=out(0,0); + * b2=out(1,0); + * b3=out(2,0); + * s11=out(3,0); + * s22=out(4,0); + * s33=out(5,0); + * + * \param[in] Data: pointer to raw data matrix object Data + * \param[in] k: radius of sphere to be approximated. If 0, it is calculated automatically + * \param[in] X0: pointer to starting vector X0 (usually [0 0 0 1 1 1]). If NULL it is calculated automatically + * \param[in] nmax: maximum number of iterations (200 is generally fine, even if it usually converges within 10 iterations) + * \param[in] tol: stopping tolerance (1e-6 is generally fine) + * \param[out] result: pointer to result matrix object S + * + * \return UTILS_STATUS_SUCCESS if success, UTILS_STATUS_TIMEOUT if nmax is reached, UTILS_STATUS_ERROR if errors + */ +utilsStatus_t GaussNewton_Sens_Cal_6(matrix_t* Data, float k, matrix_t* X0, uint16_t nmax, float tol, matrix_t* result); +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ + +#ifdef ADVUTILS_USE_STATIC_ALLOCATION +/** + * \brief Calculate L (lower triangular) and U (upper triangular) matrices such that A = LU with Cormen's Method and static allocation + * + * \param[in] A: pointer to A matrix object + * \param[out] L: pointer to L matrix object + * \param[out] U: pointer to U matrix object + * + * \return UTILS_STATUS_SUCCESS if success, UTILS_STATUS_ERROR if errors + */ +utilsStatus_t LU_CormenStatic(matrix_t* A, matrix_t* L, matrix_t* U); + +/** + * \brief Calculate L (lower triangular), U (upper triangular) and P (permutation) matrices such that A = LUP with Cormen's Method and static allocation + * + * \param[in] A: pointer to A matrix object + * \param[out] L: pointer to L matrix object + * \param[out] U: pointer to U matrix object + * \param[in] P: pointer to P matrix object + * + * \return factor to be multiplied by determinant of U to obtain determinant of A + */ +int8_t LUP_CormenStatic(matrix_t* A, matrix_t* L, matrix_t* U, matrix_t* P); + +/** + * \brief Solve AX = B system using LU factorization with static allocation * * \param[in] A: pointer to A matrix object * \param[in] B: pointer to B matrix object * \param[out] result: pointer to result matrix object */ -void QuadProd(matrix_t* A, matrix_t* B, matrix_t* result); +void LinSolveLUStatic(matrix_t* A, matrix_t* B, matrix_t* result); /** - * \brief Gauss-Newton sensor calibration with 9 parameters + * \brief Solve AX = B system using LUP factorization with static allocation + * + * \param[in] A: pointer to A matrix object + * \param[in] B: pointer to B matrix object + * \param[out] result: pointer to result matrix object + */ +void LinSolveLUPStatic(matrix_t* A, matrix_t* B, matrix_t* result); + +/** + * \brief Solve AX = B system using Gauss elimination with partial pivoting and static allocation + * + * \param[in] A: pointer to A matrix object + * \param[in] B: pointer to B matrix object + * \param[out] result: pointer to result matrix object + */ +void LinSolveGaussStatic(matrix_t* A, matrix_t* B, matrix_t* result); + +/** + * \brief Gauss-Newton sensor calibration with 9 parameters and static allocation * \attention Approximates Data to a sphere of radius k by calculating 6 gains (s) and 3 biases (b), useful to calibrate some sensors (meas_sphere=S*(meas-B) with S symmetric) * \attention Data has n>=9 rows corresponding to the number of measures and 3 columns corresponding to the 3 axes * \attention b1=out(0,0); @@ -183,10 +283,11 @@ void QuadProd(matrix_t* A, matrix_t* B, matrix_t* result); * * \return UTILS_STATUS_SUCCESS if success, UTILS_STATUS_TIMEOUT if nmax is reached, UTILS_STATUS_ERROR if errors */ -utilsStatus_t GaussNewton_Sens_Cal_9(matrix_t* Data, float k, matrix_t* X0, uint16_t nmax, float tol, matrix_t* result); +utilsStatus_t GaussNewton_Sens_Cal_9Static(matrix_t* Data, float k, matrix_t* X0, uint16_t nmax, float tol, + matrix_t* result); /** - * \brief Gauss-Newton sensor calibration with 6 parameters + * \brief Gauss-Newton sensor calibration with 6 parameters and static allocation * \attention Approximates Data to a sphere of radius k by calculating 3 gains (s) and 3 biases (b), useful to calibrate some sensors (meas_sphere=S*(meas-B) with S symmetric) * \attention Data has n>=6 rows corresponding to the number of measures and 3 columns corresponding to the 3 axes * \attention b1=out(0,0); @@ -205,7 +306,10 @@ utilsStatus_t GaussNewton_Sens_Cal_9(matrix_t* Data, float k, matrix_t* X0, uint * * \return UTILS_STATUS_SUCCESS if success, UTILS_STATUS_TIMEOUT if nmax is reached, UTILS_STATUS_ERROR if errors */ -utilsStatus_t GaussNewton_Sens_Cal_6(matrix_t* Data, float k, matrix_t* X0, uint16_t nmax, float tol, matrix_t* result); +utilsStatus_t GaussNewton_Sens_Cal_6Static(matrix_t* Data, float k, matrix_t* X0, uint16_t nmax, float tol, + matrix_t* result); + +#endif /* ADVUTILS_USE_STATIC_ALLOCATION */ #ifdef __cplusplus } diff --git a/inc/queue.h b/inc/queue.h index efce33c..a1201a2 100644 --- a/inc/queue.h +++ b/inc/queue.h @@ -61,8 +61,9 @@ typedef struct { /* Function prototypes -------------------------------------------------------*/ +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION /** - * \brief Init queue structure + * \brief Init queue structure with dynamic memory allocation * * \param[in] queue: pointer to queue object * \param[in] itemSize: size in bytes of each item in the queue @@ -71,6 +72,21 @@ typedef struct { * \return UTILS_STATUS_SUCCESS if queue is initialized, UTILS_STATUS_ERROR if data was not allocated correctly */ utilsStatus_t queueInit(queue_t* queue, size_t itemSize, QUEUE_STYPE size); +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ + +#ifdef ADVUTILS_USE_STATIC_ALLOCATION +/** + * \brief Init queue structure with static data + * + * \param[in] queue: pointer to queue object + * \param[in] data: pointer to data array with size = size * itemSize + * \param[in] itemSize: size in bytes of each item in the queue + * \param[in] size: required queue size (number of objects) + * + * \return UTILS_STATUS_SUCCESS if queue is initialized, UTILS_STATUS_ERROR if data was not allocated correctly + */ +void queueInitStatic(queue_t* queue, uint8_t* data, size_t itemSize, QUEUE_STYPE size); +#endif /* ADVUTILS_USE_STATIC_ALLOCATION */ /** * \brief Add data to end of queue @@ -186,6 +202,7 @@ static inline void queueInfo(queue_t* queue, QUEUE_STYPE* size, QUEUE_STYPE* ite */ utilsStatus_t queueFlush(queue_t* queue); +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION /** * \brief Delete queue * @@ -194,6 +211,7 @@ utilsStatus_t queueFlush(queue_t* queue); * \return UTILS_STATUS_SUCCESS if queue data is deleted, UTILS_STATUS_ERROR if data was not deleted correctly */ utilsStatus_t queueDelete(queue_t* queue); +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ #ifdef __cplusplus } diff --git a/src/LKHashTable.c b/src/LKHashTable.c index a0c0dae..5ca44e6 100644 --- a/src/LKHashTable.c +++ b/src/LKHashTable.c @@ -31,26 +31,58 @@ */ /* END Header */ +/* Configuration check -------------------------------------------------------*/ +#if !defined(ADVUTILS_USE_DYNAMIC_ALLOCATION) && !defined(ADVUTILS_USE_STATIC_ALLOCATION) +#error Either ADVUTILS_USE_DYNAMIC_ALLOCATION or ADVUTILS_USE_STATIC_ALLOCATION must be set for ADVUtils to work +#endif + /* Includes ------------------------------------------------------------------*/ #include "LKHashTable.h" -#include #include #include "hashFunctions.h" +#ifdef ADVUTILS_MEMORY_MGMT_HEADER +#if !defined(ADVUTILS_MALLOC) || !defined(ADVUTILS_CALLOC) || !defined(ADVUTILS_FREE) +#error ADVUTILS_MALLOC, ADVUTILS_CALLOC and ADVUTILS_FREE must be defined by the user! +#else +#include ADVUTILS_MEMORY_MGMT_HEADER +#endif /* !defined(ADVUTILS_MALLOC) || !defined(ADVUTILS_CALLOC) || !defined(ADVUTILS_FREE) */ +#else +#include +#endif /* ADVUTILS_MEMORY_MGMT_HEADER */ /* Macros --------------------------------------------------------------------*/ +#ifndef ADVUTILS_MEMORY_MGMT_HEADER +#define ADVUTILS_MALLOC malloc +#define ADVUTILS_CALLOC calloc +#define ADVUTILS_FREE free +#endif /* ADVUTILS_MEMORY_MGMT_HEADER */ + #define LPHT_HASHFUN(x) hash_FNV1A(x) +/* Private Functions ---------------------------------------------------------*/ + +static inline char* lkHashTableStrdup(const char* s) { + size_t bufsize = strlen(s) + 1; + char* retval = ADVUTILS_MALLOC(bufsize); + if (retval) { + memcpy(retval, s, bufsize); + } + return retval; +} + /* Functions -----------------------------------------------------------------*/ +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION + utilsStatus_t lkHashTableInit(lkHashTable_t* lkht, size_t itemSize, uint32_t size) { uint32_t ii; lkht->items = 0; lkht->size = size; lkht->itemSize = itemSize; - lkht->entries = calloc(lkht->size, sizeof(list_t)); + lkht->entries = ADVUTILS_CALLOC(lkht->size, sizeof(list_t)); if (lkht->entries == NULL) { return UTILS_STATUS_ERROR; } @@ -89,8 +121,8 @@ utilsStatus_t lkHashTablePut(lkHashTable_t* lkht, char* key, void* value) { } /* set entry and update length */ - entry.key = strdup(key); - entry.value = calloc(1, lkht->itemSize); + entry.key = lkHashTableStrdup(key); + entry.value = ADVUTILS_CALLOC(1, lkht->itemSize); memcpy(entry.value, value, lkht->itemSize); if (listPush(&(lkht->entries[ii]), &entry) == UTILS_STATUS_SUCCESS) { @@ -126,8 +158,8 @@ utilsStatus_t lkHashTableGet(lkHashTable_t* lkht, char* key, void* value, lkHash if (remove == LKHT_REMOVE_ITEM) { listRemove(&(lkht->entries[ii]), &entry, iterator.idx); lkht->items--; - free(entry.key); - free(entry.value); + ADVUTILS_FREE(entry.key); + ADVUTILS_FREE(entry.value); return UTILS_STATUS_SUCCESS; } return UTILS_STATUS_SUCCESS; @@ -152,8 +184,8 @@ utilsStatus_t lkHashTableFlush(lkHashTable_t* lkht) { while (idx--) { listPop(&(lkht->entries[ii]), &entry); - free(entry.key); - free(entry.value); + ADVUTILS_FREE(entry.key); + ADVUTILS_FREE(entry.value); } } @@ -161,3 +193,5 @@ utilsStatus_t lkHashTableFlush(lkHashTable_t* lkht) { return UTILS_STATUS_SUCCESS; } + +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ diff --git a/src/LPHashTable.c b/src/LPHashTable.c index e5d8908..096f96e 100644 --- a/src/LPHashTable.c +++ b/src/LPHashTable.c @@ -31,15 +31,34 @@ */ /* END Header */ +/* Configuration check -------------------------------------------------------*/ +#if !defined(ADVUTILS_USE_DYNAMIC_ALLOCATION) && !defined(ADVUTILS_USE_STATIC_ALLOCATION) +#error Either ADVUTILS_USE_DYNAMIC_ALLOCATION or ADVUTILS_USE_STATIC_ALLOCATION must be set for ADVUtils to work +#endif + /* Includes ------------------------------------------------------------------*/ #include "LPHashTable.h" -#include #include #include "hashFunctions.h" +#ifdef ADVUTILS_MEMORY_MGMT_HEADER +#if !defined(ADVUTILS_MALLOC) || !defined(ADVUTILS_CALLOC) || !defined(ADVUTILS_FREE) +#error ADVUTILS_MALLOC, ADVUTILS_CALLOC and ADVUTILS_FREE must be defined by the user! +#else +#include ADVUTILS_MEMORY_MGMT_HEADER +#endif /* !defined(ADVUTILS_MALLOC) || !defined(ADVUTILS_CALLOC) || !defined(ADVUTILS_FREE) */ +#else +#include +#endif /* ADVUTILS_MEMORY_MGMT_HEADER */ /* Macros --------------------------------------------------------------------*/ +#ifndef ADVUTILS_MEMORY_MGMT_HEADER +#define ADVUTILS_MALLOC malloc +#define ADVUTILS_CALLOC calloc +#define ADVUTILS_FREE free +#endif /* ADVUTILS_MEMORY_MGMT_HEADER */ + #define LPHT_HASHFUN(x) hash_FNV1A(x) /* Function prototypes -------------------------------------------------------*/ @@ -47,8 +66,21 @@ static utilsStatus_t lpHashTableSetEntry(lpHashTable_t* lpht, char* key, void* value); static utilsStatus_t lpHashTableXpand(lpHashTable_t* lpht, uint8_t increase); +/* Private Functions ---------------------------------------------------------*/ + +static inline char* lpHashTableStrdup(const char* s) { + size_t bufsize = strlen(s) + 1; + char* retval = ADVUTILS_MALLOC(bufsize); + if (retval) { + memcpy(retval, s, bufsize); + } + return retval; +} + /* Functions -----------------------------------------------------------------*/ +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION + utilsStatus_t lpHashTableInit(lpHashTable_t* lpht, size_t itemSize, uint32_t init_items, lpHashTableResizable_t resizable) { lpht->items = 0; @@ -56,7 +88,7 @@ utilsStatus_t lpHashTableInit(lpHashTable_t* lpht, size_t itemSize, uint32_t ini lpht->itemSize = itemSize; lpht->resizable = resizable; - lpht->entries = calloc(lpht->size, sizeof(lpHashTableEntry_t)); + lpht->entries = ADVUTILS_CALLOC(lpht->size, sizeof(lpHashTableEntry_t)); if (lpht->entries == NULL) { return UTILS_STATUS_ERROR; } @@ -100,8 +132,8 @@ utilsStatus_t lpHashTableGet(lpHashTable_t* lpht, char* key, void* value, lpHash memcpy(value, lpht->entries[ii].value, lpht->itemSize); if (remove == LPHT_REMOVE_ITEM) { - free(lpht->entries[ii].key); - free(lpht->entries[ii].value); + ADVUTILS_FREE(lpht->entries[ii].key); + ADVUTILS_FREE(lpht->entries[ii].value); lpht->entries[ii].key = NULL; lpht->entries[ii].value = NULL; lpht->items--; @@ -127,8 +159,8 @@ utilsStatus_t lpHashTableFlush(lpHashTable_t* lpht) { } for (ii = 0; ii < lpht->size; ii++) { - free(lpht->entries[ii].key); - free(lpht->entries[ii].value); + ADVUTILS_FREE(lpht->entries[ii].key); + ADVUTILS_FREE(lpht->entries[ii].value); lpht->entries[ii].key = NULL; lpht->entries[ii].value = NULL; } @@ -156,12 +188,12 @@ static utilsStatus_t lpHashTableSetEntry(lpHashTable_t* lpht, char* key, void* v } /* Didn't find key, allocate+copy if needed, then insert it. */ - lpht->entries[ii].key = strdup(key); - lpht->entries[ii].value = calloc(1, lpht->itemSize); + lpht->entries[ii].key = lpHashTableStrdup(key); + lpht->entries[ii].value = ADVUTILS_CALLOC(1, lpht->itemSize); if ((lpht->entries[ii].key == NULL) || (lpht->entries[ii].value == NULL)) { - free(lpht->entries[ii].key); - free(lpht->entries[ii].value); + ADVUTILS_FREE(lpht->entries[ii].key); + ADVUTILS_FREE(lpht->entries[ii].value); return UTILS_STATUS_ERROR; } @@ -197,7 +229,7 @@ static utilsStatus_t lpHashTableXpand(lpHashTable_t* lpht, uint8_t increase) { } lpHashTableEntry_t* old_entries = lpht->entries; - lpht->entries = calloc(lpht->size, sizeof(lpHashTableEntry_t)); + lpht->entries = ADVUTILS_CALLOC(lpht->size, sizeof(lpHashTableEntry_t)); if (lpht->entries == NULL) { lpht->size = old_size; lpht->entries = old_entries; @@ -210,12 +242,12 @@ static utilsStatus_t lpHashTableXpand(lpHashTable_t* lpht, uint8_t increase) { for (ii = 0; ii < old_size; ii++) { if (old_entries[ii].key != NULL) { lpHashTableSetEntry(lpht, old_entries[ii].key, old_entries[ii].value); - free(old_entries[ii].key); - free(old_entries[ii].value); + ADVUTILS_FREE(old_entries[ii].key); + ADVUTILS_FREE(old_entries[ii].value); } } - free(old_entries); + ADVUTILS_FREE(old_entries); return UTILS_STATUS_SUCCESS; } @@ -240,3 +272,5 @@ utilsStatus_t lpHashTableItNext(lpHashTableIterator_t* it) { } return UTILS_STATUS_ERROR; } + +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ diff --git a/src/list.c b/src/list.c index f06fbf2..dc71b02 100644 --- a/src/list.c +++ b/src/list.c @@ -31,14 +31,37 @@ */ /* END Header */ +/* Configuration check -------------------------------------------------------*/ +#if !defined(ADVUTILS_USE_DYNAMIC_ALLOCATION) && !defined(ADVUTILS_USE_STATIC_ALLOCATION) +#error Either ADVUTILS_USE_DYNAMIC_ALLOCATION or ADVUTILS_USE_STATIC_ALLOCATION must be set for ADVUtils to work +#endif + /* Includes ------------------------------------------------------------------*/ #include "list.h" -#include #include +#ifdef ADVUTILS_MEMORY_MGMT_HEADER +#if !defined(ADVUTILS_MALLOC) || !defined(ADVUTILS_CALLOC) || !defined(ADVUTILS_FREE) +#error ADVUTILS_MALLOC, ADVUTILS_CALLOC and ADVUTILS_FREE must be defined by the user! +#else +#include ADVUTILS_MEMORY_MGMT_HEADER +#endif /* !defined(ADVUTILS_MALLOC) || !defined(ADVUTILS_CALLOC) || !defined(ADVUTILS_FREE) */ +#else +#include +#endif /* ADVUTILS_MEMORY_MGMT_HEADER */ + +/* Macros --------------------------------------------------------------------*/ + +#ifndef ADVUTILS_MEMORY_MGMT_HEADER +#define ADVUTILS_MALLOC malloc +#define ADVUTILS_CALLOC calloc +#define ADVUTILS_FREE free +#endif /* ADVUTILS_MEMORY_MGMT_HEADER */ /* Functions -----------------------------------------------------------------*/ +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION + void listInit(list_t* list, size_t itemSize, LIST_STYPE size) { list->_front = NULL; list->_rear = NULL; @@ -53,8 +76,8 @@ utilsStatus_t listPush(list_t* list, void* value) { } listNode_t* ptr; - ptr = malloc(sizeof(listNode_t)); - ptr->data = calloc(1, list->itemSize); + ptr = ADVUTILS_MALLOC(sizeof(listNode_t)); + ptr->data = ADVUTILS_CALLOC(1, list->itemSize); memcpy(ptr->data, value, list->itemSize); ptr->next = NULL; @@ -74,8 +97,8 @@ utilsStatus_t listPushFront(list_t* list, void* value) { } listNode_t* ptr; - ptr = malloc(sizeof(listNode_t)); - ptr->data = calloc(1, list->itemSize); + ptr = ADVUTILS_MALLOC(sizeof(listNode_t)); + ptr->data = ADVUTILS_CALLOC(1, list->itemSize); memcpy(ptr->data, value, list->itemSize); ptr->next = NULL; @@ -102,8 +125,8 @@ utilsStatus_t listInsert(list_t* list, void* value, LIST_STYPE position) { } listNode_t* ptr; - ptr = malloc(sizeof(listNode_t)); - ptr->data = calloc(1, list->itemSize); + ptr = ADVUTILS_MALLOC(sizeof(listNode_t)); + ptr->data = ADVUTILS_CALLOC(1, list->itemSize); memcpy(ptr->data, value, list->itemSize); ptr->next = NULL; @@ -153,8 +176,8 @@ utilsStatus_t listPop(list_t* list, void* value) { listNode_t* ptr = list->_front; memcpy(value, ptr->data, list->itemSize); list->_front = ptr->next; - free(ptr->data); - free(ptr); + ADVUTILS_FREE(ptr->data); + ADVUTILS_FREE(ptr); list->items--; if (!list->items) { @@ -179,8 +202,8 @@ utilsStatus_t listPopBack(list_t* list, void* value) { } prev->next = NULL; - free(list->_rear->data); - free(list->_rear); + ADVUTILS_FREE(list->_rear->data); + ADVUTILS_FREE(list->_rear); list->_rear = prev; list->items--; @@ -220,8 +243,8 @@ utilsStatus_t listRemove(list_t* list, void* value, LIST_STYPE position) { prev->next = ptr->next; } - free(ptr->data); - free(ptr); + ADVUTILS_FREE(ptr->data); + ADVUTILS_FREE(ptr); list->items--; if (!list->items) { @@ -281,8 +304,8 @@ utilsStatus_t listFlush(list_t* list) { listNode_t* ptr = list->_front; while (ptr != NULL) { list->_front = ptr->next; - free(ptr->data); - free(ptr); + ADVUTILS_FREE(ptr->data); + ADVUTILS_FREE(ptr); ptr = list->_front; } list->items = 0; @@ -311,3 +334,5 @@ utilsStatus_t listItNext(listIterator_t* it) { } return UTILS_STATUS_ERROR; } + +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ diff --git a/src/matrix.c b/src/matrix.c index 2b34486..b2f3e9c 100644 --- a/src/matrix.c +++ b/src/matrix.c @@ -31,20 +31,42 @@ */ /* END Header */ +/* Configuration check -------------------------------------------------------*/ +#if !defined(ADVUTILS_USE_DYNAMIC_ALLOCATION) && !defined(ADVUTILS_USE_STATIC_ALLOCATION) +#error Either ADVUTILS_USE_DYNAMIC_ALLOCATION or ADVUTILS_USE_STATIC_ALLOCATION must be set for ADVUtils to work +#endif + /* Includes ------------------------------------------------------------------*/ #include "matrix.h" #include -#include #include "numMethods.h" +#ifdef ADVUTILS_MEMORY_MGMT_HEADER +#if !defined(ADVUTILS_MALLOC) || !defined(ADVUTILS_CALLOC) || !defined(ADVUTILS_FREE) +#error ADVUTILS_MALLOC, ADVUTILS_CALLOC and ADVUTILS_FREE must be defined by the user! +#else +#include ADVUTILS_MEMORY_MGMT_HEADER +#endif /* !defined(ADVUTILS_MALLOC) || !defined(ADVUTILS_CALLOC) || !defined(ADVUTILS_FREE) */ +#else +#include +#endif /* ADVUTILS_MEMORY_MGMT_HEADER */ + +/* Macros --------------------------------------------------------------------*/ + +#ifndef ADVUTILS_MEMORY_MGMT_HEADER +#define ADVUTILS_MALLOC malloc +#define ADVUTILS_CALLOC calloc +#define ADVUTILS_FREE free +#endif /* ADVUTILS_MEMORY_MGMT_HEADER */ /* ==========================================Assignment============================================= */ +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION /* -----------------------Constructor----------------------- */ utilsStatus_t matrixInit(matrix_t* matrix, uint8_t rows, uint8_t cols) { matrix->rows = rows; matrix->cols = cols; - matrix->data = calloc(rows * cols, sizeof(float)); + matrix->data = ADVUTILS_CALLOC(rows * cols, sizeof(float)); if (matrix->data == NULL) { return UTILS_STATUS_ERROR; @@ -53,6 +75,20 @@ utilsStatus_t matrixInit(matrix_t* matrix, uint8_t rows, uint8_t cols) { return UTILS_STATUS_SUCCESS; } +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ + +#ifdef ADVUTILS_USE_STATIC_ALLOCATION + +/* --------------------Static Constructor------------------- */ +void matrixInitStatic(matrix_t* matrix, float* data, uint8_t rows, uint8_t cols) { + matrix->rows = rows; + matrix->cols = cols; + matrix->data = data; + return; +} + +#endif /* ADVUTILS_USE_STATIC_ALLOCATION */ + /* ---------------------Identity Matrix---------------------- */ void matrixIdentity(matrix_t* matrix) { uint16_t ii; @@ -246,6 +282,8 @@ void matrixMultScalar(matrix_t* lhs, float sc, matrix_t* result) { return; } +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION + /* --------------------Inverse LU------------------------ */ void matrixInversed(matrix_t* lhs, matrix_t* result) { matrix_t Eye; @@ -266,6 +304,33 @@ void matrixInversed_rob(matrix_t* lhs, matrix_t* result) { return; } +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ + +#ifdef ADVUTILS_USE_STATIC_ALLOCATION + +/* -----------------Static Inverse LU-------------------- */ +void matrixInversedStatic(matrix_t* lhs, matrix_t* result) { + float _eyeData[lhs->rows * lhs->cols]; + matrix_t Eye; + matrixInitStatic(&Eye, _eyeData, lhs->rows, lhs->cols); + matrixIdentity(&Eye); + LinSolveLUStatic(lhs, &Eye, result); + return; +} + +/* --------------Static Robust Inverse LUP--------------- */ +void matrixInversedStatic_rob(matrix_t* lhs, matrix_t* result) { + float _eyeData[lhs->rows * lhs->cols]; + matrix_t Eye; + matrixInitStatic(&Eye, _eyeData, lhs->rows, lhs->cols); + matrixIdentity(&Eye); + LinSolveLUPStatic(lhs, &Eye, result); + matrixDelete(&Eye); + return; +} + +#endif /* ADVUTILS_USE_STATIC_ALLOCATION */ + /* -----------------Transposed-------------------- */ void matrixTrans(matrix_t* lhs, matrix_t* result) { uint8_t ii, jj; @@ -284,8 +349,10 @@ void matrixNormalized(matrix_t* lhs, matrix_t* result) { return; } +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION + /* -------Moore-Penrose pseudo inverse--------- */ -void matrixPseudo_inv(matrix_t* lhs, matrix_t* result) { +void matrixPseudoInv(matrix_t* lhs, matrix_t* result) { matrix_t tran, mult1; matrixInit(&tran, lhs->cols, lhs->rows); matrixInit(&mult1, lhs->cols, lhs->cols); @@ -297,8 +364,29 @@ void matrixPseudo_inv(matrix_t* lhs, matrix_t* result) { return; } +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ + +#ifdef ADVUTILS_USE_STATIC_ALLOCATION + +/* -------Moore-Penrose pseudo inverse--------- */ +void matrixPseudoInvStatic(matrix_t* lhs, matrix_t* result) { + float _tranData[lhs->cols * lhs->rows]; + float _mult1Data[lhs->cols * lhs->cols]; + matrix_t tran, mult1; + matrixInitStatic(&tran, _tranData, lhs->cols, lhs->rows); + matrixInitStatic(&mult1, _mult1Data, lhs->cols, lhs->cols); + matrixTrans(lhs, &tran); + matrixMult(&tran, lhs, &mult1); + LinSolveLUStatic(&mult1, &tran, result); + return; +} + +#endif /* ADVUTILS_USE_STATIC_ALLOCATION */ + /* =======================================matrix_t Data========================================= */ +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION + /* -----------Returns the determinant---------- */ float matrixDet(matrix_t* matrix) { matrix_t L, U, P; @@ -341,6 +429,49 @@ float matrixDet(matrix_t* matrix) { return determinant; } +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ + +#ifdef ADVUTILS_USE_STATIC_ALLOCATION + +/* -----------Returns the determinant---------- */ +float matrixDetStatic(matrix_t* matrix) { + float _LData[matrix->rows * matrix->rows]; + float _UData[matrix->rows * matrix->rows]; + float _PData[matrix->rows]; + matrix_t L, U, P; + matrixInitStatic(&L, _LData, matrix->rows, matrix->rows); + matrixInitStatic(&U, _UData, matrix->rows, matrix->rows); + matrixInitStatic(&P, _PData, matrix->rows, 1); + int16_t ii; + int8_t det_f; + float determinant = 1.0f; + + if (matrix->rows != matrix->cols) { + return 0.0f; + } + + if (LU_CormenStatic(matrix, &L, &U)) { + for (ii = 0; ii < matrix->rows; ii++) { + determinant *= ELEM(U, ii, ii); + } + } + + else { + det_f = LUP_CormenStatic(matrix, &L, &U, &P); + if (det_f) { + for (ii = 0; ii < matrix->rows; ii++) { + determinant *= ELEM(U, ii, ii); + } + determinant *= det_f; + } else { + determinant = 0.0f; + } + } + return determinant; +} + +#endif /* ADVUTILS_USE_STATIC_ALLOCATION */ + /* -------------Returns the norm-------------- */ float matrixNorm(matrix_t* matrix) { float result = 0.0f; @@ -352,13 +483,17 @@ float matrixNorm(matrix_t* matrix) { return result; } +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION + /* -------------Deletes the data-------------- */ utilsStatus_t matrixDelete(matrix_t* matrix) { if (matrix->data == NULL) { return UTILS_STATUS_ERROR; } - free(matrix->data); + ADVUTILS_FREE(matrix->data); return UTILS_STATUS_SUCCESS; } + +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ diff --git a/src/movingAvg.c b/src/movingAvg.c index e4eb115..650df92 100644 --- a/src/movingAvg.c +++ b/src/movingAvg.c @@ -31,18 +31,40 @@ */ /* END Header */ +/* Configuration check -------------------------------------------------------*/ +#if !defined(ADVUTILS_USE_DYNAMIC_ALLOCATION) && !defined(ADVUTILS_USE_STATIC_ALLOCATION) +#error Either ADVUTILS_USE_DYNAMIC_ALLOCATION or ADVUTILS_USE_STATIC_ALLOCATION must be set for ADVUtils to work +#endif + /* Includes ------------------------------------------------------------------*/ #include "movingAvg.h" -#include #include +#ifdef ADVUTILS_MEMORY_MGMT_HEADER +#if !defined(ADVUTILS_MALLOC) || !defined(ADVUTILS_CALLOC) || !defined(ADVUTILS_FREE) +#error ADVUTILS_MALLOC, ADVUTILS_CALLOC and ADVUTILS_FREE must be defined by the user! +#else +#include ADVUTILS_MEMORY_MGMT_HEADER +#endif /* !defined(ADVUTILS_MALLOC) || !defined(ADVUTILS_CALLOC) || !defined(ADVUTILS_FREE) */ +#else +#include +#endif /* ADVUTILS_MEMORY_MGMT_HEADER */ + +/* Macros --------------------------------------------------------------------*/ + +#ifndef ADVUTILS_MEMORY_MGMT_HEADER +#define ADVUTILS_MALLOC malloc +#define ADVUTILS_CALLOC calloc +#define ADVUTILS_FREE free +#endif /* ADVUTILS_MEMORY_MGMT_HEADER */ /* Functions -----------------------------------------------------------------*/ +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION + utilsStatus_t movingAvgInit(movingAvg_t* movingAvg, MOVAVG_IND_TYPE size) { movingAvg->data = NULL; - movingAvg->data = calloc(size, sizeof(MOVAVG_TYPE)); - /* movingAvg->data = malloc(size * sizeof(MOVAVG_TYPE)); */ + movingAvg->data = ADVUTILS_CALLOC(size, sizeof(MOVAVG_TYPE)); if (movingAvg->data == NULL) { movingAvg->size = 0; return UTILS_STATUS_ERROR; @@ -55,6 +77,21 @@ utilsStatus_t movingAvgInit(movingAvg_t* movingAvg, MOVAVG_IND_TYPE size) { return UTILS_STATUS_SUCCESS; } +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ + +#ifdef ADVUTILS_USE_STATIC_ALLOCATION + +void movingAvgInitStatic(movingAvg_t* movingAvg, MOVAVG_TYPE* data, MOVAVG_IND_TYPE size) { + movingAvg->data = data; + movingAvg->size = size; + movingAvg->sum = 0; + movingAvg->inv_size = (MOVAVG_TYPE)1.0 / size; + movingAvg->_write = 0; + return; +} + +#endif /* ADVUTILS_USE_STATIC_ALLOCATION */ + MOVAVG_TYPE movingAvgCalc(movingAvg_t* movingAvg, MOVAVG_TYPE value) { movingAvg->sum -= movingAvg->data[movingAvg->_write]; movingAvg->sum += value; @@ -77,13 +114,17 @@ utilsStatus_t movingAvgFlush(movingAvg_t* movingAvg) { return UTILS_STATUS_SUCCESS; } +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION + utilsStatus_t movingAvgDelete(movingAvg_t* movingAvg) { if (movingAvg->data == NULL) { return UTILS_STATUS_ERROR; } - free(movingAvg->data); + ADVUTILS_FREE(movingAvg->data); return UTILS_STATUS_SUCCESS; } + +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ diff --git a/src/numMethods.c b/src/numMethods.c index f62c15f..ec4ad9d 100644 --- a/src/numMethods.c +++ b/src/numMethods.c @@ -31,6 +31,11 @@ */ /* END Header */ +/* Configuration check -------------------------------------------------------*/ +#if !defined(ADVUTILS_USE_DYNAMIC_ALLOCATION) && !defined(ADVUTILS_USE_STATIC_ALLOCATION) +#error Either ADVUTILS_USE_DYNAMIC_ALLOCATION or ADVUTILS_USE_STATIC_ALLOCATION must be set for ADVUtils to work +#endif + /* Includes ------------------------------------------------------------------*/ #include "numMethods.h" @@ -113,6 +118,27 @@ void bksubPerm(matrix_t* A, matrix_t* B, matrix_t* P, matrix_t* result) { return; } +/* ------------------Quadratic form (sort of)---------------------- */ +/* returns matrix C=A*B*(~A) */ + +void QuadProd(matrix_t* A, matrix_t* B, matrix_t* result) { + int16_t i, j, n, ii; + float tmp; + matrixZeros(result); + for (n = 0; n < A->rows; n++) { + for (i = 0; i < A->cols; i++) { + tmp = 0.0; + for (j = 0; j < A->cols; j++) { + tmp += ELEMP(A, n, j) * ELEMP(B, i, j); + } + for (ii = 0; ii < A->rows; ii++) { + ELEMP(result, ii, n) += ELEMP(A, ii, i) * tmp; + } + } + } + return; +} + /* -------------------------LU factorization using Crout's Method-------------------------------- */ /* factorizes the A matrix as the product of a unit upper triangular matrix U and a lower triangular matrix L */ @@ -144,6 +170,8 @@ utilsStatus_t LU_Crout(matrix_t* A, matrix_t* L, matrix_t* U) { return UTILS_STATUS_SUCCESS; } +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION + /* -------------------------LU factorization using Cormen's Method-------------------------------- */ /* factorizes the A matrix as the product of a unit upper triangular matrix U and a lower triangular matrix L */ @@ -444,7 +472,7 @@ utilsStatus_t GaussNewton_Sens_Cal_9(matrix_t* Data, float k, matrix_t* X0, uint t3 = ELEMP(result, 5, 0) * d1 + ELEMP(result, 7, 0) * d2 + ELEMP(result, 8, 0) * d3; ELEM(res, jj, 0) = t1 * t1 + t2 * t2 + t3 * t3 - k2; } - matrixPseudo_inv(&Jr, &tmp1); + matrixPseudoInv(&Jr, &tmp1); matrixMult(&tmp1, &res, &delta); matrixSub(result, &delta, result); if (matrixNorm(&delta) < tol) { @@ -549,7 +577,7 @@ utilsStatus_t GaussNewton_Sens_Cal_6(matrix_t* Data, float k, matrix_t* X0, uint t3 = ELEMP(result, 5, 0) * d3; ELEM(res, jj, 0) = t1 * t1 + t2 * t2 + t3 * t3 - k2; } - matrixPseudo_inv(&Jr, &tmp1); + matrixPseudoInv(&Jr, &tmp1); matrixMult(&tmp1, &res, &delta); matrixSub(result, &delta, result); if (matrixNorm(&delta) < tol) { @@ -573,23 +601,412 @@ utilsStatus_t GaussNewton_Sens_Cal_6(matrix_t* Data, float k, matrix_t* X0, uint return UTILS_STATUS_TIMEOUT; } -/* ------------------Quadratic form (sort of)---------------------- */ -/* returns matrix C=A*B*(~A) */ +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ -void QuadProd(matrix_t* A, matrix_t* B, matrix_t* result) { - int16_t i, j, n, ii; +#ifdef ADVUTILS_USE_STATIC_ALLOCATION + +/* -------------------------LU factorization using Cormen's Method-------------------------------- */ +/* factorizes the A matrix as the product of a unit upper triangular matrix U and a lower triangular matrix L */ + +utilsStatus_t LU_CormenStatic(matrix_t* A, matrix_t* L, matrix_t* U) { + int16_t i, j, k; float tmp; - matrixZeros(result); - for (n = 0; n < A->rows; n++) { - for (i = 0; i < A->cols; i++) { - tmp = 0.0; - for (j = 0; j < A->cols; j++) { - tmp += ELEMP(A, n, j) * ELEMP(B, i, j); + float _A_cp_Data[A->rows * A->cols]; + matrix_t A_cp; + matrixInitStatic(&A_cp, _A_cp_Data, A->rows, A->cols); + matrixCopy(A, &A_cp); + matrixZeros(U); + matrixIdentity(L); + + for (k = 0; k < A_cp.rows; k++) { + ELEMP(U, k, k) = ELEM(A_cp, k, k); + if (ELEM(A_cp, k, k) == 0) { + return UTILS_STATUS_ERROR; + } + tmp = 1.0 / ELEMP(U, k, k); + for (i = k + 1; i < A_cp.rows; i++) { + ELEMP(L, i, k) = ELEM(A_cp, i, k) * tmp; + ELEMP(U, k, i) = ELEM(A_cp, k, i); + } + for (i = k + 1; i < A_cp.rows; i++) { + for (j = k + 1; j < A_cp.rows; j++) { + ELEM(A_cp, i, j) -= ELEMP(L, i, k) * ELEMP(U, k, j); } - for (ii = 0; ii < A->rows; ii++) { - ELEMP(result, ii, n) += ELEMP(A, ii, i) * tmp; + } + } + return UTILS_STATUS_SUCCESS; +} + +/* -----------------------LUP factorization using Cormen's Method------------------------------ */ +/* factorizes the A matrix as the product of a upper triangular matrix U and a unit lower triangular matrix L */ +/* returns the factor that has to be multiplied to the determinant of U in order to obtain the correct value */ + +int8_t LUP_CormenStatic(matrix_t* A, matrix_t* L, matrix_t* U, matrix_t* P) { + int16_t i, j, k; + float tmp, tmp2; + int16_t pivrow; + int8_t d_mult = 1; /* determinant multiplying factor */ + float _A_cp_Data[A->rows * A->cols]; + matrix_t A_cp; + matrixInitStatic(&A_cp, _A_cp_Data, A->rows, A->cols); + matrixCopy(A, &A_cp); + matrixZeros(U); + matrixIdentity(L); + /* initialization */ + for (i = 0; i < A_cp.rows; i++) { + ELEMP(P, i, 0) = i; + } + + /* outer loop over diagonal pivots */ + for (k = 0; k < A_cp.rows - 1; k++) { + + /* inner loop to find the largest pivot */ + pivrow = k; + tmp = fabsf(ELEM(A_cp, k, k)); + for (i = k + 1; i < A_cp.rows; i++) { + tmp2 = fabsf(ELEM(A_cp, i, k)); + if (tmp2 > tmp) { + tmp = tmp2; + pivrow = i; + } + } + /* check for singularity */ + if (ELEM(A_cp, pivrow, k) == 0) { + return 0; + } + + /* swap rows */ + if (pivrow != k) { + tmp = ELEMP(P, k, 0); + ELEMP(P, k, 0) = ELEMP(P, pivrow, 0); + ELEMP(P, pivrow, 0) = tmp; + d_mult *= -1; + + for (j = 0; j < A_cp.rows; j++) { + tmp = ELEM(A_cp, k, j); + ELEM(A_cp, k, j) = ELEM(A_cp, pivrow, j); + ELEM(A_cp, pivrow, j) = tmp; + } + } + tmp = 1.0 / ELEM(A_cp, k, k); + /* Gaussian elimination */ + for (i = k + 1; i < A_cp.rows; i++) { /* iterate down rows */ + ELEM(A_cp, i, k) *= tmp; + for (j = k + 1; j < A_cp.rows; j++) { /* iterate across rows */ + ELEM(A_cp, i, j) -= ELEM(A_cp, i, k) * ELEM(A_cp, k, j); + } + } + } + for (k = 0; k < A_cp.rows; k++) { + ELEMP(U, k, k) = ELEM(A_cp, k, k); + for (j = k + 1; j < A_cp.rows; j++) { + ELEMP(L, j, k) = ELEM(A_cp, j, k); + ELEMP(U, k, j) = ELEM(A_cp, k, j); + } + } + return d_mult; +} + +/* -----------------------Linear system solver using LU factorization--------------------------- */ +/* solves the linear system A*X=B, where A is a n-by-n matrix and B an n-by-m matrix, giving the n-by-m matrix X */ + +void LinSolveLUStatic(matrix_t* A, matrix_t* B, matrix_t* result) { + float _LData[A->rows * A->cols]; + float _UData[A->cols * A->cols]; + matrix_t L, U; + matrixInitStatic(&L, _LData, A->rows, A->cols); + matrixInitStatic(&U, _UData, A->cols, A->cols); + /* matrix_t *tmp1 = matrixInit(A->rows, B->cols); */ + LU_CormenStatic(A, &L, &U); + /* fwsub(L, B, tmp1); */ + /* bksub(U, tmp1, result); */ + fwsub(&L, B, result); + bksub(&U, result, result); /* hope it can work in-place */ + return; +} + +/* ----------------------Linear system solver using LUP factorization-------------------------- */ +/* solves the linear system A*X=B, where A is a n-by-n matrix and B an n-by-m matrix, giving the n-by-m matrix X */ + +void LinSolveLUPStatic(matrix_t* A, matrix_t* B, matrix_t* result) { + float _LData[A->rows * A->cols]; + float _UData[A->cols * A->cols]; + float _PData[A->rows]; + float _tmpData[A->rows * B->cols]; + matrix_t L, U, P, tmp; + matrixInitStatic(&L, _LData, A->rows, A->cols); + matrixInitStatic(&U, _UData, A->cols, A->cols); + matrixInitStatic(&P, _PData, A->rows, 1); + matrixInitStatic(&tmp, _tmpData, A->rows, B->cols); + + LUP_CormenStatic(A, &L, &U, &P); + fwsubPerm(&L, B, &P, &tmp); + bksub(&U, &tmp, result); + return; +} + +/* ------------Linear system solver using Gauss elimination with partial pivoting--------------- */ +/* solves the linear system A*X=B, where A is a n-by-n matrix and B an n-by-m matrix, giving the n-by-m matrix X */ + +void LinSolveGaussStatic(matrix_t* A, matrix_t* B, matrix_t* result) { + uint8_t pivrow = 0; /* keeps track of current pivot row */ + uint8_t k, i, j; /* k: overall index along diagonals; i: row index; j: col index */ + float tmp; /* used for finding max value and making row swaps */ + float tmp2; /* used to store abs when finding max value and to store coefficient value when eliminating values */ + + float _A_cp_Data[A->rows * A->cols]; + float _B_cp_Data[B->rows * B->cols]; + matrix_t A_cp, B_cp; + matrixInitStatic(&A_cp, _A_cp_Data, A->rows, A->cols); + matrixInitStatic(&B_cp, _B_cp_Data, B->rows, B->cols); + matrixCopy(A, &A_cp); + matrixCopy(B, &B_cp); + + for (k = 0; k < (A_cp.cols - 1); k++) { + + /* find pivot row, the row with biggest entry in current column */ + tmp = fabsf(ELEM(A_cp, k, k)); + pivrow = k; + for (i = k + 1; i < A_cp.cols; i++) { + tmp2 = fabsf(ELEM(A_cp, i, k)); /* 'Avoid using other functions inside abs()?' */ + if (tmp2 > tmp) { + tmp = tmp2; + pivrow = i; + } + } + + /* check for singular Matrix */ + if (ELEM(A_cp, pivrow, k) == 0.0) { + matrixZeros(result); + return; + } + + /* Execute pivot (row swap) if needed */ + if (pivrow != k) { + /* swap row k of matrix A with pivrow */ + for (j = k; j < A_cp.cols; j++) { + tmp = ELEM(A_cp, k, j); + ELEM(A_cp, k, j) = ELEM(A_cp, pivrow, j); + ELEM(A_cp, pivrow, j) = tmp; + } + /* swap row k of matrix B with pivrow */ + for (j = 0; j < B_cp.cols; j++) { + tmp = ELEM(B_cp, k, j); + ELEM(B_cp, k, j) = ELEM(B_cp, pivrow, j); + ELEM(B_cp, pivrow, j) = tmp; + } + } + + /* Row reduction */ + tmp = 1.0 / ELEM(A_cp, k, k); /* invert pivot element */ + for (i = k + 1; i < A_cp.cols; i++) { /* along rows */ + tmp2 = ELEM(A_cp, i, k) * tmp; + /* Perform row reduction of A */ + for (j = k + 1; j < A_cp.cols; j++) { /* along columns of A */ + ELEM(A_cp, i, j) -= tmp2 * ELEM(A_cp, k, j); + } + /* Perform row reduction of B */ + for (j = 0; j < B_cp.cols; j++) { /* along columns of B */ + ELEM(B_cp, i, j) -= tmp2 * ELEM(B_cp, k, j); } } } + bksub(&A_cp, &B_cp, result); return; } + +/* ------------Gauss-Newton sensors calibration with 9 parameters--------------- */ +/* approximates Data to a sphere of radius k by calculating 6 gains (s) and 3 biases (b), useful to calibrate some sensors (meas_sphere=S*(meas-B) with S symmetric) */ +/* Data has n>=9 rows corresponding to the number of measures and 3 columns corresponding to the 3 axes */ +/* X0 is the starting guess vector (usually [0 0 0 1 0 0 1 0 1]), nmax the maximum number of iterations (200 is generally fine, even if it usually converges within 10 iterations), and tol the stopping tolerance (1e-6 is usually more than fine) */ +/*b1=out(0,0); + b2=out(1,0); + b3=out(2,0); + s11=out(3,0); + s12=out(4,0); + s13=out(5,0); + s22=out(6,0); + s23=out(7,0); + s33=out(8,0);*/ + +utilsStatus_t GaussNewton_Sens_Cal_9Static(matrix_t* Data, float k, matrix_t* X0, uint16_t nmax, float tol, + matrix_t* result) { + float d1 = 0, d2 = 0, d3 = 0, rx1, rx2, rx3, t1, t2, t3; + float k2; + float _JrData[Data->rows * 9]; + float _resData[Data->rows]; + float _deltaData[9]; + float _tmp1Data[9 * Data->rows]; + matrix_t Jr, res, delta, tmp1; + matrixInitStatic(&Jr, _JrData, Data->rows, 9); + matrixInitStatic(&res, _resData, Data->rows, 1); + matrixInitStatic(&delta, _deltaData, 9, 1); + matrixInitStatic(&tmp1, _tmp1Data, 9, Data->rows); + + if ((Data->rows < 9) || (Data->cols != 3)) { + return UTILS_STATUS_ERROR; + } + + /* Set starting point if not given as input */ + if (X0 != NULL) { + matrixCopy(X0, result); + } else { + matrixZeros(result); + for (uint8_t ii = 0; ii < Data->rows; ii++) { + ELEMP(result, 0, 0) += matrixGet(Data, ii, 0) / Data->rows; + ELEMP(result, 1, 0) += matrixGet(Data, ii, 1) / Data->rows; + ELEMP(result, 2, 0) += matrixGet(Data, ii, 2) / Data->rows; + } + matrixSet(result, 3, 0, 1); + matrixSet(result, 6, 0, 1); + matrixSet(result, 8, 0, 1); + } + + /* Set target radius if not given as input */ + if (k != 0) { + k2 = k * k; + } else { + float max = matrixGet(Data, 0, 0) - matrixGet(result, 0, 0); + float min = matrixGet(Data, 0, 0) - matrixGet(result, 0, 0); + for (uint8_t ii = 0; ii < Data->rows; ii++) { + for (uint8_t jj = 0; jj < 3; jj++) { + float data = matrixGet(Data, ii, jj) - matrixGet(result, jj, 0); + if (data > max) { + max = data; + } else if (data < min) { + min = data; + } + } + } + k2 = 0.25 * (max - min) * (max - min); + } + + /* Perform best-fit algorithm */ + for (uint16_t n_iter = 0; n_iter < nmax; n_iter++) { + for (uint8_t jj = 0; jj < Data->rows; jj++) { + d1 = ELEMP(Data, jj, 0) - ELEMP(result, 0, 0); + d2 = ELEMP(Data, jj, 1) - ELEMP(result, 1, 0); + d3 = ELEMP(Data, jj, 2) - ELEMP(result, 2, 0); + rx1 = -2 * (ELEMP(result, 3, 0) * d1 + ELEMP(result, 4, 0) * d2 + ELEMP(result, 5, 0) * d3); + rx2 = -2 * (ELEMP(result, 4, 0) * d1 + ELEMP(result, 6, 0) * d2 + ELEMP(result, 7, 0) * d3); + rx3 = -2 * (ELEMP(result, 5, 0) * d1 + ELEMP(result, 7, 0) * d2 + ELEMP(result, 8, 0) * d3); + ELEM(Jr, jj, 0) = ELEMP(result, 3, 0) * rx1 + ELEMP(result, 4, 0) * rx2 + ELEMP(result, 5, 0) * rx3; + ELEM(Jr, jj, 1) = ELEMP(result, 4, 0) * rx1 + ELEMP(result, 6, 0) * rx2 + ELEMP(result, 7, 0) * rx3; + ELEM(Jr, jj, 2) = ELEMP(result, 5, 0) * rx1 + ELEMP(result, 7, 0) * rx2 + ELEMP(result, 8, 0) * rx3; + ELEM(Jr, jj, 3) = -d1 * rx1; + ELEM(Jr, jj, 4) = -d2 * rx1 - d1 * rx2; + ELEM(Jr, jj, 5) = -d3 * rx1 - d1 * rx3; + ELEM(Jr, jj, 6) = -d2 * rx2; + ELEM(Jr, jj, 7) = -d3 * rx2 - d2 * rx3; + ELEM(Jr, jj, 8) = -d3 * rx3; + t1 = ELEMP(result, 3, 0) * d1 + ELEMP(result, 4, 0) * d2 + ELEMP(result, 5, 0) * d3; + t2 = ELEMP(result, 4, 0) * d1 + ELEMP(result, 6, 0) * d2 + ELEMP(result, 7, 0) * d3; + t3 = ELEMP(result, 5, 0) * d1 + ELEMP(result, 7, 0) * d2 + ELEMP(result, 8, 0) * d3; + ELEM(res, jj, 0) = t1 * t1 + t2 * t2 + t3 * t3 - k2; + } + matrixPseudoInvStatic(&Jr, &tmp1); + matrixMult(&tmp1, &res, &delta); + matrixSub(result, &delta, result); + if (matrixNorm(&delta) < tol) { + return UTILS_STATUS_SUCCESS; + } else if (isnan(d1) || isnan(d2) || isnan(d3)) { + return UTILS_STATUS_ERROR; + } + } + return UTILS_STATUS_TIMEOUT; +} + +/* ------------Gauss-Newton sensors calibration with 6 parameters--------------- */ +/* approximates Data to a sphere of radius k by calculating 3 gains (s) and 3 biases (b), useful to calibrate some sensors (meas_sphere=S*(meas-B) with S diagonal) */ +/* Data has n>=6 rows corresponding to the number of measures and 3 columns corresponding to the 3 axes */ +/* X0 is the starting guess vector (usually [0 0 0 1 1 1]), nmax the maximum number of iterations (200 is generally fine, even if it usually converges within 10 iterations), and tol the stopping tolerance (1e-6 is usually more than fine) */ +/*b1=out(0,0); + b2=out(1,0); + b3=out(2,0); + s11=out(3,0); + s22=out(4,0); + s33=out(5,0);*/ + +utilsStatus_t GaussNewton_Sens_Cal_6Static(matrix_t* Data, float k, matrix_t* X0, uint16_t nmax, float tol, + matrix_t* result) { + float d1 = 0, d2 = 0, d3 = 0, t1, t2, t3; + float k2; + + float _JrData[Data->rows * 6]; + float _resData[Data->rows]; + float _deltaData[6]; + float _tmp1Data[6 * Data->rows]; + matrix_t Jr, res, delta, tmp1; + matrixInitStatic(&Jr, _JrData, Data->rows, 6); + matrixInitStatic(&res, _resData, Data->rows, 1); + matrixInitStatic(&delta, _deltaData, 6, 1); + matrixInitStatic(&tmp1, _tmp1Data, 6, Data->rows); + + if ((Data->rows < 6) || (Data->cols != 3)) { + return UTILS_STATUS_ERROR; + } + + /* Set starting point if not given as input */ + if (X0 != NULL) { + matrixCopy(X0, result); + } else { + matrixZeros(result); + for (uint8_t ii = 0; ii < Data->rows; ii++) { + ELEMP(result, 0, 0) += matrixGet(Data, ii, 0) / Data->rows; + ELEMP(result, 1, 0) += matrixGet(Data, ii, 1) / Data->rows; + ELEMP(result, 2, 0) += matrixGet(Data, ii, 2) / Data->rows; + } + matrixSet(result, 3, 0, 1); + matrixSet(result, 4, 0, 1); + matrixSet(result, 5, 0, 1); + } + + /* Set target radius if not given as input */ + if (k != 0) { + k2 = k * k; + } else { + float max = matrixGet(Data, 0, 0) - matrixGet(result, 0, 0); + float min = matrixGet(Data, 0, 0) - matrixGet(result, 0, 0); + for (uint8_t ii = 0; ii < Data->rows; ii++) { + for (uint8_t jj = 0; jj < 3; jj++) { + float data = matrixGet(Data, ii, jj) - matrixGet(result, jj, 0); + if (data > max) { + max = data; + } else if (data < min) { + min = data; + } + } + } + k2 = 0.25 * (max - min) * (max - min); + } + + /* Perform best-fit algorithm */ + for (uint16_t n_iter = 0; n_iter < nmax; n_iter++) { + for (uint8_t jj = 0; jj < Data->rows; jj++) { + d1 = ELEMP(Data, jj, 0) - ELEMP(result, 0, 0); + d2 = ELEMP(Data, jj, 1) - ELEMP(result, 1, 0); + d3 = ELEMP(Data, jj, 2) - ELEMP(result, 2, 0); + ELEM(Jr, jj, 0) = -2 * d1 * ELEMP(result, 3, 0) * ELEMP(result, 3, 0); + ELEM(Jr, jj, 1) = -2 * d2 * ELEMP(result, 4, 0) * ELEMP(result, 4, 0); + ELEM(Jr, jj, 2) = -2 * d3 * ELEMP(result, 5, 0) * ELEMP(result, 5, 0); + ELEM(Jr, jj, 3) = 2 * ELEMP(result, 3, 0) * d1 * d1; + ELEM(Jr, jj, 4) = 2 * ELEMP(result, 4, 0) * d2 * d2; + ELEM(Jr, jj, 5) = 2 * ELEMP(result, 5, 0) * d3 * d3; + t1 = ELEMP(result, 3, 0) * d1; + t2 = ELEMP(result, 4, 0) * d2; + t3 = ELEMP(result, 5, 0) * d3; + ELEM(res, jj, 0) = t1 * t1 + t2 * t2 + t3 * t3 - k2; + } + matrixPseudoInvStatic(&Jr, &tmp1); + matrixMult(&tmp1, &res, &delta); + matrixSub(result, &delta, result); + if (matrixNorm(&delta) < tol) { + return UTILS_STATUS_SUCCESS; + } else if (isnan(d1) || isnan(d2) || isnan(d3)) { + return UTILS_STATUS_ERROR; + } + } + return UTILS_STATUS_TIMEOUT; +} + +#endif /* ADVUTILS_USE_STATIC_ALLOCATION */ diff --git a/src/queue.c b/src/queue.c index b2cab61..9f679ce 100644 --- a/src/queue.c +++ b/src/queue.c @@ -9,7 +9,7 @@ * * Copyright 2023 Andrea Vivani * - * Permission is hereby granted, free of charge, to any person obtaining a copy + * Permission is hereby granted, ADVUTILS_FREE of charge, to any person obtaining a copy * of this software and associated documentation files (the “Software”), to * deal in the Software without restriction, including without limitation the * rights to use, copy, modify, merge, publish, distribute, sublicense, and/or @@ -31,18 +31,41 @@ */ /* END Header */ +/* Configuration check -------------------------------------------------------*/ +#if !defined(ADVUTILS_USE_DYNAMIC_ALLOCATION) && !defined(ADVUTILS_USE_STATIC_ALLOCATION) +#error Either ADVUTILS_USE_DYNAMIC_ALLOCATION or ADVUTILS_USE_STATIC_ALLOCATION must be set for ADVUtils to work +#endif + /* Includes ------------------------------------------------------------------*/ #include "queue.h" -#include #include +#ifdef ADVUTILS_MEMORY_MGMT_HEADER +#if !defined(ADVUTILS_MALLOC) || !defined(ADVUTILS_CALLOC) || !defined(ADVUTILS_FREE) +#error ADVUTILS_MEMORY_MGMT_HEADER, ADVUTILS_MALLOC, ADVUTILS_CALLOC and ADVUTILS_FREE must be defined by the user! +#else +#include ADVUTILS_MEMORY_MGMT_HEADER +#endif +#else +#include +#endif /* ADVUTILS_MEMORY_MGMT_HEADER */ + +/* Macros --------------------------------------------------------------------*/ + +#ifndef ADVUTILS_MEMORY_MGMT_HEADER +#define ADVUTILS_MALLOC malloc +#define ADVUTILS_CALLOC calloc +#define ADVUTILS_FREE free +#endif /* ADVUTILS_MEMORY_MGMT_HEADER */ /* Functions -----------------------------------------------------------------*/ +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION + utilsStatus_t queueInit(queue_t* queue, size_t itemSize, QUEUE_STYPE size) { queue->data = NULL; - queue->data = calloc(size, itemSize); - /* queue->data = malloc(size * itemSize); */ + queue->data = ADVUTILS_CALLOC(size, itemSize); + /* queue->data = ADVUTILS_MALLOC(size * itemSize); */ if (queue->data == NULL) { queue->size = 0; return UTILS_STATUS_ERROR; @@ -56,6 +79,22 @@ utilsStatus_t queueInit(queue_t* queue, size_t itemSize, QUEUE_STYPE size) { return UTILS_STATUS_SUCCESS; } +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */ + +#ifdef ADVUTILS_USE_STATIC_ALLOCATION + +void queueInitStatic(queue_t* queue, uint8_t* data, size_t itemSize, QUEUE_STYPE size) { + queue->data = data; + queue->size = size * itemSize; + queue->itemSize = itemSize; + queue->items = 0; + queue->_front = 0; + queue->_rear = 0; + return; +} + +#endif /* ADVUTILS_USE_STATIC_ALLOCATION */ + utilsStatus_t queuePush(queue_t* queue, void* value) { if (queue->items == queue->size) { return UTILS_STATUS_FULL; @@ -226,13 +265,17 @@ utilsStatus_t queueFlush(queue_t* queue) { return UTILS_STATUS_SUCCESS; } +#ifdef ADVUTILS_USE_DYNAMIC_ALLOCATION + utilsStatus_t queueDelete(queue_t* queue) { if (queue->data == NULL) { return UTILS_STATUS_ERROR; } - free(queue->data); + ADVUTILS_FREE(queue->data); return UTILS_STATUS_SUCCESS; } + +#endif /* ADVUTILS_USE_DYNAMIC_ALLOCATION */