Rev 2059 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2059 | Rev 2062 | ||
|---|---|---|---|
| Line 211... | Line 211... | ||
| 211 | attitude[axis] += PITCHROLLOVER360; |
211 | attitude[axis] += PITCHROLLOVER360; |
| 212 | } |
212 | } |
| 213 | } |
213 | } |
| 214 | } |
214 | } |
| Line -... | Line 215... | ||
| - | 215 | ||
| - | 216 | void correctIntegralsByAcc0thOrder_old(void) { |
|
| - | 217 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
|
| - | 218 | // are less than ....., or reintroduce Kalman. |
|
| - | 219 | // Well actually the Z axis acc. check is not so silly. |
|
| - | 220 | uint8_t axis; |
|
| - | 221 | int32_t temp; |
|
| - | 222 | ||
| - | 223 | uint8_t ca = controlActivity >> 8; |
|
| - | 224 | uint8_t highControlActivity = (ca > staticParams.maxControlActivity); |
|
| - | 225 | ||
| - | 226 | if (highControlActivity) { |
|
| - | 227 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
|
| - | 228 | } else { |
|
| - | 229 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
|
| - | 230 | } |
|
| - | 231 | ||
| - | 232 | if (accVector <= staticParams.maxAccVector) { |
|
| - | 233 | debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
|
| - | 234 | ||
| - | 235 | uint8_t permilleAcc = staticParams.zerothOrderCorrection; |
|
| - | 236 | int32_t accDerived; |
|
| - | 237 | ||
| - | 238 | /* |
|
| - | 239 | if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
|
| - | 240 | permilleAcc /= 2; |
|
| - | 241 | debugFullWeight = 0; |
|
| - | 242 | } |
|
| - | 243 | ||
| - | 244 | if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
|
| - | 245 | permilleAcc /= 2; |
|
| - | 246 | debugFullWeight = 0; |
|
| - | 247 | */ |
|
| - | 248 | ||
| - | 249 | if (highControlActivity) { // reduce effect during stick control activity |
|
| - | 250 | permilleAcc /= 4; |
|
| - | 251 | if (controlActivity > staticParams.maxControlActivity * 2) { // reduce effect during stick control activity |
|
| - | 252 | permilleAcc /= 4; |
|
| - | 253 | } |
|
| - | 254 | } |
|
| - | 255 | ||
| - | 256 | /* |
|
| - | 257 | * Add to each sum: The amount by which the angle is changed just below. |
|
| - | 258 | */ |
|
| - | 259 | for (axis = PITCH; axis <= ROLL; axis++) { |
|
| - | 260 | accDerived = getAngleEstimateFromAcc(axis); |
|
| - | 261 | //debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10); |
|
| - | 262 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
|
| - | 263 | temp = attitude[axis]; |
|
| - | 264 | attitude[axis] = ((int32_t) (1000L - permilleAcc) * temp |
|
| - | 265 | + (int32_t) permilleAcc * accDerived) / 1000L; |
|
| - | 266 | correctionSum[axis] += attitude[axis] - temp; |
|
| - | 267 | } |
|
| - | 268 | } else { |
|
| - | 269 | // experiment: Kill drift compensation updates when not flying smooth. |
|
| - | 270 | // correctionSum[PITCH] = correctionSum[ROLL] = 0; |
|
| - | 271 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
|
| - | 272 | } |
|
| - | 273 | } |
|
| - | 274 | ||
| 215 | 275 | ||
| 216 | /************************************************************************ |
276 | /************************************************************************ |
| 217 | * A kind of 0'th order integral correction, that corrects the integrals |
277 | * A kind of 0'th order integral correction, that corrects the integrals |
| 218 | * directly. This is the "gyroAccFactor" stuff in the original code. |
278 | * directly. This is the "gyroAccFactor" stuff in the original code. |
| 219 | * There is (there) also a drift compensation |
279 | * There is (there) also a drift compensation |
| 220 | * - it corrects the differential of the integral = the gyro offsets. |
280 | * - it corrects the differential of the integral = the gyro offsets. |
| 221 | * That should only be necessary with drifty gyros like ENC-03. |
281 | * That should only be necessary with drifty gyros like ENC-03. |
| 222 | ************************************************************************/ |
282 | ************************************************************************/ |
| 223 | #define LOG_DIVIDER 12 |
283 | #define LOG_DIVIDER 12 |
| 224 | #define DIVIDER (1L << LOG_DIVIDER) |
284 | #define DIVIDER (1L << LOG_DIVIDER) |
| 225 | void correctIntegralsByAcc0thOrder(void) { |
285 | void correctIntegralsByAcc0thOrder_new(void) { |
| 226 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
286 | // 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. |
287 | // are less than ....., or reintroduce Kalman. |
| 228 | // Well actually the Z axis acc. check is not so silly. |
288 | // Well actually the Z axis acc. check is not so silly. |
| 229 | uint8_t axis; |
289 | uint8_t axis; |
| Line 407... | Line 467... | ||
| 407 | getAnalogData(); |
467 | getAnalogData(); |
| 408 | calculateAccVector(); |
468 | calculateAccVector(); |
| 409 | integrate(); |
469 | integrate(); |
| Line 410... | Line 470... | ||
| 410 | 470 | ||
| - | 471 | #ifdef ATTITUDE_USE_ACC_SENSORS |
|
| 411 | #ifdef ATTITUDE_USE_ACC_SENSORS |
472 | if (staticParams.maxControlActivity) { |
| - | 473 | correctIntegralsByAcc0thOrder_old(); |
|
| - | 474 | } else { |
|
| - | 475 | correctIntegralsByAcc0thOrder_new(); |
|
| 412 | correctIntegralsByAcc0thOrder(); |
476 | } |
| 413 | driftCorrection(); |
477 | driftCorrection(); |
| Line 414... | Line 478... | ||
| 414 | #endif |
478 | #endif |
| 415 | 479 | ||