Rev 1956 | Rev 1964 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1956 | Rev 1960 | ||
---|---|---|---|
Line 59... | Line 59... | ||
59 | //#include "analog.h" |
59 | //#include "analog.h" |
60 | //#include "rc.h" |
60 | //#include "rc.h" |
Line 61... | Line 61... | ||
61 | 61 | ||
62 | // Necessary for external control and motor test |
62 | // Necessary for external control and motor test |
63 | #include "uart0.h" |
- | |
64 | - | ||
65 | // for scope debugging |
- | |
66 | // #include "rc.h" |
- | |
67 | 63 | #include "uart0.h" |
|
68 | #include "twimaster.h" |
64 | #include "twimaster.h" |
69 | #include "attitude.h" |
65 | #include "attitude.h" |
70 | #include "controlMixer.h" |
66 | #include "controlMixer.h" |
71 | #include "commands.h" |
67 | #include "commands.h" |
Line 84... | Line 80... | ||
84 | uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control |
80 | uint8_t gyroPFactor, gyroIFactor; // the PD factors for the attitude control |
85 | uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control |
81 | uint8_t yawPFactor, yawIFactor; // the PD factors for the yaw control |
Line 86... | Line 82... | ||
86 | 82 | ||
87 | // Some integral weight constant... |
83 | // Some integral weight constant... |
88 | uint16_t Ki = 10300 / 33; |
- | |
Line 89... | Line 84... | ||
89 | uint8_t RequiredMotors = 0; |
84 | uint16_t Ki = 10300 / 33; |
90 | 85 | ||
91 | /************************************************************************/ |
86 | /************************************************************************/ |
92 | /* Filter for motor value smoothing (necessary???) */ |
87 | /* Filter for motor value smoothing (necessary???) */ |
93 | /************************************************************************/ |
88 | /************************************************************************/ |
94 | int16_t motorFilter(int16_t newvalue, int16_t oldvalue) { |
89 | int16_t motorFilter(int16_t newvalue, int16_t oldvalue) { |
95 | switch (dynamicParams.UserParams[5]) { |
90 | switch (dynamicParams.motorSmoothing) { |
96 | case 0: |
91 | case 0: |
97 | return newvalue; |
92 | return newvalue; |
98 | case 1: |
93 | case 1: |
Line 116... | Line 111... | ||
116 | /* Neutral Readings */ |
111 | /* Neutral Readings */ |
117 | /************************************************************************/ |
112 | /************************************************************************/ |
118 | void flight_setNeutral() { |
113 | void flight_setNeutral() { |
119 | MKFlags |= MKFLAG_CALIBRATE; |
114 | MKFlags |= MKFLAG_CALIBRATE; |
120 | // not really used here any more. |
115 | // not really used here any more. |
- | 116 | /* |
|
121 | dynamicParams.KalmanK = -1; |
117 | dynamicParams.KalmanK = -1; |
122 | dynamicParams.KalmanMaxDrift = 0; |
118 | dynamicParams.KalmanMaxDrift = 0; |
123 | dynamicParams.KalmanMaxFusion = 32; |
119 | dynamicParams.KalmanMaxFusion = 32; |
- | 120 | */ |
|
124 | controlMixer_initVariables(); |
121 | controlMixer_initVariables(); |
125 | } |
122 | } |
Line 126... | Line 123... | ||
126 | 123 | ||
127 | void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, |
124 | void setFlightParameters(uint8_t _Ki, uint8_t _gyroPFactor, |
Line 134... | Line 131... | ||
134 | } |
131 | } |
Line 135... | Line 132... | ||
135 | 132 | ||
136 | void setNormalFlightParameters(void) { |
133 | void setNormalFlightParameters(void) { |
137 | setFlightParameters( |
134 | setFlightParameters( |
138 | dynamicParams.IFactor, |
135 | dynamicParams.IFactor, |
139 | dynamicParams.GyroP, |
136 | dynamicParams.gyroP, |
140 | staticParams.GlobalConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.GyroI, |
137 | staticParams.bitConfig & CFG_HEADING_HOLD ? 0 : dynamicParams.gyroI, |
141 | dynamicParams.GyroP, |
138 | dynamicParams.gyroP, |
142 | dynamicParams.UserParams[6] |
139 | dynamicParams.yawIFactor |
143 | ); |
140 | ); |
Line 144... | Line 141... | ||
144 | } |
141 | } |
145 | 142 | ||
Line 177... | Line 174... | ||
177 | calculateFlightAttitude(); |
174 | calculateFlightAttitude(); |
178 | controlMixer_update(); |
175 | controlMixer_update(); |
179 | throttleTerm = controls[CONTROL_THROTTLE]; |
176 | throttleTerm = controls[CONTROL_THROTTLE]; |
Line 180... | Line 177... | ||
180 | 177 | ||
181 | // This check removed. Is done on a per-motor basis, after output matrix multiplication. |
178 | // This check removed. Is done on a per-motor basis, after output matrix multiplication. |
182 | if (throttleTerm < staticParams.MinThrottle + 10) |
179 | if (throttleTerm < staticParams.minThrottle + 10) |
183 | throttleTerm = staticParams.MinThrottle + 10; |
180 | throttleTerm = staticParams.minThrottle + 10; |
184 | else if (throttleTerm > staticParams.MaxThrottle - 20) |
181 | else if (throttleTerm > staticParams.maxThrottle - 20) |
Line 185... | Line 182... | ||
185 | throttleTerm = (staticParams.MaxThrottle - 20); |
182 | throttleTerm = (staticParams.maxThrottle - 20); |
186 | 183 | ||
187 | /************************************************************************/ |
184 | /************************************************************************/ |
188 | /* RC-signal is bad */ |
185 | /* RC-signal is bad */ |
Line 197... | Line 194... | ||
197 | if (emergencyFlightTime) { |
194 | if (emergencyFlightTime) { |
198 | // continue emergency flight |
195 | // continue emergency flight |
199 | emergencyFlightTime--; |
196 | emergencyFlightTime--; |
200 | if (isFlying > 256) { |
197 | if (isFlying > 256) { |
201 | // We're probably still flying. Descend slowly. |
198 | // We're probably still flying. Descend slowly. |
202 | throttleTerm = staticParams.EmergencyGas; // Set emergency throttle |
199 | throttleTerm = staticParams.emergencyThrottle; // Set emergency throttle |
203 | MKFlags |= (MKFLAG_EMERGENCY_LANDING); // Set flag for emergency landing |
200 | MKFlags |= (MKFLAG_EMERGENCY_FLIGHT); // Set flag for emergency landing |
204 | setStableFlightParameters(); |
201 | setStableFlightParameters(); |
205 | } else { |
202 | } else { |
206 | MKFlags &= ~(MKFLAG_MOTOR_RUN); // Probably not flying, and bad R/C signal. Kill motors. |
203 | MKFlags &= ~(MKFLAG_MOTOR_RUN); // Probably not flying, and bad R/C signal. Kill motors. |
207 | } |
204 | } |
208 | } else { |
205 | } else { |
209 | // end emergency flight (just cut the motors???) |
206 | // end emergency flight (just cut the motors???) |
210 | MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_LANDING); |
207 | MKFlags &= ~(MKFLAG_MOTOR_RUN | MKFLAG_EMERGENCY_FLIGHT); |
211 | } |
208 | } |
212 | } else { |
209 | } else { |
213 | // signal is acceptable |
210 | // signal is acceptable |
214 | if (controlMixer_getSignalQuality() > SIGNAL_BAD) { |
211 | if (controlMixer_getSignalQuality() > SIGNAL_BAD) { |
215 | // Reset emergency landing control variables. |
212 | // Reset emergency landing control variables. |
216 | MKFlags &= ~(MKFLAG_EMERGENCY_LANDING); // clear flag for emergency landing |
213 | MKFlags &= ~(MKFLAG_EMERGENCY_FLIGHT); // clear flag for emergency landing |
217 | // The time is in whole seconds. |
214 | // The time is in whole seconds. |
218 | emergencyFlightTime = (uint16_t) staticParams.EmergencyGasDuration * 488; |
215 | emergencyFlightTime = (uint16_t) staticParams.emergencyFlightDuration * 488; |
219 | } |
216 | } |
Line 220... | Line 217... | ||
220 | 217 | ||
221 | // If some throttle is given, and the motor-run flag is on, increase the probability that we are flying. |
218 | // If some throttle is given, and the motor-run flag is on, increase the probability that we are flying. |
222 | if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) { |
219 | if (throttleTerm > 40 && (MKFlags & MKFLAG_MOTOR_RUN)) { |
Line 227... | Line 224... | ||
227 | /* |
224 | /* |
228 | * When standing on the ground, do not apply I controls and zero the yaw stick. |
225 | * When standing on the ground, do not apply I controls and zero the yaw stick. |
229 | * Probably to avoid integration effects that will cause the copter to spin |
226 | * Probably to avoid integration effects that will cause the copter to spin |
230 | * or flip when taking off. |
227 | * or flip when taking off. |
231 | */ |
228 | */ |
232 | if (isFlying < 256) { |
229 | if (isFlying < 256) { |
233 | IPart[PITCH] = IPart[ROLL] = 0; |
230 | IPart[PITCH] = IPart[ROLL] = 0; |
234 | // TODO: Don't stomp on other modules' variables!!! |
231 | // TODO: Don't stomp on other modules' variables!!! |
235 | // controlYaw = 0; |
232 | // controlYaw = 0; |
236 | PDPartYaw = 0; // instead. |
233 | PDPartYaw = 0; // instead. |
237 | if (isFlying == 250) { |
234 | if (isFlying == 250) { |
238 | // HC_setGround(); |
235 | // HC_setGround(); |
239 | updateCompassCourse = 1; |
236 | updateCompassCourse = 1; |
240 | yawAngleDiff = 0; |
237 | yawAngleDiff = 0; |
- | 238 | } |
|
- | 239 | } else { |
|
- | 240 | // Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag? |
|
- | 241 | // Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe. |
|
- | 242 | MKFlags |= (MKFLAG_FLY); |
|
241 | } |
243 | } |
242 | } else { |
- | |
243 | // Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag? |
- | |
244 | // Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe. |
- | |
245 | MKFlags |= (MKFLAG_FLY); |
- | |
246 | } |
- | |
247 | 244 | ||
248 | commands_handleCommands(); |
245 | commands_handleCommands(); |
249 | - | ||
250 | // if(controlMixer_getSignalQuality() >= SIGNAL_GOOD) { |
- | |
251 | setNormalFlightParameters(); |
246 | setNormalFlightParameters(); |
252 | // } |
- | |
253 | } // end else (not bad signal case) |
247 | } // end else (not bad signal case) |
254 | // end part1a: 750-800 usec. |
- | |
255 | /* |
- | |
256 | * Looping the H&I way basically is just a matter of turning off attitude angle measurement |
- | |
257 | * by integration (because 300 deg/s gyros are too slow) and turning down the throttle. |
- | |
258 | * This is the throttle part. |
- | |
259 | */ |
- | |
260 | if (looping) { |
- | |
261 | if (throttleTerm > staticParams.LoopGasLimit) |
- | |
262 | throttleTerm = staticParams.LoopGasLimit; |
- | |
263 | } |
- | |
264 | 248 | ||
265 | /************************************************************************/ |
249 | /************************************************************************/ |
266 | /* Yawing */ |
250 | /* Yawing */ |
267 | /************************************************************************/ |
251 | /************************************************************************/ |
268 | if (abs(controls[CONTROL_YAW]) > 4 * staticParams.StickYawP) { // yaw stick is activated |
252 | if (abs(controls[CONTROL_YAW]) > 4 * staticParams.stickYawP) { // yaw stick is activated |
269 | ignoreCompassTimer = 1000; |
253 | ignoreCompassTimer = 1000; |
270 | if (!(staticParams.GlobalConfig & CFG_COMPASS_FIX)) { |
254 | if (!(staticParams.bitConfig & CFG_COMPASS_FIX)) { |
271 | updateCompassCourse = 1; |
255 | updateCompassCourse = 1; |
272 | } |
256 | } |
273 | } |
257 | } |
Line 274... | Line 258... | ||
274 | 258 | ||
275 | // yawControlRate = controlYaw; |
- | |
276 | 259 | // yawControlRate = controlYaw; |
|
277 | // Trim drift of yawAngleDiff with controlYaw. |
260 | // Trim drift of yawAngleDiff with controlYaw. |
278 | // TODO: We want NO feedback of control related stuff to the attitude related stuff. |
261 | // TODO: We want NO feedback of control related stuff to the attitude related stuff. |
279 | // This seems to be used as: Difference desired <--> real heading. |
262 | // This seems to be used as: Difference desired <--> real heading. |
Line 283... | Line 266... | ||
283 | CHECK_MIN_MAX(yawAngleDiff, -50000, 50000); |
266 | CHECK_MIN_MAX(yawAngleDiff, -50000, 50000); |
Line 284... | Line 267... | ||
284 | 267 | ||
285 | /************************************************************************/ |
268 | /************************************************************************/ |
286 | /* Compass is currently not supported. */ |
269 | /* Compass is currently not supported. */ |
287 | /************************************************************************/ |
270 | /************************************************************************/ |
288 | if (staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
271 | if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
289 | updateCompass(); |
272 | updateCompass(); |
Line 290... | Line 273... | ||
290 | } |
273 | } |
291 | 274 | ||
Line 308... | Line 291... | ||
308 | /* and angular velocity (gyro signal) */ |
291 | /* and angular velocity (gyro signal) */ |
309 | /************************************************************************/ |
292 | /************************************************************************/ |
310 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
293 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
Line 311... | Line 294... | ||
311 | 294 | ||
312 | for (axis = PITCH; axis <= ROLL; axis++) { |
- | |
313 | if (looping & ((1 << 4) << axis)) { |
- | |
314 | PPart[axis] = 0; |
- | |
315 | } else { // TODO: Where do the 44000 come from??? |
295 | for (axis = PITCH; axis <= ROLL; axis++) { |
316 | PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
- | |
317 | } |
296 | PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
318 | 297 | ||
319 | /* |
298 | /* |
320 | * Now blend in the D-part - proportional to the Differential of the integral = the rate. |
299 | * Now blend in the D-part - proportional to the Differential of the integral = the rate. |
321 | * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING |
300 | * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING |
322 | * where pfactor is in [0..1]. |
301 | * where pfactor is in [0..1]. |
323 | */ |
- | |
324 | PDPart[axis] = PPart[axis] + (int32_t) ((int32_t) rate_PID[axis] |
302 | */ |
325 | * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] |
303 | PDPart[axis] = PPart[axis] + (int32_t) ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] |
326 | * (int16_t) dynamicParams.GyroD) / 16; |
304 | * (int16_t) dynamicParams.gyroD) / 16; |
327 | 305 | ||
328 | CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
306 | CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
Line 329... | Line 307... | ||
329 | } |
307 | } |
330 | 308 | ||
Line 341... | Line 319... | ||
341 | */ |
319 | */ |
342 | if (missingMotor) { |
320 | if (missingMotor) { |
343 | // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway??? |
321 | // if we are in the lift off condition. Hmmmmmm when is throttleTerm == 0 anyway??? |
344 | if (isFlying > 1 && isFlying < 50 && throttleTerm > 0) |
322 | if (isFlying > 1 && isFlying < 50 && throttleTerm > 0) |
345 | isFlying = 1; // keep within lift off condition |
323 | isFlying = 1; // keep within lift off condition |
346 | throttleTerm = staticParams.MinThrottle; // reduce gas to min to avoid lift of |
324 | throttleTerm = staticParams.minThrottle; // reduce gas to min to avoid lift of |
347 | } |
325 | } |
Line 348... | Line 326... | ||
348 | 326 | ||
349 | // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already? |
327 | // Scale up to higher resolution. Hmm why is it not (from controlMixer and down) scaled already? |
Line 379... | Line 357... | ||
379 | } |
357 | } |
380 | //CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
358 | //CHECK_MIN_MAX(yawTerm, - (MIN_YAWGAS / 2), (MIN_YAWGAS / 2)); |
381 | } |
359 | } |
Line 382... | Line 360... | ||
382 | 360 | ||
383 | // FIXME: Throttle may exceed maxThrottle (there is no check no more). |
361 | // FIXME: Throttle may exceed maxThrottle (there is no check no more). |
384 | tmp_int = staticParams.MaxThrottle * CONTROL_SCALING; |
362 | tmp_int = staticParams.maxThrottle * CONTROL_SCALING; |
385 | if (yawTerm < -(tmp_int - throttleTerm)) { |
363 | if (yawTerm < -(tmp_int - throttleTerm)) { |
386 | yawTerm = -(tmp_int - throttleTerm); |
364 | yawTerm = -(tmp_int - throttleTerm); |
387 | debugOut.digital[0] |= DEBUG_CLIP; |
365 | debugOut.digital[0] |= DEBUG_CLIP; |
388 | } else if (yawTerm > (tmp_int - throttleTerm)) { |
366 | } else if (yawTerm > (tmp_int - throttleTerm)) { |
Line 405... | Line 383... | ||
405 | // "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos. |
383 | // "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos. |
406 | // To keep up with a full stick PDPart should be about 156... |
384 | // To keep up with a full stick PDPart should be about 156... |
407 | IPart[axis] += PDPart[axis] - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
385 | IPart[axis] += PDPart[axis] - controls[axis]; // With gyroIFactor == 0, PDPart is really just a D-part. Integrate D-part (the rot. rate) and the stick pos. |
408 | } |
386 | } |
Line 409... | Line 387... | ||
409 | 387 | ||
410 | tmp_int = (int32_t) ((int32_t) dynamicParams.DynamicStability |
388 | tmp_int = (int32_t) ((int32_t) dynamicParams.dynamicStability |
Line 411... | Line 389... | ||
411 | * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64; |
389 | * (int32_t) (throttleTerm + abs(yawTerm) / 2)) / 64; |
412 | 390 | ||
413 | // TODO: From which planet comes the 16000? |
391 | // TODO: From which planet comes the 16000? |
Line 441... | Line 419... | ||
441 | 419 | ||
442 | for (i = 0; i < MAX_MOTORS; i++) { |
420 | for (i = 0; i < MAX_MOTORS; i++) { |
443 | int32_t tmp; |
421 | int32_t tmp; |
Line 444... | Line 422... | ||
444 | uint8_t throttle; |
422 | uint8_t throttle; |
445 | 423 | ||
446 | tmp = (int32_t)throttleTerm * Mixer.Motor[i][MIX_THROTTLE]; |
424 | tmp = (int32_t)throttleTerm * mixerMatrix.motor[i][MIX_THROTTLE]; |
447 | tmp += (int32_t)term[PITCH] * Mixer.Motor[i][MIX_PITCH]; |
425 | tmp += (int32_t)term[PITCH] * mixerMatrix.motor[i][MIX_PITCH]; |
448 | tmp += (int32_t)term[ROLL] * Mixer.Motor[i][MIX_ROLL]; |
426 | tmp += (int32_t)term[ROLL] * mixerMatrix.motor[i][MIX_ROLL]; |
449 | tmp += (int32_t)yawTerm * Mixer.Motor[i][MIX_YAW]; |
427 | tmp += (int32_t)yawTerm * mixerMatrix.motor[i][MIX_YAW]; |
450 | tmp = tmp >> 6; |
428 | tmp = tmp >> 6; |
451 | motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
429 | motorFilters[i] = motorFilter(tmp, motorFilters[i]); |
Line 462... | Line 440... | ||
462 | CHECK_MIN_MAX(tmp, 1, 255); |
440 | CHECK_MIN_MAX(tmp, 1, 255); |
463 | throttle = tmp; |
441 | throttle = tmp; |
Line 464... | Line 442... | ||
464 | 442 | ||
Line 465... | Line 443... | ||
465 | if (i < 4) debugOut.analog[22 + i] = throttle; |
443 | if (i < 4) debugOut.analog[22 + i] = throttle; |
466 | 444 | ||
467 | if ((MKFlags & MKFLAG_MOTOR_RUN) && Mixer.Motor[i][MIX_THROTTLE] > 0) { |
445 | if ((MKFlags & MKFLAG_MOTOR_RUN) && mixerMatrix.motor[i][MIX_THROTTLE] > 0) { |
468 | motor[i].SetPoint = throttle; |
446 | motor[i].SetPoint = throttle; |
469 | } else if (motorTestActive) { |
447 | } else if (motorTestActive) { |
470 | motor[i].SetPoint = motorTest[i]; |
448 | motor[i].SetPoint = motorTest[i]; |
Line 484... | Line 462... | ||
484 | debugOut.analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
462 | debugOut.analog[1] = (10 * angle[ROLL]) / GYRO_DEG_FACTOR_PITCHROLL; // in 0.1 deg |
485 | debugOut.analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; |
463 | debugOut.analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; |
Line 486... | Line 464... | ||
486 | 464 | ||
487 | debugOut.analog[16] = gyroPFactor; |
465 | debugOut.analog[16] = gyroPFactor; |
488 | debugOut.analog[17] = gyroIFactor; |
466 | debugOut.analog[17] = gyroIFactor; |
489 | debugOut.analog[18] = dynamicParams.GyroD; |
467 | debugOut.analog[18] = dynamicParams.gyroD; |
490 | } |
468 | } |