WebSVN - FlightCtrl - Path Comparison - /branches/dongfang_FC_rewrite Rev 2014 and /branches/dongfang_FC

Ignore whitespace Rev 2014 → Rev 2015

 /branches/dongfang_FC_rewrite/ADXRS610_FC2.0.c
 ,9 → 1,6
 #include "ADXRS610_FC2.0.h"
 #include "configuration.h"
-const uint8_t GYRO_REVERSED[3] = { 0, 0, 0 };
-const uint8_t ACC_REVERSED[3] = { 0, 1, 0 };
 void gyro_calibrate(void) {
   // Nothing to calibrate.
 }

 /branches/dongfang_FC_rewrite/ENC-03_FC1.3.c
 ,9 → 9,6
 #define PITCHROLL_MINLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 510
 #define PITCHROLL_MAXLIMIT GYRO_SUMMATION_FACTOR_PITCHROLL * 515
-const uint8_t GYRO_REVERSED[3] = { 0, 0, 1 };
-const uint8_t ACC_REVERSED[3] = { 0, 1, 0 };
 void gyro_calibrate(void) {
         printf("gyro_calibrate");
         uint8_t i, axis, factor, numberOfAxesInRange = 0;
 ,9 → 15,10
         // GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0;
         for (i = 140; i != 0; i--) {
+                delay_ms_with_adc_measurement(i <= 10 ? 10 : 2, 1);
                 // If all 3 axis are in range, shorten the remaining number of iterations.
-                if (numberOfAxesInRange == 3 && i > 10)
-                        i = 10;
+                if (numberOfAxesInRange == 3 && i > 10) i = 10;
                 numberOfAxesInRange = 0;
 ,9 → 28,9
                         else
                                 factor = GYRO_SUMMATION_FACTOR_PITCHROLL;
-                        if (rawGyroSum[axis] < 510 * factor)
+                        if (rawGyroValue(axis) < 510 * factor)
                                 gyroAmplifierOffset.offsets[axis]--;
-                        else if (rawGyroSum[axis] > 515 * factor)
+                        else if (rawGyroValue(axis) > 515 * factor)
                                 gyroAmplifierOffset.offsets[axis]++;
                         else
                                 numberOfAxesInRange++;
 ,11 → 44,9
                 }
                 gyro_loadAmplifierOffsets(0);
-                delay_ms_with_adc_measurement(i <= 10 ? 10 : 2);
         }
         gyroAmplifierOffset_writeToEEProm();
-        delay_ms_with_adc_measurement(70);
+        delay_ms_with_adc_measurement(70, 0);
 }
 void gyro_loadAmplifierOffsets(uint8_t overwriteWithDefaults) {

 /branches/dongfang_FC_rewrite/analog.c
 ,10 → 84,8
  * the offsets with the DAC.
  */
 volatile uint16_t sensorInputs[8];
-volatile int16_t rawGyroSum[3];
-volatile int16_t acc[3];
-volatile int16_t filteredAcc[3] = { 0,0,0 };
-// volatile int32_t stronglyFilteredAcc[3] = { 0,0,0 };
+int16_t acc[3];
+int16_t filteredAcc[3] = { 0,0,0 };
 /*
  * These 4 exported variables are zero-offset. The "PID" ones are used
 ,10 → 92,10
  * 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;
+int16_t gyro_PID[2];
+int16_t gyro_ATT[2];
+int16_t gyroD[2];
+int16_t yawGyro;
 /*
  * Offset values. These are the raw gyro and acc. meter sums when the copter is
 ,10 → 108,57
 sensorOffset_t gyroAmplifierOffset;
 /*
- * This allows some experimentation with the gyro filters.
- * Should be replaced by #define's later on...
+ * 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:
+ * 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
+ * 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
  */
 ,16 → 168,16
 // 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.
 ,7 → 184,7
  * 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.
 ,8 → 195,8
 /*
  * Experiment: Measuring vibration-induced sensor noise.
  */
-volatile uint16_t gyroNoisePeak[3];
-volatile uint16_t accNoisePeak[3];
+uint16_t gyroNoisePeak[3];
+uint16_t accNoisePeak[3];
 volatile uint8_t adState;
 volatile uint8_t adChannel;
 ,6 → 270,14
         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)) {
 ,12 → 335,12
 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.
      */
 ,53 → 349,49
     if (staticParams.bitConfig & CFG_GYRO_SATURATION_PREVENTION) {
       if (tempGyro < SENSOR_MIN_PITCHROLL) {
-        debugOut.digital[0] |= DEBUG_SENSORLIMIT;
-        tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT;
+                debugOut.digital[0] |= DEBUG_SENSORLIMIT;
+                tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT;
       } else if (tempGyro > SENSOR_MAX_PITCHROLL) {
-        debugOut.digital[0] |= DEBUG_SENSORLIMIT;
-        tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE
-          + SENSOR_MAX_PITCHROLL;
+                debugOut.digital[0] |= DEBUG_SENSORLIMIT;
+                tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE + SENSOR_MAX_PITCHROLL;
       }
     }
     // 2) Apply sign and offset, scale before filtering.
-    if (GYRO_REVERSED[axis]) {
-      tempOffsetGyro = (gyroOffset.offsets[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL;
-    } else {
-      tempOffsetGyro = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
-    }
-    // 3) Scale and filter.
-    tempOffsetGyro = (gyro_PID[axis] * (staticParams.gyroPIDFilterConstant - 1) + tempOffsetGyro) / staticParams.gyroPIDFilterConstant;
+    tempOffsetGyro[axis] = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
+  }
+  // 2.1: Transform axes.
+  rotate(tempOffsetGyro, staticParams.gyroQuadrant, staticParams.imuReversedFlags & 1);
+  for (uint8_t axis=0; axis<2; axis++) {
+        // 3) Filter.
+    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.
-     */
-    tempGyro = rawGyroSum[axis];
-    // 1) Apply sign and offset, scale before filtering.
-    if (GYRO_REVERSED[axis]) {
-      tempOffsetGyro = (gyroOffset.offsets[axis] - tempGyro) * GYRO_FACTOR_PITCHROLL;
-    } else {
-      tempOffsetGyro = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
-    }
-    // 2) Filter.
-    gyro_ATT[axis] = (gyro_ATT[axis] * (staticParams.gyroATTFilterConstant - 1) + tempOffsetGyro) / staticParams.gyroATTFilterConstant;
+    gyro_PID[axis] = tempOffsetGyro[axis];
+    // 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. 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[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];
-  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];
 ,24 → 398,23
 }
 void analog_updateAccelerometers(void) {
-  // Z acc.
-  if (ACC_REVERSED[Z])
-    acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z];
-  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])
-      acc[axis] = accOffset.offsets[axis] - sensorInputs[AD_ACC_PITCH-axis];
-    else
-      acc[axis] = sensorInputs[AD_ACC_PITCH-axis] - accOffset.offsets[axis];
+    acc[axis] = rawAccValue(axis) - accOffset.offsets[axis];
   }
-  for (uint8_t axis=0; axis<3; 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.
+  if (staticParams.imuReversedFlags & 8)
+    acc[Z] = accOffset.offsets[Z] - sensorInputs[AD_ACC_Z];
+  else
+    acc[Z] = sensorInputs[AD_ACC_Z] - accOffset.offsets[Z];
 }
 void analog_updateAirPressure(void) {
 ,7 → 536,7
   // 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);
 ,9 → 551,9
   // 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);
     }
   }
 ,6 → 562,7
   }
   gyroOffset_writeToEEProm();
+  startAnalogConversionCycle();
 }
 /*
 ,23 → 573,22
  * 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)
-      / ACC_OFFSET_CYCLES;
-    accOffset.offsets[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta;
+    accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES;
   }
-  accOffset_writeToEEProm();
+  accOffset_writeToEEProm();
+  startAnalogConversionCycle();
 }

 /branches/dongfang_FC_rewrite/analog.h
 ,7 → 16,6
  * Temporarily replaced by userparam-configurable variable.
  */
 // #define GYROS_ATT_FILTER 1
 // Temporarily replaced by userparam-configurable variable.
 // #define ACC_FILTER 4
 ,12 → 174,12
  * in filtering, and when a gyro becomes near saturated.
  * Maybe this distinction is not really necessary.
  */
-extern volatile int16_t gyro_PID[2];
-extern volatile int16_t gyro_ATT[2];
-extern volatile int16_t gyroD[2];
-extern volatile int16_t yawGyro;
+extern int16_t gyro_PID[2];
+extern int16_t gyro_ATT[2];
+extern int16_t gyroD[2];
+extern int16_t yawGyro;
 extern volatile uint16_t ADCycleCount;
-extern volatile int16_t UBat;
+extern int16_t UBat;
 // 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3.
 #define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3))
 ,13 → 191,13
 /*
  * This is not really for external use - but the ENC-03 gyro modules needs it.
  */
-extern volatile int16_t rawGyroSum[3];
+//extern volatile int16_t rawGyroSum[3];
 /*
  * The acceleration values that this module outputs. They are zero based.
  */
-extern volatile int16_t acc[3];
-extern volatile int16_t filteredAcc[3];
+extern int16_t acc[3];
+extern int16_t filteredAcc[3];
 // extern volatile int32_t stronglyFilteredAcc[3];
 /*
 ,8 → 205,8
  * only really reflect the noise level when the copter stands still but with
  * its motors running.
  */
-extern volatile uint16_t gyroNoisePeak[3];
-extern volatile uint16_t accNoisePeak[3];
+extern uint16_t gyroNoisePeak[3];
+extern uint16_t accNoisePeak[3];
 /*
  * Air pressure.
 ,11 → 253,11
 #define ABS_ALTITUDE_OFFSET 108205
-extern volatile uint16_t simpleAirPressure;
+extern uint16_t simpleAirPressure;
 /*
  * At saturation, the filteredAirPressure may actually be (simulated) negative.
  */
-extern volatile int32_t filteredAirPressure;
+extern int32_t filteredAirPressure;
 /*
  * Flag: Interrupt handler has done all A/D conversion and processing.
 ,6 → 267,12
 void analog_init(void);
 /*
+ * This is really only for use for the ENC-03 code module, which needs to get the raw value
+ * for its calibration. The raw value should not be used for anything else.
+ */
+uint16_t rawGyroValue(uint8_t axis);
+/*
  * Start the conversion cycle. It will stop automatically.
  */
 void startAnalogConversionCycle(void);

 /branches/dongfang_FC_rewrite/attitude.c
 ,10 → 214,6
   averageAccCount++;
   yawRate = yawGyro + driftCompYaw;
-  // We are done reading variables from the analog module.
-  // Interrupt-driven sensor reading may restart.
-  startAnalogConversionCycle();
 }
 /*
 ,6 → 411,10
   correctIntegralsByAcc0thOrder();
   driftCorrection();
 #endif
+  // We are done reading variables from the analog module.
+  // Interrupt-driven sensor reading may restart.
+  startAnalogConversionCycle();
 }
 void updateCompass(void) {

/branches/dongfang_FC_rewrite/commands.c
68,7 → 68,6
if (!(MKFlags & MKFLAG_MOTOR_RUN)) {
if (command == COMMAND_GYROCAL && !repeated) {
// Run gyro calibration but do not repeat it.

// TODO: out of here. Anyway, MKFLAG_MOTOR_RUN is cleared. Not enough?
// isFlying = 0;
// check roll/pitch stick position

 /branches/dongfang_FC_rewrite/configuration.c
 ,7 → 230,7
   // IMU
   staticParams.gyroPIDFilterConstant = 1;
-  staticParams.gyroATTFilterConstant = 1;
+  // staticParams.gyroATTFilterConstant = 1;
   staticParams.gyroDFilterConstant = 1;
   staticParams.accFilterConstant = 10;

 /branches/dongfang_FC_rewrite/configuration.h
 ,6 → 113,11
   uint8_t motorSmoothing;
   // IMU
+  // IMU stuff
+  uint8_t gyroQuadrant;
+  uint8_t accQuadrant;
+  uint8_t imuReversedFlags;
   uint8_t gyroPIDFilterConstant;
   uint8_t gyroATTFilterConstant;
   uint8_t gyroDFilterConstant;

 /branches/dongfang_FC_rewrite/flight.c
 ,7 → 153,7
   int16_t yawTerm, throttleTerm, term[2];
   // PID controller variables
-  int16_t PPArt[2], DPart[2], PPartYaw, DPartYaw;
+  int16_t PDPart[2],/* DPart[2],*/ PDPartYaw /*, DPartYaw */;
   static int32_t IPart[2] = { 0, 0 };
   static uint16_t emergencyFlightTime;
   static int8_t debugDataTimer = 1;
 ,7 → 285,7
   /************************************************************************/
   // The P-part is the P of the PID controller. That's the angle integrals (not rates).
   for (axis = PITCH; axis <= ROLL; axis++) {
-    PPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral
+    PDPart[axis] = angle[axis] * gyroIFactor / (44000 / CONTROL_SCALING); // P-Part - Proportional to Integral
     /*
      * Now blend in the D-part - proportional to the Differential of the integral = the rate.
 ,12 → 292,12
      * Read this as: PDPart = PPart + rate_PID * pfactor * CONTROL_SCALING
      * where pfactor is in [0..1].
      */
-    DPart[axis] = (int32_t) ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
+    PDPart[axis] += (int32_t) ((int32_t) rate_PID[axis] * gyroPFactor / (256L / CONTROL_SCALING)) + (differential[axis] * (int16_t) dynamicParams.gyroD) / 16;
     CHECK_MIN_MAX(PDPart[axis], -SENSOR_LIMIT, SENSOR_LIMIT);
   }
-  PPartYaw = (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING));
-  DPartYaw = (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING);
+  PDPartYaw = (int32_t) (yawAngleDiff * yawIFactor) / (2 * (44000 / CONTROL_SCALING));
+  PDPartYaw += (int32_t) (yawRate * 2 * (int32_t) yawPFactor) / (256L / CONTROL_SCALING);
   // limit control feedback
   // CHECK_MIN_MAX(PDPartYaw, -SENSOR_LIMIT, SENSOR_LIMIT);
 ,7 → 367,7
       // Integration mode: Integrate (angle - stick) = the difference between angle and stick pos.
       // That means: Holding the stick a little forward will, at constant flight attitude, cause this to grow (decline??) over time.
       // TODO: Find out why this seems to be proportional to stick position - not integrating it at all.
-      IPart[axis] += PPart[axis] - controls[axis]; // Integrate difference between P part (the angle) and the stick pos.
+      IPart[axis] += PDPart[axis] - controls[axis]; // Integrate difference between P part (the angle) and the stick pos.
     } else {
       // "HH" mode: Integrate (rate - stick) = the difference between rotation rate and stick pos.
       // To keep up with a full stick PDPart should be about 156...

 /branches/dongfang_FC_rewrite/invenSense.c
 ,9 → 8,8
  * Configuration for my prototype board with InvenSense gyros.
  * The FC 1.3 board is installed upside down, therefore Z acc is reversed but not roll.
  */
-const uint8_t GYRO_REVERSED[3] = { 0, 0, 0 };
-const uint8_t ACC_REVERSED[3] = { 0, 0, 1 };
+// The special auto-zero feature of this gyro needs a port.
 #define AUTOZERO_PORT PORTD
 #define AUTOZERO_DDR DDRD
 #define AUTOZERO_BIT 5

 /branches/dongfang_FC_rewrite/timer0.c
 ,13 → 213,18
 }
 // -----------------------------------------------------------------------
-void delay_ms_with_adc_measurement(uint16_t w) {
+void delay_ms_with_adc_measurement(uint16_t w, uint8_t stop) {
   uint16_t t_stop;
   t_stop = setDelay(w);
   while (!checkDelay(t_stop)) {
-    if (analogDataReady) {
-      analog_update();
-      startAnalogConversionCycle();
-    }
+        if (analogDataReady) {
+          analog_update();
+          startAnalogConversionCycle();
+        }
   }
+  if (stop) {
+  // Wait for new samples to get prepared but do not restart AD conversion after that!
+  // Caller MUST to that.
+        while (!analogDataReady);
+  }
 }

 /branches/dongfang_FC_rewrite/timer0.h
 ,7 → 20,7
 extern void timer0_init(void);
 extern void delay_ms(uint16_t w);
-extern void delay_ms_with_adc_measurement(uint16_t w);
+extern void delay_ms_with_adc_measurement(uint16_t w, uint8_t stop);
 extern uint16_t setDelay(uint16_t t);
 extern int8_t checkDelay(uint16_t t);

Subversion Repositories FlightCtrl

Compare Revisions

Ignore whitespace Rev 2014 → Rev 2015