Rev 1870 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1870 | Rev 1872 | ||
---|---|---|---|
Line 214... | Line 214... | ||
214 | yawRate = yawGyro + driftCompYaw; |
214 | yawRate = yawGyro + driftCompYaw; |
Line 215... | Line 215... | ||
215 | 215 | ||
216 | // We are done reading variables from the analog module. |
216 | // We are done reading variables from the analog module. |
217 | // Interrupt-driven sensor reading may restart. |
217 | // Interrupt-driven sensor reading may restart. |
218 | analogDataReady = 0; |
- | |
219 | J4HIGH; |
218 | analogDataReady = 0; |
220 | analog_start(); |
219 | analog_start(); |
Line 221... | Line 220... | ||
221 | } |
220 | } |
222 | 221 | ||
Line 237... | Line 236... | ||
237 | ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll |
236 | ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll |
238 | + (int32_t)yawRate * cosroll) >> MATH_UNIT_FACTOR_LOG) * int_tan( |
237 | + (int32_t)yawRate * cosroll) >> MATH_UNIT_FACTOR_LOG) * int_tan( |
239 | angle[PITCH])) >> MATH_UNIT_FACTOR_LOG); |
238 | angle[PITCH])) >> MATH_UNIT_FACTOR_LOG); |
Line 240... | Line 239... | ||
240 | 239 | ||
- | 240 | ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
|
- | 241 | ||
241 | ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
242 | ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch; |
Line 242... | Line 243... | ||
242 | } |
243 | } |
243 | 244 | ||
244 | // 480 usec with axis coupling - almost no time without. |
245 | // 480 usec with axis coupling - almost no time without. |
245 | void integrate(void) { |
246 | void integrate(void) { |
- | 247 | // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
|
246 | // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
248 | uint8_t axis; |
247 | uint8_t axis; |
249 | |
248 | if (!looping && (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) { |
250 | if (!looping && (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) { |
249 | trigAxisCoupling(); |
251 | trigAxisCoupling(); |
250 | } else { |
252 | } else { |
Line 355... | Line 357... | ||
355 | // 2 times / sec. = 488/2 |
357 | // 2 times / sec. = 488/2 |
356 | #define DRIFTCORRECTION_TIME 256L |
358 | #define DRIFTCORRECTION_TIME 256L |
357 | void driftCorrection(void) { |
359 | void driftCorrection(void) { |
358 | static int16_t timer = DRIFTCORRECTION_TIME; |
360 | static int16_t timer = DRIFTCORRECTION_TIME; |
359 | int16_t deltaCorrection; |
361 | int16_t deltaCorrection; |
- | 362 | int16_t round; |
|
360 | uint8_t axis; |
363 | uint8_t axis; |
- | 364 | ||
- | 365 | DebugOut.Analog[6] = (DRIFTCORRECTION_TIME + DRIFTCORRECTION_TIME/2) / DRIFTCORRECTION_TIME; |
|
- | 366 | DebugOut.Analog[7] = (-DRIFTCORRECTION_TIME + DRIFTCORRECTION_TIME/2) / DRIFTCORRECTION_TIME; |
|
- | 367 | ||
361 | if (!--timer) { |
368 | if (!--timer) { |
362 | timer = DRIFTCORRECTION_TIME; |
369 | timer = DRIFTCORRECTION_TIME; |
363 | for (axis = PITCH; axis <= ROLL; axis++) { |
370 | for (axis = PITCH; axis <= ROLL; axis++) { |
364 | // Take the sum of corrections applied, add it to delta |
371 | // Take the sum of corrections applied, add it to delta |
- | 372 | if (correctionSum[axis] >=0) |
|
365 | deltaCorrection = (correctionSum[axis] + DRIFTCORRECTION_TIME / 2) |
373 | round = DRIFTCORRECTION_TIME / 2; |
- | 374 | else |
|
366 | / DRIFTCORRECTION_TIME; |
375 | round = -DRIFTCORRECTION_TIME / 2; |
- | 376 | deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
|
367 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
377 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
368 | driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
378 | driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
369 | CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
379 | CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
370 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
380 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
371 | DebugOut.Analog[16 + axis] = correctionSum[axis]; |
381 | DebugOut.Analog[16 + axis] = correctionSum[axis]; |
372 | DebugOut.Analog[18 + axis] = deltaCorrection / staticParams.GyroAccTrim; |
- | |
373 | DebugOut.Analog[28 + axis] = driftComp[axis]; |
382 | DebugOut.Analog[28 + axis] = driftComp[axis]; |
Line 374... | Line 383... | ||
374 | 383 | ||
375 | correctionSum[axis] = 0; |
384 | correctionSum[axis] = 0; |
376 | } |
385 | } |
Line 379... | Line 388... | ||
379 | 388 | ||
380 | /************************************************************************ |
389 | /************************************************************************ |
381 | * Main procedure. |
390 | * Main procedure. |
382 | ************************************************************************/ |
391 | ************************************************************************/ |
383 | void calculateFlightAttitude(void) { |
- | |
384 | // part1: 550 usec. |
- | |
385 | // part1a: 550 usec. |
- | |
386 | // part1b: 60 usec. |
392 | void calculateFlightAttitude(void) { |
387 | getAnalogData(); |
- | |
388 | // end part1b |
393 | getAnalogData(); |
389 | integrate(); |
- | |
390 | // end part1a |
- | |
391 | - | ||
392 | DebugOut.Analog[6] = stronglyFilteredAcc[PITCH]; |
- | |
393 | DebugOut.Analog[7] = stronglyFilteredAcc[ROLL]; |
- | |
Line 394... | Line 394... | ||
394 | DebugOut.Analog[8] = stronglyFilteredAcc[Z]; |
394 | integrate(); |
395 | 395 | ||
396 | DebugOut.Analog[3] = rate_PID[PITCH]; |
396 | DebugOut.Analog[3] = rate_PID[PITCH]; |
Line 397... | Line 397... | ||
397 | DebugOut.Analog[4] = rate_PID[ROLL]; |
397 | DebugOut.Analog[4] = rate_PID[ROLL]; |
398 | DebugOut.Analog[5] = yawRate; |
398 | DebugOut.Analog[5] = yawRate; |
399 | 399 | ||
400 | #ifdef ATTITUDE_USE_ACC_SENSORS |
400 | #ifdef ATTITUDE_USE_ACC_SENSORS |
401 | correctIntegralsByAcc0thOrder(); |
- | |
402 | driftCorrection(); |
401 | correctIntegralsByAcc0thOrder(); |
Line 403... | Line 402... | ||
403 | #endif |
402 | driftCorrection(); |
404 | // end part1 |
403 | #endif |