Rev 1955 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1955 | Rev 1960 | ||
---|---|---|---|
Line 171... | Line 171... | ||
171 | /************************************************************************ |
171 | /************************************************************************ |
172 | * Neutral Readings |
172 | * Neutral Readings |
173 | ************************************************************************/ |
173 | ************************************************************************/ |
174 | void attitude_setNeutral(void) { |
174 | void attitude_setNeutral(void) { |
175 | // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway. |
175 | // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway. |
176 | dynamicParams.AxisCoupling1 = dynamicParams.AxisCoupling2 = 0; |
176 | dynamicParams.axisCoupling1 = dynamicParams.axisCoupling2 = 0; |
Line 177... | Line 177... | ||
177 | 177 | ||
178 | driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0; |
178 | driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0; |
Line 179... | Line 179... | ||
179 | correctionSum[PITCH] = correctionSum[ROLL] = 0; |
179 | correctionSum[PITCH] = correctionSum[ROLL] = 0; |
Line 215... | Line 215... | ||
215 | averageAccCount++; |
215 | averageAccCount++; |
216 | yawRate = yawGyro + driftCompYaw; |
216 | yawRate = yawGyro + driftCompYaw; |
Line 217... | Line 217... | ||
217 | 217 | ||
218 | // We are done reading variables from the analog module. |
218 | // We are done reading variables from the analog module. |
219 | // Interrupt-driven sensor reading may restart. |
- | |
220 | analogDataReady = 0; |
219 | // Interrupt-driven sensor reading may restart. |
221 | startAnalogConversionCycle(); |
220 | startAnalogConversionCycle(); |
Line 222... | Line 221... | ||
222 | } |
221 | } |
223 | 222 | ||
Line 247... | Line 246... | ||
247 | // 480 usec with axis coupling - almost no time without. |
246 | // 480 usec with axis coupling - almost no time without. |
248 | void integrate(void) { |
247 | void integrate(void) { |
249 | // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
248 | // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
250 | uint8_t axis; |
249 | uint8_t axis; |
Line 251... | Line 250... | ||
251 | 250 | ||
252 | if (!looping && (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) { |
251 | if (/*!looping && */ (staticParams.bitConfig & CFG_AXIS_COUPLING_ACTIVE)) { |
253 | trigAxisCoupling(); |
252 | trigAxisCoupling(); |
254 | } else { |
253 | } else { |
255 | ACRate[PITCH] = rate_ATT[PITCH]; |
254 | ACRate[PITCH] = rate_ATT[PITCH]; |
256 | ACRate[ROLL] = rate_ATT[ROLL]; |
255 | ACRate[ROLL] = rate_ATT[ROLL]; |
Line 298... | Line 297... | ||
298 | uint8_t axis; |
297 | uint8_t axis; |
299 | int32_t temp; |
298 | int32_t temp; |
300 | debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
299 | debugOut.digital[0] &= ~DEBUG_ACC0THORDER; |
301 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
300 | debugOut.digital[1] &= ~DEBUG_ACC0THORDER; |
Line 302... | Line 301... | ||
302 | 301 | ||
303 | if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] |
- | |
304 | <= dynamicParams.UserParams[7]) { |
- | |
305 | 302 | if (1 /*controlActivity <= dynamicParams.maxControlActivityForAcc*/) { |
|
306 | uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!! |
303 | uint8_t permilleAcc = staticParams.zerothOrderCorrection; |
Line 307... | Line 304... | ||
307 | int32_t accDerived; |
304 | int32_t accDerived; |
308 | 305 | ||
309 | /* |
306 | /* |
Line 374... | Line 371... | ||
374 | round = DRIFTCORRECTION_TIME / 2; |
371 | round = DRIFTCORRECTION_TIME / 2; |
375 | else |
372 | else |
376 | round = -DRIFTCORRECTION_TIME / 2; |
373 | round = -DRIFTCORRECTION_TIME / 2; |
377 | deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
374 | deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
378 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
375 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
379 | driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
376 | driftComp[axis] += deltaCorrection / staticParams.driftCompDivider; |
380 | CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
377 | CHECK_MIN_MAX(driftComp[axis], -staticParams.driftCompLimit, staticParams.driftCompLimit); |
381 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
378 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
382 | // DebugOut.Analog[16 + axis] = correctionSum[axis]; |
379 | // DebugOut.Analog[16 + axis] = correctionSum[axis]; |
383 | debugOut.analog[28 + axis] = driftComp[axis]; |
380 | debugOut.analog[28 + axis] = driftComp[axis]; |
Line 384... | Line 381... | ||
384 | 381 | ||
Line 442... | Line 439... | ||
442 | 439 | ||
443 | /* |
440 | /* |
444 | w = (w * dynamicParams.CompassYawEffect) / 32; |
441 | w = (w * dynamicParams.CompassYawEffect) / 32; |
445 | w = dynamicParams.CompassYawEffect - w; |
442 | w = dynamicParams.CompassYawEffect - w; |
446 | */ |
443 | */ |
447 | w = dynamicParams.CompassYawEffect - (w * dynamicParams.CompassYawEffect) |
444 | w = dynamicParams.compassYawEffect - (w * dynamicParams.compassYawEffect) |
Line 448... | Line 445... | ||
448 | / 32; |
445 | / 32; |
449 | 446 | ||
Line 458... | Line 455... | ||
458 | r |
455 | r |
459 | = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) |
456 | = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) |
460 | % 360) - 180; |
457 | % 360) - 180; |
461 | v = (r * w) / v; // align to compass course |
458 | v = (r * w) / v; // align to compass course |
462 | // limit yaw rate |
459 | // limit yaw rate |
463 | w = 3 * dynamicParams.CompassYawEffect; |
460 | w = 3 * dynamicParams.compassYawEffect; |
464 | if (v > w) |
461 | if (v > w) |
465 | v = w; |
462 | v = w; |
466 | else if (v < -w) |
463 | else if (v < -w) |
467 | v = -w; |
464 | v = -w; |
468 | yawAngleDiff += v; |
465 | yawAngleDiff += v; |