Subversion Repositories FlightCtrl

/*

/*

 * Offset values. These are the raw gyro and acc. meter sums when the copter is

 * 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

 * standing still. They are used for adjusting the gyro and acc. meter values

 * to be centered on zero.

 * to be centered on zero.

 */

 */

};

                * ACC_SUMMATION_FACTOR_PITCHROLL, 512 * ACC_SUMMATION_FACTOR_Z };

/*

/*

 * This allows some experimentation with the gyro filters.

 * This allows some experimentation with the gyro filters.

 * re-enabling ADC, the real limit is (how much?) lower.

 * re-enabling ADC, the real limit is (how much?) lower.

 * The acc. sensor is sampled even if not used - or installed

 * The acc. sensor is sampled even if not used - or installed

 * at all. The cost is not significant.

 * at all. The cost is not significant.

 */

 */

  AD_GYRO_ROLL,

const uint8_t channelsForStates[] PROGMEM = { AD_GYRO_PITCH, AD_GYRO_ROLL,

  AD_GYRO_ROLL,

  AD_GYRO_YAW,    // at 11, finish yaw gyro

                AD_GYRO_PITCH, AD_GYRO_ROLL, AD_GYRO_YAW, // at 11, finish yaw gyro

  ADMUX &= ~((1 << REFS1)|(1 << REFS0)|(1 << ADLAR));

        ADMUX &= ~((1 << REFS1) | (1 << REFS0) | (1 << ADLAR));

  // set muxer to ADC adc_channel 0 (0 to 7 is a valid choice)

        // set muxer to ADC adc_channel 0 (0 to 7 is a valid choice)

  ADMUX = (ADMUX & 0xE0) | AD_GYRO_PITCH;

        ADMUX = (ADMUX & 0xE0) | AD_GYRO_PITCH;

  //Set ADC Control and Status Register A

        //Set ADC Control and Status Register A

  //Auto Trigger Enable, Prescaler Select Bits to Division Factor 128, i.e. ADC clock = SYSCKL/128 = 156.25 kHz

        //Auto Trigger Enable, Prescaler Select Bits to Division Factor 128, i.e. ADC clock = SYSCKL/128 = 156.25 kHz

  //Set ADC Control and Status Register B

        //Set ADC Control and Status Register B

  //Trigger Source to Free Running Mode

        //Trigger Source to Free Running Mode

  ADCSRB &= ~((1 << ADTS2)|(1 << ADTS1)|(1 << ADTS0));

        ADCSRB &= ~((1 << ADTS2) | (1 << ADTS1) | (1 << ADTS0));

  // Start AD conversion

        // Start AD conversion

  analog_start();

        analog_start();

  // restore global interrupt flags

        // restore global interrupt flags

  SREG = sreg;

        SREG = sreg;

}

}

void measureNoise(const int16_t sensor, volatile uint16_t* const noiseMeasurement, const uint8_t damping) {

                volatile uint16_t* const noiseMeasurement, const uint8_t damping) {

  if (sensor > (int16_t)(*noiseMeasurement)) {

        if (sensor > (int16_t) (*noiseMeasurement)) {

    *noiseMeasurement = sensor;

                *noiseMeasurement = sensor;

  } else if (-sensor > (int16_t)(*noiseMeasurement)) {

        } else if (-sensor > (int16_t) (*noiseMeasurement)) {

    *noiseMeasurement = -sensor;

                *noiseMeasurement = -sensor;

 * Interrupt Service Routine for ADC            

 * Interrupt Service Routine for ADC            

 * Runs at 312.5 kHz or 3.2 µs. When all states are

 * Runs at 312.5 kHz or 3.2 µs. When all states are

 * processed the interrupt is disabled and further

 * processed the interrupt is disabled and further

 * AD conversions are stopped.

 * AD conversions are stopped.

 *****************************************************/

 *****************************************************/

  static uint8_t ad_channel = AD_GYRO_PITCH, state = 0;

        static uint8_t ad_channel = AD_GYRO_PITCH, state = 0;

  static uint16_t sensorInputs[8] = {0,0,0,0,0,0,0,0};

        static uint16_t sensorInputs[8] = { 0, 0, 0, 0, 0, 0, 0, 0 };

  static uint16_t pressureAutorangingWait = 25;

        static uint16_t pressureAutorangingWait = 25;

  uint16_t rawAirPressure;

        uint16_t rawAirPressure;

  uint8_t i, axis;

        uint8_t i, axis;

    if (ACC_REVERSED[PITCH])

                if (ACC_REVERSED[PITCH])

      acc[PITCH] = accOffset[PITCH] - sensorInputs[AD_ACC_PITCH];

                        acc[PITCH] = accOffset[PITCH] - sensorInputs[AD_ACC_PITCH];

    else

                else

      acc[PITCH] = sensorInputs[AD_ACC_PITCH] - accOffset[PITCH];

                        acc[PITCH] = sensorInputs[AD_ACC_PITCH] - accOffset[PITCH];

    filteredAcc[PITCH] = (filteredAcc[PITCH] * (ACC_FILTER-1) + acc[PITCH]) / ACC_FILTER;

                                / ACC_FILTER;

    measureNoise(acc[PITCH], &accNoisePeak[PITCH], 1);

                measureNoise(acc[PITCH], &accNoisePeak[PITCH], 1);

    break;

                break;

  case 13: // roll axis acc.

        case 13: // roll axis acc.

    if (ACC_REVERSED[ROLL])

                if (ACC_REVERSED[ROLL])

      acc[ROLL] = accOffset[ROLL] - sensorInputs[AD_ACC_ROLL];

                        acc[ROLL] = accOffset[ROLL] - sensorInputs[AD_ACC_ROLL];

      acc[ROLL] = sensorInputs[AD_ACC_ROLL] - accOffset[ROLL];

                filteredAcc[ROLL] = (filteredAcc[ROLL] * (ACC_FILTER - 1) + acc[ROLL])

    filteredAcc[ROLL] = (filteredAcc[ROLL] * (ACC_FILTER-1) + acc[ROLL]) / ACC_FILTER;

                                / ACC_FILTER;

    measureNoise(acc[ROLL], &accNoisePeak[ROLL], 1);

                measureNoise(acc[ROLL], &accNoisePeak[ROLL], 1);

    break;

                break;

    DebugOut.Analog[31] = simpleAirPressure;

                DebugOut.Analog[31] = simpleAirPressure;

    if (simpleAirPressure < MIN_RANGES_EXTRAPOLATION * rangewidth) {

                if (simpleAirPressure < 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

      airPressureSum += (int16_t)MIN_RANGES_EXTRAPOLATION * rangewidth + (simpleAirPressure - (int16_t)MIN_RANGES_EXTRAPOLATION * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;

                                                        * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;

    } else if (simpleAirPressure > MAX_RANGES_EXTRAPOLATION * rangewidth) {

                } else if (simpleAirPressure > 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

      airPressureSum += (int16_t)MAX_RANGES_EXTRAPOLATION * rangewidth + (simpleAirPressure - (int16_t)MAX_RANGES_EXTRAPOLATION * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;

                                                        * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;

    } else {

                } else {

      // normal case.

                        // normal case.

      // If AIRPRESSURE_SUMMATION_FACTOR is an odd number we only want to add half the double sample.

                        // If AIRPRESSURE_SUMMATION_FACTOR is an odd number we only want to add half the double sample.

      // The 2 cases above (end of range) are ignored for this.

                        // The 2 cases above (end of range) are ignored for this.

    }

                }

    // 2 samples were added.

                // 2 samples were added.

    pressureMeasurementCount += 2;

                pressureMeasurementCount += 2;

      filteredAirPressure = (filteredAirPressure * (AIRPRESSURE_FILTER-1) + airPressureSum + AIRPRESSURE_FILTER/2) / AIRPRESSURE_FILTER;

                                        + airPressureSum + AIRPRESSURE_FILTER / 2) / AIRPRESSURE_FILTER;

      pressureMeasurementCount = airPressureSum = 0;

                        pressureMeasurementCount = airPressureSum = 0;

    }

                }

    //    gyro with a wider range, and helps counter saturation at full control.

                //    gyro with a wider range, and helps counter saturation at full control.

    if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) {

                if (staticParams.GlobalConfig & CFG_ROTARY_RATE_LIMITER) {

      if (tempGyro < SENSOR_MIN_PITCHROLL) {

                        if (tempGyro < SENSOR_MIN_PITCHROLL) {

        tempGyro = (tempGyro - SENSOR_MAX_PITCHROLL) * EXTRAPOLATION_SLOPE + SENSOR_MAX_PITCHROLL;

                                                + SENSOR_MAX_PITCHROLL;

      }

                        }

    }

                }

    } else {

                } else {

      tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL;

                        tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL;

    }

                }

    tempOffsetGyro = (gyro_PID[axis] * (GYROS_PID_FILTER-1) + tempOffsetGyro) / GYROS_PID_FILTER;

                                / GYROS_PID_FILTER;

    // 4) Measure noise.

                measureNoise(tempOffsetGyro, &gyroNoisePeak[axis],

                // 5) Differential measurement.

    // 5) Differential measurement. 

                gyroD[axis] = (gyroD[axis] * (GYROS_D_FILTER - 1) + (tempOffsetGyro

    gyroD[axis] = (gyroD[axis] * (GYROS_D_FILTER-1) + (tempOffsetGyro - gyro_PID[axis])) / GYROS_D_FILTER;

                                - gyro_PID[axis])) / GYROS_D_FILTER;

    } else {

                } else {

      tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL;

                        tempOffsetGyro = (tempGyro - gyroOffset[axis]) * GYRO_FACTOR_PITCHROLL;

    }

                }

    gyro_ATT[axis] = (gyro_ATT[axis] * (GYROS_ATT_FILTER-1) + tempOffsetGyro) / GYROS_ATT_FILTER;

                                / GYROS_ATT_FILTER;

    break;

                break;

  case 17:

        case 17:

    state = 0;

                state = 0;

    for (i=0; i<8; i++) {

                for (i = 0; i < 8; i++) {

      sensorInputs[i] = 0;

                        sensorInputs[i] = 0;

    }

                }

    break;

                break;

  }

        }

  // set up for next state.

        // set up for next state.

  ad_channel = pgm_read_byte(&channelsForStates[state]);

        ad_channel = pgm_read_byte(&channelsForStates[state]);

  // ad_channel = channelsForStates[state];

        // ad_channel = channelsForStates[state];

  // set adc muxer to next ad_channel

        // set adc muxer to next ad_channel

  // after full cycle stop further interrupts

        if (state)

  if(state) analog_start();

                analog_start();

}

}

      deltaOffsets[axis] += rawGyroSum[axis];

                        deltaOffsets[axis] += rawGyroSum[axis];

    }

                }

  }

        }

    gyroOffset[axis] =  (deltaOffsets[axis] + GYRO_OFFSET_CYCLES/2) / GYRO_OFFSET_CYCLES;

                                / GYRO_OFFSET_CYCLES;

    DebugOut.Analog[20+axis] = gyroOffset[axis];

                DebugOut.Analog[20 + axis] = gyroOffset[axis];

  }

        }

      deltaOffset[axis] += acc[axis];

                        deltaOffset[axis] += acc[axis];

    }

                }

  }

        }

    filteredDelta = (deltaOffset[axis] + ACC_OFFSET_CYCLES / 2) / ACC_OFFSET_CYCLES;

                                / ACC_OFFSET_CYCLES;

    accOffset[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta;

                accOffset[axis] += ACC_REVERSED[axis] ? -filteredDelta : filteredDelta;

  }

        }