Rev 2045 | Rev 2049 | Go to most recent revision | Show entire file | Regard 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 | */ |
Line 241... | Line -... | ||
241 | // This seems to be used as: Difference desired <--> real heading. |
- | |
242 | yawAngleDiff -= controls[CONTROL_YAW]; |
241 | |
243 | - | ||
244 | // limit the effect |
- | |
245 | CHECK_MIN_MAX(yawAngleDiff, -50000, 50000); |
- | |
246 | - | ||
247 | /************************************************************************/ |
- | |
248 | /* Compass is currently not supported. */ |
- | |
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 | } |