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