Subversion Repositories FlightCtrl

Rev

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

Rev 2052 Rev 2055
Line 199... Line 199...
199
  heading += ACYawRate;
199
  heading += ACYawRate;
200
  intervalWrap(&heading, YAWOVER360);
200
  intervalWrap(&heading, YAWOVER360);
Line 201... Line 201...
201
 
201
 
Line 202... Line -...
202
  headingError += ACYawRate;
-
 
203
 
-
 
204
  debugOut.analog[27] = heading / 100;
202
  headingError += ACYawRate;
205
 
203
 
206
  /*
204
  /*
207
   * Pitch axis integration and range boundary wrap.
205
   * Pitch axis integration and range boundary wrap.
208
   */
206
   */
Line 266... Line 264...
266
    /*
264
    /*
267
     * Add to each sum: The amount by which the angle is changed just below.
265
     * Add to each sum: The amount by which the angle is changed just below.
268
     */
266
     */
269
    for (axis = PITCH; axis <= ROLL; axis++) {
267
    for (axis = PITCH; axis <= ROLL; axis++) {
270
      accDerived = getAngleEstimateFromAcc(axis);
268
      accDerived = getAngleEstimateFromAcc(axis);
271
      debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
269
      //debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
272
      // 1000 * the correction amount that will be added to the gyro angle in next line.
270
      // 1000 * the correction amount that will be added to the gyro angle in next line.
273
      temp = attitude[axis];
271
      temp = attitude[axis];
274
      attitude[axis] = ((int32_t) (1000L - permilleAcc) * temp
272
      attitude[axis] = ((int32_t) (1000L - permilleAcc) * temp
275
          + (int32_t) permilleAcc * accDerived) / 1000L;
273
          + (int32_t) permilleAcc * accDerived) / 1000L;
276
      correctionSum[axis] += attitude[axis] - temp;
274
      correctionSum[axis] += attitude[axis] - temp;
277
    }
275
    }
278
  } else {
276
  } else {
279
    debugOut.analog[9] = 0;
-
 
280
    debugOut.analog[10] = 0;
-
 
281
    // experiment: Kill drift compensation updates when not flying smooth.
277
    // experiment: Kill drift compensation updates when not flying smooth.
282
    // correctionSum[PITCH] = correctionSum[ROLL] = 0;
278
    // correctionSum[PITCH] = correctionSum[ROLL] = 0;
283
    debugOut.digital[0] |= DEBUG_ACC0THORDER;
279
    debugOut.digital[0] |= DEBUG_ACC0THORDER;
284
  }
280
  }
285
}
281
}
Line 355... Line 351...
355
 
351
 
356
void correctHeadingToMagnetic(void) {
352
void correctHeadingToMagnetic(void) {
Line 357... Line 353...
357
  int32_t error;
353
  int32_t error;
358
 
354
 
359
  if (commands_isCalibratingCompass()) {
355
  if (commands_isCalibratingCompass()) {
360
    debugOut.analog[29] = 1;
356
    //debugOut.analog[29] = 1;
Line 361... Line 357...
361
    return;
357
    return;
362
  }
358
  }
363
 
359
 
364
  // Compass is off, skip.
360
  // Compass is off, skip.
Line 365... Line 361...
365
  // Naaah this is assumed.
361
  // Naaah this is assumed.
366
  // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE))
362
  // if (!(staticParams.bitConfig & CFG_COMPASS_ACTIVE))
367
  //     return;
363
  //     return;
368
 
364
 
369
  // Compass is invalid, skip.
365
  // Compass is invalid, skip.
Line 370... Line 366...
370
  if (magneticHeading < 0) {
366
  if (magneticHeading < 0) {
371
    debugOut.analog[29] = 2;
367
    //debugOut.analog[29] = 2;
372
    return;
368
    return;
373
  }
369
  }
374
 
370
 
Line 375... Line 371...
375
  // Spinning fast, skip
371
  // Spinning fast, skip
376
  if (abs(yawRate) > 128) {
372
  if (abs(yawRate) > 128) {
377
    debugOut.analog[29] = 3;
373
    //debugOut.analog[29] = 3;
378
    return;
374
    return;
379
  }
375
  }
380
 
376
 
Line 381... Line 377...
381
  // Otherwise invalidated, skip
377
  // Otherwise invalidated, skip
382
  if (ignoreCompassTimer) {
378
  if (ignoreCompassTimer) {
Line 394... Line 390...
394
  // better resolution of the gyros to the worse resolution of the compass all the time.
390
  // better resolution of the gyros to the worse resolution of the compass all the time.
395
  // The correction should really only serve to compensate for gyro drift.
391
  // The correction should really only serve to compensate for gyro drift.
396
  if(abs(error) < GYRO_DEG_FACTOR_YAW) return;
392
  if(abs(error) < GYRO_DEG_FACTOR_YAW) return;
Line 397... Line 393...
397
 
393
 
398
  int32_t correction = (error * staticParams.compassYawCorrection) >> 8;
394
  int32_t correction = (error * staticParams.compassYawCorrection) >> 8;
Line 399... Line 395...
399
  debugOut.analog[30] = correction;
395
  //debugOut.analog[30] = correction;
400
 
396
 
401
  // The correction is added both to current heading (the direction in which the copter thinks it is pointing)
397
  // The correction is added both to current heading (the direction in which the copter thinks it is pointing)
402
  // and to the target heading (the direction to which it maneuvers to point). That means, this correction has
398
  // and to the target heading (the direction to which it maneuvers to point). That means, this correction has
Line 407... Line 403...
407
 
403
 
408
  // If we want a transparent flight wrt. compass correction (meaning the copter does not change attitude all
404
  // If we want a transparent flight wrt. compass correction (meaning the copter does not change attitude all
409
  // when the compass corrects the heading - it only corrects numbers!) we want to add:
405
  // when the compass corrects the heading - it only corrects numbers!) we want to add:
410
  // This will however cause drift to remain uncorrected!
406
  // This will however cause drift to remain uncorrected!
411
  // headingError += correction;
407
  // headingError += correction;
412
  debugOut.analog[29] = 0;
408
  //debugOut.analog[29] = 0;
413
}
409
}
Line 414... Line 410...
414
#endif
410
#endif
415
 
411