1 /* 2 * Copyright (c) 2021 Marti Riba Pons 3 * 4 * SPDX-License-Identifier: Apache-2.0 5 */ 6 7 /** 8 * @defgroup FUSION_ALG_COMP Complementary 9 * 10 * @brief Complementary sensor fuion algorithm. 11 * 12 * @ingroup FUSION_ALGORITHMS 13 * @{ 14 */ 15 16 /** 17 * @file 18 * @brief Complementary sensor fusion algorithm. 19 * 20 * This file implements the complementary sensor fusion algorithm. 21 */ 22 23 #ifndef ZEPHYR_INCLUDE_ZSL_FUSION_COMPLEMENTARY_H_ 24 #define ZEPHYR_INCLUDE_ZSL_FUSION_COMPLEMENTARY_H_ 25 26 #include <zsl/zsl.h> 27 #include <zsl/vectors.h> 28 #include <zsl/matrices.h> 29 #include <zsl/orientation/quaternions.h> 30 #include <zsl/orientation/ahrs.h> 31 32 #ifdef __cplusplus 33 extern "C" { 34 #endif 35 36 /* Source: https://ahrs.readthedocs.io/en/latest/filters/complementary.html */ 37 38 /** 39 * @brief Config settings for the complementary sensor fusion algorithm. 40 */ 41 struct zsl_fus_comp_cfg { 42 /** 43 * @brief Value between 0 and 1 used to decide which part of the 44 * complementary test is more important, the gyrpscope 45 * predicted orientation or the accel/magpredicted orientation. 46 * For more accurate gyroscope data, alpha should be closer to 47 * zero. For more accurate accelerometer and magnetometer data, 48 * alpha should be closer to 1. 49 */ 50 zsl_real_t alpha; 51 }; 52 53 /** 54 * @brief Sets the sample frequency (in Hz) for the algorithm. 55 * 56 * @param freq Sampling frequency in Hz. 57 * @param cfg Config struct for this filter. 58 * 59 * @return int 0 if everything executed correctly, otherwise an appropriate 60 * negative error code. 61 */ 62 int zsl_fus_comp_init(uint32_t freq, void *cfg); 63 64 /** 65 * @brief Complementary sensor fusion algorithm implementation. 66 * 67 * @param a Input accelerometer vector (3 samples required). NULL if none. 68 * @param m Input magnetometer vector (3 samples required). NULL if none. 69 * @param g Input gyroscope vector (3 samples required). NULL if none. 70 * @param incl Input magnetic inclination, in degrees. NULL for none. 71 * @param q Pointer to the output @ref zsl_quat. 72 * @param cfg Pointer to the config struct for this algorithm. 73 * 74 * @return int 0 if everything executed correctly, otherwise an appropriate 75 * negative error code. 76 */ 77 int zsl_fus_comp_feed(struct zsl_vec *a, struct zsl_vec *m, 78 struct zsl_vec *g, zsl_real_t *incl, struct zsl_quat *q, 79 void *cfg); 80 81 /** 82 * @brief Default error handler for the complementary sensor fusion driver. 83 * 84 * @note Normally you will want to implement your own version of this function. 85 * 86 * @param error The error code genereated. 87 */ 88 void zsl_fus_comp_error(int error); 89 90 #ifdef __cplusplus 91 } 92 #endif 93 94 #endif /* ZEPHYR_INCLUDE_ZSL_FUSION_COMPLEMENTARY_H_ */ 95 96 /** @} */ /* End of algorithms group */ 97
