Rev 2132 | Rev 2138 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2132 | Rev 2135 | ||
---|---|---|---|
Line 20... | Line 20... | ||
20 | int32_t target[3]; |
20 | int32_t target[3]; |
Line 21... | Line 21... | ||
21 | 21 | ||
22 | /* |
22 | /* |
23 | * Error integrals. |
23 | * Error integrals. |
24 | */ |
- | |
Line 25... | Line 24... | ||
25 | int32_t error[3]; |
24 | */ |
26 | 25 | ||
27 | uint8_t reverse[3]; |
26 | uint8_t reverse[3]; |
28 | int32_t maxError[3]; |
27 | int32_t maxError[3]; |
Line 43... | Line 42... | ||
43 | target[YAW] = attitude[YAW]; |
42 | target[YAW] = attitude[YAW]; |
44 | } |
43 | } |
Line 45... | Line 44... | ||
45 | 44 | ||
46 | void flight_updateFlightParametersToFlightMode(void) { |
45 | void flight_updateFlightParametersToFlightMode(void) { |
- | 46 | debugOut.analog[16] = currentFlightMode; |
|
47 | debugOut.analog[16] = currentFlightMode; |
47 | |
48 | reverse[PITCH] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_ELEVATOR; |
48 | reverse[PITCH] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_ELEVATOR; |
49 | reverse[ROLL] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_AILERONS; |
49 | reverse[ROLL] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_AILERONS; |
Line 50... | Line 50... | ||
50 | reverse[YAW] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_RUDDER; |
50 | reverse[YAW] = staticParams.servosReverse & CONTROL_SERVO_REVERSE_RUDDER; |
51 | 51 | ||
Line 76... | Line 76... | ||
76 | airspeedPID[axis].D = dynamicParams.gyroPID[axis].D; |
76 | airspeedPID[axis].D = dynamicParams.gyroPID[axis].D; |
77 | } |
77 | } |
78 | } |
78 | } |
Line 79... | Line 79... | ||
79 | 79 | ||
80 | #define LOG_STICK_SCALE 8 |
80 | #define LOG_STICK_SCALE 8 |
81 | #define LOG_P_SCALE 6 |
81 | #define LOG_P_SCALE 8 |
82 | #define LOG_I_SCALE 10 |
82 | #define LOG_I_SCALE 12 |
Line 83... | Line 83... | ||
83 | #define LOG_D_SCALE 6 |
83 | #define LOG_D_SCALE 8 |
84 | 84 | ||
85 | /************************************************************************/ |
85 | /************************************************************************/ |
86 | /* Main Flight Control */ |
86 | /* Main Flight Control */ |
Line 120... | Line 120... | ||
120 | target[axis] -= OVER360; |
120 | target[axis] -= OVER360; |
121 | } else if (target[axis] <= -OVER180) { |
121 | } else if (target[axis] <= -OVER180) { |
122 | target[axis] += OVER360; |
122 | target[axis] += OVER360; |
123 | } |
123 | } |
Line 124... | Line 124... | ||
124 | 124 | ||
Line 125... | Line 125... | ||
125 | error[axis] = attitude[axis] - target[axis]; |
125 | int32_t error = attitude[axis] - target[axis]; |
126 | 126 | ||
Line 127... | Line 127... | ||
127 | #define ROTATETARGET 1 |
127 | #define ROTATETARGET 1 |
128 | // #define TRUNCATEERROR 1 |
128 | // #define TRUNCATEERROR 1 |
- | 129 | ||
129 | 130 | #ifdef ROTATETARGET |
|
130 | #ifdef ROTATETARGET |
131 | //if(abs(error) > OVER180) { // doesnt work!!! |
131 | if(abs(error[axis]) > OVER180) { |
132 | if(error > OVER180 || error < -OVER180) { |
132 | // The shortest way from attitude to target crosses -180. |
133 | // The shortest way from attitude to target crosses -180. |
133 | // Well there are 2 possibilities: A is >0 and T is < 0, that makes E a (too) large positive number. It should be wrapped to negative. |
134 | // Well there are 2 possibilities: A is >0 and T is < 0, that makes E a (too) large positive number. It should be wrapped to negative. |
134 | // Or A is <0 and T is >0, that makes E a (too) large negative number. It should be wrapped to positive. |
135 | // Or A is <0 and T is >0, that makes E a (too) large negative number. It should be wrapped to positive. |
135 | if (error[axis] > 0) { |
136 | if (error > 0) { |
136 | if (error[axis] < OVER360 - maxError[axis]) { |
137 | if (error < OVER360 - maxError[axis]) { |
137 | // too much err. |
138 | // too much err. |
138 | error[axis] = -maxError[axis]; |
139 | error = -maxError[axis]; |
139 | target[axis] = attitude[axis] + maxError[axis]; |
140 | target[axis] = attitude[axis] + maxError[axis]; |
140 | if (target[axis] > OVER180) target[axis] -= OVER360; |
141 | if (target[axis] > OVER180) target[axis] -= OVER360; |
141 | } else { |
142 | } else { |
142 | // Normal case, we just need to correct for the wrap. Error will be negative. |
143 | // Normal case, we just need to correct for the wrap. Error will be negative. |
143 | error[axis] -= OVER360; |
144 | error -= OVER360; |
144 | } |
145 | } |
145 | } else { |
146 | } else { |
146 | if (error[axis] > maxError[axis] - OVER360) { |
147 | if (error > maxError[axis] - OVER360) { |
147 | // too much err. |
148 | // too much err. |
148 | error[axis] = maxError[axis]; |
149 | error = maxError[axis]; |
149 | target[axis] = attitude[axis] - maxError[axis]; |
150 | target[axis] = attitude[axis] - maxError[axis]; |
150 | if (target[axis] < -OVER180) target[axis] += OVER360; |
151 | if (target[axis] < -OVER180) target[axis] += OVER360; |
151 | } else { |
152 | } else { |
152 | // Normal case, we just need to correct for the wrap. Error will be negative. |
153 | // Normal case, we just need to correct for the wrap. Error will be negative. |
153 | error[axis] += OVER360; |
154 | error += OVER360; |
154 | } |
155 | } |
155 | } |
156 | } |
156 | } else { |
157 | } else { |
157 | // Simple case, linear range. |
158 | // Simple case, linear range. |
158 | if (error[axis] > maxError[axis]) { |
159 | if (error > maxError[axis]) { |
159 | error[axis] = maxError[axis]; |
160 | error = maxError[axis]; |
160 | target[axis] = attitude[axis] - maxError[axis]; |
161 | target[axis] = attitude[axis] - maxError[axis]; |
161 | } else if (error[axis] < -maxError[axis]) { |
162 | } else if (error < -maxError[axis]) { |
162 | error[axis] = -maxError[axis]; |
163 | error = -maxError[axis]; |
163 | target[axis] = attitude[axis] + maxError[axis]; |
164 | target[axis] = attitude[axis] + maxError[axis]; |
164 | } |
165 | } |
165 | } |
166 | } |
166 | #endif |
167 | #endif |
167 | #ifdef TUNCATEERROR |
168 | #ifdef TUNCATEERROR |
168 | if (error[axis] > maxError[axis]) { |
169 | if (error > maxError[axis]) { |
169 | error[axis] = maxError[axis]; |
170 | error = maxError[axis]; |
170 | } else if (error[axis] < -maxError[axis]) { |
171 | } else if (error < -maxError[axis]) { |
171 | error[axis] = -maxError[axis]; |
172 | error = -maxError[axis]; |
172 | } else { |
173 | } else { |
Line -... | Line 174... | ||
- | 174 | // update I parts here for angles mode. I parts in rate mode is something different. |
|
- | 175 | } |
|
173 | // update I parts here for angles mode. I parts in rate mode is something different. |
176 | #endif |
174 | } |
177 | |
175 | #endif |
178 | debugOut.analog[9+axis] = error / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
176 | 179 | ||
177 | /************************************************************************/ |
180 | /************************************************************************/ |
Line 187... | Line 190... | ||
187 | } else { |
190 | } else { |
188 | PDPart[axis] = 0; |
191 | PDPart[axis] = 0; |
189 | } |
192 | } |
Line 190... | Line 193... | ||
190 | 193 | ||
191 | if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
194 | if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
192 | int16_t anglePart = (int32_t)(error[axis] * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE; |
195 | int16_t anglePart = (int32_t)(error * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE; |
193 | PDPart[axis] += anglePart; |
196 | PDPart[axis] += anglePart; |
Line 194... | Line 197... | ||
194 | } |
197 | } |
195 | 198 | ||
196 | // Add I parts here... these are integrated errors. |
199 | // Add I parts here... these are integrated errors. |
197 | if (reverse[axis]) |
200 | if (reverse[axis]) |
198 | term[axis] = controls[axis] - PDPart[axis]; // + IPart[axis]; |
201 | term[axis] = controls[axis] - PDPart[axis]; // + IPart[axis]; |
199 | else |
202 | else |
Line 200... | Line -... | ||
200 | term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis]; |
- | |
201 | } |
- | |
202 | - | ||
203 | debugOut.analog[12] = term[PITCH]; |
- | |
204 | debugOut.analog[13] = term[ROLL]; |
- | |
205 | debugOut.analog[14] = term[YAW]; |
203 | term[axis] = controls[axis] + PDPart[axis]; // + IPart[axis]; |
206 | debugOut.analog[15] = term[THROTTLE]; |
204 | } |
207 | 205 | ||
208 | for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) { |
206 | for (uint8_t i = 0; i < NUM_CONTROL_SERVOS; i++) { |
209 | int16_t tmp; |
207 | int16_t tmp; |
Line 249... | Line 247... | ||
249 | 247 | ||
250 | debugOut.analog[6] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
248 | debugOut.analog[6] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
251 | debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
249 | debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
Line 252... | Line -... | ||
252 | debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10); |
- | |
253 | - | ||
254 | debugOut.analog[9] = error[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
250 | debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10); |
255 | debugOut.analog[10] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
- | |
256 | debugOut.analog[11] = error[YAW] / (GYRO_DEG_FACTOR / 10); |
251 | |
257 | 252 | debugOut.analog[12] = term[PITCH]; |
|
258 | debugOut.analog[12] = term[PITCH]; |
253 | debugOut.analog[13] = term[ROLL]; |
Line 259... | Line 254... | ||
259 | debugOut.analog[13] = term[ROLL]; |
254 | debugOut.analog[14] = term[YAW]; |
260 | debugOut.analog[14] = term[YAW]; |
255 | debugOut.analog[15] = term[THROTTLE]; |
261 | 256 | ||
262 | //DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
257 | //DebugOut.Analog[18] = (10 * controlIntegrals[CONTROL_ELEVATOR]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |