Rev 1953 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1953 | Rev 1955 | ||
|---|---|---|---|
| Line 201... | Line 201... | ||
| 201 | * The rate variable end up in a range of about [-1024, 1023]. |
201 | * The rate variable end up in a range of about [-1024, 1023]. |
| 202 | *************************************************************************/ |
202 | *************************************************************************/ |
| 203 | void getAnalogData(void) { |
203 | void getAnalogData(void) { |
| 204 | uint8_t axis; |
204 | uint8_t axis; |
| Line -... | Line 205... | ||
| - | 205 | ||
| - | 206 | analog_update(); |
|
| 205 | 207 | ||
| 206 | for (axis = PITCH; axis <= ROLL; axis++) { |
208 | for (axis = PITCH; axis <= ROLL; axis++) { |
| 207 | rate_PID[axis] = gyro_PID[axis] /* / HIRES_GYRO_INTEGRATION_FACTOR */ + driftComp[axis]; |
209 | rate_PID[axis] = gyro_PID[axis] /* / HIRES_GYRO_INTEGRATION_FACTOR */ + driftComp[axis]; |
| 208 | rate_ATT[axis] = gyro_ATT[axis] /* / HIRES_GYRO_INTEGRATION_FACTOR */ + driftComp[axis]; |
210 | rate_ATT[axis] = gyro_ATT[axis] /* / HIRES_GYRO_INTEGRATION_FACTOR */ + driftComp[axis]; |
| 209 | differential[axis] = gyroD[axis]; |
211 | differential[axis] = gyroD[axis]; |
| Line 214... | Line 216... | ||
| 214 | yawRate = yawGyro + driftCompYaw; |
216 | yawRate = yawGyro + driftCompYaw; |
| Line 215... | Line 217... | ||
| 215 | 217 | ||
| 216 | // We are done reading variables from the analog module. |
218 | // We are done reading variables from the analog module. |
| 217 | // Interrupt-driven sensor reading may restart. |
219 | // Interrupt-driven sensor reading may restart. |
| 218 | analogDataReady = 0; |
220 | analogDataReady = 0; |
| 219 | startAnalogConversionCycle(0); |
221 | startAnalogConversionCycle(); |
| Line 220... | Line 222... | ||
| 220 | } |
222 | } |
| 221 | 223 | ||
| 222 | /* |
224 | /* |
| Line 293... | Line 295... | ||
| 293 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
295 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
| 294 | // are less than ....., or reintroduce Kalman. |
296 | // are less than ....., or reintroduce Kalman. |
| 295 | // Well actually the Z axis acc. check is not so silly. |
297 | // Well actually the Z axis acc. check is not so silly. |
| 296 | uint8_t axis; |
298 | uint8_t axis; |
| 297 | int32_t temp; |
299 | int32_t temp; |
| 298 | DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER; |
300 | debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
| 299 | DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER; |
301 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
| Line 300... | Line 302... | ||
| 300 | 302 | ||
| 301 | if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] |
303 | if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] |
| Line 302... | Line 304... | ||
| 302 | <= dynamicParams.UserParams[7]) { |
304 | <= dynamicParams.UserParams[7]) { |
| Line 308... | Line 310... | ||
| 308 | if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
310 | if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active |
| 309 | permilleAcc /= 2; |
311 | permilleAcc /= 2; |
| 310 | debugFullWeight = 0; |
312 | debugFullWeight = 0; |
| 311 | } |
313 | } |
| Line 312... | Line -... | ||
| 312 | - | ||
| 313 | /* |
- | |
| 314 | 314 | ||
| 315 | if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
315 | if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity. |
| 316 | permilleAcc /= 2; |
316 | permilleAcc /= 2; |
| 317 | debugFullWeight = 0; |
317 | debugFullWeight = 0; |
| Line 318... | Line 318... | ||
| 318 | */ |
318 | */ |
| 319 | 319 | ||
| 320 | if (controlActivity > 10000) { // reduce effect during stick commands |
320 | if (controlActivity > 10000) { // reduce effect during stick commands |
| 321 | permilleAcc /= 4; |
321 | permilleAcc /= 4; |
| 322 | DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
322 | debugOut.digital[0] |= DEBUG_ACC0THORDER; |
| 323 | if (controlActivity > 20000) { // reduce effect during stick commands |
323 | if (controlActivity > 20000) { // reduce effect during stick commands |
| 324 | permilleAcc /= 4; |
324 | permilleAcc /= 4; |
| 325 | DebugOut.Digital[1] |= DEBUG_ACC0THORDER; |
325 | debugOut.digital[1] |= DEBUG_ACC0THORDER; |
| Line 326... | Line 326... | ||
| 326 | } |
326 | } |
| 327 | } |
327 | } |
| 328 | 328 | ||
| 329 | /* |
329 | /* |
| 330 | * Add to each sum: The amount by which the angle is changed just below. |
330 | * Add to each sum: The amount by which the angle is changed just below. |
| 331 | */ |
331 | */ |
| Line 332... | Line 332... | ||
| 332 | for (axis = PITCH; axis <= ROLL; axis++) { |
332 | for (axis = PITCH; axis <= ROLL; axis++) { |
| 333 | accDerived = getAngleEstimateFromAcc(axis); |
333 | accDerived = getAngleEstimateFromAcc(axis); |
| 334 | DebugOut.Analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL; |
334 | debugOut.analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL; |
| 335 | 335 | ||
| 336 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
336 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
| 337 | temp = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000; |
337 | temp = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000; |
| 338 | angle[axis] = ((int32_t) (1000L - permilleAcc) * temp |
338 | angle[axis] = ((int32_t) (1000L - permilleAcc) * temp |
| 339 | + (int32_t) permilleAcc * accDerived) / 1000L; |
339 | + (int32_t) permilleAcc * accDerived) / 1000L; |
| 340 | correctionSum[axis] += angle[axis] - temp; |
340 | correctionSum[axis] += angle[axis] - temp; |
| Line 341... | Line -... | ||
| 341 | } |
- | |
| 342 | } else { |
- | |
| 343 | DebugOut.Analog[9] = 0; |
341 | } |
| 344 | DebugOut.Analog[10] = 0; |
342 | } else { |
| 345 | 343 | debugOut.analog[9] = 0; |
|
| 346 | DebugOut.Analog[16] = 0; |
344 | debugOut.analog[10] = 0; |
| Line 379... | Line 377... | ||
| 379 | deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
377 | deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
| 380 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
378 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
| 381 | driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
379 | driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
| 382 | CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
380 | CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
| 383 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
381 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
| 384 | DebugOut.Analog[16 + axis] = correctionSum[axis]; |
382 | // DebugOut.Analog[16 + axis] = correctionSum[axis]; |
| 385 | DebugOut.Analog[28 + axis] = driftComp[axis]; |
383 | debugOut.analog[28 + axis] = driftComp[axis]; |
| Line 386... | Line 384... | ||
| 386 | 384 | ||
| 387 | correctionSum[axis] = 0; |
385 | correctionSum[axis] = 0; |
| 388 | } |
386 | } |
| 389 | } |
387 | } |
| Line 394... | Line 392... | ||
| 394 | ************************************************************************/ |
392 | ************************************************************************/ |
| 395 | void calculateFlightAttitude(void) { |
393 | void calculateFlightAttitude(void) { |
| 396 | getAnalogData(); |
394 | getAnalogData(); |
| 397 | integrate(); |
395 | integrate(); |
| Line 398... | Line -... | ||
| 398 | - | ||
| 399 | DebugOut.Analog[3] = rate_PID[PITCH]; |
- | |
| 400 | DebugOut.Analog[4] = rate_PID[ROLL]; |
- | |
| 401 | DebugOut.Analog[5] = yawRate; |
- | |
| 402 | 396 | ||
| 403 | #ifdef ATTITUDE_USE_ACC_SENSORS |
397 | #ifdef ATTITUDE_USE_ACC_SENSORS |
| 404 | correctIntegralsByAcc0thOrder(); |
398 | correctIntegralsByAcc0thOrder(); |
| 405 | driftCorrection(); |
399 | driftCorrection(); |
| 406 | #endif |
400 | #endif |