WebSVN - FlightCtrl - Diff - Rev 1955 and 1960 - /branches/dongfang_FC

 /*
  * 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] = { 512 * GYRO_SUMMATION_FACTOR_PITCHROLL, 512
                 * GYRO_SUMMATION_FACTOR_PITCHROLL, 512 * GYRO_SUMMATION_FACTOR_YAW };
 volatile int16_t accOffset[3] = { 512 * ACC_SUMMATION_FACTOR_PITCHROLL, 512
+                * ACC_SUMMATION_FACTOR_PITCHROLL, 512 * ACC_SUMMATION_FACTOR_Z };
+                */
+sensorOffset_t gyroOffset;
-                * ACC_SUMMATION_FACTOR_PITCHROLL, 512 * ACC_SUMMATION_FACTOR_Z };
+sensorOffset_t accOffset;
 /*
  * This allows some experimentation with the gyro filters.
- * Should be replaced by #define's later on...
- */
-volatile uint8_t GYROS_PID_FILTER;
-volatile uint8_t GYROS_ATT_FILTER;
-volatile uint8_t GYROS_D_FILTER;
+ * Should be replaced by #define's later on...
-volatile uint8_t ACC_FILTER;
+ */
 /*
 uint16_t getSimplePressure(int advalue) {
         return (uint16_t) OCR0A * (uint16_t) rangewidth + advalue;
+}
+void startAnalogConversionCycle(void) {
-void startAnalogConversionCycle(void) {
+  analogDataReady = 0;
   // Stop the sampling. Cycle is over.
   for (uint8_t i = 0; i < 8; i++) {
     sensorInputs[i] = 0;
+  }
      * Process the gyro data for the PID controller.
      */
     // 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a
     //    gyro with a wider range, and helps counter saturation at full control.
-    if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) {
+    if (staticParams.bitConfig & CFG_GYRO_SATURATION_PREVENTION) {
       if (tempGyro < SENSOR_MIN_PITCHROLL) {
         debugOut.digital[0] |= DEBUG_SENSORLIMIT;
         tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT;
       } else if (tempGyro > SENSOR_MAX_PITCHROLL) {
+      }
+    }
     // 2) Apply sign and offset, scale before filtering.
     if (GYRO_REVERSED[axis]) {
-      tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL;
+      tempOffsetGyro = (gyroOffset.offsets[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL;
     } else {
-      tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL;
+      tempOffsetGyro = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
+    }
     // 3) Scale and filter.
-    tempOffsetGyro = (gyro_PID[axis] * (GYROS_PID_FILTER - 1) + tempOffsetGyro) / GYROS_PID_FILTER;
+    tempOffsetGyro = (gyro_PID[axis] * (staticParams.gyroPIDFilterConstant - 1) + tempOffsetGyro) / staticParams.gyroPIDFilterConstant;
     // 4) Measure noise.
     measureNoise(tempOffsetGyro, &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING);
     // 5) Differential measurement.
      */
     tempGyro = rawGyroSum[axis];
     // 1) Apply sign and offset, scale before filtering.
     if (GYRO_REVERSED[axis]) {
-      tempOffsetGyro = (gyroOffset[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL;
+      tempOffsetGyro = (gyroOffset.offsets[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL;
     } else {
-      tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL;
+      tempOffsetGyro = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
+    }
     // 2) Filter.
-    gyro_ATT[axis] = (gyro_ATT[axis] * (GYROS_ATT_FILTER - 1) + tempOffsetGyro) / GYROS_ATT_FILTER;
+    gyro_ATT[axis] = (gyro_ATT[axis] * (staticParams.gyroATTFilterConstant - 1) + tempOffsetGyro) / staticParams.gyroATTFilterConstant;
+  }
   // Yaw gyro.
   rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW];
   if (GYRO_REVERSED[YAW])
-    yawGyro = gyroOffset[YAW] - sensorInputs[AD_GYRO_YAW];
+    yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW];
   else
-    yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset[YAW];
+    yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW];
   debugOut.analog[3] = gyro_ATT[PITCH];
   debugOut.analog[4] = gyro_ATT[ROLL];
   debugOut.analog[5] = yawGyro;
+}
 void analog_updateAccelerometers(void) {
   // Pitch and roll axis accelerations.
   for (uint8_t axis=0; axis<2; axis++) {
     if (ACC_REVERSED[axis])
-      acc[axis] = accOffset[axis] - sensorInputs[AD_ACC_PITCH-axis];
+      acc[axis] = accOffset.offsets[axis] - sensorInputs[AD_ACC_PITCH-axis];
     else
-      acc[axis] = sensorInputs[AD_ACC_PITCH-axis] - accOffset[axis];
+      acc[axis] = sensorInputs[AD_ACC_PITCH-axis] - accOffset.offsets[axis];
     measureNoise(acc[axis], &accNoisePeak[axis], 1);
+  }
   // Z acc.
   if (ACC_REVERSED[Z])
-    acc[Z] = accOffset[Z] - sensorInputs[AD_ACC_Z];
+    acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z];
   else
-    acc[Z] = sensorInputs[AD_ACC_Z] - accOffset[Z];
+    acc[Z] = sensorInputs[AD_ACC_Z] - accOffset.offsets[Z];
   /*
     stronglyFilteredAcc[Z] =
-    (stronglyFilteredAcc[Z] * 99 + acc[Z] * 10) / 100;
-  */
-  debugOut.analog[6] = acc[PITCH];
+    (stronglyFilteredAcc[Z] * 99 + acc[Z] * 10) / 100;
-  debugOut.analog[7] = acc[ROLL];
+  */
+}
 void analog_calibrate(void) {
 #define GYRO_OFFSET_CYCLES 32
   uint8_t i, axis;
-  int32_t deltaOffsets[3] = { 0, 0, 0 };
-  // Set the filters... to be removed again, once some good settings are found.
-  GYROS_PID_FILTER = (dynamicParams.UserParams[4] & 0b00000011) + 1;
-  GYROS_ATT_FILTER = ((dynamicParams.UserParams[4] & 0b00001100) >> 2) + 1;
-  GYROS_D_FILTER = ((dynamicParams.UserParams[4] & 0b00110000) >> 4) + 1;
-  ACC_FILTER = ((dynamicParams.UserParams[4] & 0b11000000) >> 6) + 1;
   int32_t deltaOffsets[3] = { 0, 0, 0 };
   gyro_calibrate();
   // determine gyro bias by averaging (requires that the copter does not rotate around any axis!)
       deltaOffsets[axis] += rawGyroSum[axis];
+    }
+  }
   for (axis = PITCH; axis <= YAW; axis++) {
-    gyroOffset[axis] = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
+    gyroOffset.offsets[axis] = (deltaOffsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
-    // DebugOut.Analog[20 + axis] = gyroOffset[axis];
+    debugOut.analog[6+axis] = gyroOffset.offsets[axis];
+  }
+  // Noise is relativ to offset. So, reset noise measurements when changing offsets.
-  // Noise is relativ to offset. So, reset noise measurements when changing offsets.
+  gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0;
-  gyroNoisePeak[PITCH] = gyroNoisePeak[ROLL] = 0;
+  accOffset_readFromEEProm();
-  accOffset[PITCH] = GetParamWord(PID_ACC_PITCH);
+  // accOffset[PITCH] = getParamWord(PID_ACC_PITCH);
-  accOffset[ROLL] = GetParamWord(PID_ACC_ROLL);
+  // accOffset[ROLL] = getParamWord(PID_ACC_ROLL);
-  accOffset[Z] = GetParamWord(PID_ACC_Z);
+  // accOffset[Z] = getParamWord(PID_ACC_Z);
  * 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, axis;
+  uint8_t i, axis;
-        int32_t deltaOffset[3] = { 0, 0, 0 };
+  int32_t deltaOffset[3] = { 0, 0, 0 };
-        int16_t filteredDelta;
+  int16_t filteredDelta;
-        // int16_t pressureDiff, savedRawAirPressure;
+  // int16_t pressureDiff, savedRawAirPressure;
-        for (i = 0; i < ACC_OFFSET_CYCLES; i++) {
+  for (i = 0; i < ACC_OFFSET_CYCLES; i++) {
-                delay_ms_Mess(10);
+    delay_ms_Mess(10);
-                for (axis = PITCH; axis <= YAW; axis++) {
+    for (axis = PITCH; axis <= YAW; axis++) {
-                        deltaOffset[axis] += acc[axis];
+      deltaOffset[axis] += acc[axis];
-                }
+    }
-        }
+  }
-        for (axis = PITCH; axis <= YAW; axis++) {
+  for (axis = PITCH; axis <= YAW; axis++) {
-                filteredDelta = (deltaOffset[axis] + ACC_OFFSET_CYCLES / 2)
+    filteredDelta = (deltaOffset[axis] + ACC_OFFSET_CYCLES / 2)
-                                / ACC_OFFSET_CYCLES;
+      / ACC_OFFSET_CYCLES;
-                accOffset[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta;
+    accOffset.offsets[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta;
-        }
+  }
-        // Save ACC neutral settings to eeprom
+  // Save ACC neutral settings to eeprom
-        SetParamWord(PID_ACC_PITCH, accOffset[PITCH]);
+  // setParamWord(PID_ACC_PITCH, accOffset[PITCH]);
-        SetParamWord(PID_ACC_ROLL, accOffset[ROLL]);
+  // setParamWord(PID_ACC_ROLL, accOffset[ROLL]);
-        SetParamWord(PID_ACC_Z, accOffset[Z]);
+  // setParamWord(PID_ACC_Z, accOffset[Z]);
+  accOffset_writeToEEProm();
-        // 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
+  // Setting offset values has an influence in the analog.c ISR
-        // Therefore run measurement for 100ms to achive stable readings
+  // Therefore run measurement for 100ms to achive stable readings
-        delay_ms_Mess(100);
-        // Set the feedback so that air pressure ends up in the middle of the range.
-        // (raw pressure high --> OCR0A also high...)
-        /*
-         OCR0A += ((rawAirPressure - 1024) / rangewidth) - 1;
-         delay_ms_Mess(1000);
+  delay_ms_Mess(100);
-         pressureDiff = 0;
-         // DebugOut.Analog[16] = rawAirPressure;
-         #define PRESSURE_CAL_CYCLE_COUNT 5
-         for (i=0; i<PRESSURE_CAL_CYCLE_COUNT; i++) {
-         savedRawAirPressure = rawAirPressure;
-         OCR0A+=2;
-         delay_ms_Mess(500);
-         // raw pressure will decrease.
-         pressureDiff += (savedRawAirPressure - rawAirPressure);
-         savedRawAirPressure = rawAirPressure;
-         OCR0A-=2;
-         delay_ms_Mess(500);
-         // raw pressure will increase.
-         pressureDiff += (rawAirPressure - savedRawAirPressure);
-         }
-         rangewidth = (pressureDiff + PRESSURE_CAL_CYCLE_COUNT * 2 * 2 - 1) / (PRESSURE_CAL_CYCLE_COUNT * 2 * 2);
-         DebugOut.Analog[27] = rangewidth;
-         */
+}

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/analog.c @ 1792 - Rev 1955 → 1960