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