134,9 → 134,12 |
} |
|
void setNormalFlightParameters(void) { |
setFlightParameters(dynamicParams.IFactor, dynamicParams.GyroP, |
setFlightParameters(dynamicParams.IFactor, |
dynamicParams.GyroP, |
staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI, |
dynamicParams.GyroP, dynamicParams.UserParams[6]); |
dynamicParams.GyroP, |
dynamicParams.UserParams[6] |
); |
} |
|
void setStableFlightParameters(void) { |
355,22 → 358,22 |
#define MIN_YAWGAS (40 * CONTROL_SCALING) // yaw also below this gas value |
yawTerm = PDPartYaw - controls[CONTROL_YAW] * CONTROL_SCALING; |
// Limit yawTerm |
DebugOut.Digital[0] &= ~DEBUG_CLIP; |
debugOut.digital[0] &= ~DEBUG_CLIP; |
if (throttleTerm > MIN_YAWGAS) { |
if (yawTerm < -throttleTerm / 2) { |
DebugOut.Digital[0] |= DEBUG_CLIP; |
debugOut.digital[0] |= DEBUG_CLIP; |
yawTerm = -throttleTerm / 2; |
} else if (yawTerm > throttleTerm / 2) { |
DebugOut.Digital[0] |= DEBUG_CLIP; |
debugOut.digital[0] |= DEBUG_CLIP; |
yawTerm = throttleTerm / 2; |
} |
//CHECK_MIN_MAX(yawTerm, - (throttleTerm / 2), (throttleTerm / 2)); |
} else { |
if (yawTerm < -MIN_YAWGAS / 2) { |
DebugOut.Digital[0] |= DEBUG_CLIP; |
debugOut.digital[0] |= DEBUG_CLIP; |
yawTerm = -MIN_YAWGAS / 2; |
} else if (yawTerm > MIN_YAWGAS / 2) { |
DebugOut.Digital[0] |= DEBUG_CLIP; |
debugOut.digital[0] |= DEBUG_CLIP; |
yawTerm = MIN_YAWGAS / 2; |
} |
//CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
380,14 → 383,14 |
tmp_int = staticParams.MaxThrottle * CONTROL_SCALING; |
if (yawTerm < -(tmp_int - throttleTerm)) { |
yawTerm = -(tmp_int - throttleTerm); |
DebugOut.Digital[0] |= DEBUG_CLIP; |
debugOut.digital[0] |= DEBUG_CLIP; |
} else if (yawTerm > (tmp_int - throttleTerm)) { |
yawTerm = (tmp_int - throttleTerm); |
DebugOut.Digital[0] |= DEBUG_CLIP; |
debugOut.digital[0] |= DEBUG_CLIP; |
} |
|
// CHECK_MIN_MAX(yawTerm, -(tmp_int - throttleTerm), (tmp_int - throttleTerm)); |
DebugOut.Digital[1] &= ~DEBUG_CLIP; |
debugOut.digital[1] &= ~DEBUG_CLIP; |
for (axis = PITCH; axis <= ROLL; axis++) { |
/* |
* Compose pitch and roll terms. This is finally where the sticks come into play. |
418,9 → 421,9 |
* TODO: Why a growing function of yaw? |
*/ |
if (term[axis] < -tmp_int) { |
DebugOut.Digital[1] |= DEBUG_CLIP; |
debugOut.digital[1] |= DEBUG_CLIP; |
} else if (term[axis] > tmp_int) { |
DebugOut.Digital[1] |= DEBUG_CLIP; |
debugOut.digital[1] |= DEBUG_CLIP; |
} |
CHECK_MIN_MAX(term[axis], -tmp_int, tmp_int); |
} |
430,10 → 433,10 |
// Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING]. |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
DebugOut.Analog[12] = term[PITCH]; |
DebugOut.Analog[13] = term[ROLL]; |
//DebugOut.Analog[14] = yawTerm; |
DebugOut.Analog[15] = throttleTerm; |
debugOut.analog[12] = term[PITCH]; |
debugOut.analog[13] = term[ROLL]; |
debugOut.analog[14] = yawTerm; |
debugOut.analog[15] = throttleTerm; |
|
for (i = 0; i < MAX_MOTORS; i++) { |
int32_t tmp; |
458,9 → 461,9 |
CHECK_MIN_MAX(tmp, 1, 255); |
throttle = tmp; |
|
if (i < 4) DebugOut.Analog[22 + i] = throttle; |
if (i < 4) debugOut.analog[22 + i] = throttle; |
|
if (MKFlags & MKFLAG_MOTOR_RUN && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
if ((MKFlags & MKFLAG_MOTOR_RUN) && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
motor[i].SetPoint = throttle; |
} else if (motorTestActive) { |
motor[i].SetPoint = motorTest[i]; |
476,21 → 479,12 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if (!(--debugDataTimer)) { |
debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
DebugOut.Analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
DebugOut.Analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; |
debugOut.analog[0] = (10 * angle[PITCH]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
debugOut.analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
debugOut.analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; |
|
DebugOut.Analog[6] = 64 >> 4; |
DebugOut.Analog[7] = -64 >> 4; |
|
/* |
DebugOut.Analog[23] = (yawRate * 2 * (int32_t)yawPFactor) / (256L / CONTROL_SCALING); |
DebugOut.Analog[24] = controlYaw; |
DebugOut.Analog[25] = yawAngleDiff / 100L; |
DebugOut.Analog[26] = accNoisePeak[PITCH]; |
DebugOut.Analog[27] = accNoisePeak[ROLL]; |
DebugOut.Analog[30] = gyroNoisePeak[PITCH]; |
DebugOut.Analog[31] = gyroNoisePeak[ROLL]; |
*/ |
debugOut.analog[16] = gyroPFactor; |
debugOut.analog[17] = gyroIFactor; |
debugOut.analog[18] = dynamicParams.GyroD; |
} |
} |