Rev 2049 | Rev 2052 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2049 | Rev 2051 | ||
---|---|---|---|
Line 212... | Line 212... | ||
212 | PDPartYaw = 0; |
212 | PDPartYaw = 0; |
213 | if (isFlying == 250) { |
213 | if (isFlying == 250) { |
214 | // HC_setGround(); |
214 | // HC_setGround(); |
215 | attitude_resetHeadingToMagnetic(); |
215 | attitude_resetHeadingToMagnetic(); |
216 | // Set target heading to the one just gotten off compass. |
216 | // Set target heading to the one just gotten off compass. |
217 | targetHeading = heading; |
217 | // targetHeading = heading; |
218 | } |
218 | } |
219 | } else { |
219 | } else { |
220 | // Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag? |
220 | // Set fly flag. TODO: Hmmm what can we trust - the isFlying counter or the flag? |
221 | // Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe. |
221 | // Answer: The counter. The flag is not read from anywhere anyway... except the NC maybe. |
222 | MKFlags |= (MKFLAG_FLY); |
222 | MKFlags |= (MKFLAG_FLY); |
Line 237... | Line 237... | ||
237 | // YGBSM!!! |
237 | // YGBSM!!! |
238 | } |
238 | } |
239 | } |
239 | } |
240 | */ |
240 | */ |
Line 241... | Line -... | ||
241 | - | ||
242 | targetHeading -= controls[CONTROL_YAW]; |
- | |
243 | - | ||
244 | debugOut.analog[28] = targetHeading; |
- | |
245 | 241 | ||
246 | #if defined (USE_NAVICTRL) |
242 | #if defined (USE_NAVICTRL) |
247 | /************************************************************************/ |
243 | /************************************************************************/ |
248 | /* GPS is currently not supported. */ |
244 | /* GPS is currently not supported. */ |
249 | /************************************************************************/ |
245 | /************************************************************************/ |
Line 253... | Line 249... | ||
253 | } else { |
249 | } else { |
254 | } |
250 | } |
255 | #endif |
251 | #endif |
256 | // end part 1: 750-800 usec. |
252 | // end part 1: 750-800 usec. |
257 | // start part 3: 350 - 400 usec. |
253 | // start part 3: 350 - 400 usec. |
258 | #define SENSOR_LIMIT (4096 * 4) |
- | |
259 | /************************************************************************/ |
254 | /************************************************************************/ |
Line 260... | Line 255... | ||
260 | 255 | ||
261 | /* Calculate control feedback from angle (gyro integral) */ |
256 | /* Calculate control feedback from angle (gyro integral) */ |
262 | /* and angular velocity (gyro signal) */ |
257 | /* and angular velocity (gyro signal) */ |
263 | /************************************************************************/ |
258 | /************************************************************************/ |
264 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
259 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
265 | for (axis = PITCH; axis <= ROLL; axis++) { |
260 | for (axis = PITCH; axis <= ROLL; axis++) { |
266 | PDPart[axis] = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2); // P-Part - Proportional to Integral |
261 | PDPart[axis] = attitude[axis] * gyroIFactor / (GYRO_DEG_FACTOR_PITCHROLL << 2); // P-Part - Proportional to Integral |
267 | PDPart[axis] += (int32_t)rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 5); |
262 | PDPart[axis] += (int32_t)rate_PID[axis] * gyroPFactor / (GYRO_DEG_FACTOR_PITCHROLL >> 5); |
268 | PDPart[axis] += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
263 | PDPart[axis] += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
269 | CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
264 | CHECK_MIN_MAX(PDPart[axis], -6L*GYRO_DEG_FACTOR_PITCHROLL, 6L*GYRO_DEG_FACTOR_PITCHROLL); |
Line -... | Line 265... | ||
- | 265 | } |
|
270 | } |
266 | |
- | 267 | #define YAW_I_LIMIT (45L * GYRO_DEG_FACTOR_YAW) |
|
- | 268 | // This is where control affects the target heading. It also (later) directly controls yaw. |
|
271 | 269 | headingError -= controls[CONTROL_YAW]; |
|
272 | int32_t headingDiff = heading - targetHeading; // apparently yaw is reverse on output. |
270 | debugOut.analog[28] = headingError / 100; |
Line 273... | Line -... | ||
273 | if (headingDiff > YAWOVER180) headingDiff -= YAWOVER360; |
- | |
274 | else if (headingDiff <= -YAWOVER180) headingDiff += YAWOVER360; |
- | |
275 | - | ||
276 | // TODO: Not quite right: We want to limit targetHeading to be max. 50000 from heading. This is the wrong var. fixed. |
- | |
277 | CHECK_MIN_MAX(headingDiff, -50000, 50000); |
271 | if (headingError < -YAW_I_LIMIT) headingError = -YAW_I_LIMIT; |
- | 272 | if (headingError > YAW_I_LIMIT) headingError = YAW_I_LIMIT; |
|
278 | debugOut.analog[29] = headingDiff; |
273 | |
Line 279... | Line 274... | ||
279 | 274 | PDPartYaw = (int32_t)(headingError * yawIFactor) / (GYRO_DEG_FACTOR_PITCHROLL << 3); |
|
280 | PDPartYaw = (int32_t)(headingDiff * yawIFactor) / (GYRO_DEG_FACTOR_PITCHROLL << 3); |
275 | // Ehhhhh here is something with desired yaw rate, not?? Ahh OK it gets added in later on. |
Line 358... | Line 353... | ||
358 | // TODO: From which planet comes the 16000? |
353 | // TODO: From which planet comes the 16000? |
359 | CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L)); |
354 | CHECK_MIN_MAX(IPart[axis], -(CONTROL_SCALING * 16000L), (CONTROL_SCALING * 16000L)); |
360 | // Add (P, D) parts minus stick pos. to the scaled-down I part. |
355 | // Add (P, D) parts minus stick pos. to the scaled-down I part. |
361 | term[axis] = PDPart[axis] - controls[axis] + IPart[axis] / Ki; // PID-controller for pitch |
356 | term[axis] = PDPart[axis] - controls[axis] + IPart[axis] / Ki; // PID-controller for pitch |
362 | term[axis] += (dynamicParams.levelCorrection[axis] - 128); |
357 | term[axis] += (dynamicParams.levelCorrection[axis] - 128); |
363 | /* |
358 | /* |
364 | * Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!). |
359 | * Apply "dynamic stability" - that is: Limit pitch and roll terms to a growing function of throttle and yaw(!). |
365 | * The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity |
360 | * The higher the dynamic stability parameter, the wider the bounds. 64 seems to be a kind of unity |
366 | * (max. pitch or roll term is the throttle value). |
361 | * (max. pitch or roll term is the throttle value). |
367 | * TODO: Why a growing function of yaw? |
362 | * TODO: Why a growing function of yaw? |
368 | */ |
363 | */ |