Rev 2132 | Rev 2142 | 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; |
| Line 128... | Line 128... | ||
| 128 | target[axis] -= OVER360; |
128 | target[axis] -= OVER360; |
| 129 | } else if (target[axis] <= -OVER180) { |
129 | } else if (target[axis] <= -OVER180) { |
| 130 | target[axis] += OVER360; |
130 | target[axis] += OVER360; |
| 131 | } |
131 | } |
| Line 132... | Line 132... | ||
| 132 | 132 | ||
| Line 133... | Line 133... | ||
| 133 | error[axis] = attitude[axis] - target[axis]; |
133 | int32_t error = attitude[axis] - target[axis]; |
| 134 | 134 | ||
| Line 135... | Line 135... | ||
| 135 | #define ROTATETARGET 1 |
135 | #define ROTATETARGET 1 |
| - | 136 | // #define TRUNCATEERROR 1 |
|
| 136 | // #define TRUNCATEERROR 1 |
137 | |
| 137 | 138 | #ifdef ROTATETARGET |
|
| 138 | #ifdef ROTATETARGET |
139 | //if(abs(error) > OVER180) { // doesnt work!!! |
| 139 | if(abs(error[axis]) > OVER180) { |
140 | if(error > OVER180 || error < -OVER180) { |
| 140 | // The shortest way from attitude to target crosses -180. |
141 | // The shortest way from attitude to target crosses -180. |
| 141 | // 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. |
142 | // 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. |
| 142 | // Or A is <0 and T is >0, that makes E a (too) large negative number. It should be wrapped to positive. |
143 | // Or A is <0 and T is >0, that makes E a (too) large negative number. It should be wrapped to positive. |
| 143 | if (error[axis] > 0) { |
144 | if (error > 0) { |
| 144 | if (error[axis] < OVER360 - maxError[axis]) { |
145 | if (error < OVER360 - maxError[axis]) { |
| 145 | // too much err. |
146 | // too much err. |
| 146 | error[axis] = -maxError[axis]; |
147 | error = -maxError[axis]; |
| 147 | target[axis] = attitude[axis] + maxError[axis]; |
148 | target[axis] = attitude[axis] + maxError[axis]; |
| 148 | if (target[axis] > OVER180) target[axis] -= OVER360; |
149 | if (target[axis] > OVER180) target[axis] -= OVER360; |
| 149 | } else { |
150 | } else { |
| 150 | // Normal case, we just need to correct for the wrap. Error will be negative. |
151 | // Normal case, we just need to correct for the wrap. Error will be negative. |
| 151 | error[axis] -= OVER360; |
152 | error -= OVER360; |
| 152 | } |
153 | } |
| 153 | } else { |
154 | } else { |
| 154 | if (error[axis] > maxError[axis] - OVER360) { |
155 | if (error > maxError[axis] - OVER360) { |
| 155 | // too much err. |
156 | // too much err. |
| 156 | error[axis] = maxError[axis]; |
157 | error = maxError[axis]; |
| 157 | target[axis] = attitude[axis] - maxError[axis]; |
158 | target[axis] = attitude[axis] - maxError[axis]; |
| 158 | if (target[axis] < -OVER180) target[axis] += OVER360; |
159 | if (target[axis] < -OVER180) target[axis] += OVER360; |
| 159 | } else { |
160 | } else { |
| 160 | // Normal case, we just need to correct for the wrap. Error will be negative. |
161 | // Normal case, we just need to correct for the wrap. Error will be negative. |
| 161 | error[axis] += OVER360; |
162 | error += OVER360; |
| 162 | } |
163 | } |
| 163 | } |
164 | } |
| 164 | } else { |
165 | } else { |
| 165 | // Simple case, linear range. |
166 | // Simple case, linear range. |
| 166 | if (error[axis] > maxError[axis]) { |
167 | if (error > maxError[axis]) { |
| 167 | error[axis] = maxError[axis]; |
168 | error = maxError[axis]; |
| 168 | target[axis] = attitude[axis] - maxError[axis]; |
169 | target[axis] = attitude[axis] - maxError[axis]; |
| 169 | } else if (error[axis] < -maxError[axis]) { |
170 | } else if (error < -maxError[axis]) { |
| 170 | error[axis] = -maxError[axis]; |
171 | error = -maxError[axis]; |
| 171 | target[axis] = attitude[axis] + maxError[axis]; |
172 | target[axis] = attitude[axis] + maxError[axis]; |
| 172 | } |
173 | } |
| 173 | } |
174 | } |
| 174 | #endif |
175 | #endif |
| 175 | #ifdef TUNCATEERROR |
176 | #ifdef TUNCATEERROR |
| 176 | if (error[axis] > maxError[axis]) { |
177 | if (error > maxError[axis]) { |
| 177 | error[axis] = maxError[axis]; |
178 | error = maxError[axis]; |
| 178 | } else if (error[axis] < -maxError[axis]) { |
179 | } else if (error < -maxError[axis]) { |
| 179 | error[axis] = -maxError[axis]; |
180 | error = -maxError[axis]; |
| 180 | } else { |
181 | } else { |
| Line -... | Line 182... | ||
| - | 182 | // update I parts here for angles mode. I parts in rate mode is something different. |
|
| - | 183 | } |
|
| 181 | // update I parts here for angles mode. I parts in rate mode is something different. |
184 | #endif |
| 182 | } |
185 | |
| 183 | #endif |
186 | debugOut.analog[9+axis] = error / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
| 184 | 187 | ||
| 185 | /************************************************************************/ |
188 | /************************************************************************/ |
| Line 194... | Line 197... | ||
| 194 | } else { |
197 | } else { |
| 195 | PDPart[axis] = 0; |
198 | PDPart[axis] = 0; |
| 196 | } |
199 | } |
| Line 197... | Line 200... | ||
| 197 | 200 | ||
| 198 | if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
201 | if (currentFlightMode == FLIGHT_MODE_ANGLES) { |
| 199 | int16_t anglePart = (int32_t)(error[axis] * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE; |
202 | int16_t anglePart = (int32_t)(error * (int32_t) airspeedPID[axis].I) >> LOG_I_SCALE; |
| 200 | PDPart[axis] += anglePart; |
203 | PDPart[axis] += anglePart; |
| Line 201... | Line 204... | ||
| 201 | } |
204 | } |
| 202 | 205 | ||
| Line 256... | Line 259... | ||
| 256 | 259 | ||
| 257 | debugOut.analog[6] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
260 | debugOut.analog[6] = target[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
| 258 | debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
261 | debugOut.analog[7] = target[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
| Line 259... | Line -... | ||
| 259 | debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10); |
- | |
| 260 | - | ||
| 261 | debugOut.analog[9] = error[PITCH] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
- | |
| 262 | debugOut.analog[10] = error[ROLL] / (GYRO_DEG_FACTOR / 10); // in 0.1 deg |
- | |
| 263 | debugOut.analog[11] = error[YAW] / (GYRO_DEG_FACTOR / 10); |
262 | debugOut.analog[8] = target[YAW] / (GYRO_DEG_FACTOR / 10); |
| 264 | 263 | ||
| 265 | debugOut.analog[12] = term[PITCH]; |
264 | debugOut.analog[12] = term[PITCH]; |
| Line 266... | Line 265... | ||
| 266 | debugOut.analog[13] = term[ROLL]; |
265 | debugOut.analog[13] = term[ROLL]; |