Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2141 → Rev 2142

/branches/dongfang_FC_fixedwing/flight.c
23,7 → 23,6
* Error integrals.
*/
 
uint8_t reverse[3];
int32_t maxError[3];
int32_t IPart[3] = { 0, 0, 0 };
PID_t airspeedPID[3];
44,10 → 43,6
 
void flight_updateFlightParametersToFlightMode(void) {
debugOut.analog[16] = currentFlightMode;
reverse[PITCH] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_ELEVATOR;
reverse[ROLL] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_AILERONS;
reverse[YAW] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_RUDDER;
 
// At a switch to angles, we want to kill errors first.
// This should be triggered only once per mode change!
85,11 → 80,6
}
}
 
#define LOG_STICK_SCALE 8
#define LOG_P_SCALE 6
#define LOG_I_SCALE 10
#define LOG_D_SCALE 6
 
/************************************************************************/
/* Main Flight Control */
/************************************************************************/
115,13 → 105,13
int32_t tmp;
 
tmp = ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE;
if (reverse[PITCH]) target[PITCH] += tmp; else target[PITCH] -= tmp;
if (staticParams.servos[PITCH].reverse) target[PITCH] += tmp; else target[PITCH] -= tmp;
 
tmp = ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE;
if (reverse[ROLL]) target[ROLL] += tmp; else target[ROLL] -= tmp;
if (staticParams.servos[ROLL].reverse) target[ROLL] += tmp; else target[ROLL] -= tmp;
 
tmp = ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE;
if (reverse[YAW]) target[YAW] += tmp; else target[YAW] -= tmp;
if (staticParams.servos[YAW].reverse) target[YAW] += tmp; else target[YAW] -= tmp;
 
for (axis = PITCH; axis <= YAW; axis++) {
if (target[axis] > OVER180) {
192,8 → 182,6
if (currentFlightMode == FLIGHT_MODE_ANGLES || currentFlightMode == FLIGHT_MODE_RATE) {
PDPart[axis] = +(((int32_t) gyro_PID[axis] * (int32_t) airspeedPID[axis].P) >> LOG_P_SCALE)
+ (((int16_t)gyroD[axis] * (int16_t) airspeedPID[axis].D) >> LOG_D_SCALE);
//if (reverse[axis])
// PDPart[axis] = -PDPart[axis];
} else {
PDPart[axis] = 0;
}
204,17 → 192,12
}
 
// Add I parts here... these are integrated errors.
if (reverse[axis])
if (staticParams.servos[axis].reverse)
term[axis] = controls[axis] - PDPart[axis]; // + IPart[axis];
else
term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis];
}
 
debugOut.analog[12] = term[PITCH];
debugOut.analog[13] = term[ROLL];
debugOut.analog[14] = term[YAW];
debugOut.analog[15] = term[THROTTLE];
 
for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) {
int16_t tmp;
if (servoTestActive) {
264,5 → 247,6
debugOut.analog[12] = term[PITCH];
debugOut.analog[13] = term[ROLL];
debugOut.analog[14] = term[YAW];
debugOut.analog[15] = term[THROTTLE];
}
}