Rev 1989 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1989 | Rev 1990 | ||
|---|---|---|---|
| Line 153... | Line 153... | ||
| 153 | * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
153 | * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
| 154 | * it is hardly worth the trouble. |
154 | * it is hardly worth the trouble. |
| 155 | ************************************************************************/ |
155 | ************************************************************************/ |
| Line 156... | Line 156... | ||
| 156 | 156 | ||
| 157 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
157 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
| 158 | int16_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 512; |
158 | int32_t correctionTerm = (dynamicParams.levelCorrection[axis] - 128) * 256L; |
| 159 | return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis] + correctionTerm; |
159 | return GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis] + correctionTerm; |
| Line 160... | Line 160... | ||
| 160 | } |
160 | } |
| 161 | 161 | ||