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 |