4 int16_t gyroADC
[3], accADC
[3], accSmooth
[3], magADC
[3];
6 uint32_t accTimeSum
= 0; // keep track for integration of acc
8 int16_t smallAngle
= 0;
9 int32_t baroPressure
= 0;
10 int32_t baroTemperature
= 0;
11 uint32_t baroPressureSum
= 0;
13 int32_t sonarAlt
; // to think about the unit
14 int32_t EstAlt
; // in cm
17 int32_t errorAltitudeI
= 0;
18 int32_t vario
= 0; // variometer in cm/s
19 int16_t throttleAngleCorrection
= 0; // correction of throttle in lateral wind,
20 float magneticDeclination
= 0.0f
; // calculated at startup from config
22 float throttleAngleScale
;
27 int16_t gyroData
[3] = { 0, 0, 0 };
28 int16_t gyroZero
[3] = { 0, 0, 0 };
29 int16_t angle
[2] = { 0, 0 }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800
30 float anglerad
[2] = { 0.0f
, 0.0f
}; // absolute angle inclination in radians
32 static void getEstimatedAttitude(void);
36 smallAngle
= lrintf(acc_1G
* cosf(RAD
* 25.0f
));
37 accVelScale
= 9.80665f
/ acc_1G
/ 10000.0f
;
38 throttleAngleScale
= (1800.0f
/ M_PI
) * (900.0f
/ cfg
.throttle_correction_angle
);
41 // if mag sensor is enabled, use it
42 if (sensors(SENSOR_MAG
))
50 static int16_t gyroYawSmooth
= 0;
53 if (sensors(SENSOR_ACC
)) {
55 getEstimatedAttitude();
62 if (mcfg
.mixerConfiguration
== MULTITYPE_TRI
) {
63 gyroData
[YAW
] = (gyroYawSmooth
* 2 + gyroADC
[YAW
]) / 3;
64 gyroYawSmooth
= gyroData
[YAW
];
65 gyroData
[ROLL
] = gyroADC
[ROLL
];
66 gyroData
[PITCH
] = gyroADC
[PITCH
];
68 for (axis
= 0; axis
< 3; axis
++)
69 gyroData
[axis
] = gyroADC
[axis
];
73 // **************************************************
74 // Simplified IMU based on "Complementary Filter"
75 // Inspired by http://starlino.com/imu_guide.html
77 // adapted by ziss_dm : http://www.multiwii.com/forum/viewtopic.php?f=8&t=198
79 // The following ideas was used in this project:
80 // 1) Rotation matrix: http://en.wikipedia.org/wiki/Rotation_matrix
82 // Currently Magnetometer uses separate CF which is used only
83 // for heading approximation.
85 // **************************************************
87 #define INV_GYR_CMPF_FACTOR (1.0f / ((float)mcfg.gyro_cmpf_factor + 1.0f))
88 #define INV_GYR_CMPFM_FACTOR (1.0f / ((float)mcfg.gyro_cmpfm_factor + 1.0f))
90 typedef struct fp_vector
{
103 // Normalize a vector
104 void normalizeV(struct fp_vector
*src
, struct fp_vector
*dest
)
108 length
= sqrtf(src
->X
* src
->X
+ src
->Y
* src
->Y
+ src
->Z
* src
->Z
);
110 dest
->X
= src
->X
/ length
;
111 dest
->Y
= src
->Y
/ length
;
112 dest
->Z
= src
->Z
/ length
;
116 // Rotate Estimated vector(s) with small angle approximation, according to the gyro data
117 void rotateV(struct fp_vector
*v
, float *delta
)
119 struct fp_vector v_tmp
= *v
;
121 // This does a "proper" matrix rotation using gyro deltas without small-angle approximation
123 float cosx
, sinx
, cosy
, siny
, cosz
, sinz
;
124 float coszcosx
, coszcosy
, sinzcosx
, coszsinx
, sinzsinx
;
126 cosx
= cosf(delta
[ROLL
]);
127 sinx
= sinf(delta
[ROLL
]);
128 cosy
= cosf(delta
[PITCH
]);
129 siny
= sinf(delta
[PITCH
]);
130 cosz
= cosf(delta
[YAW
]);
131 sinz
= sinf(delta
[YAW
]);
133 coszcosx
= cosz
* cosx
;
134 coszcosy
= cosz
* cosy
;
135 sinzcosx
= sinz
* cosx
;
136 coszsinx
= sinx
* cosz
;
137 sinzsinx
= sinx
* sinz
;
139 mat
[0][0] = coszcosy
;
140 mat
[0][1] = -cosy
* sinz
;
142 mat
[1][0] = sinzcosx
+ (coszsinx
* siny
);
143 mat
[1][1] = coszcosx
- (sinzsinx
* siny
);
144 mat
[1][2] = -sinx
* cosy
;
145 mat
[2][0] = (sinzsinx
) - (coszcosx
* siny
);
146 mat
[2][1] = (coszsinx
) + (sinzcosx
* siny
);
147 mat
[2][2] = cosy
* cosx
;
149 v
->X
= v_tmp
.X
* mat
[0][0] + v_tmp
.Y
* mat
[1][0] + v_tmp
.Z
* mat
[2][0];
150 v
->Y
= v_tmp
.X
* mat
[0][1] + v_tmp
.Y
* mat
[1][1] + v_tmp
.Z
* mat
[2][1];
151 v
->Z
= v_tmp
.X
* mat
[0][2] + v_tmp
.Y
* mat
[1][2] + v_tmp
.Z
* mat
[2][2];
154 int32_t applyDeadband(int32_t value
, int32_t deadband
)
156 if (abs(value
) < deadband
) {
158 } else if (value
> 0) {
160 } else if (value
< 0) {
166 #define F_CUT_ACCZ 10.0f // 10Hz should still be fast enough
167 static const float fc_acc
= 0.5f
/ (M_PI
* F_CUT_ACCZ
);
169 // rotate acc into Earth frame and calculate acceleration in it
170 void acc_calc(uint32_t deltaT
)
172 static int32_t accZoffset
= 0;
173 static float accz_smooth
;
175 t_fp_vector accel_ned
;
177 // the accel values have to be rotated into the earth frame
178 rpy
[0] = -(float)anglerad
[ROLL
];
179 rpy
[1] = -(float)anglerad
[PITCH
];
180 rpy
[2] = -(float)heading
* RAD
;
182 accel_ned
.V
.X
= accSmooth
[0];
183 accel_ned
.V
.Y
= accSmooth
[1];
184 accel_ned
.V
.Z
= accSmooth
[2];
186 rotateV(&accel_ned
.V
, rpy
);
188 if (cfg
.acc_unarmedcal
== 1) {
190 accZoffset
-= accZoffset
/ 64;
191 accZoffset
+= accel_ned
.V
.Z
;
193 accel_ned
.V
.Z
-= accZoffset
/ 64; // compensate for gravitation on z-axis
195 accel_ned
.V
.Z
-= acc_1G
;
197 accz_smooth
= accz_smooth
+ (deltaT
/ (fc_acc
+ deltaT
)) * (accel_ned
.V
.Z
- accz_smooth
); // low pass filter
199 // apply Deadband to reduce integration drift and vibration influence
200 accSum
[X
] += applyDeadband(lrintf(accel_ned
.V
.X
), cfg
.accxy_deadband
);
201 accSum
[Y
] += applyDeadband(lrintf(accel_ned
.V
.Y
), cfg
.accxy_deadband
);
202 accSum
[Z
] += applyDeadband(lrintf(accz_smooth
), cfg
.accz_deadband
);
204 // sum up Values for later integration to get velocity and distance
205 accTimeSum
+= deltaT
;
209 void accSum_reset(void)
218 // baseflight calculation by Luggi09 originates from arducopter
219 static int16_t calculateHeading(t_fp_vector
*vec
)
223 float cosineRoll
= cosf(anglerad
[ROLL
]);
224 float sineRoll
= sinf(anglerad
[ROLL
]);
225 float cosinePitch
= cosf(anglerad
[PITCH
]);
226 float sinePitch
= sinf(anglerad
[PITCH
]);
227 float Xh
= vec
->A
[X
] * cosinePitch
+ vec
->A
[Y
] * sineRoll
* sinePitch
+ vec
->A
[Z
] * sinePitch
* cosineRoll
;
228 float Yh
= vec
->A
[Y
] * cosineRoll
- vec
->A
[Z
] * sineRoll
;
229 float hd
= (atan2f(Yh
, Xh
) * 1800.0f
/ M_PI
+ magneticDeclination
) / 10.0f
;
237 static void getEstimatedAttitude(void)
241 static t_fp_vector EstM
;
242 static t_fp_vector EstN
= { .A
= { 1.0f
, 0.0f
, 0.0f
} };
243 static float accLPF
[3];
244 static uint32_t previousT
;
245 uint32_t currentT
= micros();
247 float scale
, deltaGyroAngle
[3];
248 deltaT
= currentT
- previousT
;
249 scale
= deltaT
* gyro
.scale
;
250 previousT
= currentT
;
253 for (axis
= 0; axis
< 3; axis
++) {
254 deltaGyroAngle
[axis
] = gyroADC
[axis
] * scale
;
255 if (cfg
.acc_lpf_factor
> 0) {
256 accLPF
[axis
] = accLPF
[axis
] * (1.0f
- (1.0f
/ cfg
.acc_lpf_factor
)) + accADC
[axis
] * (1.0f
/ cfg
.acc_lpf_factor
);
257 accSmooth
[axis
] = accLPF
[axis
];
259 accSmooth
[axis
] = accADC
[axis
];
261 accMag
+= (int32_t)accSmooth
[axis
] * accSmooth
[axis
];
263 accMag
= accMag
* 100 / ((int32_t)acc_1G
* acc_1G
);
265 rotateV(&EstG
.V
, deltaGyroAngle
);
266 if (sensors(SENSOR_MAG
)) {
267 rotateV(&EstM
.V
, deltaGyroAngle
);
269 rotateV(&EstN
.V
, deltaGyroAngle
);
270 normalizeV(&EstN
.V
, &EstN
.V
);
273 // Apply complimentary filter (Gyro drift correction)
274 // 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.
275 // To do that, we just skip filter, as EstV already rotated by Gyro
276 if (72 < (uint16_t)accMag
&& (uint16_t)accMag
< 133) {
277 for (axis
= 0; axis
< 3; axis
++)
278 EstG
.A
[axis
] = (EstG
.A
[axis
] * (float)mcfg
.gyro_cmpf_factor
+ accSmooth
[axis
]) * INV_GYR_CMPF_FACTOR
;
281 if (sensors(SENSOR_MAG
)) {
282 for (axis
= 0; axis
< 3; axis
++)
283 EstM
.A
[axis
] = (EstM
.A
[axis
] * (float)mcfg
.gyro_cmpfm_factor
+ magADC
[axis
]) * INV_GYR_CMPFM_FACTOR
;
286 f
.SMALL_ANGLE
= (EstG
.A
[Z
] > smallAngle
);
288 // Attitude of the estimated vector
289 anglerad
[ROLL
] = atan2f(EstG
.V
.Y
, EstG
.V
.Z
);
290 anglerad
[PITCH
] = atan2f(-EstG
.V
.X
, sqrtf(EstG
.V
.Y
* EstG
.V
.Y
+ EstG
.V
.Z
* EstG
.V
.Z
));
291 angle
[ROLL
] = lrintf(anglerad
[ROLL
] * (1800.0f
/ M_PI
));
292 angle
[PITCH
] = lrintf(anglerad
[PITCH
] * (1800.0f
/ M_PI
));
294 if (sensors(SENSOR_MAG
))
295 heading
= calculateHeading(&EstM
);
297 heading
= calculateHeading(&EstN
);
299 acc_calc(deltaT
); // rotate acc vector into earth frame
301 if (cfg
.throttle_correction_value
) {
303 float cosZ
= EstG
.V
.Z
/ sqrtf(EstG
.V
.X
* EstG
.V
.X
+ EstG
.V
.Y
* EstG
.V
.Y
+ EstG
.V
.Z
* EstG
.V
.Z
);
305 if (cosZ
<= 0.015f
) { // we are inverted, vertical or with a small angle < 0.86 deg
306 throttleAngleCorrection
= 0;
308 int angle
= lrintf(acosf(cosZ
) * throttleAngleScale
);
311 throttleAngleCorrection
= lrintf(cfg
.throttle_correction_value
* sinf(angle
/ (900.0f
* M_PI
/ 2.0f
))) ;
318 #define UPDATE_INTERVAL 25000 // 40hz update rate (20hz LPF on acc)
320 int getEstimatedAltitude(void)
322 static uint32_t previousT
;
323 uint32_t currentT
= micros();
333 static float accZ_old
= 0.0f
;
334 static float vel
= 0.0f
;
335 static float accAlt
= 0.0f
;
336 static int32_t lastBaroAlt
;
337 static int32_t baroGroundAltitude
= 0;
338 static int32_t baroGroundPressure
= 0;
340 dTime
= currentT
- previousT
;
341 if (dTime
< UPDATE_INTERVAL
)
343 previousT
= currentT
;
345 if (calibratingB
> 0) {
346 baroGroundPressure
-= baroGroundPressure
/ 8;
347 baroGroundPressure
+= baroPressureSum
/ (cfg
.baro_tab_size
- 1);
348 baroGroundAltitude
= (1.0f
- powf((baroGroundPressure
/ 8) / 101325.0f
, 0.190295f
)) * 4433000.0f
;
355 // calculates height from ground via baro readings
356 // see: https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Baro/AP_Baro.cpp#L140
357 BaroAlt_tmp
= lrintf((1.0f
- powf((float)(baroPressureSum
/ (cfg
.baro_tab_size
- 1)) / 101325.0f
, 0.190295f
)) * 4433000.0f
); // in cm
358 BaroAlt_tmp
-= baroGroundAltitude
;
359 BaroAlt
= lrintf((float)BaroAlt
* cfg
.baro_noise_lpf
+ (float)BaroAlt_tmp
* (1.0f
- cfg
.baro_noise_lpf
)); // additional LPF to reduce baro noise
361 dt
= accTimeSum
* 1e-6f
; // delta acc reading time in seconds
363 // Integrator - velocity, cm/sec
364 accZ_tmp
= (float)accSum
[2] / (float)accSumCount
;
365 vel_acc
= accZ_tmp
* accVelScale
* (float)accTimeSum
;
367 // Integrator - Altitude in cm
368 accAlt
+= (vel_acc
* 0.5f
) * dt
+ vel
* dt
; // integrate velocity to get distance (x= a/2 * t^2)
369 accAlt
= accAlt
* cfg
.baro_cf_alt
+ (float)BaroAlt
* (1.0f
- cfg
.baro_cf_alt
); // complementary filter for Altitude estimation (baro & acc)
374 debug
[0] = accSum
[2] / accSumCount
; // acceleration
375 debug
[1] = vel
; // velocity
376 debug
[2] = accAlt
; // height
381 baroVel
= (BaroAlt
- lastBaroAlt
) * 1000000.0f
/ dTime
;
382 lastBaroAlt
= BaroAlt
;
384 baroVel
= constrain(baroVel
, -300, 300); // constrain baro velocity +/- 300cm/s
385 baroVel
= applyDeadband(baroVel
, 10); // to reduce noise near zero
387 // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
388 // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
389 vel
= vel
* cfg
.baro_cf_vel
+ baroVel
* (1 - cfg
.baro_cf_vel
);
390 vel_tmp
= lrintf(vel
);
393 vario
= applyDeadband(vel_tmp
, 5);
395 if (abs(angle
[ROLL
]) < 800 && abs(angle
[PITCH
]) < 800) { // only calculate pid if the copters thrust is facing downwards(<80deg)
396 // Altitude P-Controller
397 error
= constrain(AltHold
- EstAlt
, -500, 500);
398 error
= applyDeadband(error
, 10); // remove small P parametr to reduce noise near zero position
399 setVel
= constrain((cfg
.P8
[PIDALT
] * error
/ 128), -300, +300); // limit velocity to +/- 3 m/s
401 // Velocity PID-Controller
403 error
= setVel
- vel_tmp
;
404 BaroPID
= constrain((cfg
.P8
[PIDVEL
] * error
/ 32), -300, +300);
407 errorAltitudeI
+= (cfg
.I8
[PIDVEL
] * error
) / 8;
408 errorAltitudeI
= constrain(errorAltitudeI
, -(1024 * 200), (1024 * 200));
409 BaroPID
+= errorAltitudeI
/ 1024; // I in range +/-200
412 BaroPID
-= constrain(cfg
.D8
[PIDVEL
] * (accZ_tmp
+ accZ_old
) / 64, -150, 150);