| 45,9 → 45,9 |
|---|
| |
| void flight_updateFlightParametersToFlightMode(void) { |
| debugOut.analog[16] = currentFlightMode; |
| reverse[PITCH] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_ELEVATOR; |
| reverse[ROLL] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_AILERONS; |
| reverse[YAW] = staticParams.controlServosReverse & CONTROL_SERVO_REVERSE_RUDDER; |
| 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! |
| 64,7 → 64,7 |
|---|
| |
| // Normal at airspeed = 10. |
| uint8_t calcAirspeedPID(uint8_t pid) { |
| if (staticParams.bitConfig & CFG_USE_AIRSPEED_PID) { |
| if (!(staticParams.bitConfig & CFG_USE_AIRSPEED_PID)) { |
| return pid; |
| } |
| |
| 85,6 → 85,11 |
|---|
| } |
| } |
| |
| #define LOG_STICK_SCALE 8 |
| #define LOG_P_SCALE 6 |
| #define LOG_I_SCALE 10 |
| #define LOG_D_SCALE 6 |
| |
| /************************************************************************/ |
| /* Main Flight Control */ |
| /************************************************************************/ |
| 107,9 → 112,9 |
|---|
| term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE]; |
| |
| // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway. |
| target[PITCH] += ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> 7; |
| target[ROLL] += ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> 7; |
| target[YAW] += ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> 7; |
| target[PITCH] += ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE; |
| target[ROLL] += ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE; |
| target[YAW] += ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE; |
| |
| for (axis = PITCH; axis <= YAW; axis++) { |
| if (target[axis] > OVER180) { |
| 119,32 → 124,25 |
|---|
| } |
| |
| if (reverse[axis]) |
| error[axis] = attitude[axis] + target[axis]; |
| else |
| error[axis] = attitude[axis] - target[axis]; |
| error[axis] = target[axis] - attitude[axis]; |
| else |
| error[axis] = attitude[axis] - target[axis]; |
| |
| if (error[axis] > maxError[axis]) { |
| error[axis] = maxError[axis]; |
| error[axis] = maxError[axis]; |
| } else if (error[axis] < -maxError[axis]) { |
| error[axis] =- maxError[axis]; |
| error[axis] =- maxError[axis]; |
| } else { |
| // update I parts here for angles mode. Ĩ parts in rate mode is something different. |
| } |
| |
| #define LOG_P_SCALE 6 |
| #define LOG_I_SCALE 6 |
| #define LOG_D_SCALE 4 |
| |
| /************************************************************************/ |
| /* Calculate control feedback from angle (gyro integral) */ |
| /* and angular velocity (gyro signal) */ |
| /************************************************************************/ |
| if (currentFlightMode == FLIGHT_MODE_ANGLES || currentFlightMode |
| == FLIGHT_MODE_RATE) { |
| PDPart[axis] = (((int32_t) gyro_PID[axis] |
| * (int16_t) airspeedPID[axis].P) >> LOG_P_SCALE) |
| + ((gyroD[axis] * (int16_t) airspeedPID[axis].D) |
| >> LOG_D_SCALE); |
| 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 { |
| 152,9 → 150,7 |
|---|
| } |
| |
| if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
| int16_t anglePart = (int32_t)( |
| error[axis] * (int32_t) airspeedPID[axis].I) |
| >> LOG_I_SCALE; |
| int16_t anglePart = (int32_t)(error[axis] * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE; |
| if (reverse[axis]) |
| PDPart[axis] += anglePart; |
| else |
| 162,7 → 158,7 |
|---|
| } |
| |
| // Add I parts here... these are integrated errors. |
| term[axis] = controls[axis] + PDPart[axis] + IPart[axis]; |
| term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis]; |
| } |
| |
| debugOut.analog[12] = term[PITCH]; |
| 173,7 → 169,7 |
|---|
| for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) { |
| int16_t tmp; |
| if (servoTestActive) { |
| controlServos[i] = ((int16_t) servoTest[i] - 128) * 4; |
| controlServos[i] = ((int16_t) servoTest[i] - 128) * 8; |
| } else { |
| // Follow the normal order of servos: Ailerons, elevator, throttle, rudder. |
| switch (i) { |