Rev 1953 | 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 |