Subversion Repositories FlightCtrl

Rev

Rev 1874 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1874 Rev 1908
Line 293... Line 293...
293
  // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities
293
  // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities
294
  // are less than ....., or reintroduce Kalman.
294
  // are less than ....., or reintroduce Kalman.
295
  // Well actually the Z axis acc. check is not so silly.
295
  // Well actually the Z axis acc. check is not so silly.
296
  uint8_t axis;
296
  uint8_t axis;
297
  int32_t temp;
297
  int32_t temp;
-
 
298
  DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER;
-
 
299
  DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER;
-
 
300
 
298
  if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z]
301
  if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z]
299
      <= dynamicParams.UserParams[7]) {
302
      <= dynamicParams.UserParams[7]) {
300
    DebugOut.Digital[0] |= DEBUG_ACC0THORDER;
-
 
Line 301... Line 303...
301
 
303
 
302
    uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!!
-
 
303
    uint8_t debugFullWeight = 1;
304
    uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!!
Line -... Line 305...
-
 
305
    int32_t accDerived;
304
    int32_t accDerived;
306
 
305
 
307
    /*
306
    if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active
308
    if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active
307
      permilleAcc /= 2;
309
      permilleAcc /= 2;
308
      debugFullWeight = 0;
310
      debugFullWeight = 0;
309
    }
311
    }
310
 
312
    */
-
 
313
    if (controlActivity > 10000) { // reduce effect during stick commands
-
 
314
      permilleAcc /= 4;
311
    if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands
315
      DebugOut.Digital[0] |= DEBUG_ACC0THORDER;
-
 
316
      if (controlActivity > 20000) { // reduce effect during stick commands
-
 
317
        permilleAcc /= 4;
312
      permilleAcc /= 2;
318
        DebugOut.Digital[1] |= DEBUG_ACC0THORDER;
Line 313... Line -...
313
      debugFullWeight = 0;
-
 
314
    }
-
 
315
 
-
 
316
    if (debugFullWeight)
-
 
317
      DebugOut.Digital[1] |= DEBUG_ACC0THORDER;
-
 
318
    else
319
      }
319
      DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER;
320
    }
320
 
321
 
321
    /*
322
    /*
322
     * Add to each sum: The amount by which the angle is changed just below.
323
     * Add to each sum: The amount by which the angle is changed just below.
Line 330... Line 331...
330
      angle[axis] = ((int32_t) (1000L - permilleAcc) * temp
331
      angle[axis] = ((int32_t) (1000L - permilleAcc) * temp
331
          + (int32_t) permilleAcc * accDerived) / 1000L;
332
          + (int32_t) permilleAcc * accDerived) / 1000L;
332
      correctionSum[axis] += angle[axis] - temp;
333
      correctionSum[axis] += angle[axis] - temp;
333
    }
334
    }
334
  } else {
335
  } else {
335
    DebugOut.Digital[0] &= ~DEBUG_ACC0THORDER;
-
 
336
    DebugOut.Digital[1] &= ~DEBUG_ACC0THORDER;
-
 
337
    DebugOut.Analog[9] = 0;
336
    DebugOut.Analog[9] = 0;
338
    DebugOut.Analog[10] = 0;
337
    DebugOut.Analog[10] = 0;
Line 339... Line 338...
339
 
338
 
340
    DebugOut.Analog[16] = 0;
339
    DebugOut.Analog[16] = 0;
Line 450... Line 449...
450
    // As readable formula:
449
    // As readable formula:
451
    // w = dynamicParams.CompassYawEffect * (1-w/32);
450
    // w = dynamicParams.CompassYawEffect * (1-w/32);
Line 452... Line 451...
452
 
451
 
453
    if (w >= 0) { // maxAttitudeAngle < 32
452
    if (w >= 0) { // maxAttitudeAngle < 32
454
      if (!ignoreCompassTimer) {
453
      if (!ignoreCompassTimer) {
-
 
454
        /*v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8;*/
455
        v = 64 + (maxControl[PITCH] + maxControl[ROLL]) / 8;
455
        v = 64 + controlActivity / 100;
456
        // yawGyroHeading - compassCourse on a -180..179 degree interval.
456
        // yawGyroHeading - compassCourse on a -180..179 degree interval.
457
        r
457
        r
458
            = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse)
458
            = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse)
459
                % 360) - 180;
459
                % 360) - 180;