Rev 2138 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2138 | Rev 2139 | ||
|---|---|---|---|
| Line 21... | Line 21... | ||
| 21 | 21 | ||
| 22 | /* |
22 | /* |
| 23 | * Error integrals. |
23 | * Error integrals. |
| Line 24... | Line -... | ||
| 24 | */ |
- | |
| 25 | 24 | */ |
|
| 26 | uint8_t reverse[3]; |
25 | |
| 27 | int32_t maxError[3]; |
26 | int32_t maxError[3]; |
| Line 28... | Line 27... | ||
| 28 | int32_t IPart[3] = { 0, 0, 0 }; |
27 | int32_t IPart[3] = { 0, 0, 0 }; |
| Line 43... | Line 42... | ||
| 43 | } |
42 | } |
| Line 44... | Line 43... | ||
| 44 | 43 | ||
| 45 | void flight_updateFlightParametersToFlightMode(void) { |
44 | void flight_updateFlightParametersToFlightMode(void) { |
| Line 46... | Line -... | ||
| 46 | debugOut.analog[16] = currentFlightMode; |
- | |
| 47 | - | ||
| 48 | reverse[PITCH] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_ELEVATOR; |
- | |
| 49 | reverse[ROLL] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_AILERONS; |
- | |
| 50 | reverse[YAW] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_RUDDER; |
45 | debugOut.analog[16] = currentFlightMode; |
| 51 | 46 | ||
| 52 | // At a switch to angles, we want to kill errors first. |
47 | // At a switch to angles, we want to kill errors first. |
| 53 | // This should be triggered only once per mode change! |
48 | // This should be triggered only once per mode change! |
| 54 | if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
49 | if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
| Line 100... | Line 95... | ||
| 100 | 95 | ||
| 101 | // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway. |
96 | // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway. |
| Line 102... | Line 97... | ||
| 102 | int32_t tmp; |
97 | int32_t tmp; |
| 103 | 98 | ||
| Line 104... | Line 99... | ||
| 104 | tmp = ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE; |
99 | tmp = ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> LOG_STICK_SCALE; |
| 105 | if (reverse[PITCH]) target[PITCH] += tmp; else target[PITCH] -= tmp; |
100 | if (staticParams.servos[PITCH].reverse) target[PITCH] += tmp; else target[PITCH] -= tmp; |
| Line 106... | Line 101... | ||
| 106 | 101 | ||
| 107 | tmp = ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE; |
102 | tmp = ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> LOG_STICK_SCALE; |
| Line 108... | Line 103... | ||
| 108 | if (reverse[ROLL]) target[ROLL] += tmp; else target[ROLL] -= tmp; |
103 | if (staticParams.servos[ROLL].reverse) target[ROLL] += tmp; else target[ROLL] -= tmp; |
| 109 | 104 | ||
| 110 | tmp = ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE; |
105 | tmp = ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> LOG_STICK_SCALE; |
| 111 | if (reverse[YAW]) target[YAW] += tmp; else target[YAW] -= tmp; |
106 | if (staticParams.servos[YAW].reverse) target[YAW] += tmp; else target[YAW] -= tmp; |
| Line 190... | Line 185... | ||
| 190 | int16_t anglePart = (int32_t)(error * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE; |
185 | int16_t anglePart = (int32_t)(error * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE; |
| 191 | PDPart[axis] += anglePart; |
186 | PDPart[axis] += anglePart; |
| 192 | } |
187 | } |
| Line 193... | Line 188... | ||
| 193 | 188 | ||
| 194 | // Add I parts here... these are integrated errors. |
189 | // Add I parts here... these are integrated errors. |
| 195 | if (reverse[axis]) |
190 | if (staticParams.servos[axis].reverse) |
| 196 | term[axis] = controls[axis] - PDPart[axis]; // + IPart[axis]; |
191 | term[axis] = controls[axis] - PDPart[axis]; // + IPart[axis]; |
| 197 | else |
192 | else |
| 198 | term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis]; |
193 | term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis]; |