Subversion Repositories FlightCtrl

Rev

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

Rev 1955 Rev 1960
Line 171... Line 171...
171 
/************************************************************************
 171 
/************************************************************************
172 
 * Neutral Readings                                                    
 172 
 * Neutral Readings                                                    
173 
 ************************************************************************/
 173 
 ************************************************************************/
174 
void attitude_setNeutral(void) {
 174 
void attitude_setNeutral(void) {
175 
  // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway.
 175 
  // Servo_Off(); // disable servo output. TODO: Why bother? The servos are going to make a jerk anyway.
176 
  dynamicParams.AxisCoupling1 = dynamicParams.AxisCoupling2 = 0;
 176 
  dynamicParams.axisCoupling1 = dynamicParams.axisCoupling2 = 0;
Line 177... Line 177...
177 
 
 177 
 
178 
  driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0;
 178 
  driftComp[PITCH] = driftComp[ROLL] = yawGyroDrift = driftCompYaw = 0;
Line 179... Line 179...
179 
  correctionSum[PITCH] = correctionSum[ROLL] = 0;
 179 
  correctionSum[PITCH] = correctionSum[ROLL] = 0;
Line 215... Line 215...
215 
  averageAccCount++;
 215 
  averageAccCount++;
216 
  yawRate = yawGyro + driftCompYaw;
 216 
  yawRate = yawGyro + driftCompYaw;
Line 217... Line 217...
217 
 
 217 
 
218 
  // We are done reading variables from the analog module.
 218 
  // We are done reading variables from the analog module.
219 
  // Interrupt-driven sensor reading may restart.
 - 
 
220 
  analogDataReady = 0;
 219 
  // Interrupt-driven sensor reading may restart.
221 
  startAnalogConversionCycle();
 220 
  startAnalogConversionCycle();
Line 222... Line 221...
222 
}
 221 
}
223 
 
 222 
 
Line 247... Line 246...
247 
// 480 usec with axis coupling - almost no time without.
 246 
// 480 usec with axis coupling - almost no time without.
248 
void integrate(void) {
 247 
void integrate(void) {
249 
  // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
 248 
  // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
250 
  uint8_t axis;
 249 
  uint8_t axis;
Line 251... Line 250...
251 
 
 250 
 
252 
  if (!looping && (staticParams.GlobalConfig & CFG_AXIS_COUPLING_ACTIVE)) {
 251 
  if (/*!looping && */ (staticParams.bitConfig & CFG_AXIS_COUPLING_ACTIVE)) {
253 
    trigAxisCoupling();
 252 
    trigAxisCoupling();
254 
  } else {
 253 
  } else {
255 
    ACRate[PITCH] = rate_ATT[PITCH];
 254 
    ACRate[PITCH] = rate_ATT[PITCH];
256 
    ACRate[ROLL] = rate_ATT[ROLL];
 255 
    ACRate[ROLL] = rate_ATT[ROLL];
Line 298... Line 297...
298 
  uint8_t axis;
 297 
  uint8_t axis;
299 
  int32_t temp;
 298 
  int32_t temp;
300 
  debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
 299 
  debugOut.digital[0] &= ~DEBUG_ACC0THORDER;
301 
  debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
 300 
  debugOut.digital[1] &= ~DEBUG_ACC0THORDER;
Line 302... Line 301...
302 
 
 301 
 
303 
  if (!looping && acc[Z] >= -dynamicParams.UserParams[7] && acc[Z]
 - 
 
304 
      <= dynamicParams.UserParams[7]) {
 - 
 
305 
 
 302 
  if (1 /*controlActivity <= dynamicParams.maxControlActivityForAcc*/) {
306 
    uint8_t permilleAcc = staticParams.GyroAccFactor; // NOTE!!! The meaning of this value has changed!!
 303 
    uint8_t permilleAcc = staticParams.zerothOrderCorrection;
Line 307... Line 304...
307 
    int32_t accDerived;
 304 
    int32_t accDerived;
308 
 
 305 
 
309 
    /*
 306 
    /*
Line 374... Line 371...
374 
        round = DRIFTCORRECTION_TIME / 2;
 371 
        round = DRIFTCORRECTION_TIME / 2;
375 
      else
 372 
      else
376 
        round = -DRIFTCORRECTION_TIME / 2;
 373 
        round = -DRIFTCORRECTION_TIME / 2;
377 
      deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME;
 374 
      deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME;
378 
      // Add the delta to the compensation. So positive delta means, gyro should have higher value.
 375 
      // Add the delta to the compensation. So positive delta means, gyro should have higher value.
379 
      driftComp[axis] += deltaCorrection / staticParams.GyroAccTrim;
 376 
      driftComp[axis] += deltaCorrection / staticParams.driftCompDivider;
380 
      CHECK_MIN_MAX(driftComp[axis], -staticParams.DriftComp, staticParams.DriftComp);
 377 
      CHECK_MIN_MAX(driftComp[axis], -staticParams.driftCompLimit, staticParams.driftCompLimit);
381 
      // DebugOut.Analog[11 + axis] = correctionSum[axis];
 378 
      // DebugOut.Analog[11 + axis] = correctionSum[axis];
382 
      // DebugOut.Analog[16 + axis] = correctionSum[axis];
 379 
      // DebugOut.Analog[16 + axis] = correctionSum[axis];
383 
      debugOut.analog[28 + axis] = driftComp[axis];
 380 
      debugOut.analog[28 + axis] = driftComp[axis];
Line 384... Line 381...
384 
 
 381 
 
Line 442... Line 439...
442 
 
 439 
 
443 
    /*
 440 
    /*
444 
     w = (w * dynamicParams.CompassYawEffect) / 32;
 441 
     w = (w * dynamicParams.CompassYawEffect) / 32;
445 
     w = dynamicParams.CompassYawEffect - w;
 442 
     w = dynamicParams.CompassYawEffect - w;
446 
     */
 443 
     */
447 
    w = dynamicParams.CompassYawEffect - (w * dynamicParams.CompassYawEffect)
 444 
    w = dynamicParams.compassYawEffect - (w * dynamicParams.compassYawEffect)
Line 448... Line 445...
448 
        / 32;
 445 
        / 32;
449 
 
 446 
 
Line 458... Line 455...
458 
        r
 455 
        r
459 
            = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse)
 456 
            = ((540 + yawGyroHeading / GYRO_DEG_FACTOR_YAW - compassCourse)
460 
                % 360) - 180;
 457 
                % 360) - 180;
461 
        v = (r * w) / v; // align to compass course
 458 
        v = (r * w) / v; // align to compass course
462 
        // limit yaw rate
 459 
        // limit yaw rate
463 
        w = 3 * dynamicParams.CompassYawEffect;
 460 
        w = 3 * dynamicParams.compassYawEffect;
464 
        if (v > w)
 461 
        if (v > w)
465 
          v = w;
 462 
          v = w;
466 
        else if (v < -w)
 463 
        else if (v < -w)
467 
          v = -w;
 464 
          v = -w;
468 
        yawAngleDiff += v;
 465 
        yawAngleDiff += v;