2 * This file is part of Cleanflight.
4 * Cleanflight is free software: you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation, either version 3 of the License, or
7 * (at your option) any later version.
9 * Cleanflight is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
14 * You should have received a copy of the GNU General Public License
15 * along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
18 // Inertial Measurement Unit (IMU)
24 #include "common/maths.h"
28 #include "common/axis.h"
30 #include "drivers/system.h"
31 #include "drivers/sensor.h"
32 #include "drivers/accgyro.h"
33 #include "drivers/compass.h"
35 #include "sensors/sensors.h"
36 #include "sensors/gyro.h"
37 #include "sensors/compass.h"
38 #include "sensors/acceleration.h"
39 #include "sensors/barometer.h"
40 #include "sensors/sonar.h"
42 #include "flight/mixer.h"
43 #include "flight/pid.h"
44 #include "flight/imu.h"
46 #include "config/runtime_config.h"
49 extern int16_t debug
[4];
51 int16_t accSmooth
[XYZ_AXIS_COUNT
];
52 int32_t accSum
[XYZ_AXIS_COUNT
];
53 int16_t gyroData
[FLIGHT_DYNAMICS_INDEX_COUNT
] = { 0, 0, 0 };
55 uint32_t accTimeSum
= 0; // keep track for integration of acc
59 int16_t smallAngle
= 0;
61 float throttleAngleScale
;
64 float magneticDeclination
= 0.0f
; // calculated at startup from config
68 rollAndPitchInclination_t inclination
= { { 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
69 float anglerad
[2] = { 0.0f
, 0.0f
}; // absolute angle inclination in radians
71 imuRuntimeConfig_t
*imuRuntimeConfig
;
72 pidProfile_t
*pidProfile
;
73 accDeadband_t
*accDeadband
;
76 imuRuntimeConfig_t
*initialImuRuntimeConfig
,
77 pidProfile_t
*initialPidProfile
,
78 accDeadband_t
*initialAccDeadband
,
79 float accz_lpf_cutoff
,
80 uint16_t throttle_correction_angle
83 imuRuntimeConfig
= initialImuRuntimeConfig
;
84 pidProfile
= initialPidProfile
;
85 accDeadband
= initialAccDeadband
;
86 fc_acc
= calculateAccZLowPassFilterRCTimeConstant(accz_lpf_cutoff
);
87 throttleAngleScale
= calculateThrottleAngleScale(throttle_correction_angle
);
92 smallAngle
= lrintf(acc_1G
* cosf(degreesToRadians(imuRuntimeConfig
->small_angle
)));
93 accVelScale
= 9.80665f
/ acc_1G
/ 10000.0f
;
94 gyroScaleRad
= gyro
.scale
* (M_PIf
/ 180.0f
) * 0.000001f
;
97 float calculateThrottleAngleScale(uint16_t throttle_correction_angle
)
99 return (1800.0f
/ M_PIf
) * (900.0f
/ throttle_correction_angle
);
103 * Calculate RC time constant used in the accZ lpf.
105 float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff
)
107 return 0.5f
/ (M_PIf
* accz_lpf_cutoff
);
110 // **************************************************
111 // Simplified IMU based on "Complementary Filter"
112 // Inspired by http://starlino.com/imu_guide.html
114 // adapted by ziss_dm : http://www.multiwii.com/forum/viewtopic.php?f=8&t=198
116 // The following ideas was used in this project:
117 // 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix
119 // Currently Magnetometer uses separate CF which is used only
120 // for heading approximation.
122 // **************************************************
127 void imuResetAccelerationSum(void)
136 // rotate acc into Earth frame and calculate acceleration in it
137 void imuCalculateAcceleration(uint32_t deltaT
)
139 static int32_t accZoffset
= 0;
140 static float accz_smooth
= 0;
143 t_fp_vector accel_ned
;
145 // deltaT is measured in us ticks
146 dT
= (float)deltaT
* 1e-6f
;
148 // the accel values have to be rotated into the earth frame
149 rpy
.angles
.roll
= -(float)anglerad
[AI_ROLL
];
150 rpy
.angles
.pitch
= -(float)anglerad
[AI_PITCH
];
151 rpy
.angles
.yaw
= -(float)heading
* RAD
;
153 accel_ned
.V
.X
= accSmooth
[0];
154 accel_ned
.V
.Y
= accSmooth
[1];
155 accel_ned
.V
.Z
= accSmooth
[2];
157 rotateV(&accel_ned
.V
, &rpy
);
159 if (imuRuntimeConfig
->acc_unarmedcal
== 1) {
160 if (!ARMING_FLAG(ARMED
)) {
161 accZoffset
-= accZoffset
/ 64;
162 accZoffset
+= accel_ned
.V
.Z
;
164 accel_ned
.V
.Z
-= accZoffset
/ 64; // compensate for gravitation on z-axis
166 accel_ned
.V
.Z
-= acc_1G
;
168 accz_smooth
= accz_smooth
+ (dT
/ (fc_acc
+ dT
)) * (accel_ned
.V
.Z
- accz_smooth
); // low pass filter
170 // apply Deadband to reduce integration drift and vibration influence
171 accSum
[X
] += applyDeadband(lrintf(accel_ned
.V
.X
), accDeadband
->xy
);
172 accSum
[Y
] += applyDeadband(lrintf(accel_ned
.V
.Y
), accDeadband
->xy
);
173 accSum
[Z
] += applyDeadband(lrintf(accz_smooth
), accDeadband
->z
);
175 // sum up Values for later integration to get velocity and distance
176 accTimeSum
+= deltaT
;
181 * Baseflight calculation by Luggi09 originates from arducopter
182 * ============================================================
183 * This function rotates magnetic vector to cancel actual yaw and
184 * pitch of craft. Then it computes it's direction in X/Y plane.
185 * This value is returned as compass heading, value is 0-360 degrees.
187 * Note that Earth's magnetic field is not parallel with ground unless
188 * you are near equator. Its inclination is considerable, >60 degrees
189 * towards ground in most of Europe.
191 * First we consider it in 2D:
193 * An example, the vector <1, 1> would be turned into the heading
194 * 45 degrees, representing it's angle clockwise from north.
196 * ***************** *
206 * *******************
208 * //TODO: Add explanation for how it uses the Z dimension.
210 int16_t imuCalculateHeading(t_fp_vector
*vec
)
214 float cosineRoll
= cosf(anglerad
[AI_ROLL
]);
215 float sineRoll
= sinf(anglerad
[AI_ROLL
]);
216 float cosinePitch
= cosf(anglerad
[AI_PITCH
]);
217 float sinePitch
= sinf(anglerad
[AI_PITCH
]);
218 float Xh
= vec
->A
[X
] * cosinePitch
+ vec
->A
[Y
] * sineRoll
* sinePitch
+ vec
->A
[Z
] * sinePitch
* cosineRoll
;
219 float Yh
= vec
->A
[Y
] * cosineRoll
- vec
->A
[Z
] * sineRoll
;
220 //TODO: Replace this comment with an explanation of why Yh and Xh can never simultanoeusly be zero,
221 // or handle the case in which they are and (atan2f(0, 0) is undefined.
222 float hd
= (atan2f(Yh
, Xh
) * 1800.0f
/ M_PIf
+ magneticDeclination
) / 10.0f
;
225 // Arctan returns a value in the range -180 to 180 degrees. We 'normalize' negative angles to be positive.
232 static void imuCalculateEstimatedAttitude(void)
236 static t_fp_vector EstM
;
237 static t_fp_vector EstN
= { .A
= { 1.0f
, 0.0f
, 0.0f
} };
238 static float accLPF
[3];
239 static uint32_t previousT
;
240 uint32_t currentT
= micros();
243 fp_angles_t deltaGyroAngle
;
244 deltaT
= currentT
- previousT
;
245 scale
= deltaT
* gyroScaleRad
;
246 previousT
= currentT
;
249 for (axis
= 0; axis
< 3; axis
++) {
250 deltaGyroAngle
.raw
[axis
] = gyroADC
[axis
] * scale
;
251 if (imuRuntimeConfig
->acc_lpf_factor
> 0) {
252 accLPF
[axis
] = accLPF
[axis
] * (1.0f
- (1.0f
/ imuRuntimeConfig
->acc_lpf_factor
)) + accADC
[axis
] * (1.0f
/ imuRuntimeConfig
->acc_lpf_factor
);
253 accSmooth
[axis
] = accLPF
[axis
];
255 accSmooth
[axis
] = accADC
[axis
];
257 accMag
+= (int32_t)accSmooth
[axis
] * accSmooth
[axis
];
259 accMag
= accMag
* 100 / ((int32_t)acc_1G
* acc_1G
);
261 rotateV(&EstG
.V
, &deltaGyroAngle
);
263 // Apply complimentary filter (Gyro drift correction)
264 // If accel magnitude >1.15G or <0.85G and ACC vector outside of the limit range => we neutralize the effect of accelerometers in the angle estimation.
265 // To do that, we just skip filter, as EstV already rotated by Gyro
267 float invGyroComplimentaryFilterFactor
= (1.0f
/ (imuRuntimeConfig
->gyro_cmpf_factor
+ 1.0f
));
269 if (72 < (uint16_t)accMag
&& (uint16_t)accMag
< 133) {
270 for (axis
= 0; axis
< 3; axis
++)
271 EstG
.A
[axis
] = (EstG
.A
[axis
] * imuRuntimeConfig
->gyro_cmpf_factor
+ accSmooth
[axis
]) * invGyroComplimentaryFilterFactor
;
274 if (EstG
.A
[Z
] > smallAngle
) {
275 ENABLE_STATE(SMALL_ANGLE
);
277 DISABLE_STATE(SMALL_ANGLE
);
280 // Attitude of the estimated vector
281 anglerad
[AI_ROLL
] = atan2f(EstG
.V
.Y
, EstG
.V
.Z
);
282 anglerad
[AI_PITCH
] = atan2f(-EstG
.V
.X
, sqrtf(EstG
.V
.Y
* EstG
.V
.Y
+ EstG
.V
.Z
* EstG
.V
.Z
));
283 inclination
.values
.rollDeciDegrees
= lrintf(anglerad
[AI_ROLL
] * (1800.0f
/ M_PIf
));
284 inclination
.values
.pitchDeciDegrees
= lrintf(anglerad
[AI_PITCH
] * (1800.0f
/ M_PIf
));
286 if (sensors(SENSOR_MAG
)) {
287 rotateV(&EstM
.V
, &deltaGyroAngle
);
288 // FIXME what does the _M_ mean?
289 float invGyroComplimentaryFilter_M_Factor
= (1.0f
/ (imuRuntimeConfig
->gyro_cmpfm_factor
+ 1.0f
));
290 for (axis
= 0; axis
< 3; axis
++) {
291 EstM
.A
[axis
] = (EstM
.A
[axis
] * imuRuntimeConfig
->gyro_cmpfm_factor
+ magADC
[axis
]) * invGyroComplimentaryFilter_M_Factor
;
293 heading
= imuCalculateHeading(&EstM
);
295 rotateV(&EstN
.V
, &deltaGyroAngle
);
296 normalizeV(&EstN
.V
, &EstN
.V
);
297 heading
= imuCalculateHeading(&EstN
);
300 imuCalculateAcceleration(deltaT
); // rotate acc vector into earth frame
303 void imuUpdate(rollAndPitchTrims_t
*accelerometerTrims
, uint8_t mixerMode
)
305 static int16_t gyroYawSmooth
= 0;
308 if (sensors(SENSOR_ACC
)) {
309 updateAccelerationReadings(accelerometerTrims
); // TODO rename to accelerometerUpdate and rename many other 'Acceleration' references to be 'Accelerometer'
310 imuCalculateEstimatedAttitude();
317 gyroData
[FD_ROLL
] = gyroADC
[FD_ROLL
];
318 gyroData
[FD_PITCH
] = gyroADC
[FD_PITCH
];
320 if (mixerMode
== MIXER_TRI
) {
321 gyroData
[FD_YAW
] = (gyroYawSmooth
* 2 + gyroADC
[FD_YAW
]) / 3;
322 gyroYawSmooth
= gyroData
[FD_YAW
];
324 gyroData
[FD_YAW
] = gyroADC
[FD_YAW
];
328 int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value
)
330 float cosZ
= EstG
.V
.Z
/ sqrtf(EstG
.V
.X
* EstG
.V
.X
+ EstG
.V
.Y
* EstG
.V
.Y
+ EstG
.V
.Z
* EstG
.V
.Z
);
333 * Use 0 as the throttle angle correction if we are inverted, vertical or with a
334 * small angle < 0.86 deg
335 * TODO: Define this small angle in config.
337 if (cosZ
<= 0.015f
) {
340 int angle
= lrintf(acosf(cosZ
) * throttleAngleScale
);
343 return lrintf(throttle_correction_value
* sinf(angle
/ (900.0f
* M_PIf
/ 2.0f
)));