Subversion Repositories FlightCtrl

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Copyright (c) 04.2007 Holger Buss

// + Nur für den privaten Gebrauch

// + www.MikroKopter.com

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),

// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.

// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt

// + bzgl. der Nutzungsbedingungen aufzunehmen.

// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,

// + Verkauf von Luftbildaufnahmen, usw.

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,

// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts

// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"

// + eindeutig als Ursprung verlinkt werden

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion

// + Benutzung auf eigene Gefahr

// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur

// + mit unserer Zustimmung zulässig

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

// + Redistributions of source code (with or without modifications) must retain the above copyright notice,

// + this list of conditions and the following disclaimer.

// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived

// +     from this software without specific prior written permission.

// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet

// +     for non-commercial use (directly or indirectly)

// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted

// +     with our written permission

// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be

// +     clearly linked as origin

// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed

// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"

// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE

// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE

// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE

// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR

// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF

// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS

// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)

// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE

// +  POSSIBILITY OF SUCH DAMAGE.

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

#include <stdlib.h>

#include <avr/io.h>

#include <avr/interrupt.h>

#include "analog.h"

#include "main.h"

#include "timer0.h"

#include "fc.h"

#include "printf_P.h"

#include "eeprom.h"

#include "twimaster.h"

volatile uint16_t Test = 0;

volatile int16_t UBat = 100;

volatile int16_t AdValueGyroNick = 0, AdValueGyroRoll = 0,  AdValueGyroYaw = 0;

volatile int16_t FilterHiResGyroNick = 0, FilterHiResGyroRoll = 0;

volatile int16_t HiResGyroNick = 2500, HiResGyroRoll = 2500;

volatile int16_t AdValueAccRoll = 0,  AdValueAccNick = 0, AdValueAccTop = 0, AdValueAccZ = 0;

volatile int32_t AirPressure = 32000;

volatile uint8_t average_pressure = 0;

volatile int16_t StartAirPressure;

volatile uint16_t ReadingAirPressure = 1023;

volatile int16_t HeightD = 0;

volatile uint16_t MeasurementCounter = 0;

volatile uint8_t ADReady = 1;

uint8_t DacOffsetGyroNick = 115, DacOffsetGyroRoll = 115, DacOffsetGyroYaw = 115;

uint8_t GyroDefectNick = 0, GyroDefectRoll = 0, GyroDefectYaw = 0;

int8_t ExpandBaro = 0;

uint8_t PressureSensorOffset;

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

/*     Initialize Analog Digital Converter           */

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

void ADC_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) | 0x00;

    //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 = (0<<ADEN)|(0<<ADSC)|(0<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(0<<ADIE);

        //Set ADC Control and Status Register B

        //Trigger Source to Free Running Mode

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

        // Start AD conversion

        ADC_Enable();

    // restore global interrupt flags

    SREG = sreg;

}

void SearchAirPressureOffset(void)

{

        uint8_t off;

        off = GetParamByte(PID_PRESSURE_OFFSET);

        if(off > 20) off -= 10;

        OCR0A = off;

        ExpandBaro = 0;

        Delay_ms_Mess(100);

        if(ReadingAirPressure < 850) off = 0;

        for(; off < 250;off++)

        {

                OCR0A = off;

                Delay_ms_Mess(50);

                printf(".");

                if(ReadingAirPressure < 850) break;

        }

        SetParamByte(PID_PRESSURE_OFFSET, off);

        PressureSensorOffset = off;

        Delay_ms_Mess(300);

}

void SearchDacGyroOffset(void)

{

        uint8_t i, ready = 0;

        GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0;

        if(BoardRelease == 13) // the auto offset calibration is available only at board release 1.3

        {

                for(i = 140; i != 0; i--)

                {

                        if(ready == 3 && i > 10) i = 9;

                        ready = 0;

                        if(AdValueGyroNick < 1020) DacOffsetGyroNick--; else if(AdValueGyroNick > 1030) DacOffsetGyroNick++; else ready++;

                        if(AdValueGyroRoll < 1020) DacOffsetGyroRoll--; else if(AdValueGyroRoll > 1030) DacOffsetGyroRoll++; else ready++;

                        if(AdValueGyroYaw  < 1020) DacOffsetGyroYaw-- ; else if(AdValueGyroYaw  > 1030) DacOffsetGyroYaw++ ; else ready++;

                        twi_state = TWI_STATE_GYRO_OFFSET_TX; // set twi_state in TWI ISR to start of Gyro Offset

                        I2C_Start();   // initiate data transmission

                        if(DacOffsetGyroNick < 10)  { GyroDefectNick = 1; DacOffsetGyroNick = 10;}; if(DacOffsetGyroNick > 245) { GyroDefectNick = 1; DacOffsetGyroNick = 245;};

                        if(DacOffsetGyroRoll < 10)  { GyroDefectRoll = 1; DacOffsetGyroRoll = 10;}; if(DacOffsetGyroRoll > 245) { GyroDefectRoll = 1; DacOffsetGyroRoll = 245;};

                        if(DacOffsetGyroYaw  < 10)  { GyroDefectYaw  = 1; DacOffsetGyroYaw  = 10;}; if(DacOffsetGyroYaw  > 245) { GyroDefectYaw  = 1; DacOffsetGyroYaw  = 245;};

                        while(twi_state); // wait for end of data transmission

                        average_pressure = 0;

                        ADC_Enable();

                        while(average_pressure == 0);

                        if(i < 10) Delay_ms_Mess(10);

                }

                Delay_ms_Mess(70);

        }

}

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

/*     Interrupt Service Routine for ADC             */

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

// runs at 312.5 kHz or 3.2 µs

// if after (60.8µs) all 19 states are processed the interrupt is disabled

// and the update of further ads is stopped

/*

1  rollgyro

2  yawgyro

3  accroll

4  accnick

5  nickgyro

6  rollgyro

7  ubat

8  acctop

9  air pressure

10 nickgyro

11 rollgyro

12 yawgyro

13 accroll

14 accnick

15 gyronick

16 gyroroll

17 airpressure

*/

#define AD_GYRO_YAW             0

#define AD_GYRO_ROLL    1

#define AD_GYRO_NICK    2

#define AD_AIRPRESS             3

#define AD_UBAT                 4

#define AD_ACC_TOP              5

#define AD_ACC_ROLL             6

#define AD_ACC_NICK             7

ISR(ADC_vect)

{

    static uint8_t ad_channel = AD_GYRO_NICK, state = 0;

    static uint16_t gyroyaw, gyroroll, gyronick, accroll, accnick;

    static int32_t filtergyronick, filtergyroroll;

    static int16_t tmpAirPressure = 0;

    // state machine

        switch(state++)

        {

                case 0:

                        gyronick = ADC; // get nick gyro voltage 1st sample

                        ad_channel = AD_GYRO_ROLL;

                        break;

                case 1:

                        gyroroll = ADC; // get roll gyro voltage 1st sample

                        ad_channel = AD_GYRO_YAW;

                        break;

                case 2:

                        gyroyaw = ADC; // get yaw gyro voltage 1st sample

                        ad_channel = AD_ACC_ROLL;

                        break;

                case 3:

                        accroll = ADC; // get roll acc voltage 1st sample

                        ad_channel = AD_ACC_NICK;

            break;

                case 4:

                        accnick = ADC; // get nick acc voltage 1st sample

                        ad_channel = AD_GYRO_NICK;

                        break;

                case 5:

                        gyronick += ADC; // get nick gyro voltage 2nd sample

                        ad_channel = AD_GYRO_ROLL;

                        break;

                case 6:

                        gyroroll += ADC; // get roll gyro voltage 2nd sample

                        ad_channel = AD_UBAT;

                        break;

                case 7:

                        // get actual UBat (Volts*10) is ADC*30V/1024*10 = ADC/3

                        UBat = (3 * UBat + ADC / 3) / 4; // low pass filter updates UBat only to 1 quater with actual ADC value

                        ad_channel = AD_ACC_TOP;

            break;

                case 8:

                        AdValueAccZ = ADC; // get plain acceleration in Z direction

                        AdValueAccTop =  (int16_t)ADC - AdBiasAccTop; // get acceleration in Z direction

                        if(AdValueAccTop > 1)

                        {

                                if(AdBiasAccTop < 750)

                                {

                                        AdBiasAccTop += 0.02;

                                        if(ModelIsFlying < 500) AdBiasAccTop += 0.1;

                                }

                        }

                        else if(AdValueAccTop < -1)

                        {

                                if(AdBiasAccTop > 550)

                                {

                                        AdBiasAccTop -= 0.02;

                                        if(ModelIsFlying < 500) AdBiasAccTop -= 0.1;

                                }

                        }

                        ReadingIntegralTop += AdValueAccTop;      // load

                        ReadingIntegralTop -= ReadingIntegralTop / 1024; // discharge

                        ad_channel = AD_AIRPRESS;

                        break;

                // case 9 is moved to the end

                case 10:

                        gyronick += ADC; // get nick gyro voltage 3rd sample

                        ad_channel = AD_GYRO_ROLL;

                        break;

                case 11:

                        gyroroll += ADC; // get roll gyro voltage 3rd sample

                        ad_channel = AD_GYRO_YAW;

                        break;

                case 12:

                        gyroyaw += ADC; // get yaw gyro voltage 2nd sample

                        if(BoardRelease == 10) AdValueGyroYaw = (gyroyaw + 1) / 2; // analog gain on board 1.0 is 2 times higher

                        else

                        if(BoardRelease == 20) AdValueGyroYaw = 2047 - gyroyaw; // 2 times higher than a single sample

                        else                   AdValueGyroYaw = gyroyaw;        // 2 times higher than a single sample

                        ad_channel = AD_ACC_ROLL;

                        break;

                case 13:

                        accroll += ADC; // get roll acc voltage 2nd sample

                        AdValueAccRoll = AdBiasAccRoll - accroll; // subtract bias

                        ad_channel = AD_ACC_NICK;

                        break;

                case 14:

                        accnick += ADC; // get nick acc voltage 2nd sample

                        AdValueAccNick = accnick  - AdBiasAccNick; // subtract bias

                        ad_channel = AD_GYRO_NICK;

            break;

        case 15:

                gyronick += ADC; // get nick gyro voltage 4th sample

                if(BoardRelease == 10) gyronick *= 2; // 8 times higer than a single sample, HW gain x2

                else gyronick *= 4; // 16 times higer than a single sample

                        AdValueGyroNick = gyronick / 8; // 2 times higher than a single sample

                        filtergyronick = (filtergyronick + gyronick) / 2; //(16 samples)/2 results in a factor of 8 higher than a single sample) see HIRES_GYRO_AMPLIFY

                        HiResGyroNick = filtergyronick - BiasHiResGyroNick;

                        FilterHiResGyroNick = (FilterHiResGyroNick + HiResGyroNick) / 2;

                        ad_channel = AD_GYRO_ROLL;

                        break;

        case 16:

                gyroroll += ADC; // get roll gyro voltage 4th sample

                if(BoardRelease == 10) gyroroll *= 2; // 8 times higer than a single sample, HW gain x2

                else gyroroll *= 4; // 16 times higer than a single sample

                        AdValueGyroRoll = gyroroll / 8; // 2 times higher than a single sample

                        filtergyroroll = (filtergyroroll + gyroroll) / 2; //(16 samples)/2 results in a factor of 8 higher than a single sample) see HIRES_GYRO_AMPLIFY

                        HiResGyroRoll = filtergyroroll - BiasHiResGyroRoll;

                        FilterHiResGyroRoll = (FilterHiResGyroRoll + HiResGyroRoll) / 2;

                        ad_channel = AD_AIRPRESS;

                        break;

                case 17:

                        state = 0; // restart sequence from beginning

            ADReady = 1; // mark

            MeasurementCounter++; // increment total measurement counter

                        // "break;" is missing to enable fall thru case 9 at the end of the sequence

                case 9:

                        tmpAirPressure += ADC; // sum adc values

                        if(++average_pressure >= 5) // if 5 values are summerized for averaging

                        {

                                tmpAirPressure /= 2;

                                ReadingAirPressure = ADC; // update meassured air pressure

                                HeightD = (31 * HeightD + (int16_t)FCParam.HeightD * (int16_t)(255 * ExpandBaro + StartAirPressure - tmpAirPressure - ReadingHeight)) / 32;  // D-Part = CurrentValue - OldValue

                                AirPressure = (tmpAirPressure + 7 * AirPressure + 4) / 8; // averaging using history

                                ReadingHeight = 255 * ExpandBaro + StartAirPressure - AirPressure;

                                average_pressure = 0; // reset air pressure measurement counter

                                tmpAirPressure /= 2;

                        }

                        ad_channel = AD_GYRO_NICK;

                        break;

                default:

                        ad_channel = AD_GYRO_NICK;

                        state = 0;

                        break;

        }

    // set adc muxer to next ad_channel

    ADMUX = (ADMUX & 0xE0) | ad_channel;

    // after full cycle stop further interrupts

    if(state != 0) ADC_Enable();

}