1 /* 2 * Copyright (c) 2021 Kevin Townsend 3 * 4 * SPDX-License-Identifier: Apache-2.0 5 */ 6 7 /** 8 * @defgroup FUSION_ALG_MADG Madgwick 9 * 10 * @brief Madgwick sensor fuion algorithm. 11 * 12 * @ingroup FUSION_ALGORITHMS 13 * @{ 14 */ 15 16 /** 17 * @file 18 * @brief Madgwick sensor fusion algorithm. 19 * 20 * This file implements the Madgwick sensor fusion algorithm. 21 */ 22 23 #ifndef ZEPHYR_INCLUDE_ZSL_FUSION_MADGWICK_H_ 24 #define ZEPHYR_INCLUDE_ZSL_FUSION_MADGWICK_H_ 25 26 #include <zsl/zsl.h> 27 #include <zsl/vectors.h> 28 #include <zsl/matrices.h> 29 #include <zsl/orientation/quaternions.h> 30 31 #ifdef __cplusplus 32 extern "C" { 33 #endif 34 35 /* Source: https://www.x-io.co.uk/res/doc/madgwick_internal_report.pdf */ 36 37 /** 38 * @brief Config settings for the Madwick filter. 39 */ 40 struct zsl_fus_madg_cfg { 41 /** 42 * @brief The filter gain beta is a positive number that represents the 43 * divergence rate of the gyroscope. 44 */ 45 zsl_real_t beta; 46 }; 47 48 /** 49 * @brief Sets the sample frequency (in Hz) for the algorithm. 50 * 51 * @param freq Sampling frequency in Hz. 52 * @param cfg Pointer to the config struct for this algorithm. 53 * 54 * @return int 0 if everything executed correctly, otherwise an appropriate 55 * negative error code. 56 */ 57 int zsl_fus_madg_init(uint32_t freq, void *cfg); 58 59 /** 60 * @brief Madgwick sensor fusion algorithm implementation. 61 * 62 * @param a Input accelerometer vector (3 samples required). NULL if none. 63 * @param m Input magnetometer vector (3 samples required). NULL if none. 64 * @param g Input gyroscope vector (3 samples required). NULL if none. 65 * @param incl Input magnetic inclination, in degrees. NULL for none. 66 * @param q Pointer to the output @ref zsl_quat. 67 * @param cfg Pointer to the config struct for this algorithm. 68 * 69 * @return int 0 if everything executed correctly, otherwise an appropriate 70 * negative error code. 71 */ 72 int zsl_fus_madg_feed(struct zsl_vec *a, struct zsl_vec *m, 73 struct zsl_vec *g, zsl_real_t *incl, struct zsl_quat *q, 74 void *cfg); 75 76 /** 77 * @brief Default error handler for the Madgwick sensor fusion driver. 78 * 79 * @note Normally you will want to implement your own version of this function. 80 * 81 * @param error The error code genereated. 82 */ 83 void zsl_fus_madg_error(int error); 84 85 #ifdef __cplusplus 86 } 87 #endif 88 89 #endif /* ZEPHYR_INCLUDE_ZSL_FUSION_MADGWICK_H_ */ 90 91 /** @} */ /* End of algorithms group */ 92
