Rev 2095 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2095 | Rev 2160 | ||
|---|---|---|---|
| Line 226... | Line 226... | ||
| 226 | // are less than ....., or reintroduce Kalman. |
226 | // are less than ....., or reintroduce Kalman. |
| 227 | // Well actually the Z axis acc. check is not so silly. |
227 | // Well actually the Z axis acc. check is not so silly. |
| 228 | uint8_t axis; |
228 | uint8_t axis; |
| 229 | int32_t temp; |
229 | int32_t temp; |
| Line 230... | Line 230... | ||
| 230 | 230 | ||
| 231 | debugOut.analog[13] = gyroActivity / 65536L; |
- | |
| 232 | - | ||
| 233 | uint16_t ca; |
- | |
| 234 | ca = gyroActivity >> 12; |
- | |
| Line -... | Line 231... | ||
| - | 231 | debugOut.analog[12] = IMUConfig.zerothOrderCorrection; |
|
| 235 | debugOut.analog[14] = ca; |
232 | |
| - | 233 | uint16_t ca = gyroActivity >> 14; |
|
| - | 234 | uint8_t gyroActivityWeighted = ca / IMUConfig.rateTolerance; |
|
| 236 | 235 | debugOut.analog[15] = gyroActivityWeighted; |
|
| Line 237... | Line 236... | ||
| 237 | uint8_t gyroActivityWeighted = ca / IMUConfig.rateTolerance; |
236 | |
| Line 238... | Line 237... | ||
| 238 | if (!gyroActivityWeighted) gyroActivityWeighted = 1; |
237 | if (!gyroActivityWeighted) gyroActivityWeighted = 1; |
| 239 | - | ||
| 240 | uint8_t accPart = IMUConfig.zerothOrderCorrection / gyroActivityWeighted; |
238 | |
| 241 | 239 | uint8_t accPart = IMUConfig.zerothOrderCorrection / gyroActivityWeighted; |
|
| Line 242... | Line 240... | ||
| 242 | debugOut.analog[28] = IMUConfig.rateTolerance; |
240 | |
| 243 | debugOut.analog[15] = gyroActivityWeighted; |
241 | debugOut.analog[28] = IMUConfig.rateTolerance; |
| Line 293... | Line 291... | ||
| 293 | deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
291 | deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
| 294 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
292 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
| 295 | driftComp[axis] += deltaCorrection / IMUConfig.driftCompDivider; |
293 | driftComp[axis] += deltaCorrection / IMUConfig.driftCompDivider; |
| 296 | CHECK_MIN_MAX(driftComp[axis], -IMUConfig.driftCompLimit, IMUConfig.driftCompLimit); |
294 | CHECK_MIN_MAX(driftComp[axis], -IMUConfig.driftCompLimit, IMUConfig.driftCompLimit); |
| 297 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
295 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
| 298 | // DebugOut.Analog[16 + axis] = correctionSum[axis]; |
296 | debugOut.analog[6 + axis] = correctionSum[axis]; |
| 299 | // debugOut.analog[28 + axis] = driftComp[axis]; |
297 | debugOut.analog[13 + axis] = driftComp[axis]; |
| 300 | correctionSum[axis] = 0; |
298 | correctionSum[axis] = 0; |
| 301 | } |
299 | } |
| 302 | } |
300 | } |
| 303 | } |
301 | } |