WebSVN - FlightCtrl - Diff - Rev 1645 and 1646 - /branches/dongfang_FC

  * Here are those for the gyros and the acc. meters. They are not zero-offset.
  * They are exported in the analog.h file - but please do not use them! The only
  * reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating
  * the offsets with the DAC.
  */
-volatile int16_t rawGyroSum[2], rawYawGyroSum;
+volatile int16_t rawGyroSum[3];
-volatile int16_t acc[2] = {0,0}, ZAcc = 0;
+volatile int16_t acc[3];
-volatile int16_t filteredAcc[2] = {0,0};
+volatile int16_t filteredAcc[2]={0,0};
 /*
  * These 4 exported variables are zero-offset. The "PID" ones are used
  * in the attitude control as rotation rates. The "ATT" ones are for
  * integration to angles.
  */
 volatile int16_t gyro_PID[2];
 volatile int16_t gyro_ATT[2];
 volatile int16_t gyroD[2];
-volatile int16_t yawGyro = 0;
+volatile int16_t yawGyro;
 /*
  * Offset values. These are the raw gyro and acc. meter sums when the copter is
  * standing still. They are used for adjusting the gyro and acc. meter values
  * to be centered on zero.
+ */
+volatile int16_t gyroOffset[3] = {
+* GYRO_SUMMATION_FACTOR_PITCHROLL,
+* GYRO_SUMMATION_FACTOR_PITCHROLL,
+* GYRO_SUMMATION_FACTOR_YAW
- */
+};
+volatile int16_t accOffset[3] = {
+* ACC_SUMMATION_FACTOR_PITCHROLL,
+* ACC_SUMMATION_FACTOR_PITCHROLL,
-volatile int16_t gyroOffset[2], yawGyroOffset;
+* ACC_SUMMATION_FACTOR_Z
-volatile int16_t accOffset[2], ZAccOffset;
+};
 /*
  * This allows some experimentation with the gyro filters.
  * Should be replaced by #define's later on...
  */
-volatile uint8_t GYROS_FIRSTORDERFILTER;
+volatile uint8_t GYROS_PID_FILTER;
-volatile uint8_t GYROS_SECONDORDERFILTER;
+volatile uint8_t GYROS_ATT_FILTER;
-volatile uint8_t GYROS_DFILTER;
+volatile uint8_t GYROS_D_FILTER;
 volatile uint8_t ACC_FILTER;
    * Actually we don't need this "switch". We could do all the sampling into the
    * sensorInputs array first, and all the processing after the last sample.
    */
   switch(state++) {
   case 7: // Z acc
-#ifdef ACC_REVERSE_ZAXIS
+  if (ACC_REVERSED[Z])
-    ZAcc = -ZAccOffset - sensorInputs[AD_ACC_Z];
+    acc[Z] = accOffset[Z] - sensorInputs[AD_ACC_Z];
-#else
+  else
-    ZAcc = sensorInputs[AD_ACC_Z] - ZAccOffset;
+    acc[Z] = sensorInputs[AD_ACC_Z] - accOffset[Z];
-#endif
-    break;
+  break;
   case 10: // yaw gyro
-    rawYawGyroSum = sensorInputs[AD_GYRO_YAW];
+    rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW];
-#ifdef GYRO_REVERSE_YAW
+    if (GYRO_REVERSED[YAW])
-    yawGyro = rawYawGyroSum - yawGyroOffset;
+      yawGyro = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW];
-#else
+    else
-    yawGyro = yawGyroOffset - rawYawGyroSum; // negative is "default" (FC 1.0-1.3).
-#endif
+      yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW];
     break;
   case 11: // pitch axis acc.
-#ifdef ACC_REVERSE_PITCHAXIS
+    if (ACC_REVERSED[PITCH])
-    acc[PITCH] = -accOffset[PITCH] - sensorInputs[AD_ACC_PITCH];
+      acc[PITCH] = accOffset[PITCH] - sensorInputs[AD_ACC_PITCH];
-#else
-    acc[PITCH] = sensorInputs[AD_ACC_PITCH] - accOffset[PITCH];
+    else
-#endif
+      acc[PITCH] = sensorInputs[AD_ACC_PITCH] - accOffset[PITCH];
     filteredAcc[PITCH] = (filteredAcc[PITCH] * (ACC_FILTER-1) + acc[PITCH]) / ACC_FILTER;
     filteredAcc[PITCH] = (filteredAcc[PITCH] * (ACC_FILTER-1) + acc[PITCH]) / ACC_FILTER;
     measureNoise(acc[PITCH], &accNoisePeak[PITCH], 1);
     break;
   case 12: // roll axis acc.
-#ifdef ACC_REVERSE_ROLLAXIS
-    acc[ROLL] = sensorInputs[AD_ACC_ROLL] - accOffset[ROLL];
+    if (ACC_REVERSED[ROLL])
-#else
+      acc[ROLL] = accOffset[ROLL] - sensorInputs[AD_ACC_ROLL];
-    acc[ROLL] = -accOffset[ROLL] - sensorInputs[AD_ACC_ROLL];
+    else
-#endif
+      acc[ROLL] = sensorInputs[AD_ACC_ROLL] - accOffset[ROLL];
       OCR0A = range;
     } else {
       filteredAirPressure = filterAirPressure(getAbsPressure(rawAirPressure));
+    }
-    DebugOut.Analog[12] = range;
+    DebugOut.Analog[13] = range;
-    DebugOut.Analog[13] = rawAirPressure;
+    DebugOut.Analog[14] = rawAirPressure;
-    DebugOut.Analog[14] = filteredAirPressure;
+    DebugOut.Analog[15] = filteredAirPressure;
     break;
   case 14:
         tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE + SENSOR_MAX_PITCHROLL;
+      }
+    }
     // 2) Apply sign and offset, scale before filtering.
-    if (GYROS_REVERSE[axis]) {
+    if (GYRO_REVERSED[axis]) {
       tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL;
     } else {
       tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL;
+    }
     // 3) Scale and filter.
-    tempOffsetGyro = (gyro_PID[axis] * (GYROS_PIDFILTER-1) + tempOffsetGyro) / GYROS_PIDFILTER;
+    tempOffsetGyro = (gyro_PID[axis] * (GYROS_PID_FILTER-1) + tempOffsetGyro) / GYROS_PID_FILTER;
     // 4) Measure noise.
     measureNoise(tempOffsetGyro, &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING);
     // 5) Differential measurement.
-    gyroD[axis] = (gyroD[axis] * (GYROS_DFILTER-1) + (tempOffsetGyro - gyro_PID[axis])) / GYROS_DFILTER;
+    gyroD[axis] = (gyroD[axis] * (GYROS_D_FILTER-1) + (tempOffsetGyro - gyro_PID[axis])) / GYROS_D_FILTER;
     // 6) Done.
     gyro_PID[axis] = tempOffsetGyro;
     /*
      * Now process the data for attitude angles.
      */
     tempGyro = rawGyroSum[axis];
     // 1) Apply sign and offset, scale before filtering.
-    if (GYROS_REVERSE[axis]) {
+    if (GYRO_REVERSED[axis]) {
       tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL;
     } else {
       tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL;
   if(state) analog_start();
+}
 void analog_calibrate(void) {
 #define GYRO_OFFSET_CYCLES 32
-  uint8_t i;
+  uint8_t i, axis;
+  int32_t deltaOffsets[3] = {0,0,0};
-  int32_t _pitchOffset = 0, _rollOffset = 0, _yawOffset = 0;
+  int16_t filteredDelta;
   // Set the filters... to be removed again, once some good settings are found.
-  GYROS_FIRSTORDERFILTER = (dynamicParams.UserParams[4]   & 0b00000011)       + 1;
+  GYROS_PID_FILTER = (dynamicParams.UserParams[4]   & 0b00000011)       + 1;
-  GYROS_SECONDORDERFILTER = ((dynamicParams.UserParams[4] & 0b00001100) >> 2) + 1;
+  GYROS_ATT_FILTER = ((dynamicParams.UserParams[4]  & 0b00001100) >> 2) + 1;
-  GYROS_DFILTER = ((dynamicParams.UserParams[4]           & 0b00110000) >> 4) + 1;
-  ACC_FILTER = ((dynamicParams.UserParams[4]              & 0b11000000) >> 6) + 1;
   GYROS_D_FILTER = ((dynamicParams.UserParams[4]    & 0b00110000) >> 4) + 1;
-  gyroOffset[PITCH] = gyroOffset[ROLL] = yawGyroOffset = 0;
+  ACC_FILTER = ((dynamicParams.UserParams[4]        & 0b11000000) >> 6) + 1;
   gyro_calibrate();
   // determine gyro bias by averaging (requires that the copter does not rotate around any axis!)
   for(i=0; i < GYRO_OFFSET_CYCLES; i++) {
     Delay_ms_Mess(10);
-    _pitchOffset += rawGyroSum[PITCH];
+    for (axis=0; axis<=YAW; axis++) {
-    _rollOffset  += rawGyroSum[ROLL];
+      deltaOffsets[axis] += rawGyroSum[axis] - gyroOffset[axis];
-    _yawOffset   += rawYawGyroSum;
+    }
   }
-  gyroOffset[PITCH] = (_pitchOffset + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
-  gyroOffset[ROLL] = (_rollOffset  + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
-  yawGyroOffset   = (_yawOffset   + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
+  for (axis=0; axis<=YAW; axis++) {
+    filteredDelta = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
-  gyro_PID[PITCH] = gyro_PID[ROLL] = 0;
-  gyro_ATT[PITCH] = gyro_ATT[ROLL] = 0;
+    gyroOffset[axis] += filteredDelta;
+  }
-  // Noise is relative to offset. So, reset noise measurements when
-  // changing offsets.
+  // Noise is relative to offset. So, reset noise measurements when changing offsets.
+  gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0;
-  gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0;
+  accOffset[PITCH] = GetParamWord(PID_ACC_PITCH);
   accOffset[ROLL]  = GetParamWord(PID_ACC_ROLL);
-  accOffset[PITCH] = (int16_t)GetParamWord(PID_ACC_PITCH);
+  accOffset[Z]     = GetParamWord(PID_ACC_Z);
   accOffset[ROLL]  = (int16_t)GetParamWord(PID_ACC_ROLL);
  * anyway. There would be nothing wrong with updating the variables
  * directly from here, though.
  */
 void analog_calibrateAcc(void) {
 #define ACC_OFFSET_CYCLES 10
-  uint8_t i;
+  uint8_t i, axis;
-  int32_t _pitchAxisOffset = 0, _rollAxisOffset = 0, _ZAxisOffset = 0;
+  int32_t deltaOffset[3] = {0,0,0};
+  int16_t filteredDelta;
   // int16_t pressureDiff, savedRawAirPressure;
-  accOffset[PITCH] = accOffset[ROLL] = ZAccOffset = 0;
   for(i=0; i < ACC_OFFSET_CYCLES; i++) {
     Delay_ms_Mess(10);
-    _pitchAxisOffset += acc[PITCH];
+    for (axis=0; axis<=YAW; axis++) {
+          deltaOffset[axis] += acc[axis];
+        }
+  }
-    _rollAxisOffset  += acc[ROLL];
+  for (axis=0; axis<=YAW; axis++) {
+    filteredDelta = (deltaOffset[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES;
-    _ZAxisOffset += ZAcc;
+    accOffset[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta;
+  }
   // Save ACC neutral settings to eeprom
-  SetParamWord(PID_ACC_PITCH, (uint16_t)((_pitchAxisOffset + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES));
+  SetParamWord(PID_ACC_PITCH, accOffset[PITCH]);
-  SetParamWord(PID_ACC_ROLL, (uint16_t)((_rollAxisOffset  + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES));
+  SetParamWord(PID_ACC_ROLL,  accOffset[ROLL]);
-  SetParamWord(PID_ACC_TOP,  (uint16_t)((_ZAxisOffset     + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES));
+  SetParamWord(PID_ACC_Z,     accOffset[Z]);
+  // Noise is relative to offset. So, reset noise measurements when
-  // Noise is relative to offset. So, reset noise measurements when
+  // changing offsets.
-  // changing offsets.
+  accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0;
-  accNoisePeak[PITCH] = accNoisePeak[ROLL] = 0;
   // Setting offset values has an influence in the analog.c ISR
   // Therefore run measurement for 100ms to achive stable readings
-  // Delay_ms_Mess(100);
+  Delay_ms_Mess(100);
     Delay_ms_Mess(200);
     // raw pressure will increase.
     pressureDiff += (rawAirPressure - savedRawAirPressure);
+    }
-    DebugOut.Analog[15] = rangewidth =
+    DebugOut.Analog[16] = rangewidth =
     (pressureDiff + PRESSURE_CAL_CYCLE_COUNT * 2 - 1) / (PRESSURE_CAL_CYCLE_COUNT * 2);
   */

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/analog.c - Rev 1645 → 1646