Subversion Repositories FlightCtrl

Rev

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

Rev 1179 Rev 1180
Line 1... Line 1...
1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2
// + Copyright (c) 04.2007 Holger Buss
2
// + Copyright (c) 04.2007 Holger Buss
3
// + only for non-profit use
3
// + Nur für den privaten Gebrauch
4
// + www.MikroKopter.com
4
// + www.MikroKopter.com
-
 
5
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
6
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
-
 
7
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
-
 
8
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
-
 
9
// + bzgl. der Nutzungsbedingungen aufzunehmen.
-
 
10
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
-
 
11
// + Verkauf von Luftbildaufnahmen, usw.
-
 
12
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
13
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
-
 
14
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
-
 
15
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
16
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
-
 
17
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
-
 
18
// + eindeutig als Ursprung verlinkt werden
-
 
19
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
20
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
-
 
21
// + Benutzung auf eigene Gefahr
-
 
22
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
-
 
23
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
24
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
-
 
25
// + mit unserer Zustimmung zulässig
-
 
26
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
27
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
-
 
28
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
-
 
29
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
-
 
30
// + this list of conditions and the following disclaimer.
-
 
31
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
5
// + see the File "License.txt" for further Informations
32
// +     from this software without specific prior written permission.
-
 
33
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
-
 
34
// +     for non-commercial use (directly or indirectly)
-
 
35
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
-
 
36
// +     with our written permission
-
 
37
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
-
 
38
// +     clearly linked as origin
-
 
39
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
-
 
40
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
-
 
41
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
-
 
42
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
-
 
43
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
-
 
44
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
-
 
45
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
-
 
46
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
-
 
47
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
-
 
48
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
-
 
49
// +  POSSIBILITY OF SUCH DAMAGE.
6
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
50
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
7
#include <stdlib.h>
51
#include <stdlib.h>
8
#include <avr/io.h>
52
#include <avr/io.h>
9
#include <avr/interrupt.h>
53
#include <avr/interrupt.h>
Line 14... Line 58...
14
#include "fc.h"
58
#include "fc.h"
15
#include "printf_P.h"
59
#include "printf_P.h"
16
#include "eeprom.h"
60
#include "eeprom.h"
17
#include "twimaster.h"
61
#include "twimaster.h"
Line 18... Line 62...
18
 
62
 
-
 
63
volatile uint16_t Test = 0;
19
volatile int16_t Current_AccZ = 0;
64
 
20
volatile int16_t UBat = 100;
65
volatile int16_t UBat = 100;
21
volatile int16_t AdValueGyrNick = 0, AdValueGyrRoll = 0,  AdValueGyrYaw = 0;
66
volatile int16_t AdValueGyroNick = 0, AdValueGyroRoll = 0,  AdValueGyroYaw = 0;
22
uint8_t AnalogOffsetNick = 115, AnalogOffsetRoll = 115, AnalogOffsetYaw = 115;
67
volatile int16_t FilterHiResGyroNick = 0, FilterHiResGyroRoll = 0;
23
uint8_t GyroDefectNick = 0, GyroDefectRoll = 0, GyroDefectYaw = 0;
68
volatile int16_t HiResGyroNick = 2500, HiResGyroRoll = 2500;
24
volatile int16_t AdValueAccRoll = 0,  AdValueAccNick = 0, AdValueAccTop = 0;
69
volatile int16_t AdValueAccRoll = 0,  AdValueAccNick = 0, AdValueAccTop = 0, AdValueAccZ = 0;
25
volatile int32_t AirPressure = 32000;
70
volatile int32_t AirPressure = 32000;
26
volatile uint8_t average_pressure = 0;
71
volatile uint8_t average_pressure = 0;
27
volatile int16_t StartAirPressure;
72
volatile int16_t StartAirPressure;
28
volatile uint16_t ReadingAirPressure = 1023;
-
 
29
int8_t ExpandBaro = 0;
-
 
30
uint8_t PressureSensorOffset;
73
volatile uint16_t ReadingAirPressure = 1023;
31
volatile int16_t HeightD = 0;
74
volatile int16_t HeightD = 0;
-
 
75
volatile uint16_t MeasurementCounter = 0;
-
 
76
volatile uint8_t ADReady = 1;
-
 
77
 
-
 
78
uint8_t DacOffsetGyroNick = 115, DacOffsetGyroRoll = 115, DacOffsetGyroYaw = 115;
-
 
79
uint8_t GyroDefectNick = 0, GyroDefectRoll = 0, GyroDefectYaw = 0;
-
 
80
int8_t ExpandBaro = 0;
Line 32... Line 81...
32
volatile uint16_t MeasurementCounter = 0;
81
uint8_t PressureSensorOffset;
33
 
82
 
34
/*****************************************************/
83
/*****************************************************/
35
/*     Initialize Analog Digital Converter           */
84
/*     Initialize Analog Digital Converter           */
Line 49... Line 98...
49
    ADMUX &= ~((1 << REFS1)|(1 << REFS0)|(1 << ADLAR));
98
    ADMUX &= ~((1 << REFS1)|(1 << REFS0)|(1 << ADLAR));
50
    // set muxer to ADC adc_channel 0 (0 to 7 is a valid choice)
99
    // set muxer to ADC adc_channel 0 (0 to 7 is a valid choice)
51
    ADMUX = (ADMUX & 0xE0) | 0x00;
100
    ADMUX = (ADMUX & 0xE0) | 0x00;
52
    //Set ADC Control and Status Register A
101
    //Set ADC Control and Status Register A
53
    //Auto Trigger Enable, Prescaler Select Bits to Division Factor 128, i.e. ADC clock = SYSCKL/128 = 156.25 kHz
102
    //Auto Trigger Enable, Prescaler Select Bits to Division Factor 128, i.e. ADC clock = SYSCKL/128 = 156.25 kHz
54
    ADCSRA = (1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0);
103
        ADCSRA = (0<<ADEN)|(0<<ADSC)|(0<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(0<<ADIE);
55
        //Set ADC Control and Status Register B
104
        //Set ADC Control and Status Register B
56
        //Trigger Source to Free Running Mode
105
        //Trigger Source to Free Running Mode
57
        ADCSRB &= ~((1 << ADTS2)|(1 << ADTS1)|(1 << ADTS0));
106
        ADCSRB &= ~((1 << ADTS2)|(1 << ADTS1)|(1 << ADTS0));
58
        // Enable AD conversion
107
        // Start AD conversion
59
        ADC_Enable();
108
        ADC_Enable();
60
    // restore global interrupt flags
109
    // restore global interrupt flags
61
    SREG = sreg;
110
    SREG = sreg;
62
}
111
}
Line 81... Line 130...
81
        PressureSensorOffset = off;
130
        PressureSensorOffset = off;
82
        Delay_ms_Mess(300);
131
        Delay_ms_Mess(300);
83
}
132
}
Line 84... Line 133...
84
 
133
 
85
 
134
 
86
void SearchGyroOffset(void)
135
void SearchDacGyroOffset(void)
Line 87... Line 136...
87
{
136
{
-
 
137
        uint8_t i, ready = 0;
-
 
138
 
88
        uint8_t i, ready = 0;
139
        GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0;
89
 
140
        if(BoardRelease == 13) // the auto offset calibration is available only at board release 1.3
90
        GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0;
141
        {
91
        for(i = 140; i != 0; i--)
142
                for(i = 140; i != 0; i--)
92
        {
143
                {
93
                if(ready == 3 && i > 10) i = 9;
144
                        if(ready == 3 && i > 10) i = 9;
94
                ready = 0;
145
                        ready = 0;
95
                if(AdValueGyrNick < 1020) AnalogOffsetNick--; else if(AdValueGyrNick > 1030) AnalogOffsetNick++; else ready++;
146
                        if(AdValueGyroNick < 1020) DacOffsetGyroNick--; else if(AdValueGyroNick > 1030) DacOffsetGyroNick++; else ready++;
96
                if(AdValueGyrRoll < 1020) AnalogOffsetRoll--; else if(AdValueGyrRoll > 1030) AnalogOffsetRoll++; else ready++;
147
                        if(AdValueGyroRoll < 1020) DacOffsetGyroRoll--; else if(AdValueGyroRoll > 1030) DacOffsetGyroRoll++; else ready++;
97
                if(AdValueGyrYaw  < 1020) AnalogOffsetYaw-- ; else if(AdValueGyrYaw  > 1030) AnalogOffsetYaw++ ; else ready++;
148
                        if(AdValueGyroYaw  < 1020) DacOffsetGyroYaw-- ; else if(AdValueGyroYaw  > 1030) DacOffsetGyroYaw++ ; else ready++;
98
                twi_state = TWI_STATE_GYRO_OFFSET_TX; // set twi_state in TWI ISR to start of Gyro Offset
149
                        twi_state = TWI_STATE_GYRO_OFFSET_TX; // set twi_state in TWI ISR to start of Gyro Offset
99
                I2C_Start();   // initiate data transmission
150
                        I2C_Start();   // initiate data transmission
100
                if(AnalogOffsetNick < 10)  { GyroDefectNick = 1; AnalogOffsetNick = 10;}; if(AnalogOffsetNick > 245) { GyroDefectNick = 1; AnalogOffsetNick = 245;};
151
                        if(DacOffsetGyroNick < 10)  { GyroDefectNick = 1; DacOffsetGyroNick = 10;}; if(DacOffsetGyroNick > 245) { GyroDefectNick = 1; DacOffsetGyroNick = 245;};
101
                if(AnalogOffsetRoll < 10)  { GyroDefectRoll = 1; AnalogOffsetRoll = 10;}; if(AnalogOffsetRoll > 245) { GyroDefectRoll = 1; AnalogOffsetRoll = 245;};
152
                        if(DacOffsetGyroRoll < 10)  { GyroDefectRoll = 1; DacOffsetGyroRoll = 10;}; if(DacOffsetGyroRoll > 245) { GyroDefectRoll = 1; DacOffsetGyroRoll = 245;};
102
                if(AnalogOffsetYaw  < 10)  { GyroDefectYaw  = 1; AnalogOffsetYaw  = 10;}; if(AnalogOffsetYaw  > 245) { GyroDefectYaw  = 1; AnalogOffsetYaw  = 245;};
153
                        if(DacOffsetGyroYaw  < 10)  { GyroDefectYaw  = 1; DacOffsetGyroYaw  = 10;}; if(DacOffsetGyroYaw  > 245) { GyroDefectYaw  = 1; DacOffsetGyroYaw  = 245;};
103
                while(twi_state); // wait for end of data transmission
154
                        while(twi_state); // wait for end of data transmission
104
                average_pressure = 0;
155
                        average_pressure = 0;
-
 
156
                        ADC_Enable();
-
 
157
                        while(average_pressure == 0);
105
                ADC_Enable();
158
                        if(i < 10) Delay_ms_Mess(10);
106
                while(average_pressure == 0);
-
 
107
                if(i < 10) Delay_ms_Mess(10);
159
                }
Line 108... Line 160...
108
        }
160
                Delay_ms_Mess(70);
109
        Delay_ms_Mess(70);
161
        }
110
}
162
}
111
 
163
 
112
 
164
 
113
 
165
 
-
 
166
 
-
 
167
/*****************************************************/
114
 
168
/*     Interrupt Service Routine for ADC             */
115
/*****************************************************/
169
/*****************************************************/
116
/*     Interrupt Service Routine for ADC             */
170
// runs at 312.5 kHz or 3.2 µs
-
 
171
// if after (60.8µs) all 19 states are processed the interrupt is disabled
-
 
172
// and the update of further ads is stopped
-
 
173
 
117
/*****************************************************/
174
/*
-
 
175
0  nickgyro
-
 
176
1  rollgyro
-
 
177
2  yawgyro
118
// runs at 156.25 kHz or 6.4 µs
178
3  accroll
-
 
179
4  accnick
-
 
180
5  nickgyro
-
 
181
6  rollgyro
-
 
182
7  ubat
-
 
183
8  acctop
-
 
184
9  air pressure
-
 
185
10 nickgyro
-
 
186
11 rollgyro
-
 
187
12 yawgyro
-
 
188
13 accroll
119
// if after (70.4µs) all 11 states are processed the interrupt is disabled
189
14 accnick
120
// and the update of further ads is stopped
190
15 gyronick
121
// The routine changes the ADC input muxer running
191
16 gyroroll
122
// thru the state machine by the following order.
192
17 airpressure
123
// state 0: ch0 (yaw gyro)
193
*/
124
// state 1: ch1 (roll gyro)
194
 
125
// state 2: ch2 (nick gyro)
195
 
126
// state 3: ch4 (battery voltage -> UBat)
196
#define AD_GYRO_YAW             0
Line 127... Line 197...
127
// state 4: ch6 (acc y -> Current_AccY)
197
#define AD_GYRO_ROLL    1
128
// state 5: ch7 (acc x -> Current_AccX)
198
#define AD_GYRO_NICK    2
129
// state 6: ch0 (yaw gyro average with first reading   -> AdValueGyrYaw)
199
#define AD_AIRPRESS             3
130
// state 7: ch1 (roll gyro average with first reading  -> AdValueGyrRoll)
200
#define AD_UBAT                 4
-
 
201
#define AD_ACC_TOP              5
131
// state 8: ch2 (nick gyro average with first reading -> AdValueGyrNick)
202
#define AD_ACC_ROLL             6
132
// state 9: ch5 (acc z add also 4th part of acc x and acc y to reading)
-
 
133
// state10: ch3 (air pressure averaging over 5 single readings -> tmpAirPressure)
-
 
-
 
203
#define AD_ACC_NICK             7
134
 
204
 
135
ISR(ADC_vect)
205
ISR(ADC_vect)
136
{
206
{
137
    static uint8_t adc_channel = 0, state = 0;
207
    static uint8_t ad_channel = AD_GYRO_NICK, state = 0;
138
    static uint16_t yaw1, roll1, nick1;
208
    static uint16_t gyroyaw, gyroroll, gyronick, accroll, accnick;
139
    static int16_t tmpAirPressure = 0;
209
    static int32_t filtergyronick, filtergyroroll;
140
    // disable further AD conversion
-
 
141
    ADC_Disable();
210
    static int16_t tmpAirPressure = 0;
142
    // state machine
211
 
143
    switch(state++)
212
    // state machine
144
        {
213
        switch(state++)
145
        case 0:
214
        {
146
            yaw1 = ADC; // get Gyro Yaw Voltage 1st sample
215
                case 0:
147
            adc_channel = 1; // set next channel to ADC1 = ROLL GYRO
216
                        gyronick = ADC; // get nick gyro voltage 1st sample
148
            MeasurementCounter++; // increment total measurement counter
217
                        ad_channel = AD_GYRO_ROLL;
149
            break;
218
                        break;
150
        case 1:
219
                case 1:
151
            roll1 = ADC; // get Gyro Roll Voltage 1st sample
220
                        gyroroll = ADC; // get roll gyro voltage 1st sample
152
            adc_channel = 2; // set next channel to ADC2 = NICK GYRO
-
 
153
            break;
221
                        ad_channel = AD_GYRO_YAW;
154
        case 2:
222
                        break;
155
            nick1 = ADC; // get Gyro Nick Voltage 1st sample
223
                case 2:
156
            adc_channel = 4; // set next channel to ADC4 = UBAT
224
                        gyroyaw = ADC; // get yaw gyro voltage 1st sample
157
            break;
225
                        ad_channel = AD_ACC_ROLL;
158
        case 3:
226
                        break;
159
                // get actual UBat (Volts*10) is ADC*30V/1024*10 = ADC/3
227
                case 3:
160
            UBat = (3 * UBat + ADC / 3) / 4; // low pass filter updates UBat only to 1 quater with actual ADC value
228
                        accroll = ADC; // get roll acc voltage 1st sample
161
            adc_channel = 6; // set next channel to ADC6 = ACC_Y
229
                        ad_channel = AD_ACC_NICK;
162
            break;
230
            break;
163
        case 4:
231
                case 4:
164
            AdValueAccRoll = NeutralAccY - ADC; // get acceleration in Y direction
-
 
165
            adc_channel = 7; // set next channel to ADC7 = ACC_X
-
 
166
            break;
-
 
167
        case 5:
232
                        accnick = ADC; // get nick acc voltage 1st sample
168
            AdValueAccNick = ADC - NeutralAccX; // get acceleration in X direction
-
 
169
                    adc_channel = 0; // set next channel to ADC7 = YAW GYRO
233
                        ad_channel = AD_GYRO_NICK;
170
            break;
-
 
171
        case 6:
-
 
172
                // average over two samples to create current AdValueGyrYaw
-
 
173
            if(BoardRelease == 10)  AdValueGyrYaw = (ADC + yaw1) / 2;
-
 
174
            else if (BoardRelease == 20)  AdValueGyrYaw = 1023 - (ADC + yaw1);
-
 
175
                        else                                    AdValueGyrYaw = ADC + yaw1; // gain is 2 times lower on FC 1.1
234
                        break;
176
            adc_channel = 1; // set next channel to ADC7 = ROLL GYRO
235
                case 5:
177
            break;
236
                        gyronick += ADC; // get nick gyro voltage 2nd sample
178
        case 7:
-
 
179
                // average over two samples to create current ADValueGyrRoll
237
                        ad_channel = AD_GYRO_ROLL;
180
            if(BoardRelease == 10)  AdValueGyrRoll = (ADC + roll1) / 2;
238
                        break;
181
                        else                                    AdValueGyrRoll = ADC + roll1; // gain is 2 times lower on FC 1.1
239
                case 6:
182
            adc_channel = 2; // set next channel to ADC2 = NICK GYRO
240
                        gyroroll += ADC; // get roll gyro voltage 2nd sample
183
            break;
241
                        ad_channel = AD_UBAT;
184
        case 8:
242
                        break;
185
                // average over two samples to create current ADValueNick
-
 
186
            if(BoardRelease == 10)  AdValueGyrNick = (ADC + nick1) / 2;
243
                case 7:
187
                        else                                    AdValueGyrNick = ADC + nick1; // gain is 2 times lower on FC 1.1
244
                        // get actual UBat (Volts*10) is ADC*30V/1024*10 = ADC/3
188
            adc_channel = 5; // set next channel to ADC5 = ACC_Z
245
                        UBat = (3 * UBat + ADC / 3) / 4; // low pass filter updates UBat only to 1 quater with actual ADC value
189
            break;
246
                        ad_channel = AD_ACC_TOP;
190
       case 9:
247
            break;
191
                // get z acceleration
248
                case 8:
192
            AdValueAccTop =  (int16_t) ADC - NeutralAccZ; // get plain acceleration in Z direction
249
                        AdValueAccZ = ADC; // get plain acceleration in Z direction
193
            AdValueAccTop += abs(AdValueAccNick) / 4 + abs(AdValueAccRoll) / 4;
250
                        AdValueAccTop =  (int16_t)ADC - AdBiasAccTop; // get acceleration in Z direction
194
            if(AdValueAccTop > 1)
251
                        if(AdValueAccTop > 1)
195
             {
252
                        {
196
                if(NeutralAccZ < 750)
253
                                if(AdBiasAccTop < 750)
197
                {
254
                                {
198
                                        NeutralAccZ += 0.02;
255
                                        AdBiasAccTop += 0.02;
199
                                        if(Model_Is_Flying < 500) NeutralAccZ += 0.1;
256
                                        if(ModelIsFlying < 500) AdBiasAccTop += 0.1;
200
                                }
257
                                }
-
 
258
                        }
-
 
259
                        else if(AdValueAccTop < -1)
-
 
260
                        {
-
 
261
                                if(AdBiasAccTop > 550)
-
 
262
                                {
-
 
263
                                        AdBiasAccTop -= 0.02;
-
 
264
                                        if(ModelIsFlying < 500) AdBiasAccTop -= 0.1;
-
 
265
                                }
-
 
266
                        }
-
 
267
                        ReadingIntegralTop += AdValueAccTop;      // load
-
 
268
                        ReadingIntegralTop -= ReadingIntegralTop / 1024; // discharge
-
 
269
                        ad_channel = AD_AIRPRESS;
-
 
270
                        break;
-
 
271
                // case 9 is moved to the end
-
 
272
                case 10:
-
 
273
                        gyronick += ADC; // get nick gyro voltage 3rd sample
-
 
274
                        ad_channel = AD_GYRO_ROLL;
-
 
275
                        break;
-
 
276
                case 11:
-
 
277
                        gyroroll += ADC; // get roll gyro voltage 3rd sample
-
 
278
                        ad_channel = AD_GYRO_YAW;
-
 
279
                        break;
-
 
280
                case 12:
-
 
281
                        gyroyaw += ADC; // get yaw gyro voltage 2nd sample
-
 
282
                        if(BoardRelease == 10) AdValueGyroYaw = (gyroyaw + 1) / 2; // analog gain on board 1.0 is 2 times higher
-
 
283
                        else
-
 
284
                        if(BoardRelease == 20) AdValueGyroYaw = 2047 - gyroyaw; // 2 times higher than a single sample
-
 
285
                        else                   AdValueGyroYaw = gyroyaw;        // 2 times higher than a single sample
-
 
286
                        ad_channel = AD_ACC_ROLL;
-
 
287
                        break;
-
 
288
                case 13:
201
             }
289
                        accroll += ADC; // get roll acc voltage 2nd sample
202
             else if(AdValueAccTop < -1)
290
                        AdValueAccRoll = AdBiasAccRoll - accroll; // subtract bias
203
             {
291
                        ad_channel = AD_ACC_NICK;
204
                if(NeutralAccZ > 550)
292
                        break;
205
                {
293
                case 14:
-
 
294
                        accnick += ADC; // get nick acc voltage 2nd sample
-
 
295
                        AdValueAccNick = accnick  - AdBiasAccNick; // subtract bias
-
 
296
                        ad_channel = AD_GYRO_NICK;
-
 
297
            break;
-
 
298
        case 15:
206
                                        NeutralAccZ-= 0.02;
299
                gyronick += ADC; // get nick gyro voltage 4th sample
207
                                        if(Model_Is_Flying < 500) NeutralAccZ -= 0.1;
300
                if(BoardRelease == 10) gyronick *= 2; // 8 times higer than a single sample, HW gain x2
-
 
301
                else gyronick *= 4; // 16 times higer than a single sample
-
 
302
                        AdValueGyroNick = gyronick / 8; // 2 times higher than a single sample
-
 
303
                        filtergyronick = (filtergyronick + gyronick) / 2; //(16 samples)/2 results in a factor of 8 higher than a single sample) see HIRES_GYRO_AMPLIFY
-
 
304
                        HiResGyroNick = filtergyronick - BiasHiResGyroNick;
-
 
305
                        FilterHiResGyroNick = (FilterHiResGyroNick + HiResGyroNick) / 2;
-
 
306
                        ad_channel = AD_GYRO_ROLL;
-
 
307
                        break;
-
 
308
        case 16:
-
 
309
                gyroroll += ADC; // get roll gyro voltage 4th sample
-
 
310
                if(BoardRelease == 10) gyroroll *= 2; // 8 times higer than a single sample, HW gain x2
-
 
311
                else gyroroll *= 4; // 16 times higer than a single sample
-
 
312
                        AdValueGyroRoll = gyroroll / 8; // 2 times higher than a single sample
-
 
313
                        filtergyroroll = (filtergyroroll + gyroroll) / 2; //(16 samples)/2 results in a factor of 8 higher than a single sample) see HIRES_GYRO_AMPLIFY
-
 
314
                        HiResGyroRoll = filtergyroroll - BiasHiResGyroRoll;
-
 
315
                        FilterHiResGyroRoll = (FilterHiResGyroRoll + HiResGyroRoll) / 2;
208
                                }
316
                        ad_channel = AD_AIRPRESS;
209
             }
317
                        break;
-
 
318
                case 17:
210
            Current_AccZ = ADC;
319
                        state = 0; // restart sequence from beginning
211
            Reading_Integral_Top += AdValueAccTop;      // Integrieren
320
            ADReady = 1; // mark
212
            Reading_Integral_Top -= Reading_Integral_Top / 1024; // dämfen
321
            MeasurementCounter++; // increment total measurement counter
213
                adc_channel = 3; // set next channel to ADC3 = air pressure
322
                        // "break;" is missing to enable fall thru case 9 at the end of the sequence
214
            break;
323
                case 9:
215
        case 10:
324
                        tmpAirPressure += ADC; // sum adc values
216
            tmpAirPressure += ADC; // sum vadc values
325
                        if(++average_pressure >= 5) // if 5 values are summerized for averaging
217
            if(++average_pressure >= 5) // if 5 values are summerized for averaging
326
                        {
218
            {
327
                                tmpAirPressure /= 2;
219
                ReadingAirPressure = ADC; // update measured air pressure
-
 
220
                                HeightD = (7 * HeightD + (int16_t)FCParam.Height_D * (int16_t)(255 * ExpandBaro + StartAirPressure - tmpAirPressure - ReadingHeight))/8;  // D-Part = CurrentValue - OldValue
328
                                ReadingAirPressure = ADC; // update meassured air pressure
221
                AirPressure = (tmpAirPressure + 3 * AirPressure) / 4; // averaging using history
329
                                HeightD = (31 * HeightD + (int16_t)FCParam.HeightD * (int16_t)(255 * ExpandBaro + StartAirPressure - tmpAirPressure - ReadingHeight)) / 32;  // D-Part = CurrentValue - OldValue
222
                ReadingHeight = 255 * ExpandBaro + StartAirPressure - AirPressure;
330
                                AirPressure = (tmpAirPressure + 7 * AirPressure + 4) / 8; // averaging using history
223
                average_pressure = 0; // reset air pressure measurement counter
331
                                ReadingHeight = 255 * ExpandBaro + StartAirPressure - AirPressure;
224
                tmpAirPressure = 0;
332
                                average_pressure = 0; // reset air pressure measurement counter
225
            }
333
                                tmpAirPressure /= 2;
226
            adc_channel = 0; // set next channel to ADC0 = GIER GYRO
334
                        }
227
            state = 0; // reset state machine
335
                        ad_channel = AD_GYRO_NICK;
228
            break;
336
                        break;
229
        default:
337
                default:
230
            adc_channel = 0;
338
                        ad_channel = AD_GYRO_NICK;