WebSVN - FlightCtrl - Diff - Rev 1991 and 2015 - /branches/dongfang_FC

  * 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 uint16_t sensorInputs[8];
-volatile int16_t rawGyroSum[3];
-volatile int16_t acc[3];
+int16_t acc[3];
-volatile int16_t filteredAcc[3] = { 0,0,0 };
+int16_t filteredAcc[3] = { 0,0,0 };
-// volatile int32_t stronglyFilteredAcc[3] = { 0,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];
+int16_t gyro_PID[2];
-volatile int16_t gyro_ATT[2];
+int16_t gyro_ATT[2];
-volatile int16_t gyroD[2];
+int16_t gyroD[2];
-volatile int16_t yawGyro;
+int16_t yawGyro;
 /*
  * Offset values. These are the raw gyro and acc. meter sums when the copter is
 sensorOffset_t gyroOffset;
 sensorOffset_t accOffset;
 sensorOffset_t gyroAmplifierOffset;
+/*
-/*
+ * In the MK coordinate system, nose-down is positive and left-roll is positive.
+ * If a sensor is used in an orientation where one but not both of the axes has
+ * an opposite sign, PR_ORIENTATION_REVERSED is set to 1 (true).
+ * Transform:
- * This allows some experimentation with the gyro filters.
+ * pitch <- pp*pitch + pr*roll
+ * roll  <- rp*pitch + rr*roll
+ * Not reversed, GYRO_QUADRANT:
+ * 0: pp=1, pr=0, rp=0, rr=1  // 0    degrees
+ * 1: pp=1, pr=-1,rp=1, rr=1  // +45  degrees
+ * 2: pp=0, pr=-1,rp=1, rr=0  // +90  degrees
+ * 3: pp=-1,pr=-1,rp=1, rr=1  // +135 degrees
+ * 4: pp=-1,pr=0, rp=0, rr=-1 // +180 degrees
+ * 5: pp=-1,pr=1, rp=-1,rr=-1 // +225 degrees
+ * 6: pp=0, pr=1, rp=-1,rr=0  // +270 degrees
+ * 7: pp=1, pr=1, rp=-1,rr=1  // +315 degrees
+ * Reversed, GYRO_QUADRANT:
+ * 0: pp=-1,pr=0, rp=0, rr=1  // 0    degrees with pitch reversed
+ * 1: pp=-1,pr=-1,rp=-1,rr=1  // +45  degrees with pitch reversed
+ * 2: pp=0, pr=-1,rp=-1,rr=0  // +90  degrees with pitch reversed
+ * 3: pp=1, pr=-1,rp=-1,rr=1  // +135 degrees with pitch reversed
+ * 4: pp=1, pr=0, rp=0, rr=-1 // +180 degrees with pitch reversed
+ * 5: pp=1, pr=1, rp=1, rr=-1 // +225 degrees with pitch reversed
+ * 6: pp=0, pr=1, rp=1, rr=0  // +270 degrees with pitch reversed
- * Should be replaced by #define's later on...
+ * 7: pp=-1,pr=1, rp=1, rr=1  // +315 degrees with pitch reversed
+ */
+void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) {
+  static const int8_t rotationTab[] = {1,1,0,-1,-1,-1,0,1};
+  // Pitch to Pitch part
+  int8_t xx = (reverse & 1) ? rotationTab[(quadrant+4)%8] : rotationTab[quadrant];
+  // Roll to Pitch part
+  int8_t xy = rotationTab[(quadrant+2)%8];
+  // Pitch to Roll part
+  int8_t yx = reverse ? rotationTab[(quadrant+2)%8] : rotationTab[(quadrant+6)%8];
+  // Roll to Roll part
+  int8_t yy = rotationTab[quadrant];
+  int16_t xIn = result[0];
+  result[0] = xx*result[0] + xy*result[1];
+  result[1] = yx*xIn + yy*result[1];
+  if (quadrant & 1) {
+        // A rotation was used above, where the factors were too large by sqrt(2).
+        // So, we multiply by 2^n/sqt(2) and right shift n bits, as to divide by sqrt(2).
+        // A suitable value for n: Sample is 11 bits. After transformation it is the sum
+        // of 2 11 bit numbers, so 12 bits. We have 4 bits left...
+        result[0] = (result[0]*11) >> 4;
+        result[1] = (result[1]*11) >> 4;
+  }
  */
 /*
  * Air pressure
  */
 volatile uint8_t rangewidth = 105;
 // Direct from sensor, irrespective of range.
 // volatile uint16_t rawAirPressure;
 // Value of 2 samples, with range.
-volatile uint16_t simpleAirPressure;
+uint16_t simpleAirPressure;
 // Value of AIRPRESSURE_SUMMATION_FACTOR samples, with range, filtered.
-volatile int32_t filteredAirPressure;
+int32_t filteredAirPressure;
 // Partial sum of AIRPRESSURE_SUMMATION_FACTOR samples.
-volatile int32_t airPressureSum;
+int32_t airPressureSum;
 // The number of samples summed into airPressureSum so far.
-volatile uint8_t pressureMeasurementCount;
+uint8_t pressureMeasurementCount;
 /*
  * Battery voltage, in units of: 1k/11k / 3V * 1024 = 31.03 per volt.
  * That is divided by 3 below, for a final 10.34 per volt.
  * So the initial value of 100 is for 9.7 volts.
  */
-volatile int16_t UBat = 100;
+int16_t UBat = 100;
 /*
  * Control and status.
  */
 volatile uint16_t ADCycleCount = 0;
         // restore global interrupt flags
         SREG = sreg;
+}
+uint16_t rawGyroValue(uint8_t axis) {
+        return sensorInputs[AD_GYRO_PITCH-axis];
+}
+uint16_t rawAccValue(uint8_t axis) {
+        return sensorInputs[AD_ACC_PITCH-axis];
+}
 void measureNoise(const int16_t sensor,
                 volatile uint16_t* const noiseMeasurement, const uint8_t damping) {
         if (sensor > (int16_t) (*noiseMeasurement)) {
+  }
+}
 void analog_updateGyros(void) {
   // for various filters...
-  int16_t tempOffsetGyro, tempGyro;
+  int16_t tempOffsetGyro[2], tempGyro;
   debugOut.digital[0] &= ~DEBUG_SENSORLIMIT;
   for (uint8_t axis=0; axis<2; axis++) {
-    tempGyro = rawGyroSum[axis] = sensorInputs[AD_GYRO_PITCH-axis];
+    tempGyro = rawGyroValue(axis);
     /*
      * 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.bitConfig & CFG_GYRO_SATURATION_PREVENTION) {
       if (tempGyro < SENSOR_MIN_PITCHROLL) {
-        debugOut.digital[0] |= DEBUG_SENSORLIMIT;
+                debugOut.digital[0] |= DEBUG_SENSORLIMIT;
-        tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT;
+                tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT;
-      } else if (tempGyro > SENSOR_MAX_PITCHROLL) {
-        debugOut.digital[0] |= DEBUG_SENSORLIMIT;
+      } else if (tempGyro > SENSOR_MAX_PITCHROLL) {
-        tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE
+                debugOut.digital[0] |= DEBUG_SENSORLIMIT;
-          + SENSOR_MAX_PITCHROLL;
+                tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE + SENSOR_MAX_PITCHROLL;
+      }
-    }
+    }
+    // 2) Apply sign and offset, scale before filtering.
-    // 2) Apply sign and offset, scale before filtering.
+    tempOffsetGyro[axis] = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
-    if (GYRO_REVERSED[axis]) {
+  }
-      tempOffsetGyro = (gyroOffset.offsets[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL;
-    } else {
+  // 2.1: Transform axes.
-      tempOffsetGyro = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
+  rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & 1);
-    }
+  for (uint8_t axis=0; axis<2; axis++) {
-    // 3) Scale and filter.
+        // 3) Filter.
-    tempOffsetGyro = (gyro_PID[axis] * (staticParams.gyroPIDFilterConstant - 1) + tempOffsetGyro) / staticParams.gyroPIDFilterConstant;
+    tempOffsetGyro[axis] = (gyro_PID[axis] * (staticParams.gyroPIDFilterConstant - 1) + tempOffsetGyro[axis]) / staticParams.gyroPIDFilterConstant;
     // 4) Measure noise.
-    measureNoise(tempOffsetGyro, &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING);
+    measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING);
     // 5) Differential measurement.
-    gyroD[axis] = (gyroD[axis] * (staticParams.gyroDFilterConstant - 1) + (tempOffsetGyro - gyro_PID[axis])) / staticParams.gyroDFilterConstant;
+    gyroD[axis] = (gyroD[axis] * (staticParams.gyroDFilterConstant - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / staticParams.gyroDFilterConstant;
-    // 6) Done.
-    gyro_PID[axis] = tempOffsetGyro;
-    /*
-     * Now process the data for attitude angles.
-     */
+    // 6) Done.
-    tempGyro = rawGyroSum[axis];
-    // 1) Apply sign and offset, scale before filtering.
-    if (GYRO_REVERSED[axis]) {
+    gyro_PID[axis] = tempOffsetGyro[axis];
-      tempOffsetGyro = (gyroOffset.offsets[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL;
-    } else {
-      tempOffsetGyro = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
-    }
+    // Prepare tempOffsetGyro for next calculation below...
+    tempOffsetGyro[axis] = (rawGyroValue(axis) - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
+  }
+  /*
+   * Now process the data for attitude angles.
+   */
+   rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & 1);
-    // 2) Filter.
+  // 2) Filter. This should really be quite unnecessary. The integration should gobble up any noise anyway and the values are not used for anything else.
-    gyro_ATT[axis] = (gyro_ATT[axis] * (staticParams.gyroATTFilterConstant - 1) + tempOffsetGyro) / staticParams.gyroATTFilterConstant;
-  }
+  // gyro_ATT[PITCH] = (gyro_ATT[PITCH] * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[PITCH]) / staticParams.attitudeGyroFilter;
   // gyro_ATT[ROLL]  = (gyro_ATT[ROLL]  * (staticParams.attitudeGyroFilter - 1) + tempOffsetGyro[ROLL])  / staticParams.attitudeGyroFilter;
   // Yaw gyro.
-  rawGyroSum[YAW] = sensorInputs[AD_GYRO_YAW];
+  // Yaw gyro.
-  if (GYRO_REVERSED[YAW])
+  if (staticParams.imuReversedFlags & 2)
     yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW];
-  else
-    yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW];
-}
-void analog_updateAccelerometers(void) {
-  // Z acc.
-  if (ACC_REVERSED[Z])
+  else
-    acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z];
+    yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW];
-  else
-    acc[Z] = sensorInputs[AD_ACC_Z] - accOffset.offsets[Z];
-  // Pitch and roll axis accelerations.
+}
   for (uint8_t axis=0; axis<2; axis++) {
-    if (ACC_REVERSED[axis])
+void analog_updateAccelerometers(void) {
+  // Pitch and roll axis accelerations.
+  for (uint8_t axis=0; axis<2; axis++) {
-      acc[axis] = accOffset.offsets[axis] - sensorInputs[AD_ACC_PITCH-axis];
+    acc[axis] = rawAccValue(axis) - accOffset.offsets[axis];
     else
       acc[axis] = sensorInputs[AD_ACC_PITCH-axis] - accOffset.offsets[axis];
   rotate(acc, staticParams.accQuadrant, staticParams.imuReversedFlags & 4);
+  for(uint8_t axis=0; axis<3; axis++) {
+    filteredAcc[axis] = (filteredAcc[axis] * (staticParams.accFilterConstant - 1) + acc[axis]) / staticParams.accFilterConstant;
+    measureNoise(acc[axis], &accNoisePeak[axis], 1);
+  }
   // Z acc.
-  for (uint8_t axis=0; axis<3; axis++) {
+  if (staticParams.imuReversedFlags & 8)
-    filteredAcc[axis] = (filteredAcc[axis] * (staticParams.accFilterConstant - 1) + acc[axis]) / staticParams.accFilterConstant;
+    acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z];
-    measureNoise(acc[axis], &accNoisePeak[axis], 1);
+  else
   gyroNoisePeak[PITCH] = gyroNoisePeak[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_with_adc_measurement(100);
+  delay_ms_with_adc_measurement(100, 0);
   // Rough estimate. Hmm no nothing happens at calibration anyway.
   // airPressureSum = simpleAirPressure * (AIRPRESSURE_SUMMATION_FACTOR/2);
   int32_t offsets[3] = { 0, 0, 0 };
   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_with_adc_measurement(20);
+    delay_ms_with_adc_measurement(10, 1);
     for (axis = PITCH; axis <= YAW; axis++) {
-      offsets[axis] += rawGyroSum[axis];
+      offsets[axis] += rawGyroValue(axis);
+    }
+  }
   for (axis = PITCH; axis <= YAW; axis++) {
     gyroOffset.offsets[axis] = (offsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
+  }
   gyroOffset_writeToEEProm();
-  gyroOffset_writeToEEProm();
+  startAnalogConversionCycle();
+}
 /*
  * Find acc. offsets for a neutral reading, and write them to EEPROM.
  * Does not (!} update the local variables. This must be done with a
  * call to analog_calibrate() - this always (?) is done by the caller
  * anyway. There would be nothing wrong with updating the variables
  * directly from here, though.
  */
 void analog_calibrateAcc(void) {
-#define ACC_OFFSET_CYCLES 10
+#define ACC_OFFSET_CYCLES 32
   uint8_t i, axis;
-  int32_t deltaOffset[3] = { 0, 0, 0 };
+  int32_t offsets[3] = { 0, 0, 0 };
   int16_t filteredDelta;
   for (i = 0; i < ACC_OFFSET_CYCLES; i++) {
-    delay_ms_with_adc_measurement(10);
+    delay_ms_with_adc_measurement(10, 1);
     for (axis = PITCH; axis <= YAW; axis++) {
-      deltaOffset[axis] += acc[axis];
+      offsets[axis] += rawAccValue(axis);
     }
-  }
   for (axis = PITCH; axis <= YAW; axis++) {
-    filteredDelta = (deltaOffset[axis] + ACC_OFFSET_CYCLES / 2)
+  for (axis = PITCH; axis <= YAW; axis++) {
-      / ACC_OFFSET_CYCLES;
+    accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES;

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/analog.c - Rev 1991 → 2015