/
imu.h
88 lines (73 loc) · 2.57 KB
/
imu.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
/*
* This file is part of Cleanflight and Betaflight.
*
* Cleanflight and Betaflight are free software. You can redistribute
* this software and/or modify this software under the terms of the
* GNU General Public License as published by the Free Software
* Foundation, either version 3 of the License, or (at your option)
* any later version.
*
* Cleanflight and Betaflight are distributed in the hope that they
* will be useful, but WITHOUT ANY WARRANTY; without even the implied
* warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
* See the GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this software.
*
* If not, see <http://www.gnu.org/licenses/>.
*/
#pragma once
#include "common/axis.h"
#include "common/time.h"
#include "common/maths.h"
#include "pg/pg.h"
// Exported symbols
extern bool canUseGPSHeading;
typedef struct {
float w,x,y,z;
} quaternion;
#define QUATERNION_INITIALIZE {.w=1, .x=0, .y=0,.z=0}
typedef struct {
float ww,wx,wy,wz,xx,xy,xz,yy,yz,zz;
} quaternionProducts;
#define QUATERNION_PRODUCTS_INITIALIZE {.ww=1, .wx=0, .wy=0, .wz=0, .xx=0, .xy=0, .xz=0, .yy=0, .yz=0, .zz=0}
typedef union {
int16_t raw[XYZ_AXIS_COUNT];
struct {
// absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
int16_t roll;
int16_t pitch;
int16_t yaw;
} values;
} attitudeEulerAngles_t;
#define EULER_INITIALIZE { { 0, 0, 0 } }
extern attitudeEulerAngles_t attitude;
extern float rMat[3][3];
typedef struct imuConfig_s {
uint16_t dcm_kp; // DCM filter proportional gain ( x 10000)
uint16_t dcm_ki; // DCM filter integral gain ( x 10000)
uint8_t small_angle;
uint8_t imu_process_denom;
} imuConfig_t;
PG_DECLARE(imuConfig_t, imuConfig);
typedef struct imuRuntimeConfig_s {
float dcm_ki;
float dcm_kp;
} imuRuntimeConfig_t;
void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correction_value);
float getCosTiltAngle(void);
void getQuaternion(quaternion * q);
void imuUpdateAttitude(timeUs_t currentTimeUs);
void imuInit(void);
#ifdef SIMULATOR_BUILD
void imuSetAttitudeRPY(float roll, float pitch, float yaw); // in deg
void imuSetAttitudeQuat(float w, float x, float y, float z);
#if defined(SIMULATOR_IMU_SYNC)
void imuSetHasNewData(uint32_t dt);
#endif
#endif
bool imuQuaternionHeadfreeOffsetSet(void);
void imuQuaternionHeadfreeTransformVectorEarthToBody(t_fp_vector_def * v);
bool shouldInitializeGPSHeading(void);
bool isUpright(void);