WebSVN - FlightCtrl - Diff - Rev 2164 and 2189 - /branches/dongfang_FC

 #include <avr/io.h>
 #include <avr/interrupt.h>
 #include <avr/pgmspace.h>
 #include <stdlib.h>
+#include <stdio.h>
-#include "analog.h"
-#include "attitude.h"
+#include "analog.h"
-#include "sensors.h"
-#include "printf_P.h"
-#include "mk3mag.h"
+#include "sensors.h"
-// for Delay functions
+// for Delay functions used in calibration.
-#include "timer0.h"
 #include "timer0.h"
 // For reading and writing acc. meter offsets.
-#include "eeprom.h"
-// For debugOut
+#include "eeprom.h"
-#include "output.h"
+#include "debug.h"
+// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
-// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit
+#define startADC() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE))
 #define startADC() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE))
+// TODO: Off to PROGMEM .
 const char* recal = ", recalibration needed.";
 /*
+ * Gyro and accelerometer values for attitude computation.
- * For each A/D conversion cycle, each analog channel is sampled a number of times
+ * Unfiltered (this is unnecessary as noise should get absorbed in DCM).
- * (see array channelsForStates), and the results for each channel are summed.
+ * Normalized to rad/s.
- * Here are those for the gyros and the acc. meters. They are not zero-offset.
+ * Data flow: ADCs (1 iteration) --> samplingADCData --offsetting-->  gyro_attitude_tmp
- * They are exported in the analog.h file - but please do not use them! The only
+ * --rotation-->
- * reason for the export is that the ENC-03_FC1.3 modules needs them for calibrating
+ * [filtering] --> gyro_attitude.
- * the offsets with the DAC.
- */
+ * Altimeter is also considered part of the "long" attitude loop.
-volatile uint16_t sensorInputs[8];
+ */
-int16_t acc[3];
+Vector3f gyro_attitude;
-int16_t filteredAcc[3] = { 0,0,0 };
+Vector3f accel;
+/*
-/*
+ * This stuff is for the aircraft control thread. It runs in unprocessed integers.
- * These 4 exported variables are zero-offset. The "PID" ones are used
- * in the attitude control as rotation rates. The "ATT" ones are for
+ * (well some sort of scaling will be required).
- * integration to angles.
+ * Data flow: ADCs (1 iteration) -> samplingADCData -> [offsetting and rotation] ->
- */
+ * [filtering] --> gyro_control
-int16_t gyro_PID[2];
+ */
-int16_t gyro_ATT[2];
-int16_t gyroD[2];
+int16_t gyro_control[3];
+int16_t gyroD[2];
+int16_t gyroDWindow[2][GYRO_D_WINDOW_LENGTH];
-int16_t gyroDWindow[2][GYRO_D_WINDOW_LENGTH];
+uint8_t gyroDWindowIdx;
 uint8_t gyroDWindowIdx = 0;
-int16_t yawGyro;
-int16_t magneticHeading;
 /*
-int32_t groundPressure;
+ * Air pressure. TODO: Might as well convert to floats / well known units.
-int16_t dHeight;
+ */
 int32_t groundPressure;
-uint32_t gyroActivity;
+int16_t dHeight;
 /*
- * 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
+ * Offset values. These are the raw gyro and acc. meter sums when the copter is
- * to be centered on zero.
+ * standing still. They are used for adjusting the gyro and acc. meter values
- */
-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:
- * 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
+ * to be centered on zero.
- * 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
+sensorOffset_t gyroOffset;
- * 4: pp=1, pr=0, rp=0, rr=-1 // +180 degrees with pitch reversed
+sensorOffset_t accelOffset;
- * 5: pp=1, pr=1, rp=1, rr=-1 // +225 degrees with pitch reversed
+sensorOffset_t gyroAmplifierOffset;
- * 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
+/*
- */
+ * Redo this to that quadrant 0 is normal with an FC2.x.
  */
 void rotate(int16_t* result, uint8_t quadrant, uint8_t reverse) {
-  static const int8_t rotationTab[] = {1,1,0,-1,-1,-1,0,1};
+    static const int8_t rotationTab[] = { 1, 1, 0, -1, -1, -1, 0, 1 };
-  // Pitch to Pitch part
+    // Pitch to Pitch part
-  int8_t xx = reverse ? rotationTab[(quadrant+4)%8] : rotationTab[quadrant];
+    int8_t xx = reverse ? rotationTab[(quadrant + 4) & 7] : rotationTab[quadrant]; // 1
+    // Roll to Pitch part
+    int8_t xy = rotationTab[(quadrant + 2) & 7]; // -1
-  // Roll to Pitch part
+    // Pitch to Roll part
-  int8_t xy = rotationTab[(quadrant+2)%8];
+    int8_t yx = reverse ? rotationTab[(quadrant + 2) & 7] : rotationTab[(quadrant + 6) & 7]; // -1
-  // Pitch to Roll part
+    // Roll 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];
+    int8_t yy = rotationTab[quadrant]; // -1
-  result[0] = xx*xIn + xy*result[1];
-  result[1] = yx*xIn + yy*result[1];
+    int16_t xIn = result[0];
+    int32_t tmp0, tmp1;
+    tmp0 = xx * xIn + xy * result[1];
     tmp1 = yx * xIn + yy * result[1];
   if (quadrant & 1) {
-        // A rotation was used above, where the factors were too large by sqrt(2).
+    if (quadrant & 1) {
-        // So, we multiply by 2^n/sqt(2) and right shift n bits, as to divide by sqrt(2).
+        tmp0 = (tmp0 * 181L) >> 8;
-        // A suitable value for n: Sample is 11 bits. After transformation it is the sum
+        tmp1 = (tmp1 * 181L) >> 8;
         // 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;
     result[0] = (int16_t) tmp0;
     result[1] = (int16_t) tmp1;
 // Partial sum of AIRPRESSURE_SUMMATION_FACTOR samples.
 int32_t airPressureSum;
 // The number of samples summed into airPressureSum so far.
-uint8_t pressureMeasurementCount;
+uint8_t pressureSumCount;
 /*
 int16_t UBat = 100;
 /*
  * Control and status.
  */
+volatile uint16_t samplingADCData[8];
-volatile uint8_t analogDataReady = 1;
+volatile uint16_t attitudeADCData[8];
-/*
- * Experiment: Measuring vibration-induced sensor noise.
+volatile uint8_t analog_controlDataStatus = CONTROL_SENSOR_DATA_READY;
- */
+volatile uint8_t analog_attitudeDataStatus = ATTITUDE_SENSOR_NO_DATA;
-uint16_t gyroNoisePeak[3];
+// Number of ADC iterations done for current attitude loop.
-uint16_t accNoisePeak[3];
+volatile uint8_t attitudeSumCount;
-volatile uint8_t adState;
-volatile uint8_t adChannel;
-// ADC channels
-#define AD_GYRO_YAW       0
-#define AD_GYRO_ROLL      1
-#define AD_GYRO_PITCH     2
-#define AD_AIRPRESSURE    3
-#define AD_UBAT           4
-#define AD_ACC_Z          5
-#define AD_ACC_ROLL       6
-#define AD_ACC_PITCH      7
-/*
- * Table of AD converter inputs for each state.
- * The number of samples summed for each channel is equal to
- * the number of times the channel appears in the array.
- * The max. number of samples that can be taken in 2 ms is:
- * 20e6 / 128 / 13 / (1/2e-3) = 24. Since the main control
- * loop needs a little time between reading AD values and
  * re-enabling ADC, the real limit is (how much?) lower.
+volatile uint8_t ADCSampleCount;
- * The acc. sensor is sampled even if not used - or installed
+volatile uint8_t adChannel;
+const uint8_t channelsForStates[] PROGMEM = {
+    AD_GYRO_X,
+    AD_GYRO_Y,
+    AD_GYRO_Z,
+    AD_ACCEL_X,
+    AD_ACCEL_Y,
- * at all. The cost is not significant.
+    AD_GYRO_X,
+    AD_GYRO_Y,
+    //AD_GYRO_Z,
+    AD_ACCEL_Z,
+    AD_AIRPRESSURE,
+    AD_GYRO_X,
+    AD_GYRO_Y,
+    AD_GYRO_Z,
+    AD_ACCEL_X,
+    AD_ACCEL_Y,
+    AD_GYRO_X,
+    AD_GYRO_Y,
+    //AD_GYRO_Z,
+    //AD_ACCEL_Z,
+    //AD_AIRPRESSURE,
+    AD_GYRO_X,
- */
+    AD_GYRO_Y,
+    AD_GYRO_Z,
+    AD_ACCEL_X,
-const uint8_t channelsForStates[] PROGMEM = {
+    AD_ACCEL_Y,
+    AD_GYRO_X,
+    AD_GYRO_Y,
+    //AD_GYRO_Z,
-  AD_GYRO_PITCH, AD_GYRO_ROLL, AD_GYRO_YAW,
+    AD_ACCEL_Z,
-  AD_ACC_PITCH, AD_ACC_ROLL, AD_AIRPRESSURE,
+    AD_AIRPRESSURE,
-  AD_GYRO_PITCH, AD_GYRO_ROLL, AD_ACC_Z, // at 8, measure Z acc.
+    AD_GYRO_Y,
-  AD_GYRO_PITCH, AD_GYRO_ROLL, AD_GYRO_YAW, // at 11, finish yaw gyro
+    AD_GYRO_X,
+    AD_GYRO_Z,
+    AD_ACCEL_X,
+    AD_ACCEL_Y,
-  AD_ACC_PITCH,   // at 12, finish pitch axis acc.
+    AD_GYRO_X,
-  AD_ACC_ROLL,    // at 13, finish roll axis acc.
+    AD_GYRO_Y,
-  AD_AIRPRESSURE, // at 14, finish air pressure.
+    // AD_GYRO_Z,
-        ADCSRB &= ~((1<<ADTS2)|(1<<ADTS1)|(1<<ADTS0));
+    ADCSRB &= ~((1 << ADTS2) | (1 << ADTS1) | (1 << ADTS0));
-        for (uint8_t i=0; i<MAX_AIRPRESSURE_WINDOW_LENGTH; i++) {
+    for (uint8_t i = 0; i < MAX_AIRPRESSURE_WINDOW_LENGTH; i++) {
-          airPressureWindow[i] = 0;
+        airPressureWindow[i] = 0;
+    }
     windowedAirPressure = 0;
-        startAnalogConversionCycle();
+    startADCCycle();
+    // restore global interrupt flags
-        // restore global interrupt flags
+    SREG = sreg;
+}
+// Convert axis number (X, Y, Z to ADC channel mapping (1, 2, 0)
+uint16_t gyroValue(uint8_t axis, volatile uint16_t dataArray[]) {
+    switch (axis) {
+    case X:
-        SREG = sreg;
+        return dataArray[AD_GYRO_X];
+    case Y:
+        return dataArray[AD_GYRO_Y];
     case Z:
-uint16_t rawGyroValue(uint8_t axis) {
-        return sensorInputs[AD_GYRO_PITCH-axis];
         return dataArray[AD_GYRO_Z];
+    default:
+        return 0; // should never happen.
+    }
+}
-uint16_t rawAccValue(uint8_t axis) {
-        return sensorInputs[AD_ACC_PITCH-axis];
+uint16_t gyroValueForFC13DACCalibration(uint8_t axis) {
-}
+    return gyroValue(axis, samplingADCData);
+}
-void measureNoise(const int16_t sensor,
+// Convert axis number (X, Y, Z to ADC channel mapping (6, 7, 5)
-                volatile uint16_t* const noiseMeasurement, const uint8_t damping) {
+uint16_t accValue(uint8_t axis, volatile uint16_t dataArray[]) {
+    switch (axis) {
-        if (sensor > (int16_t) (*noiseMeasurement)) {
+    case X:
-                *noiseMeasurement = sensor;
+        return dataArray[AD_ACCEL_X];
-        } else if (-sensor > (int16_t) (*noiseMeasurement)) {
+    case Y:
-                *noiseMeasurement = -sensor;
+        return dataArray[AD_ACCEL_Y];
-        } else if (*noiseMeasurement > damping) {
+    case Z:
-                *noiseMeasurement -= damping;
+        return dataArray[AD_ACCEL_Z];
-        } else {
+    default:
-                *noiseMeasurement = 0;
+        return 0; // should never happen.
+    }
+}
 /*
  * Min.: 0
  * Max: About 106 * 240 + 2047 = 27487; it is OK with just a 16 bit type.
  */
-uint16_t getSimplePressure(int advalue) {
-        uint16_t result = (uint16_t) OCR0A * (uint16_t) rangewidth + advalue;
-        result += (acc[Z] * (staticParams.airpressureAccZCorrection-128)) >> 10;
-        return result;
+uint16_t getSimplePressure(int advalue) {
-}
+    uint16_t result = (uint16_t) OCR0A * /*(uint16_t)*/ rangewidth + advalue;
     result += (/*accel.z*/0 * (staticParams.airpressureAccZCorrection - 128)) >> 10;
-void startAnalogConversionCycle(void) {
+    return result;
-  analogDataReady = 0;
+}
+void startADCCycle(void) {
+    for (uint8_t i=0; i<8; i++) {
-  // Stop the sampling. Cycle is over.
+        samplingADCData[i] = 0;
   for (uint8_t i = 0; i < 8; i++) {
-    sensorInputs[i] = 0;
+    ADCSampleCount = 0;
     adChannel = AD_GYRO_X;
-  adState = 0;
+    ADMUX = (ADMUX & 0xE0) | adChannel;
-  adChannel = AD_GYRO_PITCH;
+    analog_controlDataStatus = CONTROL_SENSOR_SAMPLING_DATA;
-  ADMUX = (ADMUX & 0xE0) | adChannel;
+    J4HIGH;
-  startADC();
+    startADC();
 }
 /*****************************************************
  * Interrupt Service Routine for ADC
- * Runs at 312.5 kHz or 3.2 �s. When all states are
+ * Runs at 12 kHz. When all states are processed
- * processed further conversions are stopped.
+ * further conversions are stopped.
  *****************************************************/
-ISR(ADC_vect) {
+ISR( ADC_vect) {
-  sensorInputs[adChannel] += ADC;
+    samplingADCData[adChannel] += ADC;
-  // set up for next state.
+    // set up for next state.
-  adState++;
+    ADCSampleCount++;
+    if (ADCSampleCount < sizeof(channelsForStates)) {
-  if (adState < sizeof(channelsForStates)) {
+        adChannel = pgm_read_byte(&channelsForStates[ADCSampleCount]);
-    adChannel = pgm_read_byte(&channelsForStates[adState]);
+        // set adc muxer to next adChannel
-    // set adc muxer to next adChannel
+        ADMUX = (ADMUX & 0xE0) | adChannel;
-    ADMUX = (ADMUX & 0xE0) | adChannel;
+        // after full cycle stop further interrupts
-    // after full cycle stop further interrupts
+        startADC();
-    startADC();
+    } else {
-  } else {
+        J4LOW;
-    analogDataReady = 1;
-    // do not restart ADC converter.
+        analog_controlDataStatus = CONTROL_SENSOR_DATA_READY;
-  }
+        // do not restart ADC converter.
-}
+    }
-void measureGyroActivity(int16_t newValue) {
+}
-  gyroActivity += newValue * newValue;
-//                abs(newValue); // (uint32_t)((int32_t)newValue * newValue);
-}
 /*
+ * Used in calibration only!
-#define GADAMPING 6
+ * Wait the specified number of millis, and then run a full sensor ADC cycle.
-void dampenGyroActivity(void) {
+ */
+void waitADCCycle(uint16_t delay) {
+    delay_ms(delay);
+    startADCCycle();
-  static uint8_t cnt = 0;
+    while(analog_controlDataStatus != CONTROL_SENSOR_DATA_READY)
+        ;
-  if (++cnt >= IMUConfig.gyroActivityDamping) {
-    cnt = 0;
+}
-    gyroActivity *= (uint32_t)((1L<<GADAMPING)-1);
+void analog_updateControlData(void) {
+    /*
+     * 1) Near-saturation boost (dont bother with Z)
-    gyroActivity >>= GADAMPING;
+     * 2) Offset
+     * 3) Rotation
-  }
-  /*
-  if (gyroActivity >= 10) gyroActivity -= 10;
+     * 4) Filter
-  else if (gyroActivity <=- 10) gyroActivity += 10;
+     * 5) Extract gyroD (should this be without near-saturation boost really? Ignore issue)
-  */
+     */
     int16_t tempOffsetGyro[2];
+    int16_t tempGyro;
 void analog_updateGyros(void) {
-  // for various filters...
+    for (uint8_t axis=X; axis<=Y; axis++) {
-  int16_t tempOffsetGyro[2], tempGyro;
+        tempGyro = gyroValue(axis, samplingADCData);
         //debugOut.analog[3 + axis] = tempGyro;
-  debugOut.digital[0] &= ~DEBUG_SENSORLIMIT;
+        //debugOut.analog[3 + 2] = gyroValue(Z, samplingADCData);
   for (uint8_t axis=0; axis<2; axis++) {
-    tempGyro = rawGyroValue(axis);
+        /*
-    /*
+         * Process the gyro data for the PID controller.
-     * Process the gyro data for the PID controller.
+         */
-     */
+        // 1) Extrapolate: Near the ends of the range, we boost the input significantly. This simulates a
-    // 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.
-    //    gyro with a wider range, and helps counter saturation at full control.
+        //    There is hardly any reason to bother extrapolating yaw.
-    if (staticParams.bitConfig & CFG_GYRO_SATURATION_PREVENTION) {
+        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) {
-                debugOut.digital[0] |= DEBUG_SENSORLIMIT;
+            if (tempGyro < SENSOR_MIN_XY) {
-                tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE + SENSOR_MAX_PITCHROLL;
+                debugOut.digital[0] |= DEBUG_SENSORLIMIT;
+                tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT;
             } else if (tempGyro > SENSOR_MAX_XY) {
-    }
+                debugOut.digital[0] |= DEBUG_SENSORLIMIT;
-    // 2) Apply sign and offset, scale before filtering.
+                tempGyro = (tempGyro - SENSOR_MAX_XY) * EXTRAPOLATION_SLOPE + SENSOR_MAX_XY;
     tempOffsetGyro[axis] = (tempGyro - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
+        }
-  // 2.1: Transform axes.
+        // 2) Apply offset (rotation will take care of signs).
+        tempOffsetGyro[axis] = tempGyro - gyroOffset.offsets[axis];
-  rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_PR);
+    }
-  for (uint8_t axis=0; axis<2; axis++) {
+    // 2.1: Transform axes.
-        // 3) Filter.
+    rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_XY);
-    tempOffsetGyro[axis] = (gyro_PID[axis] * (IMUConfig.gyroPIDFilterConstant - 1) + tempOffsetGyro[axis]) / IMUConfig.gyroPIDFilterConstant;
+    for (uint8_t axis=X; axis<=Y; axis++) {
-    // 4) Measure noise.
+        // Filter. There is no filter for Z and no need for one.
+      tempGyro = (gyro_control[axis] * (IMUConfig.gyroPIDFilterConstant - 1) + tempOffsetGyro[axis]) / IMUConfig.gyroPIDFilterConstant;
-    measureNoise(tempOffsetGyro[axis], &gyroNoisePeak[axis], GYRO_NOISE_MEASUREMENT_DAMPING);
+        // 5) Differential measurement.
-    // 5) Differential measurement.
+        int16_t diff = tempGyro - gyro_control[axis];
-    // gyroD[axis] = (gyroD[axis] * (staticParams.gyroDFilterConstant - 1) + (tempOffsetGyro[axis] - gyro_PID[axis])) / staticParams.gyroDFilterConstant;
+        gyroD[axis] -= gyroDWindow[axis][gyroDWindowIdx];
-    int16_t diff = tempOffsetGyro[axis] - gyro_PID[axis];
+        gyroD[axis] += diff;
+        gyroDWindow[axis][gyroDWindowIdx] = diff;
-    gyroD[axis] -= gyroDWindow[axis][gyroDWindowIdx];
+        // 6) Done.
-    gyroD[axis] += diff;
+        gyro_control[axis] = tempGyro;
-    gyroDWindow[axis][gyroDWindowIdx] = diff;
+    }
-    // 6) Done.
-    gyro_PID[axis] = tempOffsetGyro[axis];
+    if (++gyroDWindowIdx >= IMUConfig.gyroDWindowLength) {
-    // Prepare tempOffsetGyro for next calculation below...
-    tempOffsetGyro[axis] = (rawGyroValue(axis) - gyroOffset.offsets[axis]) * GYRO_FACTOR_PITCHROLL;
-  }
+        gyroDWindowIdx = 0;
+    }
-  /*
-   * Now process the data for attitude angles.
-   */
-   rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_PR);
-   dampenGyroActivity();
-   gyro_ATT[PITCH] = tempOffsetGyro[PITCH];
-   gyro_ATT[ROLL] = tempOffsetGyro[ROLL];
-   /*
+    if (IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_Z)
-   measureGyroActivity(tempOffsetGyro[PITCH]);
+      tempGyro = gyroOffset.offsets[Z] - gyroValue(Z, samplingADCData);
-   measureGyroActivity(tempOffsetGyro[ROLL]);
+    else
+      tempGyro = gyroValue(Z, samplingADCData) - gyroOffset.offsets[Z];
-   */
+    gyro_control[Z] = tempGyro;
-   measureGyroActivity(gyroD[PITCH]);
-   measureGyroActivity(gyroD[ROLL]);
-   // We measure activity of yaw by plain gyro value and not d/dt, because:
-   // - There is no drift correction anyway
    // - Effect of steady circular flight would vanish (it should have effect).
-   // int16_t diff = yawGyro;
+    startADCCycle();
-   // Yaw gyro.
+}
-  if (IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_YAW)
+/*
+ * The uint16s can take a max. of 1<<16-10) = 64 samples summed.
-    yawGyro = gyroOffset.offsets[YAW] - sensorInputs[AD_GYRO_YAW];
+ * Assuming a max oversampling count of 8 for the control loop, this is 8 control loop iterations
-  else
+ * summed. After 8 are reached, we just throw away all further data. This (that the attitude loop
+ * is more than 8 times slower than the control loop) should not happen anyway so there is no waste.
+ */
+#define MAX_OVEROVERSAMPLING_COUNT 8
+void analog_sumAttitudeData(void) {
+    // From when this procedure completes, there is attitude data available.
+    if (analog_attitudeDataStatus == ATTITUDE_SENSOR_NO_DATA)
+        analog_attitudeDataStatus = ATTITUDE_SENSOR_DATA_READY;
+    if (analog_attitudeDataStatus == ATTITUDE_SENSOR_DATA_READY && attitudeSumCount < MAX_OVEROVERSAMPLING_COUNT) {
+        for (uint8_t i = 0; i < 8; i++) {
+            attitudeADCData[i] += samplingADCData[i];
+        }
+        attitudeSumCount++;
+        debugOut.analog[24] = attitudeSumCount;
+    }
+}
+void clearAttitudeData(void) {
+    for (uint8_t i = 0; i < 8; i++) {
+        attitudeADCData[i] = 0;
+    }
+    attitudeSumCount = 0;
+    analog_attitudeDataStatus = ATTITUDE_SENSOR_NO_DATA;
+}
+void updateAttitudeVectors(void) {
+    /*
+     int16_t gyro_attitude_tmp[3];
+     Vector3f gyro_attitude;
+     Vector3f accel;
+     */
     yawGyro = sensorInputs[AD_GYRO_YAW] - gyroOffset.offsets[YAW];
+    int16_t tmpSensor[3];
-  // diff -= yawGyro;
-  // gyroD[YAW] -= gyroDWindow[YAW][gyroDWindowIdx];
+    // prevent gyro_attitude_tmp and attitudeSumCount from being updated.
-  // gyroD[YAW] += diff;
+    // TODO: This only prevents interrupts from starting. Well its good enough really?
+    analog_attitudeDataStatus = ATTITUDE_SENSOR_READING_DATA;
+    tmpSensor[X] = gyroValue(X, attitudeADCData) - gyroOffset.offsets[X] * attitudeSumCount;
+    tmpSensor[Y] = gyroValue(Y, attitudeADCData) - gyroOffset.offsets[Y] * attitudeSumCount;
+    rotate(tmpSensor, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_XY);
-  // gyroDWindow[YAW][gyroDWindowIdx] = diff;
+    if (IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_Z)
+        tmpSensor[Z] = gyroOffset.offsets[Z] * attitudeSumCount - gyroValue(Z, attitudeADCData);
     else
-  // gyroActivity += (uint32_t)(abs(yawGyro)* IMUConfig.yawRateFactor);
+        tmpSensor[Z] = gyroValue(Z, attitudeADCData) - gyroOffset.offsets[Z] * attitudeSumCount;
   measureGyroActivity(yawGyro);
     gyro_attitude.x = ((float) tmpSensor[X]) / (GYRO_RATE_FACTOR_XY * attitudeSumCount);
-  // air pressure
+    // air pressure
-  if (pressureAutorangingWait) {
+    if (pressureAutorangingWait) {
-    //A range switch was done recently. Wait for steadying.
+        //A range switch was done recently. Wait for steadying.
-    pressureAutorangingWait--;
+        pressureAutorangingWait--;
-  } else {
+    } else {
-    rawAirPressure = sensorInputs[AD_AIRPRESSURE];
+        rawAirPressure = attitudeADCData[AD_AIRPRESSURE] / attitudeSumCount;
-    if (rawAirPressure < MIN_RAWPRESSURE) {
+        if (rawAirPressure < MIN_RAWPRESSURE) {
-      // value is too low, so decrease voltage on the op amp minus input, making the value higher.
+            // value is too low, so decrease voltage on the op amp minus input, making the value higher.
-      newrange = OCR0A - (MAX_RAWPRESSURE - MIN_RAWPRESSURE) / (rangewidth * 4); // 4; // (MAX_RAWPRESSURE - rawAirPressure) / (rangewidth * 2) + 1;
+            newrange = OCR0A - (MAX_RAWPRESSURE - MIN_RAWPRESSURE) / (rangewidth * 4);
-      if (newrange > MIN_RANGES_EXTRAPOLATION) {
+            if (newrange > MIN_RANGES_EXTRAPOLATION) {
-        pressureAutorangingWait = (OCR0A - newrange) * AUTORANGE_WAIT_FACTOR; // = OCRA0 - OCRA0 +
+                pressureAutorangingWait = (OCR0A - newrange) * AUTORANGE_WAIT_FACTOR; // = OCRA0 - OCRA0 +
-        OCR0A = newrange;
+                OCR0A = newrange;
-      } else {
+            } else {
-        if (OCR0A) {
+                if (OCR0A) {
+                }
+            }
-    } else if (rawAirPressure > MAX_RAWPRESSURE) {
+        } else if (rawAirPressure > MAX_RAWPRESSURE) {
-      // value is too high, so increase voltage on the op amp minus input, making the value lower.
+            // value is too high, so increase voltage on the op amp minus input, making the value lower.
-      // If near the end, make a limited increase
+            // If near the end, make a limited increase
-      newrange = OCR0A + (MAX_RAWPRESSURE - MIN_RAWPRESSURE) / (rangewidth * 4); // 4;  // (rawAirPressure - MIN_RAWPRESSURE) / (rangewidth * 2) - 1;
+            newrange = OCR0A + (MAX_RAWPRESSURE - MIN_RAWPRESSURE) / (rangewidth * 4);
-      if (newrange < MAX_RANGES_EXTRAPOLATION) {
+            if (newrange < MAX_RANGES_EXTRAPOLATION) {
-        pressureAutorangingWait = (newrange - OCR0A) * AUTORANGE_WAIT_FACTOR;
+                pressureAutorangingWait = (newrange - OCR0A) * AUTORANGE_WAIT_FACTOR;
-        OCR0A = newrange;
+                OCR0A = newrange;
-      } else {
+            } else {
-        if (OCR0A < 254) {
+                if (OCR0A < 254) {
+        }
-    // Even if the sample is off-range, use it.
+        // Even if the sample is off-range, use it.
-    simpleAirPressure = getSimplePressure(rawAirPressure);
+        simpleAirPressure = getSimplePressure(rawAirPressure);
-    if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) {
+        if (simpleAirPressure < (uint16_t)(MIN_RANGES_EXTRAPOLATION * rangewidth)) {
-      // Danger: pressure near lower end of range. If the measurement saturates, the
+            // Danger: pressure near lower end of range. If the measurement saturates, the
-      // copter may climb uncontrolledly... Simulate a drastic reduction in pressure.
+            // copter may climb uncontrolledly... Simulate a drastic reduction in pressure.
-      debugOut.digital[1] |= DEBUG_SENSORLIMIT;
+            debugOut.digital[1] |= DEBUG_SENSORLIMIT;
-      airPressureSum += (int16_t) MIN_RANGES_EXTRAPOLATION * rangewidth
+            airPressureSum += (int16_t) MIN_RANGES_EXTRAPOLATION * rangewidth
-        + (simpleAirPressure - (int16_t) MIN_RANGES_EXTRAPOLATION
+                    + (simpleAirPressure - (int16_t) MIN_RANGES_EXTRAPOLATION
-           * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;
+                            * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;
-    } else if (simpleAirPressure > MAX_RANGES_EXTRAPOLATION * rangewidth) {
+        } else if (simpleAirPressure > (uint16_t)(MAX_RANGES_EXTRAPOLATION * rangewidth)) {
-      // Danger: pressure near upper end of range. If the measurement saturates, the
+            // Danger: pressure near upper end of range. If the measurement saturates, the
-      // copter may descend uncontrolledly... Simulate a drastic increase in pressure.
+            // copter may descend uncontrolledly... Simulate a drastic increase in pressure.
-      debugOut.digital[1] |= DEBUG_SENSORLIMIT;
+            debugOut.digital[1] |= DEBUG_SENSORLIMIT;
-      debugOut.digital[1] &= ~DEBUG_SENSORLIMIT;
+            debugOut.digital[1] &= ~DEBUG_SENSORLIMIT;
-          airPressureSum += simpleAirPressure;
+            airPressureSum += simpleAirPressure;
+        }
-    // 2 samples were added.
+        // 2 samples were added.
-    pressureMeasurementCount += 2;
+        pressureSumCount += 2;
-    // Assumption here: AIRPRESSURE_OVERSAMPLING is even (well we all know it's 14 haha...)
+        // Assumption here: AIRPRESSURE_OVERSAMPLING is even (well we all know it's 28...)
-    if (pressureMeasurementCount == AIRPRESSURE_OVERSAMPLING) {
+        if (pressureSumCount == AIRPRESSURE_OVERSAMPLING) {
-      // The best oversampling count is 14.5. We add a quarter of the double ADC value to get the final half.
+            // The best oversampling count is 14.5. We add a quarter of the double ADC value to get the final half.
-      airPressureSum += simpleAirPressure >> 2;
+            airPressureSum += simpleAirPressure >> 2;
+            uint32_t lastFilteredAirPressure = filteredAirPressure;
-      uint32_t lastFilteredAirPressure = filteredAirPressure;
+            if (!staticParams.airpressureWindowLength) {
+                filteredAirPressure = (filteredAirPressure
                         * (staticParams.airpressureFilterConstant - 1)
-      if (!staticParams.airpressureWindowLength) {
+                        + airPressureSum
-          filteredAirPressure = (filteredAirPressure * (staticParams.airpressureFilterConstant - 1)
+                        + staticParams.airpressureFilterConstant / 2)
-                          + airPressureSum + staticParams.airpressureFilterConstant / 2) / staticParams.airpressureFilterConstant;
+                        / staticParams.airpressureFilterConstant;
-      } else {
+            } else {
-          // use windowed.
+                // use windowed.
+                windowedAirPressure += simpleAirPressure;
-          windowedAirPressure += simpleAirPressure;
+                windowedAirPressure -= airPressureWindow[windowPtr];
-          windowedAirPressure -= airPressureWindow[windowPtr];
+                airPressureWindow[windowPtr++] = simpleAirPressure;
-          airPressureWindow[windowPtr++] = simpleAirPressure;
+                if (windowPtr >= staticParams.airpressureWindowLength)
-          if (windowPtr >= staticParams.airpressureWindowLength) windowPtr = 0;
+                    windowPtr = 0;
-          filteredAirPressure = windowedAirPressure / staticParams.airpressureWindowLength;
+                filteredAirPressure = windowedAirPressure / staticParams.airpressureWindowLength;
+            }
-      // positive diff of pressure
+            // positive diff of pressure
-      int16_t diff = filteredAirPressure - lastFilteredAirPressure;
+            int16_t diff = filteredAirPressure - lastFilteredAirPressure;
-      // is a negative diff of height.
+            // is a negative diff of height.
+            dHeight -= diff;
-      dHeight -= diff;
+            // remove old sample (fifo) from window.
-      // remove old sample (fifo) from window.
+            dHeight += dAirPressureWindow[dWindowPtr];
-      dHeight += dAirPressureWindow[dWindowPtr];
+            dAirPressureWindow[dWindowPtr++] = diff;
+            if (dWindowPtr >= staticParams.airpressureDWindowLength)
+                dWindowPtr = 0;
+            pressureSumCount = airPressureSum = 0;
+        }
       dAirPressureWindow[dWindowPtr++] = diff;
       if (dWindowPtr >= staticParams.airpressureDWindowLength) dWindowPtr = 0;
-      pressureMeasurementCount = airPressureSum = 0;
+    debugOut.analog[25] = simpleAirPressure;
     debugOut.analog[26] = OCR0A;
-  }
+    debugOut.analog[27] = filteredAirPressure;
+}
 void analog_updateBatteryVoltage(void) {
+    // Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v).
-  // Battery. The measured value is: (V * 1k/11k)/3v * 1024 = 31.03 counts per volt (max. measurable is 33v).
+    // This is divided by 3 --> 10.34 counts per volt.
-  // This is divided by 3 --> 10.34 counts per volt.
+    UBat = (3 * UBat + attitudeADCData[AD_UBAT] / 3) / 4;
   UBat = (3 * UBat + sensorInputs[AD_UBAT] / 3) / 4;
-}
+void analog_updateAttitudeData(void) {
-void analog_update(void) {
-  analog_updateGyros();
+    updateAttitudeVectors();
   analog_updateAccelerometers();
-  analog_updateAirPressure();
+    // TODO: These are waaaay off by now.
-  analog_updateBatteryVoltage();
+    analog_updateAirPressure();
-#ifdef USE_MK3MAG
+    analog_updateBatteryVoltage();
-  magneticHeading = volatileMagneticHeading;
-#endif
+    clearAttitudeData();
+}
 void analog_setNeutral() {
+    gyro_init();
 void analog_setNeutral() {
-  gyro_init();
+    if (gyroOffset_readFromEEProm()) {
+        printf("gyro offsets invalid%s", recal);
+        gyroOffset.offsets[X] = gyroOffset.offsets[Y] = 512 * GYRO_OVERSAMPLING_XY;
+        gyroOffset.offsets[Z] = 512 * GYRO_OVERSAMPLING_Z;
+        // This will get the DACs for FC1.3 to offset to a reasonable value.
+        gyro_calibrate();
+    }
   if (gyroOffset_readFromEEProm()) {
-    printf("gyro offsets invalid%s",recal);
+    if (accelOffset_readFromEEProm()) {
-    gyroOffset.offsets[PITCH] = gyroOffset.offsets[ROLL] = 512 * GYRO_OVERSAMPLING_PITCHROLL;
+        printf("acc. meter offsets invalid%s", recal);
-    gyroOffset.offsets[YAW] = 512 * GYRO_OVERSAMPLING_YAW;
+        accelOffset.offsets[X] = accelOffset.offsets[Y] = 512 * ACCEL_OVERSAMPLING_XY;
         accelOffset.offsets[Z] = 512 * ACCEL_OVERSAMPLING_Z;
         if (IMUConfig.imuReversedFlags & IMU_REVERSE_ACCEL_Z) {
-  if (accOffset_readFromEEProm()) {
+            accelOffset.offsets[Z] -= ACCEL_G_FACTOR_Z;
-    printf("acc. meter offsets invalid%s",recal);
+        } else {
-    accOffset.offsets[PITCH] = accOffset.offsets[ROLL] = 512 * ACC_OVERSAMPLING_XY;
+            accelOffset.offsets[Z] += ACCEL_G_FACTOR_Z;
     accOffset.offsets[Z] = 717 * ACC_OVERSAMPLING_Z;
+    }
-  // Noise is relative to offset. So, reset noise measurements when changing offsets.
-  for (uint8_t i=PITCH; i<=ROLL; i++) {
-          gyroNoisePeak[i] = 0;
+    // Noise is relative to offset. So, reset noise measurements when changing offsets.
-          gyroD[i] = 0;
+    for (uint8_t i=X; i<=Y; i++) {
-          for (uint8_t j=0; j<GYRO_D_WINDOW_LENGTH; j++) {
+        // gyroNoisePeak[i] = 0;
-                  gyroDWindow[i][j] = 0;
+        gyroD[i] = 0;
         for (uint8_t j=0; j<GYRO_D_WINDOW_LENGTH; j++) {
+            gyroDWindow[i][j] = 0;
+        }
+    }
+    // Setting offset values has an influence in the analog.c ISR
     // Therefore run measurement for 100ms to achive stable readings
-  // Setting offset values has an influence in the analog.c ISR
+    waitADCCycle(100);
   // Therefore run measurement for 100ms to achive stable readings
-  delay_ms_with_adc_measurement(100, 0);
+void analog_calibrateGyros(void) {
-  gyroActivity = 0;
+#define GYRO_OFFSET_CYCLES 100
     uint8_t i, axis;
     int32_t offsets[3] = { 0, 0, 0 };
-void analog_calibrateGyros(void) {
-#define GYRO_OFFSET_CYCLES 32
+    flightControlStatus = BLOCKED_FOR_CALIBRATION;
-  uint8_t i, axis;
-  int32_t offsets[3] = { 0, 0, 0 };
+    delay_ms(10);
-  gyro_calibrate();
     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++) {
+    // determine gyro bias by averaging (requires that the copter does not rotate around any axis!)
-    delay_ms_with_adc_measurement(10, 1);
+    for (i = 0; i < GYRO_OFFSET_CYCLES; i++) {
-    for (axis = PITCH; axis <= YAW; axis++) {
+        waitADCCycle(5);
-      offsets[axis] += rawGyroValue(axis);
+        for (axis=X; axis<=Z; axis++) {
             offsets[axis] += gyroValue(axis, samplingADCData);
+        }
+    }
   for (axis = PITCH; axis <= YAW; axis++) {
-    gyroOffset.offsets[axis] = (offsets[axis] + GYRO_OFFSET_CYCLES / 2) / GYRO_OFFSET_CYCLES;
+    for (axis=X; axis<=Z; axis++) {
         gyroOffset.offsets[axis] = (offsets[axis] + GYRO_OFFSET_CYCLES/2) / GYRO_OFFSET_CYCLES;
-    int16_t min = (512-200) * (axis==YAW) ? GYRO_OVERSAMPLING_YAW : GYRO_OVERSAMPLING_PITCHROLL;
+        int16_t min = (512 - 200) * (axis==Z) ? GYRO_OVERSAMPLING_Z : GYRO_OVERSAMPLING_XY;
-    int16_t max = (512+200) * (axis==YAW) ? GYRO_OVERSAMPLING_YAW : GYRO_OVERSAMPLING_PITCHROLL;
+        int16_t max = (512 + 200) * (axis==Z) ? GYRO_OVERSAMPLING_Z : GYRO_OVERSAMPLING_XY;
-    if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max)
+        if (gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max)
-      versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_PITCH << axis;
+            versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_X << axis;
+    }
+    gyroOffset_writeToEEProm();
+    //startADCCycle();
+}
-  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
- * Find acc. offsets for a neutral reading, and write them to EEPROM.
+ * anyway. There would be nothing wrong with updating the variables
- * Does not (!} update the local variables. This must be done with a
+ * directly from here, though.
- * call to analog_calibrate() - this always (?) is done by the caller
+ */
- * anyway. There would be nothing wrong with updating the variables
+void analog_calibrateAcc(void) {
- * directly from here, though.
+#define ACCEL_OFFSET_CYCLES 100
- */
+    uint8_t i, axis;
-void analog_calibrateAcc(void) {
+    int32_t offsets[3] = { 0, 0, 0 };
-#define ACC_OFFSET_CYCLES 32
-  uint8_t i, axis;
+    flightControlStatus = BLOCKED_FOR_CALIBRATION;
-  int32_t offsets[3] = { 0, 0, 0 };
+    delay_ms(10);
-  for (i = 0; i < ACC_OFFSET_CYCLES; i++) {
+    for (i = 0; i < ACCEL_OFFSET_CYCLES; i++) {
-    delay_ms_with_adc_measurement(10, 1);
+        waitADCCycle(5);
-    for (axis = PITCH; axis <= YAW; axis++) {
+        for (axis=X; axis<=Z; axis++) {
-      offsets[axis] += rawAccValue(axis);
+            offsets[axis] += accValue(axis, samplingADCData);
-    }
+        }
+    }
-  for (axis = PITCH; axis <= YAW; axis++) {
+    for (axis=X; axis<=Z; axis++) {
-    accOffset.offsets[axis] = (offsets[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES;
+        accelOffset.offsets[axis] = (offsets[axis] + ACCEL_OFFSET_CYCLES / 2) / ACCEL_OFFSET_CYCLES;
-    int16_t min,max;
+        int16_t min, max;
+        if (axis == Z) {
+            if (IMUConfig.imuReversedFlags & IMU_REVERSE_ACCEL_Z) {
+                // TODO: This assumes a sensitivity of +/- 2g.
+                min = (256 - 200) * ACCEL_OVERSAMPLING_Z;
+                max = (256 + 200) * ACCEL_OVERSAMPLING_Z;
+            } else {
-    if (axis==Z) {
+                // TODO: This assumes a sensitivity of +/- 2g.
-        if (IMUConfig.imuReversedFlags & IMU_REVERSE_ACC_Z) {
+                min = (768 - 200) * ACCEL_OVERSAMPLING_Z;
-        // TODO: This assumes a sensitivity of +/- 2g.
+                max = (768 + 200) * ACCEL_OVERSAMPLING_Z;
                 min = (256-200) * ACC_OVERSAMPLING_Z;
-                        max = (256+200) * ACC_OVERSAMPLING_Z;
+        } else {
-        } else {
+            min = (512 - 200) * ACCEL_OVERSAMPLING_XY;
-        // TODO: This assumes a sensitivity of +/- 2g.
+            max = (512 + 200) * ACCEL_OVERSAMPLING_XY;
-                min = (768-200) * ACC_OVERSAMPLING_Z;
+        }
+        if (gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) {
+            versionInfo.hardwareErrors[0] |= FC_ERROR0_ACCEL_X << axis;
                         max = (768+200) * ACC_OVERSAMPLING_Z;
+    }
-    } else {
-        min = (512-200) * ACC_OVERSAMPLING_XY;
-        max = (512+200) * ACC_OVERSAMPLING_XY;
-    }
-    if(gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) {
-      versionInfo.hardwareErrors[0] |= FC_ERROR0_ACC_X << axis;
-    }
-  }
-  accOffset_writeToEEProm();
+    if (IMUConfig.imuReversedFlags & IMU_REVERSE_ACCEL_Z) {

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/analog.c - Rev 2164 → 2189