Subversion Repositories FlightCtrl

Rev

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

Rev 1645 Rev 1646
Line 122... Line 122...
122
 
122
 
123
#define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L)
123
#define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L)
124
#define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L)
124
#define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L)
Line 125... Line 125...
125
#define YAWOVER360       (GYRO_DEG_FACTOR_YAW * 360L)
125
#define YAWOVER360       (GYRO_DEG_FACTOR_YAW * 360L)
Line 126... Line 126...
126
 
126
 
127
int32_t correctionSum[2] = {0,0};
127
int16_t correctionSum[2] = {0,0};
128
 
128
 
129
/*
129
/*
Line 163... Line 163...
163
void attitude_setNeutral(void) {
163
void attitude_setNeutral(void) {
164
  // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway.
164
  // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway.
165
  dynamicParams.AxisCoupling1 = dynamicParams.AxisCoupling2 = 0;
165
  dynamicParams.AxisCoupling1 = dynamicParams.AxisCoupling2 = 0;
Line 166... Line 166...
166
 
166
 
-
 
167
  dynamicOffset[PITCH] = dynamicOffset[ROLL] = 0;
Line 167... Line 168...
167
  dynamicOffset[PITCH] = dynamicOffset[ROLL] = 0;
168
  correctionSum[PITCH] = correctionSum[ROLL] = 0;
168
 
169
 
Line 169... Line 170...
169
  // Calibrate hardware.
170
  // Calibrate hardware.
Line 176... Line 177...
176
  setStaticAttitudeAngles();
177
  setStaticAttitudeAngles();
177
  yawAngle = 0;
178
  yawAngle = 0;
Line 178... Line 179...
178
 
179
 
179
  // update compass course to current heading
180
  // update compass course to current heading
-
 
181
  compassCourse = compassHeading;
180
  compassCourse = compassHeading;
182
 
181
  // Inititialize YawGyroIntegral value with current compass heading
183
  // Inititialize YawGyroIntegral value with current compass heading
Line 182... Line 184...
182
  yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW;
184
  yawGyroHeading = (int32_t)compassHeading * GYRO_DEG_FACTOR_YAW;
183
 
185
 
Line 197... Line 199...
197
  for (axis=PITCH; axis <=ROLL; axis++) {
199
  for (axis=PITCH; axis <=ROLL; axis++) {
198
    rate_PID[axis]     = (gyro_PID[axis] + dynamicOffset[axis]) / HIRES_GYRO_INTEGRATION_FACTOR;
200
    rate_PID[axis]     = (gyro_PID[axis] + dynamicOffset[axis]) / HIRES_GYRO_INTEGRATION_FACTOR;
199
    rate[axis]         = (gyro_ATT[axis] + dynamicOffset[axis]) / HIRES_GYRO_INTEGRATION_FACTOR;
201
    rate[axis]         = (gyro_ATT[axis] + dynamicOffset[axis]) / HIRES_GYRO_INTEGRATION_FACTOR;
200
    differential[axis] = gyroD[axis];
202
    differential[axis] = gyroD[axis];
201
  }
203
  }
-
 
204
 
202
  yawRate = yawGyro + dynamicOffsetYaw;
205
  yawRate = yawGyro + dynamicOffsetYaw;
Line 203... Line 206...
203
 
206
 
204
  // We are done reading variables from the analog module.
207
  // We are done reading variables from the analog module.
205
  // Interrupt-driven sensor reading may restart.
208
  // Interrupt-driven sensor reading may restart.
Line 235... Line 238...
235
    ACRate[PITCH] = rate[PITCH];
238
    ACRate[PITCH] = rate[PITCH];
236
    ACRate[ROLL]  = rate[ROLL];
239
    ACRate[ROLL]  = rate[ROLL];
237
    ACYawRate = yawRate;
240
    ACYawRate = yawRate;
238
  }
241
  }
Line 239... Line -...
239
 
-
 
240
  DebugOut.Analog[3] = ACRate[PITCH];
-
 
241
  DebugOut.Analog[4] = ACRate[ROLL];
-
 
242
  DebugOut.Analog[5] = ACYawRate;
-
 
243
 
242
 
244
  /*
243
  /*
245
   * Yaw
244
   * Yaw
246
   * Calculate yaw gyro integral (~ to rotation angle)
245
   * Calculate yaw gyro integral (~ to rotation angle)
247
   * Limit yawGyroHeading proportional to 0 deg to 360 deg
246
   * Limit yawGyroHeading proportional to 0 deg to 360 deg
Line 271... Line 270...
271
}
270
}
Line 272... Line 271...
272
 
271
 
273
/************************************************************************
272
/************************************************************************
274
 * A kind of 0'th order integral correction, that corrects the integrals
273
 * A kind of 0'th order integral correction, that corrects the integrals
275
 * directly. This is the "gyroAccFactor" stuff in the original code.
274
 * directly. This is the "gyroAccFactor" stuff in the original code.
276
 * There is (there) also what I would call a  "minus 1st order correction"
275
 * There is (there) also a drift compensation
277
 * - it corrects the differential of the integral = the gyro offsets.
276
 * - it corrects the differential of the integral = the gyro offsets.
278
 * That should only be necessary with drifty gyros like ENC-03.
277
 * That should only be necessary with drifty gyros like ENC-03.
279
 ************************************************************************/
278
 ************************************************************************/
280
void correctIntegralsByAcc0thOrder(void) {
279
void correctIntegralsByAcc0thOrder(void) {
281
  // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities
280
  // TODO: Consider changing this to: Only correct when integrals are less than ...., or only correct when angular velocities
282
  // are less than ....., or reintroduce Kalman.
281
  // are less than ....., or reintroduce Kalman.
283
  // Well actually the Z axis acc. check is not so silly.
282
  // Well actually the Z axis acc. check is not so silly.
284
  uint8_t axis;
283
  uint8_t axis;
285
  if(!looping && //((ZAcc >= -4) || (MKFlags & MKFLAG_MOTOR_RUN))) { // if not looping in any direction
284
  int32_t correction;
286
     ZAcc >= -dynamicParams.UserParams[7] && ZAcc <= dynamicParams.UserParams[7]) {
285
  if(!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z] <= dynamicParams.UserParams[7]) {
Line 287... Line 286...
287
    DebugOut.Digital[0] = 1;
286
    DebugOut.Digital[0] = 1;
288
   
287
   
289
    uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!!
288
    uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!!
Line 290... Line 289...
290
    uint8_t debugFullWeight = 1;
289
    uint8_t debugFullWeight = 1;
291
    int32_t accDerived[2];
290
    int32_t accDerived;
292
   
291
   
293
    if((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands
292
    if((maxControl[PITCH] > 64) || (maxControl[ROLL] > 64)) { // reduce effect during stick commands
Line 302... Line 301...
302
 
301
 
303
    /*
302
    /*
304
     * Add to each sum: The amount by which the angle is changed just below.
303
     * Add to each sum: The amount by which the angle is changed just below.
305
     */
304
     */
306
    for (axis=PITCH; axis<=ROLL; axis++) {
305
    for (axis=PITCH; axis<=ROLL; axis++) {
-
 
306
      accDerived = getAngleEstimateFromAcc(axis);
-
 
307
      DebugOut.Analog[9 + axis] = (10 * accDerived) / GYRO_DEG_FACTOR_PITCHROLL;
-
 
308
     
307
      accDerived[axis] = getAngleEstimateFromAcc(axis);
309
      // 1000 * the correction amount that will be added to the gyro angle in next line.
-
 
310
      correction = angle[axis]; //(permilleAcc * (accDerived - angle[axis])) / 1000;
-
 
311
      angle[axis] = ((int32_t)(1000 - permilleAcc) * angle[axis] + (int32_t)permilleAcc * accDerived) / 1000L;
-
 
312
 
Line 308... Line 313...
308
      correctionSum[axis] += permilleAcc * (accDerived[axis] - angle[axis]);
313
      correctionSum[axis] += angle[axis] - correction;
309
   
314
   
-
 
315
      // There should not be a risk of overflow here, since the integrals do not exceed a few 100000.
-
 
316
      // change = ((1000 - permilleAcc) * angle[axis] + permilleAcc * accDerived) / 1000 - angle[axis]
-
 
317
      // = (1000 * angle[axis] - permilleAcc * angle[axis] + permilleAcc * accDerived) / 1000 - angle[axis]
-
 
318
      // = (- permilleAcc * angle[axis] + permilleAcc * accDerived) / 1000
-
 
319
      // = permilleAcc * (accDerived - angle[axis]) / 1000
-
 
320
     
310
      // There should not be a risk of overflow here, since the integrals do not exceed a few 100000.
321
      // Experiment: Do not acutally apply the correction. See if drift compensation does that.
Line 311... Line 322...
311
      angle[axis] = ((int32_t)(1000 - permilleAcc) * angle[axis] + (int32_t)permilleAcc * accDerived[axis]) / 1000L;
322
      // angle[axis] += correction / 1000;
312
    }
323
    }
313
       
324
       
Line 320... Line 331...
320
/************************************************************************
331
/************************************************************************
321
 * This is an attempt to correct not the error in the angle integrals
332
 * This is an attempt to correct not the error in the angle integrals
322
 * (that happens in correctIntegralsByAcc0thOrder above) but rather the
333
 * (that happens in correctIntegralsByAcc0thOrder above) but rather the
323
 * cause of it: Gyro drift, vibration and rounding errors.
334
 * cause of it: Gyro drift, vibration and rounding errors.
324
 * All the corrections made in correctIntegralsByAcc0thOrder over
335
 * All the corrections made in correctIntegralsByAcc0thOrder over
325
 * MINUSFIRSTORDERCORRECTION_TIME cycles are summed up. This number is
336
 * DRIFTCORRECTION_TIME cycles are summed up. This number is
326
 * then divided by MINUSFIRSTORDERCORRECTION_TIME to get the approx.
337
 * then divided by DRIFTCORRECTION_TIME to get the approx.
327
 * correction that should have been applied to each iteration to fix
338
 * correction that should have been applied to each iteration to fix
328
 * the error. This is then added to the dynamic offsets.
339
 * the error. This is then added to the dynamic offsets.
329
 ************************************************************************/
340
 ************************************************************************/
330
// 2 times / sec.
341
// 2 times / sec. = 488/2
331
#define DRIFTCORRECTION_TIME 488/2
342
#define DRIFTCORRECTION_TIME 256L
332
void driftCompensation(void) {
343
void driftCorrection(void) {
333
  static int16_t timer = DRIFTCORRECTION_TIME;
344
  static int16_t timer = DRIFTCORRECTION_TIME;
334
  int16_t deltaCompensation;
345
  int16_t deltaCorrection;
335
  uint8_t axis;
346
  uint8_t axis;
336
  if (! --timer) {
347
  if (! --timer) {
337
    timer = DRIFTCORRECTION_TIME;
348
    timer = DRIFTCORRECTION_TIME;
338
    for (axis=PITCH; axis<=ROLL; axis++) {
349
    for (axis=PITCH; axis<=ROLL; axis++) {
-
 
350
      // Take the sum of corrections applied, add it to delta
339
      deltaCompensation = ((correctionSum[axis] + 1000L * DRIFTCORRECTION_TIME / 2) / 1000 / DRIFTCORRECTION_TIME);
351
      deltaCorrection = ((correctionSum[axis] + DRIFTCORRECTION_TIME / 2) * HIRES_GYRO_INTEGRATION_FACTOR) / DRIFTCORRECTION_TIME;
340
      CHECK_MIN_MAX(deltaCompensation, -staticParams.DriftComp, staticParams.DriftComp);
352
      // Add the delta to the compensation. So positive delta means, gyro should have higher value.
341
      dynamicOffset[axis] += deltaCompensation / staticParams.GyroAccTrim;
353
      dynamicOffset[axis] += deltaCorrection / staticParams.GyroAccTrim;
-
 
354
      CHECK_MIN_MAX(dynamicOffset[axis], -staticParams.DriftComp, staticParams.DriftComp);
-
 
355
      DebugOut.Analog[11 + axis] = correctionSum[axis];
-
 
356
      DebugOut.Analog[28 + axis] = dynamicOffset[axis];
342
      correctionSum[axis] = 0;
357
      correctionSum[axis] = 0;
343
      DebugOut.Analog[28 + axis] = dynamicOffset;
-
 
344
    }
358
    }
345
  }
359
  }
346
}
360
}
Line 347... Line 361...
347
 
361
 
348
/************************************************************************
362
/************************************************************************
349
 * Main procedure.
363
 * Main procedure.
350
 ************************************************************************/
364
 ************************************************************************/
351
void calculateFlightAttitude(void) {  
365
void calculateFlightAttitude(void) {  
352
  getAnalogData();
366
  getAnalogData();
-
 
367
  integrate();
-
 
368
 
-
 
369
  DebugOut.Analog[6] = ACRate[PITCH];
-
 
370
  DebugOut.Analog[7] = ACRate[ROLL];
-
 
371
  DebugOut.Analog[8] = ACYawRate;
-
 
372
 
-
 
373
  DebugOut.Analog[3] = rate_PID[PITCH];
-
 
374
  DebugOut.Analog[4] = rate_PID[ROLL];
-
 
375
  DebugOut.Analog[5] = yawRate;
353
  integrate();
376
 
354
#ifdef ATTITUDE_USE_ACC_SENSORS
377
#ifdef ATTITUDE_USE_ACC_SENSORS
355
  correctIntegralsByAcc0thOrder();
378
  correctIntegralsByAcc0thOrder();
356
  driftCompensation();
379
  driftCorrection();
357
#endif
380
#endif
Line 358... Line 381...
358
}
381
}
359
 
382