Rev 1873 | Rev 1908 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1873 | Rev 1874 | ||
|---|---|---|---|
| Line 434... | Line 434... | ||
| 434 | DebugOut.Analog[13] = term[ROLL]; |
434 | DebugOut.Analog[13] = term[ROLL]; |
| 435 | DebugOut.Analog[14] = yawTerm; |
435 | DebugOut.Analog[14] = yawTerm; |
| 436 | DebugOut.Analog[15] = throttleTerm; |
436 | DebugOut.Analog[15] = throttleTerm; |
| Line 437... | Line 437... | ||
| 437 | 437 | ||
| 438 | for (i = 0; i < MAX_MOTORS; i++) { |
438 | for (i = 0; i < MAX_MOTORS; i++) { |
| 439 | int16_t tmp; |
439 | int32_t tmp; |
| 440 | if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
440 | if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
| 441 | tmp = (int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE] / 64L; |
441 | tmp = (int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]; |
| 442 | tmp += (int32_t)term[PITCH] * Mixer.Motor[i][MIX_PITCH] / 64L; |
442 | tmp += (int32_t)term[PITCH] * Mixer.Motor[i][MIX_PITCH]; |
| 443 | tmp += (int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL] / 64L; |
443 | tmp += (int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL]; |
| 444 | tmp += (int32_t)yawTerm * Mixer.Motor[i][MIX_YAW] / 64L; |
444 | tmp += (int32_t)yawTerm * Mixer.Motor[i][MIX_YAW]; |
| 445 | // tmp = tmp >> 6; |
445 | tmp = tmp >> 6; |
| 446 | motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
446 | motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
| 447 | // Now we scale back down to a 0..255 range. |
447 | // Now we scale back down to a 0..255 range. |
| Line 448... | Line 448... | ||
| 448 | tmp = motorFilters[i] / MOTOR_SCALING; |
448 | tmp = motorFilters[i] / MOTOR_SCALING; |
| Line 474... | Line 474... | ||
| 474 | debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
474 | debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
| 475 | DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
475 | DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
| 476 | DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
476 | DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
| 477 | DebugOut.Analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; |
477 | DebugOut.Analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; |
| Line -... | Line 478... | ||
| - | 478 | ||
| - | 479 | DebugOut.Analog[6] = 64 >> 4; |
|
| - | 480 | DebugOut.Analog[7] = -64 >> 4; |
|
| 478 | 481 | ||
| 479 | /* |
482 | /* |
| 480 | DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING); |
483 | DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING); |
| 481 | DebugOut.Analog[24] = controlYaw; |
484 | DebugOut.Analog[24] = controlYaw; |
| 482 | DebugOut.Analog[25] = yawAngleDiff / 100L; |
485 | DebugOut.Analog[25] = yawAngleDiff / 100L; |