Rev 2045 | Rev 2049 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2045 | Rev 2048 | ||
|---|---|---|---|
| Line 210... | Line 210... | ||
| 210 | if (isFlying < 256) { |
210 | if (isFlying < 256) { |
| 211 | IPart[PITCH] = IPart[ROLL] = 0; |
211 | IPart[PITCH] = IPart[ROLL] = 0; |
| 212 | PDPartYaw = 0; |
212 | PDPartYaw = 0; |
| 213 | if (isFlying == 250) { |
213 | if (isFlying == 250) { |
| 214 | // HC_setGround(); |
214 | // HC_setGround(); |
| 215 | updateCompassCourse = 1; |
215 | attitude_resetHeadingToMagnetic(); |
| - | 216 | // Set target heading to the one just gotten off compass. |
|
| 216 | yawAngleDiff = 0; |
217 | targetHeading = heading; |
| 217 | } |
218 | } |
| 218 | } else { |
219 | } else { |
| 219 | // 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? |
| 220 | // 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. |
| 221 | MKFlags |= (MKFLAG_FLY); |
222 | MKFlags |= (MKFLAG_FLY); |
| Line 226... | Line 227... | ||
| 226 | } // end else (not bad signal case) |
227 | } // end else (not bad signal case) |
| Line 227... | Line 228... | ||
| 227 | 228 | ||
| 228 | /************************************************************************/ |
229 | /************************************************************************/ |
| 229 | /* Yawing */ |
230 | /* Yawing */ |
| - | 231 | /************************************************************************/ |
|
| 230 | /************************************************************************/ |
232 | /* |
| 231 | if (abs(controls[CONTROL_YAW]) > 4 * staticParams.stickYawP) { // yaw stick is activated |
233 | if (abs(controls[CONTROL_YAW]) > 4 * staticParams.stickYawP) { // yaw stick is activated |
| 232 | ignoreCompassTimer = 1000; |
234 | ignoreCompassTimer = 1000; |
| 233 | if (!(staticParams.bitConfig & CFG_COMPASS_FIX)) { |
235 | if (!(staticParams.bitConfig & CFG_COMPASS_FIX)) { |
| - | 236 | //targetHeading = heading; |
|
| 234 | updateCompassCourse = 1; |
237 | // YGBSM!!! |
| 235 | } |
238 | } |
| - | 239 | } |
|
| Line 236... | Line -... | ||
| 236 | } |
- | |
| 237 | - | ||
| 238 | // yawControlRate = controlYaw; |
- | |
| 239 | // Trim drift of yawAngleDiff with controlYaw. |
- | |
| 240 | // TODO: We want NO feedback of control related stuff to the attitude related stuff. |
240 | */ |
| 241 | // This seems to be used as: Difference desired <--> real heading. |
- | |
| 242 | yawAngleDiff -= controls[CONTROL_YAW]; |
- | |
| 243 | - | ||
| Line 244... | Line -... | ||
| 244 | // limit the effect |
- | |
| 245 | CHECK_MIN_MAX(yawAngleDiff, -50000, 50000); |
- | |
| 246 | - | ||
| 247 | /************************************************************************/ |
- | |
| 248 | /* Compass is currently not supported. */ |
241 | |
| 249 | /************************************************************************/ |
- | |
| Line 250... | Line 242... | ||
| 250 | if (staticParams.bitConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) { |
242 | targetHeading -= controls[CONTROL_YAW]; |
| 251 | updateCompass(); |
243 | |
| 252 | } |
244 | debugOut.analog[28] = targetHeading; |
| 253 | 245 | ||
| Line 269... | Line 261... | ||
| 269 | /* Calculate control feedback from angle (gyro integral) */ |
261 | /* Calculate control feedback from angle (gyro integral) */ |
| 270 | /* and angular velocity (gyro signal) */ |
262 | /* and angular velocity (gyro signal) */ |
| 271 | /************************************************************************/ |
263 | /************************************************************************/ |
| 272 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
264 | // The P-part is the P of the PID controller. That's the angle integrals (not rates). |
| 273 | for (axis = PITCH; axis <= ROLL; axis++) { |
265 | for (axis = PITCH; axis <= ROLL; axis++) { |
| 274 | PDPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
266 | PDPart[axis] = attitude[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral |
| 275 | PDPart[axis] += ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)); |
267 | PDPart[axis] += ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)); |
| 276 | PDPart[axis] += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
268 | PDPart[axis] += (differential[axis] * (int16_t) dynamicParams.gyroD) / 16; |
| 277 | CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
269 | CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT); |
| 278 | } |
270 | } |
| Line -... | Line 271... | ||
| - | 271 | ||
| - | 272 | int32_t headingDiff = heading - targetHeading; // apparently yaw is reverse on output. |
|
| - | 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); |
|
| - | 278 | debugOut.analog[29] = headingDiff; |
|
| 279 | 279 | ||
| 280 | PDPartYaw = (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
280 | PDPartYaw = (int32_t) (headingDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING)); |
| Line 281... | Line 281... | ||
| 281 | PDPartYaw += (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING); |
281 | PDPartYaw += (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING); |
| 282 | 282 | ||
| Line 430... | Line 430... | ||
| 430 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
430 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 431 | // Debugging |
431 | // Debugging |
| 432 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
432 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 433 | if (!(--debugDataTimer)) { |
433 | if (!(--debugDataTimer)) { |
| 434 | debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
434 | debugDataTimer = 24; // update debug outputs at 488 / 24 = 20.3 Hz. |
| 435 | debugOut.analog[0] = angle[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10); // in 0.1 deg |
435 | debugOut.analog[0] = attitude[PITCH] / (GYRO_DEG_FACTOR_PITCHROLL/10); // in 0.1 deg |
| 436 | debugOut.analog[1] = angle[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10); // in 0.1 deg |
436 | debugOut.analog[1] = attitude[ROLL] / (GYRO_DEG_FACTOR_PITCHROLL/10); // in 0.1 deg |
| 437 | debugOut.analog[2] = yawGyroHeading / GYRO_DEG_FACTOR_YAW; |
437 | debugOut.analog[2] = heading / GYRO_DEG_FACTOR_YAW; |
| 438 | } |
438 | } |
| 439 | } |
439 | } |