Subversion Repositories FlightCtrl

#include <avr/io.h>

#include <avr/interrupt.h>

#include <avr/pgmspace.h>

#include <stdlib.h>

#include <stdio.h>

#include "analog.h"

#include "sensors.h"

// for Delay functions used in calibration.

#include "timer0.h"

// For reading and writing acc. meter offsets.

#include "eeprom.h"

#include "debug.h"

// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit

#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.

 * Unfiltered (this is unnecessary as noise should get absorbed in DCM).

 * Normalized to rad/s.

 * Data flow: ADCs (1 iteration) --> samplingADCData --offsetting-->  gyro_attitude_tmp

 * --rotation-->

 * [filtering] --> gyro_attitude.

 * Altimeter is also considered part of the "long" attitude loop.

 */

Vector3f gyro_attitude;

Vector3f accel;

/*

 * This stuff is for the aircraft control thread. It runs in unprocessed integers.

 * (well some sort of scaling will be required).

 * Data flow: ADCs (1 iteration) -> samplingADCData -> [offsetting and rotation] ->

 * [filtering] --> gyro_control

 */

int16_t gyro_control[3];

int16_t gyroD[2];

int16_t gyroDWindow[2][GYRO_D_WINDOW_LENGTH];

uint8_t gyroDWindowIdx;

/*

 * Air pressure. TODO: Might as well convert to floats / well known units.

 */

int32_t groundPressure;

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

 * to be centered on zero.

 */

sensorOffset_t gyroOffset;

sensorOffset_t accelOffset;

sensorOffset_t gyroAmplifierOffset;

/*

 * 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 };

    // Pitch to Pitch part

    int8_t xx = reverse ? rotationTab[(quadrant + 4) & 7] : rotationTab[quadrant]; // 1

    // Roll to Pitch part

    int8_t xy = rotationTab[(quadrant + 2) & 7]; // -1

    // Pitch to Roll part

    int8_t yx = reverse ? rotationTab[(quadrant + 2) & 7] : rotationTab[(quadrant + 6) & 7]; // -1  

    // Roll to Roll part

    int8_t yy = rotationTab[quadrant]; // -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) {

        tmp0 = (tmp0 * 181L) >> 8;

        tmp1 = (tmp1 * 181L) >> 8;

    }

    result[0] = (int16_t) tmp0;

    result[1] = (int16_t) tmp1;

}

/*

 * Air pressure

 */

volatile uint8_t rangewidth = 105;

// Direct from sensor, irrespective of range.

// Value of 2 samples, with range.

uint16_t simpleAirPressure;

// Value of AIRPRESSURE_OVERSAMPLING samples, with range, filtered.

int32_t filteredAirPressure;

#define MAX_D_AIRPRESSURE_WINDOW_LENGTH 32

//int32_t lastFilteredAirPressure;

int16_t dAirPressureWindow[MAX_D_AIRPRESSURE_WINDOW_LENGTH];

uint8_t dWindowPtr = 0;

#define MAX_AIRPRESSURE_WINDOW_LENGTH 32

int16_t airPressureWindow[MAX_AIRPRESSURE_WINDOW_LENGTH];

int32_t windowedAirPressure;

uint8_t windowPtr = 0;

// Partial sum of AIRPRESSURE_SUMMATION_FACTOR samples.

int32_t airPressureSum;

// The number of samples summed into airPressureSum so far.

uint8_t pressureSumCount;

/*

 * 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.

 */

int16_t UBat = 100;

/*

 * Control and status.

 */

volatile uint16_t samplingADCData[8];

volatile uint16_t attitudeADCData[8];

volatile uint8_t analog_controlDataStatus = CONTROL_SENSOR_DATA_READY;

volatile uint8_t analog_attitudeDataStatus = ATTITUDE_SENSOR_NO_DATA;

// Number of ADC iterations done for current attitude loop.

volatile uint8_t attitudeSumCount;

volatile uint8_t ADCSampleCount;

volatile uint8_t adChannel;

const uint8_t channelsForStates[] PROGMEM = {

    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,

    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,

    AD_ACCEL_Y,

    AD_GYRO_X,

    AD_GYRO_Y,

    //AD_GYRO_Z,

    AD_ACCEL_Z,

    AD_AIRPRESSURE,

    AD_GYRO_Y,

    AD_GYRO_X,

    AD_GYRO_Z,

    AD_ACCEL_X,

    AD_ACCEL_Y,

    AD_GYRO_X,

    AD_GYRO_Y,

    // AD_GYRO_Z,

    //AD_ACCEL_Z,

    //AD_AIRPRESSURE,

    AD_UBAT

};

// Feature removed. Could be reintroduced later - but should work for all gyro types then.

// uint8_t GyroDefectPitch = 0, GyroDefectRoll = 0, GyroDefectYaw = 0;

void analog_init(void) {

    uint8_t sreg = SREG;

    // disable all interrupts before reconfiguration

    cli();

    //ADC0 ... ADC7 is connected to PortA pin 0 ... 7

    DDRA = 0x00;

    PORTA = 0x00;

    // Digital Input Disable Register 0

    // Disable digital input buffer for analog adc_channel pins

    DIDR0 = 0xFF;

    // external reference, adjust data to the right

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

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

    ADMUX = (ADMUX & 0xE0);

    //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

    ADCSRA = (1 << ADPS2) | (1 << ADPS1) | (1 << ADPS0);

    //Set ADC Control and Status Register B

    //Trigger Source to Free Running Mode

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

    for (uint8_t i = 0; i < MAX_AIRPRESSURE_WINDOW_LENGTH; i++) {

        airPressureWindow[i] = 0;

    }

    windowedAirPressure = 0;

    startADCCycle();

    // 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:

        return dataArray[AD_GYRO_X];

    case Y:

        return dataArray[AD_GYRO_Y];

    case Z:

        return dataArray[AD_GYRO_Z];

    default:

        return 0; // should never happen.

    }

}

uint16_t gyroValueForFC13DACCalibration(uint8_t axis) {

    return gyroValue(axis, samplingADCData);

}

// Convert axis number (X, Y, Z to ADC channel mapping (6, 7, 5)

uint16_t accValue(uint8_t axis, volatile uint16_t dataArray[]) {

    switch (axis) {

    case X:

        return dataArray[AD_ACCEL_X];

    case Y:

        return dataArray[AD_ACCEL_Y];

    case Z:

        return dataArray[AD_ACCEL_Z];

    default:

        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 += (/*accel.z*/0 * (staticParams.airpressureAccZCorrection - 128)) >> 10;

    return result;

}

void startADCCycle(void) {

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

        samplingADCData[i] = 0;

    }

    ADCSampleCount = 0;

    adChannel = AD_GYRO_X;

    ADMUX = (ADMUX & 0xE0) | adChannel;

    analog_controlDataStatus = CONTROL_SENSOR_SAMPLING_DATA;

    J4HIGH;

    startADC();

}

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

 * Interrupt Service Routine for ADC

 * Runs at 12 kHz. When all states are processed

 * further conversions are stopped.

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

ISR( ADC_vect) {

    samplingADCData[adChannel] += ADC;

    // set up for next state.

    ADCSampleCount++;

    if (ADCSampleCount < sizeof(channelsForStates)) {

        adChannel = pgm_read_byte(&channelsForStates[ADCSampleCount]);

        // set adc muxer to next adChannel

        ADMUX = (ADMUX & 0xE0) | adChannel;

        // after full cycle stop further interrupts

        startADC();

    } else {

        J4LOW;

        analog_controlDataStatus = CONTROL_SENSOR_DATA_READY;

        // do not restart ADC converter.

    }

}

/*

 * Used in calibration only!

 * Wait the specified number of millis, and then run a full sensor ADC cycle.

 */

void waitADCCycle(uint16_t delay) {

    delay_ms(delay);

    startADCCycle();

    while(analog_controlDataStatus != CONTROL_SENSOR_DATA_READY)

        ;

}

void analog_updateControlData(void) {

    /*

     * 1) Near-saturation boost (dont bother with Z)

     * 2) Offset

     * 3) Rotation

     * 4) Filter

     * 5) Extract gyroD (should this be without near-saturation boost really? Ignore issue)

     */

    int16_t tempOffsetGyro[2];

    int16_t tempGyro;

    for (uint8_t axis=X; axis<=Y; axis++) {

        tempGyro = gyroValue(axis, samplingADCData);

        //debugOut.analog[3 + axis] = tempGyro;

        //debugOut.analog[3 + 2] = gyroValue(Z, samplingADCData);

        /*

         * 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.

        //    There is hardly any reason to bother extrapolating yaw.

        if (staticParams.bitConfig & CFG_GYRO_SATURATION_PREVENTION) {

            if (tempGyro < SENSOR_MIN_XY) {

                debugOut.digital[0] |= DEBUG_SENSORLIMIT;

                tempGyro = tempGyro * EXTRAPOLATION_SLOPE - EXTRAPOLATION_LIMIT;

            } else if (tempGyro > SENSOR_MAX_XY) {

                debugOut.digital[0] |= DEBUG_SENSORLIMIT;

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

            }

        }

        // 2) Apply offset (rotation will take care of signs).

        tempOffsetGyro[axis] = tempGyro - gyroOffset.offsets[axis];

    }

    // 2.1: Transform axes.

    rotate(tempOffsetGyro, IMUConfig.gyroQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_XY);

    for (uint8_t axis=X; axis<=Y; axis++) {

        // Filter. There is no filter for Z and no need for one.

      tempGyro = (gyro_control[axis] * (IMUConfig.gyroPIDFilterConstant - 1) + tempOffsetGyro[axis]) / IMUConfig.gyroPIDFilterConstant;

        // 5) Differential measurement.

        int16_t diff = tempGyro - gyro_control[axis];

        gyroD[axis] -= gyroDWindow[axis][gyroDWindowIdx];

        gyroD[axis] += diff;

        gyroDWindow[axis][gyroDWindowIdx] = diff;

        // 6) Done.

        gyro_control[axis] = tempGyro;

    }

    if (++gyroDWindowIdx >= IMUConfig.gyroDWindowLength) {

        gyroDWindowIdx = 0;

    }

    if (IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_Z)

      tempGyro = gyroOffset.offsets[Z] - gyroValue(Z, samplingADCData);

    else

      tempGyro = gyroValue(Z, samplingADCData) - gyroOffset.offsets[Z];

    gyro_control[Z] = tempGyro;

    startADCCycle();

}

/*

 * The uint16s can take a max. of 1<<16-10) = 64 samples summed.

 * Assuming a max oversampling count of 8 for the control loop, this is 8 control loop iterations

 * 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;

     */

    int16_t tmpSensor[3];

    // prevent gyro_attitude_tmp and attitudeSumCount from being updated.

    // 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);

    if (IMUConfig.imuReversedFlags & IMU_REVERSE_GYRO_Z)

        tmpSensor[Z] = gyroOffset.offsets[Z] * attitudeSumCount - gyroValue(Z, attitudeADCData);

    else

        tmpSensor[Z] = gyroValue(Z, attitudeADCData) - gyroOffset.offsets[Z] * attitudeSumCount;

    gyro_attitude.x = ((float) tmpSensor[X]) / (GYRO_RATE_FACTOR_XY * attitudeSumCount);

    gyro_attitude.y = ((float) tmpSensor[Y]) / (GYRO_RATE_FACTOR_XY * attitudeSumCount);

    gyro_attitude.z = ((float) tmpSensor[Z]) / (GYRO_RATE_FACTOR_Z  * attitudeSumCount);

    // Done with gyros. Now accelerometer:

    tmpSensor[X] = accValue(X, attitudeADCData) - accelOffset.offsets[X] * attitudeSumCount;

    tmpSensor[Y] = accValue(Y, attitudeADCData) - accelOffset.offsets[Y] * attitudeSumCount;

    rotate(tmpSensor, IMUConfig.accQuadrant, IMUConfig.imuReversedFlags & IMU_REVERSE_ACCEL_XY);

    // Z acc.

    if (IMUConfig.imuReversedFlags & IMU_REVERSE_ACCEL_Z)

        tmpSensor[Z] = accelOffset.offsets[Z] * attitudeSumCount - accValue(Z, attitudeADCData);

    else

        tmpSensor[Z] = accValue(Z, attitudeADCData) - accelOffset.offsets[Z] * attitudeSumCount;

    // Blarh!!! We just skip acc filtering. There are trillions of samples already.

    accel.x = (float)tmpSensor[X] / (ACCEL_FACTOR_XY * attitudeSumCount); // (accel.x + (float)tmpSensor[X] / (ACCEL_FACTOR_XY * attitudeSumCount)) / 2.0;

    accel.y = (float)tmpSensor[Y] / (ACCEL_FACTOR_XY * attitudeSumCount); // (accel.y + (float)tmpSensor[Y] / (ACCEL_FACTOR_XY * attitudeSumCount)) / 2.0;

    accel.z = (float)tmpSensor[Z] / (ACCEL_FACTOR_Z  * attitudeSumCount); // (accel.z + (float)tmpSensor[Z] / (ACCEL_FACTOR_Z  * attitudeSumCount)) / 2.0;

    for (uint8_t i=0; i<3; i++) {

      debugOut.analog[3 + i] = (int16_t)(gyro_attitude[i] * 100);

      debugOut.analog[6 + i] = (int16_t)(accel[i] * 100);

    }

}

void analog_updateAirPressure(void) {

    static uint16_t pressureAutorangingWait = 25;

    uint16_t rawAirPressure;

    int16_t newrange;

    // air pressure

    if (pressureAutorangingWait) {

        //A range switch was done recently. Wait for steadying.

        pressureAutorangingWait--;

    } else {

        rawAirPressure = attitudeADCData[AD_AIRPRESSURE] / attitudeSumCount;

        if (rawAirPressure < MIN_RAWPRESSURE) {

            // 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);

            if (newrange > MIN_RANGES_EXTRAPOLATION) {

                pressureAutorangingWait = (OCR0A - newrange) * AUTORANGE_WAIT_FACTOR; // = OCRA0 - OCRA0 +

                OCR0A = newrange;

            } else {

                if (OCR0A) {

                    OCR0A--;

                    pressureAutorangingWait = AUTORANGE_WAIT_FACTOR;

                }

            }

        } else if (rawAirPressure > MAX_RAWPRESSURE) {

            // 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

            newrange = OCR0A + (MAX_RAWPRESSURE - MIN_RAWPRESSURE) / (rangewidth * 4);

            if (newrange < MAX_RANGES_EXTRAPOLATION) {

                pressureAutorangingWait = (newrange - OCR0A) * AUTORANGE_WAIT_FACTOR;

                OCR0A = newrange;

            } else {

                if (OCR0A < 254) {

                    OCR0A++;

                    pressureAutorangingWait = AUTORANGE_WAIT_FACTOR;

                }

            }

        }

        // Even if the sample is off-range, use it.

        simpleAirPressure = getSimplePressure(rawAirPressure);

        if (simpleAirPressure < (uint16_t)(MIN_RANGES_EXTRAPOLATION * rangewidth)) {

            // Danger: pressure near lower end of range. If the measurement saturates, the

            // copter may climb uncontrolledly... Simulate a drastic reduction in pressure.

            debugOut.digital[1] |= DEBUG_SENSORLIMIT;

            airPressureSum += (int16_t) MIN_RANGES_EXTRAPOLATION * rangewidth

                    + (simpleAirPressure - (int16_t) MIN_RANGES_EXTRAPOLATION

                            * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;

        } else if (simpleAirPressure > (uint16_t)(MAX_RANGES_EXTRAPOLATION * rangewidth)) {

            // Danger: pressure near upper end of range. If the measurement saturates, the

            // copter may descend uncontrolledly... Simulate a drastic increase in pressure.

            debugOut.digital[1] |= DEBUG_SENSORLIMIT;

            airPressureSum += (int16_t) MAX_RANGES_EXTRAPOLATION * rangewidth

                    + (simpleAirPressure - (int16_t) MAX_RANGES_EXTRAPOLATION

                            * rangewidth) * PRESSURE_EXTRAPOLATION_COEFF;

        } else {

            // normal case.

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

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

            debugOut.digital[1] &= ~DEBUG_SENSORLIMIT;

            airPressureSum += simpleAirPressure;

        }

        // 2 samples were added.

        pressureSumCount += 2;

        // Assumption here: AIRPRESSURE_OVERSAMPLING is even (well we all know it's 28...)

        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.

            airPressureSum += simpleAirPressure >> 2;

            uint32_t lastFilteredAirPressure = filteredAirPressure;

            if (!staticParams.airpressureWindowLength) {

                filteredAirPressure = (filteredAirPressure

                        * (staticParams.airpressureFilterConstant - 1)

                        + airPressureSum

                        + staticParams.airpressureFilterConstant / 2)

                        / staticParams.airpressureFilterConstant;

            } else {

                // use windowed.

                windowedAirPressure += simpleAirPressure;

                windowedAirPressure -= airPressureWindow[windowPtr];

                airPressureWindow[windowPtr++] = simpleAirPressure;

                if (windowPtr >= staticParams.airpressureWindowLength)

                    windowPtr = 0;

                filteredAirPressure = windowedAirPressure / staticParams.airpressureWindowLength;

            }

            // positive diff of pressure

            int16_t diff = filteredAirPressure - lastFilteredAirPressure;

            // is a negative diff of height.

            dHeight -= diff;

            // remove old sample (fifo) from window.

            dHeight += dAirPressureWindow[dWindowPtr];

            dAirPressureWindow[dWindowPtr++] = diff;

            if (dWindowPtr >= staticParams.airpressureDWindowLength)

                dWindowPtr = 0;

            pressureSumCount = 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).

    // This is divided by 3 --> 10.34 counts per volt.

    UBat = (3 * UBat + attitudeADCData[AD_UBAT] / 3) / 4;

}

void analog_updateAttitudeData(void) {

    updateAttitudeVectors();

    // TODO: These are waaaay off by now.

    analog_updateAirPressure();

    analog_updateBatteryVoltage();

    clearAttitudeData();

}

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 (accelOffset_readFromEEProm()) {

        printf("acc. meter offsets invalid%s", recal);

        accelOffset.offsets[X] = accelOffset.offsets[Y] = 512 * ACCEL_OVERSAMPLING_XY;

        accelOffset.offsets[Z] = 512 * ACCEL_OVERSAMPLING_Z;

        if (IMUConfig.imuReversedFlags & IMU_REVERSE_ACCEL_Z) {

            accelOffset.offsets[Z] -= ACCEL_G_FACTOR_Z;

        } else {

            accelOffset.offsets[Z] += ACCEL_G_FACTOR_Z;

        }

    }

    // Noise is relative to offset. So, reset noise measurements when changing offsets.

    for (uint8_t i=X; i<=Y; i++) {

        // gyroNoisePeak[i] = 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

    waitADCCycle(100);

}

void analog_calibrateGyros(void) {

#define GYRO_OFFSET_CYCLES 100

    uint8_t i, axis;

    int32_t offsets[3] = { 0, 0, 0 };

    flightControlStatus = BLOCKED_FOR_CALIBRATION;

    delay_ms(10);

    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++) {

        waitADCCycle(5);

        for (axis=X; axis<=Z; axis++) {

            offsets[axis] += gyroValue(axis, samplingADCData);

        }

    }

    for (axis=X; axis<=Z; axis++) {

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

        int16_t min = (512 - 200) * (axis==Z) ? GYRO_OVERSAMPLING_Z : GYRO_OVERSAMPLING_XY;

        int16_t max = (512 + 200) * (axis==Z) ? GYRO_OVERSAMPLING_Z : GYRO_OVERSAMPLING_XY;

        if (gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max)

            versionInfo.hardwareErrors[0] |= FC_ERROR0_GYRO_X << axis;

    }

    gyroOffset_writeToEEProm();

    //startADCCycle();

}

/*

 * 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 ACCEL_OFFSET_CYCLES 100

    uint8_t i, axis;

    int32_t offsets[3] = { 0, 0, 0 };

    flightControlStatus = BLOCKED_FOR_CALIBRATION;

    delay_ms(10);

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

        waitADCCycle(5);

        for (axis=X; axis<=Z; axis++) {

            offsets[axis] += accValue(axis, samplingADCData);

        }

    }

    for (axis=X; axis<=Z; axis++) {

        accelOffset.offsets[axis] = (offsets[axis] + ACCEL_OFFSET_CYCLES / 2) / ACCEL_OFFSET_CYCLES;

        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 {

                // TODO: This assumes a sensitivity of +/- 2g.

                min = (768 - 200) * ACCEL_OVERSAMPLING_Z;

                max = (768 + 200) * ACCEL_OVERSAMPLING_Z;

            }

        } else {

            min = (512 - 200) * ACCEL_OVERSAMPLING_XY;

            max = (512 + 200) * ACCEL_OVERSAMPLING_XY;

        }

        if (gyroOffset.offsets[axis] < min || gyroOffset.offsets[axis] > max) {

            versionInfo.hardwareErrors[0] |= FC_ERROR0_ACCEL_X << axis;

        }

    }

    if (IMUConfig.imuReversedFlags & IMU_REVERSE_ACCEL_Z) {

        accelOffset.offsets[Z] -= ACCEL_G_FACTOR_Z;

    } else {

        accelOffset.offsets[Z] += ACCEL_G_FACTOR_Z;

    }

    accelOffset_writeToEEProm();

    // startADCCycle();

}

void analog_setGround() {

    groundPressure = filteredAirPressure;

}

int32_t analog_getHeight(void) {

    int32_t height = groundPressure - filteredAirPressure;

    debugOut.analog[28] = height;

    return height;

}

int16_t analog_getDHeight(void) {

    return dHeight;

}