Rev 2109 | Rev 2119 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2109 | Rev 2118 | ||
|---|---|---|---|
| Line 105... | Line 105... | ||
| 105 | setAirspeedPIDs(); |
105 | setAirspeedPIDs(); |
| Line 106... | Line 106... | ||
| 106 | 106 | ||
| Line 107... | Line 107... | ||
| 107 | term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE]; |
107 | term[CONTROL_THROTTLE] = controls[CONTROL_THROTTLE]; |
| 108 | 108 | ||
| 109 | // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway. |
109 | // These params are just left the same in all modes. In MANUAL and RATE the results are ignored anyway. |
| 110 | target[PITCH] += (controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> 7; |
110 | target[PITCH] += ((int32_t)controls[CONTROL_ELEVATOR] * staticParams.stickIElevator) >> 7; |
| Line 111... | Line 111... | ||
| 111 | target[ROLL] += (controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> 7; |
111 | target[ROLL] += ((int32_t)controls[CONTROL_AILERONS] * staticParams.stickIAilerons) >> 7; |
| 112 | target[YAW] += (controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> 7; |
112 | target[YAW] += ((int32_t)controls[CONTROL_RUDDER] * staticParams.stickIRudder) >> 7; |
| 113 | 113 | ||
| 114 | for (axis = PITCH; axis <= YAW; axis++) { |
114 | for (axis = PITCH; axis <= YAW; axis++) { |
| Line 125... | Line 125... | ||
| 125 | 125 | ||
| 126 | if (error[axis] > maxError[axis]) { |
126 | if (error[axis] > maxError[axis]) { |
| 127 | error[axis] = maxError[axis]; |
127 | error[axis] = maxError[axis]; |
| 128 | } else if (error[axis] < -maxError[axis]) { |
128 | } else if (error[axis] < -maxError[axis]) { |
| - | 129 | error[axis] =- maxError[axis]; |
|
| - | 130 | } else { |
|
| 129 | error[axis] =- maxError[axis]; |
131 | // update I parts here for angles mode. Ĩ parts in rate mode is something different. |
| Line 130... | Line 132... | ||
| 130 | } |
132 | } |
| 131 | 133 | ||
| 132 | #define LOG_P_SCALE 6 |
134 | #define LOG_P_SCALE 6 |
| Line 156... | Line 158... | ||
| 156 | if (reverse[axis]) |
158 | if (reverse[axis]) |
| 157 | PDPart[axis] += anglePart; |
159 | PDPart[axis] += anglePart; |
| 158 | else |
160 | else |
| 159 | PDPart[axis] -= anglePart; |
161 | PDPart[axis] -= anglePart; |
| 160 | } |
162 | } |
| 161 | // Add I parts here... these are integrated errors. |
- | |
| 162 | // When an error wraps, actually its I part should be negated or something... |
- | |
| Line -... | Line 163... | ||
| - | 163 | ||
| 163 | 164 | // Add I parts here... these are integrated errors. |
|
| 164 | term[axis] = controls[axis] + PDPart[axis] + IPart[axis]; |
165 | term[axis] = controls[axis] + PDPart[axis] + IPart[axis]; |
| Line 165... | Line 166... | ||
| 165 | } |
166 | } |
| 166 | 167 | ||