Subversion Repositories FlightCtrl

Rev

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

Rev 2095 Rev 2160
Line 226... Line 226...
226
  // are less than ....., or reintroduce Kalman.
226
  // are less than ....., or reintroduce Kalman.
227
  // Well actually the Z axis acc. check is not so silly.
227
  // Well actually the Z axis acc. check is not so silly.
228
  uint8_t axis;
228
  uint8_t axis;
229
  int32_t temp;
229
  int32_t temp;
Line 230... Line 230...
230
 
230
 
231
  debugOut.analog[13] = gyroActivity / 65536L;
-
 
232
 
-
 
233
  uint16_t ca;
-
 
234
  ca = gyroActivity >> 12;
-
 
Line -... Line 231...
-
 
231
  debugOut.analog[12] = IMUConfig.zerothOrderCorrection;
235
  debugOut.analog[14] = ca;
232
 
-
 
233
  uint16_t ca = gyroActivity >> 14;
-
 
234
  uint8_t gyroActivityWeighted = ca / IMUConfig.rateTolerance;
236
 
235
  debugOut.analog[15] = gyroActivityWeighted;
Line 237... Line 236...
237
  uint8_t gyroActivityWeighted = ca / IMUConfig.rateTolerance;
236
 
Line 238... Line 237...
238
  if (!gyroActivityWeighted) gyroActivityWeighted = 1;
237
  if (!gyroActivityWeighted) gyroActivityWeighted = 1;
239
 
-
 
240
  uint8_t accPart = IMUConfig.zerothOrderCorrection / gyroActivityWeighted;
238
 
241
 
239
  uint8_t accPart = IMUConfig.zerothOrderCorrection / gyroActivityWeighted;
Line 242... Line 240...
242
  debugOut.analog[28] = IMUConfig.rateTolerance;
240
 
243
  debugOut.analog[15] = gyroActivityWeighted;
241
  debugOut.analog[28] = IMUConfig.rateTolerance;
Line 293... Line 291...
293
      deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME;
291
      deltaCorrection = (correctionSum[axis] + round) / DRIFTCORRECTION_TIME;
294
      // Add the delta to the compensation. So positive delta means, gyro should have higher value.
292
      // Add the delta to the compensation. So positive delta means, gyro should have higher value.
295
      driftComp[axis] += deltaCorrection / IMUConfig.driftCompDivider;
293
      driftComp[axis] += deltaCorrection / IMUConfig.driftCompDivider;
296
      CHECK_MIN_MAX(driftComp[axis], -IMUConfig.driftCompLimit, IMUConfig.driftCompLimit);
294
      CHECK_MIN_MAX(driftComp[axis], -IMUConfig.driftCompLimit, IMUConfig.driftCompLimit);
297
      // DebugOut.Analog[11 + axis] = correctionSum[axis];
295
      // DebugOut.Analog[11 + axis] = correctionSum[axis];
298
      // DebugOut.Analog[16 + axis] = correctionSum[axis];
296
      debugOut.analog[6 + axis] = correctionSum[axis];
299
      // debugOut.analog[28 + axis] = driftComp[axis];
297
      debugOut.analog[13 + axis] = driftComp[axis];
300
      correctionSum[axis] = 0;
298
      correctionSum[axis] = 0;
301
    }
299
    }
302
  }
300
  }
303
}
301
}