Rev 2089 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2089 | Rev 2092 | ||
|---|---|---|---|
| Line 1... | Line 1... | ||
| 1 | #include <stdlib.h> |
1 | #include <stdlib.h> |
| 2 | #include <avr/io.h> |
2 | #include <avr/io.h> |
| - | 3 | #include <stdlib.h> |
|
| Line 3... | Line 4... | ||
| 3 | 4 | ||
| 4 | #include "attitude.h" |
5 | #include "attitude.h" |
| 5 | #include "dongfangMath.h" |
6 | #include "dongfangMath.h" |
| Line 225... | Line 226... | ||
| 225 | // are less than ....., or reintroduce Kalman. |
226 | // are less than ....., or reintroduce Kalman. |
| 226 | // Well actually the Z axis acc. check is not so silly. |
227 | // Well actually the Z axis acc. check is not so silly. |
| 227 | uint8_t axis; |
228 | uint8_t axis; |
| 228 | int32_t temp; |
229 | int32_t temp; |
| Line 229... | Line 230... | ||
| 229 | 230 | ||
| 230 | uint16_t ca = gyroActivity >> 8; |
231 | uint16_t ca = gyroActivity >> 9; |
| Line 231... | Line 232... | ||
| 231 | debugOut.analog[14] = ca; |
232 | debugOut.analog[14] = ca; |
| 232 | 233 | ||
| Line 233... | Line 234... | ||
| 233 | uint8_t gyroActivityWeighted = ca / staticParams.rateTolerance; |
234 | uint8_t gyroActivityWeighted = ca / IMUConfig.rateTolerance; |
| Line -... | Line 235... | ||
| - | 235 | if (!gyroActivityWeighted) gyroActivityWeighted = 1; |
|
| 234 | if (!gyroActivityWeighted) gyroActivityWeighted = 1; |
236 | |
| 235 | 237 | uint8_t accPart = IMUConfig.zerothOrderCorrection / gyroActivityWeighted; |
|
| 236 | uint8_t accPart = staticParams.zerothOrderCorrection / gyroActivityWeighted; |
238 | |
| Line 237... | Line 239... | ||
| 237 | 239 | debugOut.analog[28] = IMUConfig.rateTolerance; |
|
| Line 285... | Line 287... | ||
| 285 | round = DRIFTCORRECTION_TIME / 2; |
287 | round = DRIFTCORRECTION_TIME / 2; |
| 286 | else |
288 | else |
| 287 | round = -DRIFTCORRECTION_TIME / 2; |
289 | round = -DRIFTCORRECTION_TIME / 2; |
| 288 | deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
290 | deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME; |
| 289 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
291 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
| 290 | driftComp[axis] += deltaCorrection / staticParams.driftCompDivider; |
292 | driftComp[axis] += deltaCorrection / IMUConfig.driftCompDivider; |
| 291 | CHECK_MIN_MAX(driftComp[axis], -staticParams.driftCompLimit, staticParams.driftCompLimit); |
293 | CHECK_MIN_MAX(driftComp[axis], -IMUConfig.driftCompLimit, IMUConfig.driftCompLimit); |
| 292 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
294 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
| 293 | // DebugOut.Analog[16 + axis] = correctionSum[axis]; |
295 | // DebugOut.Analog[16 + axis] = correctionSum[axis]; |
| 294 | // debugOut.analog[28 + axis] = driftComp[axis]; |
296 | // debugOut.analog[28 + axis] = driftComp[axis]; |
| 295 | correctionSum[axis] = 0; |
297 | correctionSum[axis] = 0; |
| 296 | } |
298 | } |