Rev 1821 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1821 | Rev 1864 | ||
|---|---|---|---|
| Line 154... | Line 154... | ||
| 154 | * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
154 | * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
| 155 | * it is hardly worth the trouble. |
155 | * it is hardly worth the trouble. |
| 156 | ************************************************************************/ |
156 | ************************************************************************/ |
| Line 157... | Line 157... | ||
| 157 | 157 | ||
| 158 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
158 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
| 159 | return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; |
159 | return GYRO_ACC_FACTOR * (int32_t)filteredAcc[axis]; |
| Line 160... | Line 160... | ||
| 160 | } |
160 | } |
| 161 | 161 | ||
| 162 | void setStaticAttitudeAngles(void) { |
162 | void setStaticAttitudeAngles(void) { |
| Line 179... | Line 179... | ||
| 179 | correctionSum[PITCH] = correctionSum[ROLL] = 0; |
179 | correctionSum[PITCH] = correctionSum[ROLL] = 0; |
| Line 180... | Line 180... | ||
| 180 | 180 | ||
| 181 | // Calibrate hardware. |
181 | // Calibrate hardware. |
| Line 182... | Line -... | ||
| 182 | analog_calibrate(); |
- | |
| 183 | - | ||
| 184 | // reset gyro readings |
- | |
| 185 | // rate_ATT[PITCH] = rate_ATT[ROLL] = yawRate = 0; |
182 | analog_calibrate(); |
| 186 | 183 | ||
| 187 | // reset gyro integrals to acc guessing |
184 | // reset gyro integrals to acc guessing |
| Line 188... | Line 185... | ||
| 188 | setStaticAttitudeAngles(); |
185 | setStaticAttitudeAngles(); |
| Line 205... | Line 202... | ||
| 205 | *************************************************************************/ |
202 | *************************************************************************/ |
| 206 | void getAnalogData(void) { |
203 | void getAnalogData(void) { |
| 207 | uint8_t axis; |
204 | uint8_t axis; |
| Line 208... | Line 205... | ||
| 208 | 205 | ||
| 209 | for (axis = PITCH; axis <= ROLL; axis++) { |
206 | for (axis = PITCH; axis <= ROLL; axis++) { |
| 210 | rate_PID[axis] = (gyro_PID[axis] + driftComp[axis]) |
- | |
| 211 | / HIRES_GYRO_INTEGRATION_FACTOR; |
207 | rate_PID[axis] = gyro_PID[axis] / HIRES_GYRO_INTEGRATION_FACTOR + driftComp[axis]; |
| 212 | rate_ATT[axis] = (gyro_ATT[axis] + driftComp[axis]) |
- | |
| 213 | / HIRES_GYRO_INTEGRATION_FACTOR; |
208 | rate_ATT[axis] = gyro_ATT[axis] / HIRES_GYRO_INTEGRATION_FACTOR + driftComp[axis]; |
| 214 | differential[axis] = gyroD[axis]; |
209 | differential[axis] = gyroD[axis]; |
| 215 | averageAcc[axis] += acc[axis]; |
210 | averageAcc[axis] += acc[axis]; |
| Line 216... | Line 211... | ||
| 216 | } |
211 | } |
| Line 295... | Line 290... | ||
| 295 | void correctIntegralsByAcc0thOrder(void) { |
290 | void correctIntegralsByAcc0thOrder(void) { |
| 296 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
291 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
| 297 | // are less than ....., or reintroduce Kalman. |
292 | // are less than ....., or reintroduce Kalman. |
| 298 | // Well actually the Z axis acc. check is not so silly. |
293 | // Well actually the Z axis acc. check is not so silly. |
| 299 | uint8_t axis; |
294 | uint8_t axis; |
| 300 | int32_t correction; |
295 | int32_t temp; |
| 301 | if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] |
296 | if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] |
| 302 | <= dynamicParams.UserParams[7]) { |
297 | <= dynamicParams.UserParams[7]) { |
| 303 | DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
298 | DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
| Line 304... | Line 299... | ||
| 304 | 299 | ||
| Line 327... | Line 322... | ||
| 327 | for (axis = PITCH; axis <= ROLL; axis++) { |
322 | for (axis = PITCH; axis <= ROLL; axis++) { |
| 328 | accDerived = getAngleEstimateFromAcc(axis); |
323 | accDerived = getAngleEstimateFromAcc(axis); |
| 329 | DebugOut.Analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL; |
324 | DebugOut.Analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL; |
| Line 330... | Line 325... | ||
| 330 | 325 | ||
| 331 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
326 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
| 332 | correction = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000; |
327 | temp = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000; |
| 333 | angle[axis] = ((int32_t) (1000L - permilleAcc) * angle[axis] |
328 | angle[axis] = ((int32_t) (1000L - permilleAcc) * temp |
| 334 | + (int32_t) permilleAcc * accDerived) / 1000L; |
329 | + (int32_t) permilleAcc * accDerived) / 1000L; |
| 335 | correctionSum[axis] += angle[axis] - correction; |
- | |
| 336 | DebugOut.Analog[16 + axis] = angle[axis] - correction; |
330 | correctionSum[axis] += angle[axis] - temp; |
| 337 | } |
331 | } |
| 338 | } else { |
332 | } else { |
| 339 | DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER; |
333 | DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER; |
| 340 | DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER; |
334 | DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER; |
| Line 366... | Line 360... | ||
| 366 | uint8_t axis; |
360 | uint8_t axis; |
| 367 | if (!--timer) { |
361 | if (!--timer) { |
| 368 | timer = DRIFTCORRECTION_TIME; |
362 | timer = DRIFTCORRECTION_TIME; |
| 369 | for (axis = PITCH; axis <= ROLL; axis++) { |
363 | for (axis = PITCH; axis <= ROLL; axis++) { |
| 370 | // Take the sum of corrections applied, add it to delta |
364 | // Take the sum of corrections applied, add it to delta |
| 371 | deltaCorrection = (correctionSum[axis] * HIRES_GYRO_INTEGRATION_FACTOR |
365 | deltaCorrection = (correctionSum[axis] + DRIFTCORRECTION_TIME / 2) / DRIFTCORRECTION_TIME; |
| 372 | + DRIFTCORRECTION_TIME / 2) / DRIFTCORRECTION_TIME; |
- | |
| 373 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
366 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
| 374 | driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
367 | driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
| 375 | CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
368 | CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
| 376 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
369 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
| 377 | - | ||
| - | 370 | DebugOut.Analog[16 + axis] = correctionSum[axis]; |
|
| 378 | DebugOut.Analog[18 + axis] = deltaCorrection / staticParams.GyroAccTrim; |
371 | DebugOut.Analog[18 + axis] = deltaCorrection / staticParams.GyroAccTrim; |
| 379 | DebugOut.Analog[28 + axis] = driftComp[axis]; |
372 | DebugOut.Analog[28 + axis] = driftComp[axis]; |
| Line 380... | Line 373... | ||
| 380 | 373 | ||
| 381 | correctionSum[axis] = 0; |
374 | correctionSum[axis] = 0; |