Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 1954 → Rev 1955

/branches/dongfang_FC_rewrite/flight.c
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;
}
}