Subversion Repositories FlightCtrl

Rev

Rev 1870 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1870 Rev 1872
Line 214... Line 214...
214 
  yawRate = yawGyro + driftCompYaw;
 214 
  yawRate = yawGyro + driftCompYaw;
Line 215... Line 215...
215 
 
 215 
 
216 
  // We are done reading variables from the analog module.
 216 
  // We are done reading variables from the analog module.
217 
  // Interrupt-driven sensor reading may restart.
 217 
  // Interrupt-driven sensor reading may restart.
218 
  analogDataReady = 0;
 - 
 
219 
  J4HIGH;
 218 
  analogDataReady = 0;
220 
  analog_start();
 219 
  analog_start();
Line 221... Line 220...
221 
}
 220 
}
222 
 
 221 
 
Line 237... Line 236...
237 
  ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll
 236 
  ACRate[ROLL] = rate_ATT[ROLL] + (((((int32_t)rate_ATT[PITCH] * sinroll
238 
      + (int32_t)yawRate * cosroll) >> MATH_UNIT_FACTOR_LOG) * int_tan(
 237 
      + (int32_t)yawRate * cosroll) >> MATH_UNIT_FACTOR_LOG) * int_tan(
239 
      angle[PITCH])) >> MATH_UNIT_FACTOR_LOG);
 238 
      angle[PITCH])) >> MATH_UNIT_FACTOR_LOG);
Line 240... Line 239...
240 
 
 239 
 
- 
 
 240 
  ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch;
- 
 
 241 
 
241 
  ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch;
 242 
  ACYawRate = ((int32_t)rate_ATT[PITCH] * sinroll + (int32_t)yawRate * cosroll) / cospitch;
Line 242... Line 243...
242 
}
 243 
}
243 
 
 244 
 
244 
// 480 usec with axis coupling - almost no time without.
 245 
// 480 usec with axis coupling - almost no time without.
245 
void integrate(void) {
 246 
void integrate(void) {
- 
 
 247 
  // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
246 
  // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
 248 
  uint8_t axis;
247 
  uint8_t axis;
 249 
 
248 
  if (!looping && (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) {
 250 
  if (!looping && (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) {
249 
    trigAxisCoupling();
 251 
    trigAxisCoupling();
250 
  } else {
 252 
  } else {
Line 355... Line 357...
355 
// 2 times / sec. = 488/2
 357 
// 2 times / sec. = 488/2
356 
#define DRIFTCORRECTION_TIME 256L
 358 
#define DRIFTCORRECTION_TIME 256L
357 
void driftCorrection(void) {
 359 
void driftCorrection(void) {
358 
  static int16_t timer = DRIFTCORRECTION_TIME;
 360 
  static int16_t timer = DRIFTCORRECTION_TIME;
359 
  int16_t deltaCorrection;
 361 
  int16_t deltaCorrection;
- 
 
 362 
  int16_t round;
360 
  uint8_t axis;
 363 
  uint8_t axis;
- 
 
 364 
 
- 
 
 365 
  DebugOut.Analog[6] = (DRIFTCORRECTION_TIME + DRIFTCORRECTION_TIME/2) / DRIFTCORRECTION_TIME;
- 
 
 366 
  DebugOut.Analog[7] = (-DRIFTCORRECTION_TIME + DRIFTCORRECTION_TIME/2) / DRIFTCORRECTION_TIME;
- 
 
 367 
 
361 
  if (!--timer) {
 368 
  if (!--timer) {
362 
    timer = DRIFTCORRECTION_TIME;
 369 
    timer = DRIFTCORRECTION_TIME;
363 
    for (axis = PITCH; axis <= ROLL; axis++) {
 370 
    for (axis = PITCH; axis <= ROLL; axis++) {
364 
      // Take the sum of corrections applied, add it to delta
 371 
      // Take the sum of corrections applied, add it to delta
- 
 
 372 
      if (correctionSum[axis] >=0)
365 
      deltaCorrection = (correctionSum[axis] + DRIFTCORRECTION_TIME / 2)
 373 
        round = DRIFTCORRECTION_TIME / 2;
- 
 
 374 
      else
366 
          / DRIFTCORRECTION_TIME;
 375 
        round = -DRIFTCORRECTION_TIME / 2;
- 
 
 376 
      deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME;
367 
      // Add the delta to the compensation. So positive delta means, gyro should have higher value.
 377 
      // Add the delta to the compensation. So positive delta means, gyro should have higher value.
368 
      driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim;
 378 
      driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim;
369 
      CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp);
 379 
      CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp);
370 
      // DebugOut.Analog[11 + axis] = correctionSum[axis];
 380 
      // DebugOut.Analog[11 + axis] = correctionSum[axis];
371 
      DebugOut.Analog[16 + axis] = correctionSum[axis];
 381 
      DebugOut.Analog[16 + axis] = correctionSum[axis];
372 
      DebugOut.Analog[18 + axis] = deltaCorrection / staticParams.GyroAccTrim;
 - 
 
373 
      DebugOut.Analog[28 + axis] = driftComp[axis];
 382 
      DebugOut.Analog[28 + axis] = driftComp[axis];
Line 374... Line 383...
374 
 
 383 
 
375 
      correctionSum[axis] = 0;
 384 
      correctionSum[axis] = 0;
376 
    }
 385 
    }
Line 379... Line 388...
379 
 
 388 
 
380 
/************************************************************************
 389 
/************************************************************************
381 
 * Main procedure.
 390 
 * Main procedure.
382 
 ************************************************************************/
 391 
 ************************************************************************/
383 
void calculateFlightAttitude(void) {
 - 
 
384 
  // part1: 550 usec.
 - 
 
385 
  // part1a: 550 usec.
 - 
 
386 
  // part1b: 60 usec.
 392 
void calculateFlightAttitude(void) {
387 
  getAnalogData();
 - 
 
388 
  // end part1b
 393 
  getAnalogData();
389 
  integrate();
 - 
 
390 
  // end part1a
 - 
 
391 
 
 - 
 
392 
  DebugOut.Analog[6] = stronglyFilteredAcc[PITCH];
 - 
 
393 
  DebugOut.Analog[7] = stronglyFilteredAcc[ROLL];
 - 
 
Line 394... Line 394...
394 
  DebugOut.Analog[8] = stronglyFilteredAcc[Z];
 394 
  integrate();
395 
 
 395 
 
396 
  DebugOut.Analog[3] = rate_PID[PITCH];
 396 
  DebugOut.Analog[3] = rate_PID[PITCH];
Line 397... Line 397...
397 
  DebugOut.Analog[4] = rate_PID[ROLL];
 397 
  DebugOut.Analog[4] = rate_PID[ROLL];
398 
  DebugOut.Analog[5] = yawRate;
 398 
  DebugOut.Analog[5] = yawRate;
399 
 
 399 
 
400 
#ifdef ATTITUDE_USE_ACC_SENSORS
 400 
#ifdef ATTITUDE_USE_ACC_SENSORS
401 
  correctIntegralsByAcc0thOrder();
 - 
 
402 
  driftCorrection();
 401 
  correctIntegralsByAcc0thOrder();
Line 403... Line 402...
403 
#endif
 402 
  driftCorrection();
404 
  // end part1
 403 
#endif