Rev 2058 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2058 | Rev 2059 | ||
---|---|---|---|
Line 218... | Line 218... | ||
218 | * directly. This is the "gyroAccFactor" stuff in the original code. |
218 | * directly. This is the "gyroAccFactor" stuff in the original code. |
219 | * There is (there) also a drift compensation |
219 | * There is (there) also a drift compensation |
220 | * - it corrects the differential of the integral = the gyro offsets. |
220 | * - it corrects the differential of the integral = the gyro offsets. |
221 | * That should only be necessary with drifty gyros like ENC-03. |
221 | * That should only be necessary with drifty gyros like ENC-03. |
222 | ************************************************************************/ |
222 | ************************************************************************/ |
- | 223 | #define LOG_DIVIDER 12 |
|
- | 224 | #define DIVIDER (1L << LOG_DIVIDER) |
|
223 | void correctIntegralsByAcc0thOrder(void) { |
225 | void correctIntegralsByAcc0thOrder(void) { |
224 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
226 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
225 | // are less than ....., or reintroduce Kalman. |
227 | // are less than ....., or reintroduce Kalman. |
226 | // Well actually the Z axis acc. check is not so silly. |
228 | // Well actually the Z axis acc. check is not so silly. |
227 | uint8_t axis; |
229 | uint8_t axis; |
228 | int32_t temp; |
230 | int32_t temp; |
Line 229... | Line 231... | ||
229 | 231 | ||
230 | uint8_t ca = controlActivity >> 8; |
232 | uint16_t ca = controlActivity >> 6; |
231 | uint8_t highControlActivity = (ca > staticParams.maxControlActivity); |
- | |
232 | 233 | uint8_t controlActivityWeighted = ca / staticParams.zerothOrderCorrectionControlTolerance; |
|
233 | if (highControlActivity) { |
234 | if (!controlActivityWeighted) controlActivityWeighted = 1; |
234 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
- | |
235 | } else { |
235 | uint8_t accVectorWeighted = accVector / staticParams.zerothOrderCorrectionAccTolerance; |
236 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
- | |
237 | } |
236 | if (!accVectorWeighted) accVectorWeighted = 1; |
238 | 237 | ||
239 | if (accVector <= dynamicParams.maxAccVector) { |
238 | uint8_t accPart = staticParams.zerothOrderCorrection; |
240 | debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
239 | int32_t accDerived; |
241 | 240 | ||
242 | uint8_t permilleAcc = staticParams.zerothOrderCorrection; |
241 | debugOut.analog[14] = controlActivity; |
243 | int32_t accDerived; |
242 | debugOut.analog[15] = accVector; |
244 | 243 | ||
245 | /* |
244 | /* |
246 | if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
245 | if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
247 | permilleAcc /= 2; |
246 | permilleAcc /= 2; |
248 | debugFullWeight = 0; |
247 | debugFullWeight = 0; |
249 | } |
248 | } |
250 | 249 | ||
251 | if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
250 | if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
252 | permilleAcc /= 2; |
251 | permilleAcc /= 2; |
253 | debugFullWeight = 0; |
252 | debugFullWeight = 0; |
254 | */ |
253 | */ |
255 | 254 | ||
256 | if (highControlActivity) { // reduce effect during stick control activity |
255 | debugOut.analog[20] = controlActivityWeighted; |
257 | permilleAcc /= 4; |
- | |
258 | if (controlActivity > staticParams.maxControlActivity * 2) { // reduce effect during stick control activity |
256 | debugOut.analog[21] = accVectorWeighted; |
259 | permilleAcc /= 4; |
- | |
260 | } |
- | |
261 | } |
257 | debugOut.analog[24] = accVector; |
- | 258 | ||
- | 259 | accPart /= controlActivityWeighted; |
|
262 | 260 | accPart /= accVectorWeighted; |
|
263 | /* |
261 | /* |
264 | * Add to each sum: The amount by which the angle is changed just below. |
262 | * Add to each sum: The amount by which the angle is changed just below. |
265 | */ |
263 | */ |
266 | for (axis = PITCH; axis <= ROLL; axis++) { |
264 | for (axis = PITCH; axis <= ROLL; axis++) { |
267 | accDerived = getAngleEstimateFromAcc(axis); |
265 | accDerived = getAngleEstimateFromAcc(axis); |
268 | //debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
266 | //debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
269 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
267 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
270 | temp = attitude[axis]; |
268 | temp = attitude[axis]; |
271 | attitude[axis] = ((int32_t) (1000L - permilleAcc) * temp |
- | |
272 | + (int32_t) permilleAcc * accDerived) / 1000L; |
269 | attitude[axis] = ((int32_t) (DIVIDER - accPart) * temp + (int32_t)accPart * accDerived) >> LOG_DIVIDER; |
273 | correctionSum[axis] += attitude[axis] - temp; |
- | |
274 | } |
- | |
275 | } else { |
- | |
276 | // experiment: Kill drift compensation updates when not flying smooth. |
- | |
277 | // correctionSum[PITCH] = correctionSum[ROLL] = 0; |
- | |
278 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
270 | correctionSum[axis] += attitude[axis] - temp; |
279 | } |
271 | } |
Line 280... | Line 272... | ||
280 | } |
272 | } |
281 | 273 | ||
Line 349... | Line 341... | ||
349 | 341 | ||
350 | void correctHeadingToMagnetic(void) { |
342 | void correctHeadingToMagnetic(void) { |
Line 351... | Line 343... | ||
351 | int32_t error; |
343 | int32_t error; |
352 | 344 | ||
353 | if (commands_isCalibratingCompass()) { |
345 | if (commands_isCalibratingCompass()) { |
354 | debugOut.analog[30] = -1; |
346 | //debugOut.analog[30] = -1; |
Line 355... | Line 347... | ||
355 | return; |
347 | return; |
356 | } |
348 | } |
357 | 349 | ||
358 | // Compass is off, skip. |
350 | // Compass is off, skip. |
Line 359... | Line 351... | ||
359 | // Naaah this is assumed. |
351 | // Naaah this is assumed. |
360 | // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
352 | // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
361 | // return; |
353 | // return; |
362 | 354 | ||
363 | // Compass is invalid, skip. |
355 | // Compass is invalid, skip. |
Line 364... | Line 356... | ||
364 | if (magneticHeading < 0) { |
356 | if (magneticHeading < 0) { |
365 | debugOut.analog[30] = -2; |
357 | //debugOut.analog[30] = -2; |
366 | return; |
358 | return; |
367 | } |
359 | } |
368 | 360 | ||
Line 369... | Line 361... | ||
369 | // Spinning fast, skip |
361 | // Spinning fast, skip |
370 | if (abs(yawRate) > 128) { |
362 | if (abs(yawRate) > 128) { |
371 | debugOut.analog[30] = -3; |
363 | // debugOut.analog[30] = -3; |
372 | return; |
364 | return; |
373 | } |
365 | } |
374 | 366 | ||
Line 375... | Line 367... | ||
375 | // Otherwise invalidated, skip |
367 | // Otherwise invalidated, skip |
Line 376... | Line 368... | ||
376 | if (ignoreCompassTimer) { |
368 | if (ignoreCompassTimer) { |
377 | ignoreCompassTimer--; |
369 | ignoreCompassTimer--; |
378 | debugOut.analog[30] = -4; |
370 | //debugOut.analog[30] = -4; |
379 | return; |
371 | return; |
Line 393... | Line 385... | ||
393 | 385 | ||
394 | int32_t correction = (error * staticParams.compassYawCorrection) >> 8; |
386 | int32_t correction = (error * staticParams.compassYawCorrection) >> 8; |
Line 395... | Line 387... | ||
395 | //debugOut.analog[30] = correction; |
387 | //debugOut.analog[30] = correction; |
396 | 388 | ||
397 | // The correction is added both to current heading (the direction in which the copter thinks it is pointing) |
- | |
398 | // and to the target heading (the direction to which it maneuvers to point). That means, this correction has |
- | |
399 | // no effect on control at all!!! It only has effect on the values of the two variables. However, these values |
389 | // The correction is added both to current heading (the direction in which the copter thinks it is pointing) |
- | 390 | // and to the heading error (the angle of yaw that the copter is off the set heading). |
|
400 | // could have effect on control elsewhere, like in compassControl.c . |
391 | heading += correction; |
Line 401... | Line 392... | ||
401 | heading += correction; |
392 | headingError += correction; |
402 | intervalWrap(&heading, YAWOVER360); |
393 | intervalWrap(&heading, YAWOVER360); |
403 | 394 |