Rev 1775 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1775 | Rev 1805 | ||
---|---|---|---|
Line 72... | Line 72... | ||
72 | #include "controlMixer.h" |
72 | #include "controlMixer.h" |
Line 73... | Line 73... | ||
73 | 73 | ||
74 | // For Servo_On / Off |
74 | // For Servo_On / Off |
Line 75... | Line -... | ||
75 | // #include "timer2.h" |
- | |
76 | - | ||
77 | #ifdef USE_MK3MAG |
- | |
78 | #include "mk3mag.h" |
- | |
79 | #include "gps.h" |
75 | // #include "timer2.h" |
Line 80... | Line 76... | ||
80 | #endif |
76 | |
81 | #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
77 | #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
82 | 78 | ||
Line 111... | Line 107... | ||
111 | */ |
107 | */ |
112 | int32_t angle[2], yawAngleDiff; |
108 | int32_t angle[2], yawAngleDiff; |
Line 113... | Line 109... | ||
113 | 109 | ||
Line 114... | Line 110... | ||
114 | int readingHeight = 0; |
110 | int readingHeight = 0; |
- | 111 | ||
115 | 112 | // Yaw angle and compass stuff. |
|
- | 113 | ||
- | 114 | // This is updated/written from MM3. Negative angle indicates invalid data. |
|
- | 115 | int16_t compassHeading = -1; |
|
116 | // compass course |
116 | |
- | 117 | // This is NOT updated from MM3. Negative angle indicates invalid data. |
|
- | 118 | int16_t compassCourse = -1; |
|
- | 119 | ||
117 | int16_t compassHeading = -1; // negative angle indicates invalid data. |
120 | // The difference between the above 2 (heading - course) on a -180..179 degree interval. |
- | 121 | // Not necessary. Never read anywhere. |
|
118 | int16_t compassCourse = -1; |
122 | // int16_t compassOffCourse = 0; |
119 | int16_t compassOffCourse = 0; |
123 | |
120 | uint16_t updateCompassCourse = 0; |
124 | uint8_t updateCompassCourse = 0; |
- | 125 | uint8_t compassCalState = 0; |
|
121 | uint8_t compassCalState = 0; |
126 | uint16_t ignoreCompassTimer = 500; |
122 | uint16_t badCompassHeading = 500; |
127 | |
Line 123... | Line 128... | ||
123 | int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass |
128 | int32_t yawGyroHeading; // Yaw Gyro Integral supported by compass |
124 | int16_t yawGyroDrift; |
129 | int16_t yawGyroDrift; |
Line 200... | Line 205... | ||
200 | *************************************************************************/ |
205 | *************************************************************************/ |
201 | void getAnalogData(void) { |
206 | void getAnalogData(void) { |
202 | uint8_t axis; |
207 | uint8_t axis; |
Line 203... | Line 208... | ||
203 | 208 | ||
204 | for (axis=PITCH; axis <=ROLL; axis++) { |
209 | for (axis = PITCH; axis <= ROLL; axis++) { |
- | 210 | rate_PID[axis] = (gyro_PID[axis] + driftComp[axis]) |
|
205 | rate_PID[axis] = (gyro_PID[axis] + driftComp[axis]) / HIRES_GYRO_INTEGRATION_FACTOR; |
211 | / HIRES_GYRO_INTEGRATION_FACTOR; |
- | 212 | rate_ATT[axis] = (gyro_ATT[axis] + driftComp[axis]) |
|
206 | rate_ATT[axis] = (gyro_ATT[axis] + driftComp[axis]) / HIRES_GYRO_INTEGRATION_FACTOR; |
213 | / HIRES_GYRO_INTEGRATION_FACTOR; |
207 | differential[axis] = gyroD[axis]; |
214 | differential[axis] = gyroD[axis]; |
208 | averageAcc[axis] += acc[axis]; |
215 | averageAcc[axis] += acc[axis]; |
Line 209... | Line 216... | ||
209 | } |
216 | } |
Line 227... | Line 234... | ||
227 | int16_t cospitch = int_cos(angle[PITCH]); |
234 | int16_t cospitch = int_cos(angle[PITCH]); |
228 | int16_t cosroll = int_cos(angle[ROLL]); |
235 | int16_t cosroll = int_cos(angle[ROLL]); |
229 | int16_t sinroll = int_sin(angle[ROLL]); |
236 | int16_t sinroll = int_sin(angle[ROLL]); |
230 | int16_t tanpitch = int_tan(angle[PITCH]); |
237 | int16_t tanpitch = int_tan(angle[PITCH]); |
231 | #define ANTIOVF 512 |
238 | #define ANTIOVF 512 |
232 | ACRate[PITCH] = ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t)yawRate * sinroll) / (int32_t)MATH_UNIT_FACTOR; |
239 | ACRate[PITCH] = ((int32_t) rate_ATT[PITCH] * cosroll - (int32_t) yawRate |
- | 240 | * sinroll) / (int32_t) MATH_UNIT_FACTOR; |
|
233 | ACRate[ROLL] = rate_ATT[ROLL] + (((int32_t)rate_ATT[PITCH] * sinroll / ANTIOVF * tanpitch + (int32_t)yawRate * int_cos(angle[ROLL]) / ANTIOVF * tanpitch) / ((int32_t)MATH_UNIT_FACTOR / ANTIOVF * MATH_UNIT_FACTOR)); |
241 | ACRate[ROLL] = rate_ATT[ROLL] + (((int32_t) rate_ATT[PITCH] * sinroll |
- | 242 | / ANTIOVF * tanpitch + (int32_t) yawRate * int_cos(angle[ROLL]) |
|
- | 243 | / ANTIOVF * tanpitch) / ((int32_t) MATH_UNIT_FACTOR / ANTIOVF |
|
- | 244 | * MATH_UNIT_FACTOR)); |
|
234 | ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch + ((int32_t)yawRate * cosroll) / cospitch; |
245 | ACYawRate = ((int32_t) rate_ATT[PITCH] * sinroll) / cospitch |
- | 246 | + ((int32_t) yawRate * cosroll) / cospitch; |
|
235 | } |
247 | } |
Line 236... | Line 248... | ||
236 | 248 | ||
237 | // 480 usec with axis coupling - almost no time without. |
249 | // 480 usec with axis coupling - almost no time without. |
238 | void integrate(void) { |
250 | void integrate(void) { |
Line 285... | Line 297... | ||
285 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
297 | // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities |
286 | // are less than ....., or reintroduce Kalman. |
298 | // are less than ....., or reintroduce Kalman. |
287 | // Well actually the Z axis acc. check is not so silly. |
299 | // Well actually the Z axis acc. check is not so silly. |
288 | uint8_t axis; |
300 | uint8_t axis; |
289 | int32_t correction; |
301 | int32_t correction; |
290 | if(!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] <= dynamicParams.UserParams[7]) { |
302 | if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] |
- | 303 | <= dynamicParams.UserParams[7]) { |
|
291 | DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
304 | DebugOut.Digital[0] |= DEBUG_ACC0THORDER; |
Line 292... | Line 305... | ||
292 | 305 | ||
293 | uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!! |
306 | uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!! |
294 | uint8_t debugFullWeight = 1; |
307 | uint8_t debugFullWeight = 1; |
Line 312... | Line 325... | ||
312 | /* |
325 | /* |
313 | * Add to each sum: The amount by which the angle is changed just below. |
326 | * Add to each sum: The amount by which the angle is changed just below. |
314 | */ |
327 | */ |
315 | for (axis=PITCH; axis<=ROLL; axis++) { |
328 | for (axis = PITCH; axis <= ROLL; axis++) { |
316 | accDerived = getAngleEstimateFromAcc(axis); |
329 | accDerived = getAngleEstimateFromAcc(axis); |
317 | DebugOut.Analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL; |
330 | DebugOut.Analog[9 + axis] = (10 * accDerived) |
- | 331 | / GYRO_DEG_FACTOR_PITCHROLL; |
|
Line 318... | Line 332... | ||
318 | 332 | ||
319 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
333 | // 1000 * the correction amount that will be added to the gyro angle in next line. |
320 | correction = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000; |
334 | correction = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000; |
- | 335 | angle[axis] = ((int32_t) (1000L - permilleAcc) * angle[axis] |
|
321 | angle[axis] = ((int32_t)(1000L - permilleAcc) * angle[axis] + (int32_t)permilleAcc * accDerived) / 1000L; |
336 | + (int32_t) permilleAcc * accDerived) / 1000L; |
322 | correctionSum[axis] += angle[axis] - correction; |
337 | correctionSum[axis] += angle[axis] - correction; |
323 | DebugOut.Analog[16+axis] = angle[axis] - correction; |
338 | DebugOut.Analog[16 + axis] = angle[axis] - correction; |
324 | } |
339 | } |
325 | } else { |
340 | } else { |
Line 353... | Line 368... | ||
353 | uint8_t axis; |
368 | uint8_t axis; |
354 | if (! --timer) { |
369 | if (!--timer) { |
355 | timer = DRIFTCORRECTION_TIME; |
370 | timer = DRIFTCORRECTION_TIME; |
356 | for (axis=PITCH; axis<=ROLL; axis++) { |
371 | for (axis = PITCH; axis <= ROLL; axis++) { |
357 | // Take the sum of corrections applied, add it to delta |
372 | // Take the sum of corrections applied, add it to delta |
- | 373 | deltaCorrection = (correctionSum[axis] |
|
358 | deltaCorrection = (correctionSum[axis] * HIRES_GYRO_INTEGRATION_FACTOR + DRIFTCORRECTION_TIME / 2) / DRIFTCORRECTION_TIME; |
374 | * HIRES_GYRO_INTEGRATION_FACTOR + DRIFTCORRECTION_TIME / 2) |
- | 375 | / DRIFTCORRECTION_TIME; |
|
359 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
376 | // Add the delta to the compensation. So positive delta means, gyro should have higher value. |
360 | driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
377 | driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim; |
361 | CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
378 | CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp); |
362 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
379 | // DebugOut.Analog[11 + axis] = correctionSum[axis]; |
Line 363... | Line 380... | ||
363 | 380 | ||
- | 381 | DebugOut.Analog[18 + axis] = deltaCorrection |
|
364 | DebugOut.Analog[18+axis] = deltaCorrection / staticParams.GyroAccTrim; |
382 | / staticParams.GyroAccTrim; |
Line 365... | Line 383... | ||
365 | DebugOut.Analog[28+axis] = driftComp[axis]; |
383 | DebugOut.Analog[28 + axis] = driftComp[axis]; |
366 | 384 | ||
367 | correctionSum[axis] = 0; |
385 | correctionSum[axis] = 0; |
Line 401... | Line 419... | ||
401 | int16_t w, v, r,correction, error; |
419 | int16_t w, v, r, correction, error; |
Line 402... | Line 420... | ||
402 | 420 | ||
403 | if(compassCalState && !(MKFlags & MKFLAG_MOTOR_RUN)) { |
421 | if (compassCalState && !(MKFlags & MKFLAG_MOTOR_RUN)) { |
404 | if (controlMixer_testCompassCalState()) { |
422 | if (controlMixer_testCompassCalState()) { |
- | 423 | compassCalState++; |
|
405 | compassCalState++; |
424 | if (compassCalState < 5) |
- | 425 | beepNumber(compassCalState); |
|
406 | if(compassCalState < 5) beepNumber(compassCalState); |
426 | else |
407 | else beep(1000); |
427 | beep(1000); |
408 | } |
428 | } |
409 | } else { |
429 | } else { |
410 | // get maximum attitude angle |
430 | // get maximum attitude angle |
411 | w = abs(angle[PITCH] / 512); |
431 | w = abs(angle[PITCH] / 512); |
412 | v = abs(angle[ROLL] / 512); |
432 | v = abs(angle[ROLL] / 512); |
- | 433 | if (v > w) |
|
413 | if(v > w) w = v; |
434 | w = v; |
414 | correction = w / 8 + 1; |
435 | correction = w/8 + 1; |
- | 436 | // calculate the deviation of the yaw gyro heading and the compass heading |
|
415 | // calculate the deviation of the yaw gyro heading and the compass heading |
437 | if (compassHeading < 0) |
416 | if (compassHeading < 0) error = 0; // disable yaw drift compensation if compass heading is undefined |
- | |
- | 438 | error = 0; // disable yaw drift compensation if compass heading is undefined |
|
417 | else error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) % 360) - 180; |
439 | else |
418 | if(abs(yawRate) > 128) { // spinning fast |
440 | if (abs(yawRate) > 128) { // spinning fast |
- | 441 | error = 0; |
|
- | 442 | } else { |
|
- | 443 | // compassHeading - yawGyroHeading, on a -180..179 deg interval. |
|
419 | error = 0; |
444 | error = ((540 + compassHeading - (yawGyroHeading / GYRO_DEG_FACTOR_YAW)) % 360) - 180; |
420 | } |
445 | } |
421 | if(!badCompassHeading && w < 25) { |
446 | if (!ignoreCompassTimer && w < 25) { |
- | 447 | yawGyroDrift += error; |
|
422 | yawGyroDrift += error; |
448 | // Basically this gets set if we are in "fix" mode, and when starting. |
423 | if(updateCompassCourse) { |
449 | if (updateCompassCourse) { |
424 | beep(200); |
450 | beep(200); |
425 | yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW; |
451 | yawGyroHeading = (int32_t) compassHeading * GYRO_DEG_FACTOR_YAW; |
426 | compassCourse = compassHeading; //(int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
452 | compassCourse = compassHeading; //(int16_t)(yawGyroHeading / GYRO_DEG_FACTOR_YAW); |
427 | updateCompassCourse = 0; |
453 | updateCompassCourse = 0; |
428 | } |
454 | } |
429 | } |
455 | } |
- | 456 | yawGyroHeading += (error * 8) / correction; |
|
- | 457 | ||
430 | yawGyroHeading += (error * 8) / correction; |
458 | /* |
431 | w = (w * dynamicParams.CompassYawEffect) / 32; |
459 | w = (w * dynamicParams.CompassYawEffect) / 32; |
- | 460 | w = dynamicParams.CompassYawEffect - w; |
|
- | 461 | */ |
|
- | 462 | w = dynamicParams.CompassYawEffect - (w * dynamicParams.CompassYawEffect) / 32; |
|
432 | w = dynamicParams.CompassYawEffect - w; |
463 | |
- | 464 | // As readable formula: |
|
- | 465 | // w = dynamicParams.CompassYawEffect * (1-w/32); |
|
- | 466 | ||
433 | if(w >= 0) { |
467 | if (w >= 0) { // maxAttitudeAngle < 32 |
434 | if(!badCompassHeading) { |
468 | if (!ignoreCompassTimer) { |
435 | v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8; |
469 | v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8; |
436 | // calc course deviation |
470 | // yawGyroHeading - compassCourse on a -180..179 degree interval. |
437 | r = ((540 + (yawGyroHeading / GYRO_DEG_FACTOR_YAW) - compassCourse) % 360) - 180; |
471 | r = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse) % 360) - 180; |
438 | v = (r * w) / v; // align to compass course |
472 | v = (r * w) / v; // align to compass course |
439 | // limit yaw rate |
473 | // limit yaw rate |
440 | w = 3 * dynamicParams.CompassYawEffect; |
474 | w = 3 * dynamicParams.CompassYawEffect; |
- | 475 | if (v > w) |
|
441 | if (v > w) v = w; |
476 | v = w; |
- | 477 | else if (v < -w) |
|
442 | else if (v < -w) v = -w; |
478 | v = -w; |
443 | yawAngleDiff += v; |
- | |
444 | } |
- | |
445 | else |
479 | yawAngleDiff += v; |
446 | { // wait a while |
480 | } else { // wait a while |
447 | badCompassHeading--; |
481 | ignoreCompassTimer--; |
448 | } |
482 | } |
449 | } else { // ignore compass at extreme attitudes for a while |
483 | } else { // ignore compass at extreme attitudes for a while |
450 | badCompassHeading = 500; |
484 | ignoreCompassTimer = 500; |
451 | } |
485 | } |
452 | } |
486 | } |
Line 453... | Line 487... | ||
453 | } |
487 | } |