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; |