Subversion Repositories FlightCtrl

Rev

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

Rev 2088 Rev 2089
Line 74... Line 74...
74
 */
74
 */
75
int16_t driftComp[2] = { 0, 0 }, driftCompYaw = 0;
75
int16_t driftComp[2] = { 0, 0 }, driftCompYaw = 0;
76
// int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0;
76
// int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0;
77
// int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw;
77
// int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw;
78
// int16_t dynamicCalCount;
78
// int16_t dynamicCalCount;
-
 
79
// uint16_t accVector;
Line 79... Line 80...
79
 
80
 
Line 80... Line 81...
80
uint16_t accVector;
81
// uint32_t gyroActivity;
81
 
82
 
82
/************************************************************************
83
/************************************************************************
83
 * Set inclination angles from the acc. sensor data.                    
84
 * Set inclination angles from the acc. sensor data.                    
Line 117... Line 118...
117
  // Calibrate hardware.
118
  // Calibrate hardware.
118
  analog_setNeutral();
119
  analog_setNeutral();
Line 119... Line 120...
119
 
120
 
120
  // reset gyro integrals to acc guessing
121
  // reset gyro integrals to acc guessing
-
 
122
  setStaticAttitudeAngles();
121
  setStaticAttitudeAngles();
123
 
122
#ifdef USE_MK3MAG
124
#ifdef USE_MK3MAG
123
  attitude_resetHeadingToMagnetic();
125
  attitude_resetHeadingToMagnetic();
124
#endif
126
#endif
125
  // Servo_On(); //enable servo output
127
  // Servo_On(); //enable servo output
Line 207... Line 209...
207
      attitude[axis] += PITCHROLLOVER360;
209
      attitude[axis] += PITCHROLLOVER360;
208
    }
210
    }
209
  }
211
  }
210
}
212
}
Line 211... Line -...
211
 
-
 
212
void correctIntegralsByAcc0thOrder_old(void) {
-
 
213
  uint8_t axis;
-
 
214
  int32_t temp;
-
 
215
 
-
 
216
  uint8_t ca = controlActivity >> 8;
-
 
217
  uint8_t highControlActivity = (ca > staticParams.maxControlActivity);
-
 
218
 
-
 
219
  if (highControlActivity) {
-
 
220
    debugOut.digital[1] |= DEBUG_ACC0THORDER;
-
 
221
  } else {
-
 
222
    debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
-
 
223
  }
-
 
224
 
-
 
225
  if (accVector <= staticParams.maxAccVector) {
-
 
226
    debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
-
 
227
 
-
 
228
    uint8_t permilleAcc = staticParams.zerothOrderCorrection / 8;
-
 
229
    int32_t accDerived;
-
 
230
 
-
 
231
    /*
-
 
232
     if ((controlYaw < -64) || (controlYaw > 64)) { // reduce further if yaw stick is active
-
 
233
     permilleAcc /= 2;
-
 
234
     debugFullWeight = 0;
-
 
235
     }
-
 
236
 
-
 
237
     if ((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands. Replace by controlActivity.
-
 
238
     permilleAcc /= 2;
-
 
239
     debugFullWeight = 0;
-
 
240
     */
-
 
241
 
-
 
242
    if (highControlActivity) { // reduce effect during stick control activity
-
 
243
      permilleAcc /= 4;
-
 
244
      if (controlActivity > staticParams.maxControlActivity * 2) { // reduce effect during stick control activity
-
 
245
        permilleAcc /= 4;
-
 
246
      }
-
 
247
    }
-
 
248
 
-
 
249
    /*
-
 
250
     * Add to each sum: The amount by which the angle is changed just below.
-
 
251
     */
-
 
252
    for (axis = PITCH; axis <= ROLL; axis++) {
-
 
253
      accDerived = getAngleEstimateFromAcc(axis);
-
 
254
      //debugOut.analog[9 + axis] = accDerived / (GYRO_DEG_FACTOR_PITCHROLL / 10);
-
 
255
      // 1000 * the correction amount that will be added to the gyro angle in next line.
-
 
256
      temp = attitude[axis];
-
 
257
      attitude[axis] = ((int32_t) (1000L - permilleAcc) * temp
-
 
258
          + (int32_t) permilleAcc * accDerived) / 1000L;
-
 
259
      correctionSum[axis] += attitude[axis] - temp;
-
 
260
    }
-
 
261
  } else {
-
 
262
    // experiment: Kill drift compensation updates when not flying smooth.
-
 
263
    // correctionSum[PITCH] = correctionSum[ROLL] = 0;
-
 
264
    debugOut.digital[0] |= DEBUG_ACC0THORDER;
-
 
265
  }
-
 
266
}
-
 
267
 
-
 
268
 
213
 
269
/************************************************************************
214
/************************************************************************
270
 * A kind of 0'th order integral correction, that corrects the integrals
215
 * A kind of 0'th order integral correction, that corrects the integrals
271
 * directly. This is the "gyroAccFactor" stuff in the original code.
216
 * directly. This is the "gyroAccFactor" stuff in the original code.
272
 * There is (there) also a drift compensation
217
 * There is (there) also a drift compensation
273
 * - it corrects the differential of the integral = the gyro offsets.
218
 * - it corrects the differential of the integral = the gyro offsets.
274
 * That should only be necessary with drifty gyros like ENC-03.
219
 * That should only be necessary with drifty gyros like ENC-03.
275
 ************************************************************************/
220
 ************************************************************************/
276
#define LOG_DIVIDER 12
221
#define LOG_DIVIDER 12
277
#define DIVIDER (1L << LOG_DIVIDER)
222
#define DIVIDER (1L << LOG_DIVIDER)
278
void correctIntegralsByAcc0thOrder_new(void) {
223
void correctIntegralsByAcc0thOrder(void) {
279
  // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities
224
  // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities
280
  // are less than ....., or reintroduce Kalman.
225
  // are less than ....., or reintroduce Kalman.
281
  // Well actually the Z axis acc. check is not so silly.
226
  // Well actually the Z axis acc. check is not so silly.
282
  uint8_t axis;
227
  uint8_t axis;
Line 283... Line -...
283
  int32_t temp;
-
 
284
 
-
 
285
  // for debug LEDs, to be removed with that.
-
 
286
  static uint8_t controlActivityFlash=1;
-
 
287
  static uint8_t accFlash=1;
-
 
288
#define CF_MAX 10
-
 
289
  // [1..n[=off    [n..10]=on
-
 
290
  // 1 -->1=on, 2=on, ..., 10=on
-
 
291
  // 2 -->1=off,2=on, ..., 10=on
-
 
292
  // 10-->1=off,2=off,..., 10=on
228
  int32_t temp;
293
  // 11-->1=off,2=off,..., 10=off
-
 
294
  uint16_t ca = controlActivity >> 6;
-
 
295
  uint8_t controlActivityWeighted = ca / staticParams.zerothOrderCorrectionControlTolerance;
-
 
296
  if (!controlActivityWeighted) controlActivityWeighted = 1;
-
 
297
  uint8_t accVectorWeighted = accVector / staticParams.zerothOrderCorrectionAccTolerance;
-
 
298
  if (!accVectorWeighted) accVectorWeighted = 1;
-
 
299
 
-
 
300
  uint8_t accPart = staticParams.zerothOrderCorrection;
-
 
301
  int32_t accDerived;
-
 
302
 
-
 
303
  debugOut.analog[14] = controlActivity;
-
 
304
  debugOut.analog[15] = accVector;
-
 
305
 
-
 
306
  debugOut.analog[20] = controlActivityWeighted;
229
 
Line 307... Line 230...
307
  debugOut.analog[21] = accVectorWeighted;
230
  uint16_t ca = gyroActivity >> 8;
308
  debugOut.analog[24] = accVector;
231
  debugOut.analog[14] = ca;
Line 309... Line 232...
309
 
232
 
310
  accPart /=  controlActivityWeighted;
-
 
311
  accPart /=  accVectorWeighted;
-
 
312
 
-
 
313
  if (controlActivityFlash < controlActivityWeighted) {
-
 
314
          debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
-
 
Line -... Line 233...
-
 
233
  uint8_t gyroActivityWeighted = ca / staticParams.rateTolerance;
-
 
234
  if (!gyroActivityWeighted) gyroActivityWeighted = 1;
-
 
235
 
-
 
236
  uint8_t accPart = staticParams.zerothOrderCorrection / gyroActivityWeighted;
315
  } else {
237
 
316
          debugOut.digital[0] |= DEBUG_ACC0THORDER;
238
  debugOut.analog[15] = gyroActivityWeighted;
317
  }
239
  debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
-
 
240
  debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
318
  if (++controlActivityFlash > CF_MAX+1) controlActivityFlash=1;
241
 
319
 
242
  if (gyroActivityWeighted < 8) {
320
  if (accFlash < accVectorWeighted) {
-
 
Line 321... Line 243...
321
          debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
243
    debugOut.digital[0] |= DEBUG_ACC0THORDER;
322
  } else {
244
  }
323
          debugOut.digital[1] |= DEBUG_ACC0THORDER;
245
  if (gyroActivityWeighted <= 2) {
324
  }
246
    debugOut.digital[1] |= DEBUG_ACC0THORDER;
325
  if (++accFlash > CF_MAX+1) accFlash=1;
247
  }
326
 
248
 
327
  /*
249
  /*
328
   * Add to each sum: The amount by which the angle is changed just below.
250
   * Add to each sum: The amount by which the angle is changed just below.
329
   */
251
   */
330
  for (axis = PITCH; axis <= ROLL; axis++) {
252
  for (axis = PITCH; axis <= ROLL; axis++) {
Line 373... Line 295...
373
      correctionSum[axis] = 0;
295
      correctionSum[axis] = 0;
374
    }
296
    }
375
  }
297
  }
376
}
298
}
Line -... Line 299...
-
 
299
 
377
 
300
/*
378
void calculateAccVector(void) {
301
void calculateAccVector(void) {
379
  int16_t temp;
302
  int16_t temp;
380
  temp = filteredAcc[0] >> 3;
303
  temp = filteredAcc[0] >> 3;
381
  accVector = temp * temp;
304
  accVector = temp * temp;
382
  temp = filteredAcc[1] >> 3;
305
  temp = filteredAcc[1] >> 3;
383
  accVector += temp * temp;
306
  accVector += temp * temp;
384
  temp = filteredAcc[2] >> 3;
307
  temp = filteredAcc[2] >> 3;
385
  accVector += temp * temp;
308
  accVector += temp * temp;
-
 
309
}
Line 386... Line 310...
386
}
310
*/
387
 
311
 
388
#ifdef USE_MK3MAG
312
#ifdef USE_MK3MAG
389
void attitude_resetHeadingToMagnetic(void) {
313
void attitude_resetHeadingToMagnetic(void) {
Line 476... Line 400...
476
/************************************************************************
400
/************************************************************************
477
 * Main procedure.
401
 * Main procedure.
478
 ************************************************************************/
402
 ************************************************************************/
479
void calculateFlightAttitude(void) {
403
void calculateFlightAttitude(void) {
480
  getAnalogData();
404
  getAnalogData();
481
  calculateAccVector();
405
  // calculateAccVector();
482
  integrate();
406
  integrate();
Line 483... Line 407...
483
 
407
 
484
#ifdef ATTITUDE_USE_ACC_SENSORS
-
 
485
  if (staticParams.maxControlActivity) {
408
#ifdef ATTITUDE_USE_ACC_SENSORS
486
    correctIntegralsByAcc0thOrder_old();
-
 
487
  } else {
-
 
488
    correctIntegralsByAcc0thOrder_new();
-
 
489
  }
409
  correctIntegralsByAcc0thOrder();
490
  driftCorrection();
410
  driftCorrection();
Line 491... Line 411...
491
#endif
411
#endif
492
 
412
 
Line 498... Line 418...
498
  if (staticParams.bitConfig & CFG_COMPASS_ENABLED) {
418
  if (staticParams.bitConfig & CFG_COMPASS_ENABLED) {
499
    correctHeadingToMagnetic();
419
    correctHeadingToMagnetic();
500
  }
420
  }
501
#endif
421
#endif
502
}
422
}
503
 
-
 
504
/*
-
 
505
 * This is part of an experiment to measure average sensor offsets caused by motor vibration,
-
 
506
 * and to compensate them away. It brings about some improvement, but no miracles.
-
 
507
 * As long as the left stick is kept in the start-motors position, the dynamic compensation
-
 
508
 * will measure the effect of vibration, to use for later compensation. So, one should keep
-
 
509
 * the stick in the start-motors position for a few seconds, till all motors run (at the wrong
-
 
510
 * speed unfortunately... must find a better way)
-
 
511
 */
-
 
512
/*
-
 
513
 void attitude_startDynamicCalibration(void) {
-
 
514
 dynamicCalPitch = dynamicCalRoll = dynamicCalYaw = dynamicCalCount = 0;
-
 
515
 savedDynamicOffsetPitch = savedDynamicOffsetRoll = 1000;
-
 
516
 }
-
 
517
 
-
 
518
 void attitude_continueDynamicCalibration(void) {
-
 
519
 // measure dynamic offset now...
-
 
520
 dynamicCalPitch += hiResPitchGyro;
-
 
521
 dynamicCalRoll += hiResRollGyro;
-
 
522
 dynamicCalYaw += rawYawGyroSum;
-
 
523
 dynamicCalCount++;
-
 
524
 
-
 
525
 // Param6: Manual mode. The offsets are taken from Param7 and Param8.
-
 
526
 if (dynamicParams.UserParam6 || 1) { // currently always enabled.
-
 
527
 // manual mode
-
 
528
 driftCompPitch = dynamicParams.UserParam7 - 128;
-
 
529
 driftCompRoll = dynamicParams.UserParam8 - 128;
-
 
530
 } else {
-
 
531
 // use the sampled value (does not seem to work so well....)
-
 
532
 driftCompPitch = savedDynamicOffsetPitch = -dynamicCalPitch / dynamicCalCount;
-
 
533
 driftCompRoll = savedDynamicOffsetRoll = -dynamicCalRoll / dynamicCalCount;
-
 
534
 driftCompYaw = -dynamicCalYaw / dynamicCalCount;
-
 
535
 }
-
 
536
 
-
 
537
 // keep resetting these meanwhile, to avoid accumulating errors.
-
 
538
 setStaticAttitudeIntegrals();
-
 
539
 yawAngle = 0;
-
 
540
 }
-
 
541
 */
-