Subversion Repositories FlightCtrl

Rev

Rev 885 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 885 Rev 886
Line 2... Line 2...
2
// + Copyright (c) 04.2007 Holger Buss
2
// + Copyright (c) 04.2007 Holger Buss
3
// + only for non-profit use
3
// + only for non-profit use
4
// + www.MikroKopter.com
4
// + www.MikroKopter.com
5
// + see the File "License.txt" for further Informations
5
// + see the File "License.txt" for further Informations
6
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
6
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
7
#include <stdlib.h>
-
 
8
#include <avr/io.h>
-
 
9
#include <avr/interrupt.h>
Line -... Line 10...
-
 
10
 
7
 
11
#include "analog.h"
-
 
12
#include "main.h"
-
 
13
#include "timer0.h"
-
 
14
#include "fc.h"
-
 
15
#include "printf_P.h"
8
#include "main.h"
16
#include "eeprom.h"
9
 
-
 
10
volatile int  Aktuell_Nick,Aktuell_Roll,Aktuell_Gier,Aktuell_ax, Aktuell_ay,Aktuell_az, UBat = 100;
17
 
11
volatile int  AdWertNick = 0, AdWertRoll = 0, AdWertGier = 0;
18
volatile int16_t Current_AccZ = 0;
12
volatile int  AdWertAccRoll = 0,AdWertAccNick = 0,AdWertAccHoch = 0;
19
volatile int16_t UBat = 100;
13
volatile char MessanzahlNick = 0, MessanzahlRoll = 0, MessanzahlGier = 0;
20
volatile int16_t AdValueGyrPitch = 0, AdValueGyrRoll = 0,  AdValueGyrYaw = 0;
14
volatile char messanzahl_AccNick = 0, messanzahl_AccRoll = 0, messanzahl_AccHoch = 0;
21
volatile int16_t AdValueAccRoll = 0,  AdValueAccPitch = 0, AdValueAccTop = 0;
15
volatile long Luftdruck = 32000;
22
volatile int32_t AirPressure = 32000;
16
volatile int  StartLuftdruck;
23
volatile int16_t StartAirPressure;
17
volatile unsigned int  MessLuftdruck = 1023;
24
volatile uint16_t ReadingAirPressure = 1023;
18
unsigned char DruckOffsetSetting;
25
uint8_t PressureSensorOffset;
19
volatile int HoeheD = 0;
-
 
20
volatile char messanzahl_Druck;
-
 
21
volatile int  tmpLuftdruck;
26
volatile int16_t HeightD = 0;
22
volatile unsigned int ZaehlMessungen = 0;
27
volatile uint16_t MeasurementCounter = 0;
23
 
28
 
24
//#######################################################################################
-
 
-
 
29
/*****************************************************/
-
 
30
/*     Initialize Analog Digital Converter           */
25
//
31
/*****************************************************/
26
void ADC_Init(void)
-
 
27
//#######################################################################################
32
void ADC_Init(void)
-
 
33
{
-
 
34
        uint8_t sreg = SREG;
-
 
35
        // disable all interrupts before reconfiguration
-
 
36
        cli();
-
 
37
        //ADC0 ... ADC7 is connected to PortA pin 0 ... 7
-
 
38
        DDRA = 0x00;
-
 
39
        PORTA = 0x00;
-
 
40
        // Digital Input Disable Register 0
-
 
41
        // Disable digital input buffer for analog adc_channel pins
-
 
42
        DIDR0 = 0xFF;
-
 
43
        // external reference, adjust data to the right
-
 
44
    ADMUX &= ~((1 << REFS1)|(1 << REFS0)|(1 << ADLAR));
28
{
45
    // set muxer to ADC adc_channel 0 (0 to 7 is a valid choice)
-
 
46
    ADMUX = (ADMUX & 0xE0) | 0x00;
-
 
47
    //Set ADC Control and Status Register A
29
    ADMUX = 0;//Referenz ist extern
48
    //Auto Trigger Enable, Prescaler Select Bits to Division Factor 128, i.e. ADC clock = SYSCKL/128 = 156.25 kHz
-
 
49
    ADCSRA = (1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);
-
 
50
        //Set ADC Control and Status Register B
-
 
51
        //Trigger Source to Free Running Mode
-
 
52
        ADCSRB &= ~((1 << ADTS2)|(1 << ADTS1)|(1 << ADTS0));
-
 
53
        // Enable AD conversion
30
    ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE);
54
        ADC_Enable();
-
 
55
    // restore global interrupt flags
31
    //Free Running Mode, Division Factor 128, Interrupt on
56
    SREG = sreg;
Line 32... Line 57...
32
}
57
}
33
 
58
 
34
void SucheLuftruckOffset(void)
59
void SearchAirPressureOffset(void)
35
{
60
{
36
 unsigned int off;
61
        uint8_t off;
37
 off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]);
62
        off = GetParamByte(PID_PRESSURE_OFFSET);
38
 if(off > 20) off -= 10;
63
        if(off > 20) off -= 10;
39
 OCR0A = off;
64
        OCR0A = off;
40
 Delay_ms_Mess(100);
65
        Delay_ms_Mess(100);
41
 if(MessLuftdruck < 850) off = 0;
66
        if(ReadingAirPressure < 850) off = 0;
42
 for(; off < 250;off++)
67
        for(; off < 250;off++)
43
  {
68
        {
44
  OCR0A = off;
69
                OCR0A = off;
45
  Delay_ms_Mess(50);
70
                Delay_ms_Mess(50);
46
  printf(".");  
71
                printf(".");
47
  if(MessLuftdruck < 900) break;
72
                if(ReadingAirPressure < 900) break;
48
  }
73
        }
49
 eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off);
74
        SetParamByte(PID_PRESSURE_OFFSET, off);
50
 DruckOffsetSetting = off;
75
        PressureSensorOffset = off;
Line -... Line 76...
-
 
76
        Delay_ms_Mess(300);
-
 
77
}
-
 
78
 
-
 
79
 
-
 
80
/*****************************************************/
-
 
81
/*     Interrupt Service Routine for ADC             */
-
 
82
/*****************************************************/
-
 
83
// runs at 156.25 kHz or 6.4 µs
-
 
84
// if after (70.4µs) all 11 states are processed the interrupt is disabled
-
 
85
// and the update of further ads is stopped
-
 
86
// The routine changes the ADC input muxer running
-
 
87
// thru the state machine by the following order.
-
 
88
// state 0: ch0 (yaw gyro)
-
 
89
// state 1: ch1 (roll gyro)
-
 
90
// state 2: ch2 (pitch gyro)
-
 
91
// state 3: ch4 (battery voltage -> UBat)
-
 
92
// state 4: ch6 (acc y -> Current_AccY)
-
 
93
// state 5: ch7 (acc x -> Current_AccX)
51
 Delay_ms_Mess(300);
94
// state 6: ch0 (yaw gyro average with first reading   -> AdValueGyrYaw)
52
}
95
// state 7: ch1 (roll gyro average with first reading  -> AdValueGyrRoll)
53
 
96
// state 8: ch2 (pitch gyro average with first reading -> AdValueGyrPitch)
54
 
-
 
55
//#######################################################################################
97
// state 9: ch5 (acc z add also 4th part of acc x and acc y to reading)
56
//
98
// state10: ch3 (air pressure averaging over 5 single readings -> tmpAirPressure)
57
SIGNAL(SIG_ADC)
99
 
-
 
100
ISR(ADC_vect)
-
 
101
{
-
 
102
    static uint8_t adc_channel = 0, state = 0;
58
//#######################################################################################
103
    static uint16_t yaw1, roll1, pitch1;
-
 
104
    static uint8_t average_pressure = 0;
59
{
105
    static int16_t tmpAirPressure = 0;
60
    static unsigned char kanal=0,state = 0;
106
    // disable further AD conversion
61
    static unsigned int gier1, roll1, nick1;
107
    ADC_Disable();
62
    ANALOG_OFF;
108
    // state machine
63
    switch(state++)
109
    switch(state++)
64
        {
110
        {
65
        case 0:
111
        case 0:
66
            gier1 = ADC;
112
            yaw1 = ADC; // get Gyro Yaw Voltage 1st sample
67
            kanal = 1;
113
            adc_channel = 1; // set next channel to ADC1 = ROLL GYRO
68
            ZaehlMessungen++;
114
            MeasurementCounter++; // increment total measurement counter
69
            break;
115
            break;
70
        case 1:
116
        case 1:
71
            roll1 = ADC;
117
            roll1 = ADC; // get Gyro Roll Voltage 1st sample
72
            kanal = 2;
118
            adc_channel = 2; // set next channel to ADC2 = PITCH GYRO
73
            break;
119
            break;
74
        case 2:
120
        case 2:
-
 
121
            pitch1 = ADC; // get Gyro Pitch Voltage 1st sample
75
            nick1 = ADC;
122
            adc_channel = 4; // set next channel to ADC4 = UBAT
76
            kanal = 4;
123
            break;
77
            break;
124
        case 3:
78
        case 3:
125
                // get actual UBat (Volts*10) is ADC*30V/1024*10 = ADC/3
79
            UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19)  / 2;
126
            UBat = (3 * UBat + ADC / 3) / 4; // low pass filter updates UBat only to 1 quater with actual ADC value
80
            kanal = 6;
127
            adc_channel = 6; // set next channel to ADC6 = ACC_Y
81
            break;
-
 
82
        case 4:
128
            break;
83
            Aktuell_ay = NeutralAccY - ADC;
129
        case 4:
84
            AdWertAccRoll = Aktuell_ay;
130
            AdValueAccRoll = NeutralAccY - ADC; // get acceleration in Y direction
85
            kanal = 7;
131
            adc_channel = 7; // set next channel to ADC7 = ACC_X
86
            break;
-
 
87
        case 5:
132
            break;
88
            Aktuell_ax = ADC - NeutralAccX;
133
        case 5:
-
 
134
            AdValueAccPitch = ADC - NeutralAccX; // get acceleration in X direction
89
            AdWertAccNick =  Aktuell_ax;
135
                    adc_channel = 0; // set next channel to ADC7 = YAW GYRO
90
                    kanal = 0;
136
            break;
91
            break;
137
        case 6:
92
        case 6:
138
                // average over two samples to create current AdValueGyrYaw
93
            if(PlatinenVersion == 10)  AdWertGier = (ADC + gier1) / 2;
139
            if(BoardRelease == 10)  AdValueGyrYaw = (ADC + yaw1) / 2;
-
 
140
                        else                                    AdValueGyrYaw = ADC + yaw1; // gain is 2 times lower on FC 1.1
94
                        else                                       AdWertGier = ADC + gier1;
141
            adc_channel = 1; // set next channel to ADC7 = ROLL GYRO
95
            kanal = 1;
142
            break;
96
            break;
143
        case 7:
97
        case 7:
144
                // average over two samples to create current ADValueGyrRoll
98
            if(PlatinenVersion == 10)  AdWertRoll = (ADC + roll1) / 2;
145
            if(BoardRelease == 10)  AdValueGyrRoll = (ADC + roll1) / 2;
-
 
146
                        else                                    AdValueGyrRoll = ADC + roll1; // gain is 2 times lower on FC 1.1
99
                        else                                       AdWertRoll = ADC + roll1;
147
            adc_channel = 2; // set next channel to ADC2 = PITCH GYRO
100
            kanal = 2;
148
            break;
101
            break;
-
 
102
        case 8:
-
 
103
            if(PlatinenVersion == 10)  AdWertNick = (ADC + nick1) / 2;
149
        case 8:
104
                        else                                       AdWertNick = ADC + nick1;
150
                // average over two samples to create current ADValuePitch
105
//AdWertNick = 0;
151
            if(BoardRelease == 10)  AdValueGyrPitch = (ADC + pitch1) / 2;
-
 
152
                        else                                    AdValueGyrPitch = ADC + pitch1; // gain is 2 times lower on FC 1.1
106
//AdWertNick += Poti2;            
153
            adc_channel = 5; // set next channel to ADC5 = ACC_Z
107
            kanal = 5;
154
            break;
108
            break;
155
       case 9:
109
       case 9:
156
                // get z acceleration
110
            AdWertAccHoch =  (signed int) ADC - NeutralAccZ;
157
            AdValueAccTop =  (int16_t) ADC - NeutralAccZ; // get plain acceleration in Z direction
111
//            AdWertAccHoch += abs(Aktuell_ay) / 4 + abs(Aktuell_ax) / 4;
158
            AdValueAccTop += abs(AdValueAccPitch) / 4 + abs(AdValueAccRoll) / 4;
112
            if(AdWertAccHoch > 1)
159
            if(AdValueAccTop > 1)
113
             {
160
             {
114
              if(NeutralAccZ < 750)
161
                if(NeutralAccZ < 750)
115
               {
162
                {
116
                NeutralAccZ += 0.02;
163
                                        NeutralAccZ += 0.02;
117
                if(modell_fliegt < 500) NeutralAccZ += 0.1;
164
                                        if(Model_Is_Flying < 500) NeutralAccZ += 0.1;
118
               }
165
                                }
119
             }  
166
             }
120
             else if(AdWertAccHoch < -1)
167
             else if(AdValueAccTop < -1)
121
             {
168
             {
122
              if(NeutralAccZ > 550)
169
                if(NeutralAccZ > 550)
123
                {
170
                {
124
                 NeutralAccZ-= 0.02;
-
 
125
                 if(modell_fliegt < 500) NeutralAccZ -= 0.1;              
171
                                        NeutralAccZ-= 0.02;
126
                }
172
                                        if(Model_Is_Flying < 500) NeutralAccZ -= 0.1;
127
             }
173
                                }
128
            messanzahl_AccHoch = 1;
174
             }
129
            Aktuell_az = ADC;
175
            Current_AccZ = ADC;
130
            Mess_Integral_Hoch += AdWertAccHoch;      // Integrieren
176
            Reading_Integral_Top += AdValueAccTop;      // Integrieren
131
            Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen
177
            Reading_Integral_Top -= Reading_Integral_Top / 1024; // dämfen
132
                kanal = 3;
178
                adc_channel = 3; // set next channel to ADC3 = air pressure
133
            break;
179
            break;
134
        case 10:
180
        case 10:
135
            tmpLuftdruck += ADC;
-
 
136
            if(++messanzahl_Druck >= 5)
181
            tmpAirPressure += ADC; // sum vadc values
137
                {
182
            if(++average_pressure >= 5) // if 5 values are summerized for averaging
138
                MessLuftdruck = ADC;
183
            {
-
 
184
                ReadingAirPressure = ADC; // update measured air pressure
139
                messanzahl_Druck = 0;
185
                                HeightD = (7 * HeightD + (int16_t)FCParam.Height_D * (int16_t)(StartAirPressure - tmpAirPressure - ReadingHeight))/8;  // D-Part = CurrentValue - OldValue
140
                                HoeheD = (7 * HoeheD + (int) Parameter_Luftdruck_D * (int)(StartLuftdruck - tmpLuftdruck - HoehenWert))/8;  // D-Anteil = neuerWert - AlterWert
186
                AirPressure = (tmpAirPressure + 3 * AirPressure) / 4; // averaging using history
141
                Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4;
187
                ReadingHeight = StartAirPressure - AirPressure;
142
                HoehenWert = StartLuftdruck - Luftdruck;
188
                average_pressure = 0; // reset air pressure measurement counter
143
                tmpLuftdruck = 0;
189
                tmpAirPressure = 0;
144
                }
190
            }
145
            kanal = 0;
191
            adc_channel = 0; // set next channel to ADC0 = GIER GYRO
146
            state = 0;
192
            state = 0; // reset state machine
147
            break;
193
            break;
148
        default:
194
        default:
-
 
195
            adc_channel = 0;
149
            kanal = 0;
196
            state = 0;
-
 
197
            break;
150
            state = 0;
198
        }
151
            break;
199
    // set adc muxer to next adc_channel