Rev 2055 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2055 | Rev 2058 | ||
|---|---|---|---|
| Line 181... | Line 181... | ||
| 181 | // 480 usec with axis coupling - almost no time without. |
181 | // 480 usec with axis coupling - almost no time without. |
| 182 | void integrate(void) { |
182 | void integrate(void) { |
| 183 | // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
183 | // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
| 184 | uint8_t axis; |
184 | uint8_t axis; |
| Line 185... | Line 185... | ||
| 185 | 185 | ||
| 186 | if (staticParams.bitConfig & CFG_AXIS_COUPLING_ACTIVE) { |
186 | if (staticParams.bitConfig & CFG_AXIS_COUPLING_ENABLED) { |
| 187 | trigAxisCoupling(); |
187 | trigAxisCoupling(); |
| 188 | } else { |
188 | } else { |
| 189 | ACRate[PITCH] = rate_ATT[PITCH]; |
189 | ACRate[PITCH] = rate_ATT[PITCH]; |
| 190 | ACRate[ROLL] = rate_ATT[ROLL]; |
190 | ACRate[ROLL] = rate_ATT[ROLL]; |
| Line 196... | Line 196... | ||
| 196 | * Calculate yaw gyro integral (~ to rotation angle) |
196 | * Calculate yaw gyro integral (~ to rotation angle) |
| 197 | * Limit heading proportional to 0 deg to 360 deg |
197 | * Limit heading proportional to 0 deg to 360 deg |
| 198 | */ |
198 | */ |
| 199 | heading += ACYawRate; |
199 | heading += ACYawRate; |
| 200 | intervalWrap(&heading, YAWOVER360); |
200 | intervalWrap(&heading, YAWOVER360); |
| 201 | - | ||
| 202 | headingError += ACYawRate; |
201 | headingError += ACYawRate; |
| Line 203... | Line 202... | ||
| 203 | 202 | ||
| 204 | /* |
203 | /* |
| 205 | * Pitch axis integration and range boundary wrap. |
204 | * Pitch axis integration and range boundary wrap. |
| Line 324... | Line 323... | ||
| 324 | accVector = temp * temp; |
323 | accVector = temp * temp; |
| 325 | temp = filteredAcc[1] >> 3; |
324 | temp = filteredAcc[1] >> 3; |
| 326 | accVector += temp * temp; |
325 | accVector += temp * temp; |
| 327 | temp = filteredAcc[2] >> 3; |
326 | temp = filteredAcc[2] >> 3; |
| 328 | accVector += temp * temp; |
327 | accVector += temp * temp; |
| 329 | //debugOut.analog[18] = accVector; |
- | |
| 330 | } |
328 | } |
| Line 331... | Line 329... | ||
| 331 | 329 | ||
| 332 | #ifdef USE_MK3MAG |
330 | #ifdef USE_MK3MAG |
| 333 | void attitude_resetHeadingToMagnetic(void) { |
331 | void attitude_resetHeadingToMagnetic(void) { |
| Line 351... | Line 349... | ||
| 351 | 349 | ||
| 352 | void correctHeadingToMagnetic(void) { |
350 | void correctHeadingToMagnetic(void) { |
| Line 353... | Line 351... | ||
| 353 | int32_t error; |
351 | int32_t error; |
| 354 | 352 | ||
| 355 | if (commands_isCalibratingCompass()) { |
353 | if (commands_isCalibratingCompass()) { |
| 356 | //debugOut.analog[29] = 1; |
354 | debugOut.analog[30] = -1; |
| Line 357... | Line 355... | ||
| 357 | return; |
355 | return; |
| 358 | } |
356 | } |
| 359 | 357 | ||
| 360 | // Compass is off, skip. |
358 | // Compass is off, skip. |
| Line 361... | Line 359... | ||
| 361 | // Naaah this is assumed. |
359 | // Naaah this is assumed. |
| 362 | // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
360 | // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE)) |
| 363 | // return; |
361 | // return; |
| 364 | 362 | ||
| 365 | // Compass is invalid, skip. |
363 | // Compass is invalid, skip. |
| Line 366... | Line 364... | ||
| 366 | if (magneticHeading < 0) { |
364 | if (magneticHeading < 0) { |
| 367 | //debugOut.analog[29] = 2; |
365 | debugOut.analog[30] = -2; |
| 368 | return; |
366 | return; |
| 369 | } |
367 | } |
| 370 | 368 | ||
| Line 371... | Line 369... | ||
| 371 | // Spinning fast, skip |
369 | // Spinning fast, skip |
| 372 | if (abs(yawRate) > 128) { |
370 | if (abs(yawRate) > 128) { |
| 373 | //debugOut.analog[29] = 3; |
371 | debugOut.analog[30] = -3; |
| 374 | return; |
372 | return; |
| 375 | } |
373 | } |
| 376 | 374 | ||
| Line -... | Line 375... | ||
| - | 375 | // Otherwise invalidated, skip |
|
| - | 376 | if (ignoreCompassTimer) { |
|
| 377 | // Otherwise invalidated, skip |
377 | ignoreCompassTimer--; |
| 378 | if (ignoreCompassTimer) { |
378 | debugOut.analog[30] = -4; |
| 379 | ignoreCompassTimer--; |
379 | return; |
| 380 | //debugOut.analog[29] = 4; |
380 | } |