Autodrone
Multi-Domain Vehicle
madgwick.h
Go to the documentation of this file.
1
10#ifndef __SENSOR_FUSION_H__
11#define __SENSOR_FUSION_H__
12
13/* Defines */
14#define sampleFreqDef 500.0f // sample frequency in Hz
15#define betaDef 5.0f // 2 * proportional gain
16
17/* Function Prototypes */
18void madgwickInit(void);
19void updateIMU(float gx, float gy, float gz, float ax, float ay, float az);
20void computeAngles(void);
21
22/* Global Variables */
23extern float beta; // algorithm gain
24extern float q0;
25extern float q1;
26extern float q2;
27extern float q3; // quaternion of sensor frame relative to auxiliary frame
28extern float invSampleFreq;
29extern float roll;
30extern float pitch;
31extern float yaw;
32extern char anglesComputed;
33
34/* Inline Functions */
35
40static inline float
41getRoll(void)
42{
43 if (!anglesComputed) computeAngles();
44 return roll * 57.29578f;
45}
46
51static inline float
52getPitch(void)
53{
54 if (!anglesComputed) computeAngles();
55 return pitch * 57.29578f;
56}
57
62static inline float
63getYaw(void)
64{
65 if (!anglesComputed) computeAngles();
66 return yaw * 57.29578f;
67}
68
73static inline float
74getRollRadians(void)
75{
76 if (!anglesComputed) computeAngles();
77 return roll;
78}
79
84static inline float
85getPitchRadians(void)
86{
87 if (!anglesComputed) computeAngles();
88 return pitch;
89}
90
95static inline float
96getYawRadians(void)
97{
98 if (!anglesComputed) computeAngles();
99 return yaw;
100}
101
102#endif /* __SENSOR_FUSION_H__ */
updateIMU
void updateIMU(float gx, float gy, float gz, float ax, float ay, float az)
Creates quaternion from gyro and accel values.
Definition: madgwick.c:59
q1
float q1
Definition: madgwick.c:17
anglesComputed
char anglesComputed
Definition: madgwick.c:24
roll
float roll
Definition: madgwick.c:21
q0
float q0
Definition: madgwick.c:16
pitch
float pitch
Definition: madgwick.c:22
invSampleFreq
float invSampleFreq
Definition: madgwick.c:20
computeAngles
void computeAngles(void)
Computes angles from quaternion.
Definition: madgwick.c:154
yaw
float yaw
Definition: madgwick.c:23
beta
float beta
Definition: madgwick.c:15
q3
float q3
Definition: madgwick.c:19
madgwickInit
void madgwickInit(void)
Initializes quaternion and other variables.
Definition: madgwick.c:34
q2
float q2
Definition: madgwick.c:18