Rev 2058 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1612 | dongfang | 1 | #include <stdlib.h> |
2 | #include <avr/io.h> |
||
3 | |||
4 | #include "attitude.h" |
||
5 | #include "dongfangMath.h" |
||
2048 | - | 6 | #include "commands.h" |
1612 | dongfang | 7 | |
1775 | - | 8 | // For scope debugging only! |
9 | #include "rc.h" |
||
10 | |||
1612 | dongfang | 11 | // where our main data flow comes from. |
12 | #include "analog.h" |
||
13 | |||
14 | #include "configuration.h" |
||
1775 | - | 15 | #include "output.h" |
1612 | dongfang | 16 | |
17 | // Some calculations are performed depending on some stick related things. |
||
18 | #include "controlMixer.h" |
||
19 | |||
20 | #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
||
21 | |||
22 | /* |
||
23 | * Gyro readings, as read from the analog module. It would have been nice to flow |
||
24 | * them around between the different calculations as a struct or array (doing |
||
25 | * things functionally without side effects) but this is shorter and probably |
||
26 | * faster too. |
||
27 | * The variables are overwritten at each attitude calculation invocation - the values |
||
28 | * are not preserved or reused. |
||
29 | */ |
||
1775 | - | 30 | int16_t rate_ATT[2], yawRate; |
1612 | dongfang | 31 | |
32 | // With different (less) filtering |
||
1645 | - | 33 | int16_t rate_PID[2]; |
34 | int16_t differential[2]; |
||
1612 | dongfang | 35 | |
36 | /* |
||
37 | * Gyro readings, after performing "axis coupling" - that is, the transfomation |
||
38 | * of rotation rates from the airframe-local coordinate system to a ground-fixed |
||
39 | * coordinate system. If axis copling is disabled, the gyro readings will be |
||
40 | * copied into these directly. |
||
41 | * These are global for the same pragmatic reason as with the gyro readings. |
||
42 | * The variables are overwritten at each attitude calculation invocation - the values |
||
43 | * are not preserved or reused. |
||
44 | */ |
||
1645 | - | 45 | int16_t ACRate[2], ACYawRate; |
1612 | dongfang | 46 | |
47 | /* |
||
48 | * Gyro integrals. These are the rotation angles of the airframe compared to the |
||
49 | * horizontal plane, yaw relative to yaw at start. |
||
50 | */ |
||
2048 | - | 51 | int32_t attitude[2]; |
1612 | dongfang | 52 | |
2048 | - | 53 | //int readingHeight = 0; |
1612 | dongfang | 54 | |
1805 | - | 55 | // Yaw angle and compass stuff. |
2051 | - | 56 | int32_t headingError; |
1805 | - | 57 | |
58 | // The difference between the above 2 (heading - course) on a -180..179 degree interval. |
||
59 | // Not necessary. Never read anywhere. |
||
60 | // int16_t compassOffCourse = 0; |
||
61 | |||
2051 | - | 62 | uint16_t ignoreCompassTimer = 0;// 500; |
1805 | - | 63 | |
2048 | - | 64 | int32_t heading; // Yaw Gyro Integral supported by compass |
1775 | - | 65 | int16_t yawGyroDrift; |
1612 | dongfang | 66 | |
1805 | - | 67 | int16_t correctionSum[2] = { 0, 0 }; |
1612 | dongfang | 68 | |
1775 | - | 69 | // For NaviCTRL use. |
1805 | - | 70 | int16_t averageAcc[2] = { 0, 0 }, averageAccCount = 0; |
1775 | - | 71 | |
1612 | dongfang | 72 | /* |
73 | * Experiment: Compensating for dynamic-induced gyro biasing. |
||
74 | */ |
||
1805 | - | 75 | int16_t driftComp[2] = { 0, 0 }, driftCompYaw = 0; |
1612 | dongfang | 76 | // int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0; |
77 | // int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw; |
||
78 | // int16_t dynamicCalCount; |
||
79 | |||
1980 | - | 80 | uint16_t accVector; |
81 | |||
1612 | dongfang | 82 | /************************************************************************ |
83 | * Set inclination angles from the acc. sensor data. |
||
84 | * If acc. sensors are not used, set to zero. |
||
85 | * TODO: One could use inverse sine to calculate the angles more |
||
1616 | dongfang | 86 | * accurately, but since: 1) the angles are rather small at times when |
87 | * it makes sense to set the integrals (standing on ground, or flying at |
||
1612 | dongfang | 88 | * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
89 | * it is hardly worth the trouble. |
||
90 | ************************************************************************/ |
||
91 | |||
1645 | - | 92 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
1991 | - | 93 | //int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L; |
2048 | - | 94 | return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; // + correctionTerm; |
2032 | - | 95 | // return 342L * filteredAcc[axis]; |
1612 | dongfang | 96 | } |
97 | |||
98 | void setStaticAttitudeAngles(void) { |
||
99 | #ifdef ATTITUDE_USE_ACC_SENSORS |
||
2048 | - | 100 | attitude[PITCH] = getAngleEstimateFromAcc(PITCH); |
101 | attitude[ROLL] = getAngleEstimateFromAcc(ROLL); |
||
1612 | dongfang | 102 | #else |
2048 | - | 103 | attitude[PITCH] = attitude[ROLL] = 0; |
1612 | dongfang | 104 | #endif |
105 | } |
||
106 | |||
107 | /************************************************************************ |
||
108 | * Neutral Readings |
||
109 | ************************************************************************/ |
||
110 | void attitude_setNeutral(void) { |
||
1869 | - | 111 | // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway. |
2032 | - | 112 | // dynamicParams.axisCoupling1 = dynamicParams.axisCoupling2 = 0; |
1612 | dongfang | 113 | |
1869 | - | 114 | driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0; |
115 | correctionSum[PITCH] = correctionSum[ROLL] = 0; |
||
1612 | dongfang | 116 | |
1869 | - | 117 | // Calibrate hardware. |
1961 | - | 118 | analog_setNeutral(); |
1612 | dongfang | 119 | |
1869 | - | 120 | // reset gyro integrals to acc guessing |
121 | setStaticAttitudeAngles(); |
||
2052 | - | 122 | #ifdef USE_MK3MAG |
2048 | - | 123 | attitude_resetHeadingToMagnetic(); |
2052 | - | 124 | #endif |
1869 | - | 125 | // Servo_On(); //enable servo output |
1612 | dongfang | 126 | } |
127 | |||
128 | /************************************************************************ |
||
129 | * Get sensor data from the analog module, and release the ADC |
||
130 | * TODO: Ultimately, the analog module could do this (instead of dumping |
||
1645 | - | 131 | * the values into variables). |
132 | * The rate variable end up in a range of about [-1024, 1023]. |
||
1612 | dongfang | 133 | *************************************************************************/ |
134 | void getAnalogData(void) { |
||
1869 | - | 135 | uint8_t axis; |
1612 | dongfang | 136 | |
1955 | - | 137 | analog_update(); |
138 | |||
1869 | - | 139 | for (axis = PITCH; axis <= ROLL; axis++) { |
1963 | - | 140 | rate_PID[axis] = gyro_PID[axis] + driftComp[axis]; |
141 | rate_ATT[axis] = gyro_ATT[axis] + driftComp[axis]; |
||
1869 | - | 142 | differential[axis] = gyroD[axis]; |
143 | averageAcc[axis] += acc[axis]; |
||
144 | } |
||
1775 | - | 145 | |
1869 | - | 146 | averageAccCount++; |
147 | yawRate = yawGyro + driftCompYaw; |
||
1612 | dongfang | 148 | } |
149 | |||
150 | /* |
||
151 | * This is the standard flight-style coordinate system transformation |
||
152 | * (from airframe-local axes to a ground-based system). For some reason |
||
153 | * the MK uses a left-hand coordinate system. The tranformation has been |
||
154 | * changed accordingly. |
||
155 | */ |
||
156 | void trigAxisCoupling(void) { |
||
2048 | - | 157 | int16_t rollAngleInDegrees = attitude[ROLL] / GYRO_DEG_FACTOR_PITCHROLL; |
158 | int16_t pitchAngleInDegrees = attitude[PITCH] / GYRO_DEG_FACTOR_PITCHROLL; |
||
1866 | - | 159 | |
2045 | - | 160 | int16_t cospitch = cos_360(pitchAngleInDegrees); |
161 | int16_t cosroll = cos_360(rollAngleInDegrees); |
||
162 | int16_t sinroll = sin_360(rollAngleInDegrees); |
||
163 | |||
2048 | - | 164 | ACRate[PITCH] = (((int32_t) rate_ATT[PITCH] * cosroll |
165 | - (int32_t) yawRate * sinroll) >> LOG_MATH_UNIT_FACTOR); |
||
1866 | - | 166 | |
2048 | - | 167 | ACRate[ROLL] = rate_ATT[ROLL] |
168 | + (((((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll) |
||
169 | >> LOG_MATH_UNIT_FACTOR) * tan_360(pitchAngleInDegrees)) |
||
170 | >> LOG_MATH_UNIT_FACTOR); |
||
1866 | - | 171 | |
2048 | - | 172 | ACYawRate = |
173 | ((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll) |
||
174 | / cospitch; |
||
1872 | - | 175 | |
2048 | - | 176 | ACYawRate = |
177 | ((int32_t) rate_ATT[PITCH] * sinroll + (int32_t) yawRate * cosroll) |
||
178 | / cospitch; |
||
1612 | dongfang | 179 | } |
180 | |||
1775 | - | 181 | // 480 usec with axis coupling - almost no time without. |
1612 | dongfang | 182 | void integrate(void) { |
1869 | - | 183 | // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
184 | uint8_t axis; |
||
1872 | - | 185 | |
2058 | - | 186 | if (staticParams.bitConfig & CFG_AXIS_COUPLING_ENABLED) { |
1869 | - | 187 | trigAxisCoupling(); |
188 | } else { |
||
189 | ACRate[PITCH] = rate_ATT[PITCH]; |
||
190 | ACRate[ROLL] = rate_ATT[ROLL]; |
||
191 | ACYawRate = yawRate; |
||
192 | } |
||
1612 | dongfang | 193 | |
1869 | - | 194 | /* |
195 | * Yaw |
||
196 | * Calculate yaw gyro integral (~ to rotation angle) |
||
2048 | - | 197 | * Limit heading proportional to 0 deg to 360 deg |
1869 | - | 198 | */ |
2048 | - | 199 | heading += ACYawRate; |
200 | intervalWrap(&heading, YAWOVER360); |
||
2051 | - | 201 | headingError += ACYawRate; |
202 | |||
1869 | - | 203 | /* |
204 | * Pitch axis integration and range boundary wrap. |
||
205 | */ |
||
206 | for (axis = PITCH; axis <= ROLL; axis++) { |
||
2048 | - | 207 | attitude[axis] += ACRate[axis]; |
208 | if (attitude[axis] > PITCHROLLOVER180) { |
||
209 | attitude[axis] -= PITCHROLLOVER360; |
||
210 | } else if (attitude[axis] <= -PITCHROLLOVER180) { |
||
211 | attitude[axis] += PITCHROLLOVER360; |
||
1869 | - | 212 | } |
213 | } |
||
1612 | dongfang | 214 | } |
215 | |||
216 | /************************************************************************ |
||
217 | * A kind of 0'th order integral correction, that corrects the integrals |
||
218 | * directly. This is the "gyroAccFactor" stuff in the original code. |
||
1646 | - | 219 | * There is (there) also a drift compensation |
1612 | dongfang | 220 | * - it corrects the differential of the integral = the gyro offsets. |
221 | * That should only be necessary with drifty gyros like ENC-03. |
||
222 | ************************************************************************/ |
||
2059 | - | 223 | #define LOG_DIVIDER 12 |
224 | #define DIVIDER (1L << LOG_DIVIDER) |
||
1612 | dongfang | 225 | void correctIntegralsByAcc0thOrder(void) { |
1869 | - | 226 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
227 | // are less than ....., or reintroduce Kalman. |
||
228 | // Well actually the Z axis acc. check is not so silly. |
||
229 | uint8_t axis; |
||
230 | int32_t temp; |
||
1908 | - | 231 | |
2059 | - | 232 | uint16_t ca = controlActivity >> 6; |
233 | uint8_t controlActivityWeighted = ca / staticParams.zerothOrderCorrectionControlTolerance; |
||
234 | if (!controlActivityWeighted) controlActivityWeighted = 1; |
||
235 | uint8_t accVectorWeighted = accVector / staticParams.zerothOrderCorrectionAccTolerance; |
||
236 | if (!accVectorWeighted) accVectorWeighted = 1; |
||
1988 | - | 237 | |
2059 | - | 238 | uint8_t accPart = staticParams.zerothOrderCorrection; |
239 | int32_t accDerived; |
||
1988 | - | 240 | |
2059 | - | 241 | debugOut.analog[14] = controlActivity; |
242 | debugOut.analog[15] = accVector; |
||
2048 | - | 243 | |
2059 | - | 244 | /* |
245 | if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
||
246 | permilleAcc /= 2; |
||
247 | debugFullWeight = 0; |
||
248 | } |
||
1612 | dongfang | 249 | |
2059 | - | 250 | if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
251 | permilleAcc /= 2; |
||
252 | debugFullWeight = 0; |
||
253 | */ |
||
1953 | - | 254 | |
2059 | - | 255 | debugOut.analog[20] = controlActivityWeighted; |
256 | debugOut.analog[21] = accVectorWeighted; |
||
257 | debugOut.analog[24] = accVector; |
||
1953 | - | 258 | |
2059 | - | 259 | accPart /= controlActivityWeighted; |
260 | accPart /= accVectorWeighted; |
||
261 | /* |
||
262 | * Add to each sum: The amount by which the angle is changed just below. |
||
263 | */ |
||
264 | for (axis = PITCH; axis <= ROLL; axis++) { |
||
265 | accDerived = getAngleEstimateFromAcc(axis); |
||
266 | //debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
||
267 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
||
268 | temp = attitude[axis]; |
||
269 | attitude[axis] = ((int32_t) (DIVIDER - accPart) * temp + (int32_t)accPart * accDerived) >> LOG_DIVIDER; |
||
270 | correctionSum[axis] += attitude[axis] - temp; |
||
1869 | - | 271 | } |
1612 | dongfang | 272 | } |
273 | |||
274 | /************************************************************************ |
||
275 | * This is an attempt to correct not the error in the angle integrals |
||
276 | * (that happens in correctIntegralsByAcc0thOrder above) but rather the |
||
277 | * cause of it: Gyro drift, vibration and rounding errors. |
||
278 | * All the corrections made in correctIntegralsByAcc0thOrder over |
||
1646 | - | 279 | * DRIFTCORRECTION_TIME cycles are summed up. This number is |
280 | * then divided by DRIFTCORRECTION_TIME to get the approx. |
||
1612 | dongfang | 281 | * correction that should have been applied to each iteration to fix |
282 | * the error. This is then added to the dynamic offsets. |
||
283 | ************************************************************************/ |
||
1646 | - | 284 | // 2 times / sec. = 488/2 |
285 | #define DRIFTCORRECTION_TIME 256L |
||
286 | void driftCorrection(void) { |
||
1869 | - | 287 | static int16_t timer = DRIFTCORRECTION_TIME; |
288 | int16_t deltaCorrection; |
||
1872 | - | 289 | int16_t round; |
1869 | - | 290 | uint8_t axis; |
1872 | - | 291 | |
1869 | - | 292 | if (!--timer) { |
293 | timer = DRIFTCORRECTION_TIME; |
||
294 | for (axis = PITCH; axis <= ROLL; axis++) { |
||
295 | // Take the sum of corrections applied, add it to delta |
||
2048 | - | 296 | if (correctionSum[axis] >= 0) |
1872 | - | 297 | round = DRIFTCORRECTION_TIME / 2; |
298 | else |
||
299 | round = -DRIFTCORRECTION_TIME / 2; |
||
300 | deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
||
1869 | - | 301 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
1960 | - | 302 | driftComp[axis] += deltaCorrection / staticParams.driftCompDivider; |
303 | CHECK_MIN_MAX(driftComp[axis], -staticParams.driftCompLimit, staticParams.driftCompLimit); |
||
1869 | - | 304 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
1955 | - | 305 | // DebugOut.Analog[16 + axis] = correctionSum[axis]; |
2035 | - | 306 | // debugOut.analog[28 + axis] = driftComp[axis]; |
1869 | - | 307 | correctionSum[axis] = 0; |
308 | } |
||
309 | } |
||
1612 | dongfang | 310 | } |
311 | |||
1980 | - | 312 | void calculateAccVector(void) { |
2048 | - | 313 | int16_t temp; |
314 | temp = filteredAcc[0] >> 3; |
||
315 | accVector = temp * temp; |
||
316 | temp = filteredAcc[1] >> 3; |
||
317 | accVector += temp * temp; |
||
318 | temp = filteredAcc[2] >> 3; |
||
319 | accVector += temp * temp; |
||
1980 | - | 320 | } |
321 | |||
2052 | - | 322 | #ifdef USE_MK3MAG |
2048 | - | 323 | void attitude_resetHeadingToMagnetic(void) { |
324 | if (commands_isCalibratingCompass()) |
||
325 | return; |
||
326 | |||
327 | // Compass is off, skip. |
||
2052 | - | 328 | if (!(staticParams.bitConfig & CFG_COMPASS_ENABLED)) |
2048 | - | 329 | return; |
330 | |||
331 | // Compass is invalid, skip. |
||
332 | if (magneticHeading < 0) |
||
333 | return; |
||
334 | |||
335 | heading = (int32_t) magneticHeading * GYRO_DEG_FACTOR_YAW; |
||
2051 | - | 336 | //targetHeading = heading; |
337 | headingError = 0; |
||
2048 | - | 338 | |
339 | debugOut.digital[0] ^= DEBUG_COMPASS; |
||
340 | } |
||
341 | |||
342 | void correctHeadingToMagnetic(void) { |
||
343 | int32_t error; |
||
344 | |||
2051 | - | 345 | if (commands_isCalibratingCompass()) { |
2059 | - | 346 | //debugOut.analog[30] = -1; |
2048 | - | 347 | return; |
2051 | - | 348 | } |
2048 | - | 349 | |
350 | // Compass is off, skip. |
||
351 | // Naaah this is assumed. |
||
352 | // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
||
353 | // return; |
||
354 | |||
355 | // Compass is invalid, skip. |
||
2051 | - | 356 | if (magneticHeading < 0) { |
2059 | - | 357 | //debugOut.analog[30] = -2; |
2048 | - | 358 | return; |
2051 | - | 359 | } |
2048 | - | 360 | |
361 | // Spinning fast, skip |
||
2051 | - | 362 | if (abs(yawRate) > 128) { |
2059 | - | 363 | // debugOut.analog[30] = -3; |
2048 | - | 364 | return; |
2051 | - | 365 | } |
2048 | - | 366 | |
367 | // Otherwise invalidated, skip |
||
368 | if (ignoreCompassTimer) { |
||
369 | ignoreCompassTimer--; |
||
2059 | - | 370 | //debugOut.analog[30] = -4; |
2048 | - | 371 | return; |
372 | } |
||
373 | |||
2059 | - | 374 | //debugOut.analog[30] = magneticHeading; |
2058 | - | 375 | |
2048 | - | 376 | // TODO: Find computational cost of this. |
2051 | - | 377 | error = ((int32_t)magneticHeading*GYRO_DEG_FACTOR_YAW - heading); |
378 | if (error <= -YAWOVER180) error += YAWOVER360; |
||
379 | else if (error > YAWOVER180) error -= YAWOVER360; |
||
2048 | - | 380 | |
381 | // We only correct errors larger than the resolution of the compass, or else we would keep rounding the |
||
382 | // better resolution of the gyros to the worse resolution of the compass all the time. |
||
383 | // The correction should really only serve to compensate for gyro drift. |
||
384 | if(abs(error) < GYRO_DEG_FACTOR_YAW) return; |
||
385 | |||
2051 | - | 386 | int32_t correction = (error * staticParams.compassYawCorrection) >> 8; |
2055 | - | 387 | //debugOut.analog[30] = correction; |
2048 | - | 388 | |
389 | // The correction is added both to current heading (the direction in which the copter thinks it is pointing) |
||
2059 | - | 390 | // and to the heading error (the angle of yaw that the copter is off the set heading). |
2048 | - | 391 | heading += correction; |
2059 | - | 392 | headingError += correction; |
2048 | - | 393 | intervalWrap(&heading, YAWOVER360); |
394 | |||
2051 | - | 395 | // If we want a transparent flight wrt. compass correction (meaning the copter does not change attitude all |
396 | // when the compass corrects the heading - it only corrects numbers!) we want to add: |
||
397 | // This will however cause drift to remain uncorrected! |
||
398 | // headingError += correction; |
||
2055 | - | 399 | //debugOut.analog[29] = 0; |
2048 | - | 400 | } |
2052 | - | 401 | #endif |
2048 | - | 402 | |
1612 | dongfang | 403 | /************************************************************************ |
404 | * Main procedure. |
||
405 | ************************************************************************/ |
||
1805 | - | 406 | void calculateFlightAttitude(void) { |
1869 | - | 407 | getAnalogData(); |
1980 | - | 408 | calculateAccVector(); |
1869 | - | 409 | integrate(); |
1775 | - | 410 | |
1612 | dongfang | 411 | #ifdef ATTITUDE_USE_ACC_SENSORS |
1869 | - | 412 | correctIntegralsByAcc0thOrder(); |
413 | driftCorrection(); |
||
1612 | dongfang | 414 | #endif |
2015 | - | 415 | |
416 | // We are done reading variables from the analog module. |
||
417 | // Interrupt-driven sensor reading may restart. |
||
418 | startAnalogConversionCycle(); |
||
1612 | dongfang | 419 | |
2052 | - | 420 | #ifdef USE_MK3MAG |
421 | if (staticParams.bitConfig & (CFG_COMPASS_ENABLED | CFG_GPS_ENABLED)) { |
||
2048 | - | 422 | correctHeadingToMagnetic(); |
1869 | - | 423 | } |
2052 | - | 424 | #endif |
1775 | - | 425 | } |
1612 | dongfang | 426 | |
427 | /* |
||
428 | * This is part of an experiment to measure average sensor offsets caused by motor vibration, |
||
429 | * and to compensate them away. It brings about some improvement, but no miracles. |
||
430 | * As long as the left stick is kept in the start-motors position, the dynamic compensation |
||
431 | * will measure the effect of vibration, to use for later compensation. So, one should keep |
||
432 | * the stick in the start-motors position for a few seconds, till all motors run (at the wrong |
||
433 | * speed unfortunately... must find a better way) |
||
434 | */ |
||
435 | /* |
||
1805 | - | 436 | void attitude_startDynamicCalibration(void) { |
437 | dynamicCalPitch = dynamicCalRoll = dynamicCalYaw = dynamicCalCount = 0; |
||
438 | savedDynamicOffsetPitch = savedDynamicOffsetRoll = 1000; |
||
439 | } |
||
1612 | dongfang | 440 | |
1805 | - | 441 | void attitude_continueDynamicCalibration(void) { |
442 | // measure dynamic offset now... |
||
443 | dynamicCalPitch += hiResPitchGyro; |
||
444 | dynamicCalRoll += hiResRollGyro; |
||
445 | dynamicCalYaw += rawYawGyroSum; |
||
446 | dynamicCalCount++; |
||
447 | |||
448 | // Param6: Manual mode. The offsets are taken from Param7 and Param8. |
||
449 | if (dynamicParams.UserParam6 || 1) { // currently always enabled. |
||
450 | // manual mode |
||
451 | driftCompPitch = dynamicParams.UserParam7 - 128; |
||
452 | driftCompRoll = dynamicParams.UserParam8 - 128; |
||
453 | } else { |
||
454 | // use the sampled value (does not seem to work so well....) |
||
455 | driftCompPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount; |
||
456 | driftCompRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount; |
||
457 | driftCompYaw = -dynamicCalYaw / dynamicCalCount; |
||
458 | } |
||
459 | |||
460 | // keep resetting these meanwhile, to avoid accumulating errors. |
||
461 | setStaticAttitudeIntegrals(); |
||
462 | yawAngle = 0; |
||
463 | } |
||
464 | */ |