Subversion Repositories FlightCtrl

Rev

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

Rev 886 Rev 911
Line 15... Line 15...
15
#include "printf_P.h"
15
#include "printf_P.h"
16
#include "eeprom.h"
16
#include "eeprom.h"
Line 17... Line 17...
17
 
17
 
18
volatile int16_t Current_AccZ = 0;
18
volatile int16_t Current_AccZ = 0;
19
volatile int16_t UBat = 100;
19
volatile int16_t UBat = 100;
20
volatile int16_t AdValueGyrPitch = 0, AdValueGyrRoll = 0,  AdValueGyrYaw = 0;
20
volatile int16_t AdValueGyrNick = 0, AdValueGyrRoll = 0,  AdValueGyrYaw = 0;
21
volatile int16_t AdValueAccRoll = 0,  AdValueAccPitch = 0, AdValueAccTop = 0;
21
volatile int16_t AdValueAccRoll = 0,  AdValueAccNick = 0, AdValueAccTop = 0;
22
volatile int32_t AirPressure = 32000;
22
volatile int32_t AirPressure = 32000;
23
volatile int16_t StartAirPressure;
23
volatile int16_t StartAirPressure;
24
volatile uint16_t ReadingAirPressure = 1023;
24
volatile uint16_t ReadingAirPressure = 1023;
25
uint8_t PressureSensorOffset;
25
uint8_t PressureSensorOffset;
Line 85... Line 85...
85
// and the update of further ads is stopped
85
// and the update of further ads is stopped
86
// The routine changes the ADC input muxer running
86
// The routine changes the ADC input muxer running
87
// thru the state machine by the following order.
87
// thru the state machine by the following order.
88
// state 0: ch0 (yaw gyro)
88
// state 0: ch0 (yaw gyro)
89
// state 1: ch1 (roll gyro)
89
// state 1: ch1 (roll gyro)
90
// state 2: ch2 (pitch gyro)
90
// state 2: ch2 (nick gyro)
91
// state 3: ch4 (battery voltage -> UBat)
91
// state 3: ch4 (battery voltage -> UBat)
92
// state 4: ch6 (acc y -> Current_AccY)
92
// state 4: ch6 (acc y -> Current_AccY)
93
// state 5: ch7 (acc x -> Current_AccX)
93
// state 5: ch7 (acc x -> Current_AccX)
94
// state 6: ch0 (yaw gyro average with first reading   -> AdValueGyrYaw)
94
// state 6: ch0 (yaw gyro average with first reading   -> AdValueGyrYaw)
95
// state 7: ch1 (roll gyro average with first reading  -> AdValueGyrRoll)
95
// state 7: ch1 (roll gyro average with first reading  -> AdValueGyrRoll)
96
// state 8: ch2 (pitch gyro average with first reading -> AdValueGyrPitch)
96
// state 8: ch2 (nick gyro average with first reading -> AdValueGyrNick)
97
// state 9: ch5 (acc z add also 4th part of acc x and acc y to reading)
97
// state 9: ch5 (acc z add also 4th part of acc x and acc y to reading)
98
// state10: ch3 (air pressure averaging over 5 single readings -> tmpAirPressure)
98
// state10: ch3 (air pressure averaging over 5 single readings -> tmpAirPressure)
Line 99... Line 99...
99
 
99
 
100
ISR(ADC_vect)
100
ISR(ADC_vect)
101
{
101
{
102
    static uint8_t adc_channel = 0, state = 0;
102
    static uint8_t adc_channel = 0, state = 0;
103
    static uint16_t yaw1, roll1, pitch1;
103
    static uint16_t yaw1, roll1, nick1;
104
    static uint8_t average_pressure = 0;
104
    static uint8_t average_pressure = 0;
105
    static int16_t tmpAirPressure = 0;
105
    static int16_t tmpAirPressure = 0;
106
    // disable further AD conversion
106
    // disable further AD conversion
107
    ADC_Disable();
107
    ADC_Disable();
Line 113... Line 113...
113
            adc_channel = 1; // set next channel to ADC1 = ROLL GYRO
113
            adc_channel = 1; // set next channel to ADC1 = ROLL GYRO
114
            MeasurementCounter++; // increment total measurement counter
114
            MeasurementCounter++; // increment total measurement counter
115
            break;
115
            break;
116
        case 1:
116
        case 1:
117
            roll1 = ADC; // get Gyro Roll Voltage 1st sample
117
            roll1 = ADC; // get Gyro Roll Voltage 1st sample
118
            adc_channel = 2; // set next channel to ADC2 = PITCH GYRO
118
            adc_channel = 2; // set next channel to ADC2 = NICK GYRO
119
            break;
119
            break;
120
        case 2:
120
        case 2:
121
            pitch1 = ADC; // get Gyro Pitch Voltage 1st sample
121
            nick1 = ADC; // get Gyro Nick Voltage 1st sample
122
            adc_channel = 4; // set next channel to ADC4 = UBAT
122
            adc_channel = 4; // set next channel to ADC4 = UBAT
123
            break;
123
            break;
124
        case 3:
124
        case 3:
125
                // get actual UBat (Volts*10) is ADC*30V/1024*10 = ADC/3
125
                // get actual UBat (Volts*10) is ADC*30V/1024*10 = ADC/3
126
            UBat = (3 * UBat + ADC / 3) / 4; // low pass filter updates UBat only to 1 quater with actual ADC value
126
            UBat = (3 * UBat + ADC / 3) / 4; // low pass filter updates UBat only to 1 quater with actual ADC value
Line 129... Line 129...
129
        case 4:
129
        case 4:
130
            AdValueAccRoll = NeutralAccY - ADC; // get acceleration in Y direction
130
            AdValueAccRoll = NeutralAccY - ADC; // get acceleration in Y direction
131
            adc_channel = 7; // set next channel to ADC7 = ACC_X
131
            adc_channel = 7; // set next channel to ADC7 = ACC_X
132
            break;
132
            break;
133
        case 5:
133
        case 5:
134
            AdValueAccPitch = ADC - NeutralAccX; // get acceleration in X direction
134
            AdValueAccNick = ADC - NeutralAccX; // get acceleration in X direction
135
                    adc_channel = 0; // set next channel to ADC7 = YAW GYRO
135
                    adc_channel = 0; // set next channel to ADC7 = YAW GYRO
136
            break;
136
            break;
137
        case 6:
137
        case 6:
138
                // average over two samples to create current AdValueGyrYaw
138
                // average over two samples to create current AdValueGyrYaw
139
            if(BoardRelease == 10)  AdValueGyrYaw = (ADC + yaw1) / 2;
139
            if(BoardRelease == 10)  AdValueGyrYaw = (ADC + yaw1) / 2;
Line 142... Line 142...
142
            break;
142
            break;
143
        case 7:
143
        case 7:
144
                // average over two samples to create current ADValueGyrRoll
144
                // average over two samples to create current ADValueGyrRoll
145
            if(BoardRelease == 10)  AdValueGyrRoll = (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
146
                        else                                    AdValueGyrRoll = ADC + roll1; // gain is 2 times lower on FC 1.1
147
            adc_channel = 2; // set next channel to ADC2 = PITCH GYRO
147
            adc_channel = 2; // set next channel to ADC2 = NICK GYRO
148
            break;
148
            break;
149
        case 8:
149
        case 8:
150
                // average over two samples to create current ADValuePitch
150
                // average over two samples to create current ADValueNick
151
            if(BoardRelease == 10)  AdValueGyrPitch = (ADC + pitch1) / 2;
151
            if(BoardRelease == 10)  AdValueGyrNick = (ADC + nick1) / 2;
152
                        else                                    AdValueGyrPitch = ADC + pitch1; // gain is 2 times lower on FC 1.1
152
                        else                                    AdValueGyrNick = ADC + nick1; // gain is 2 times lower on FC 1.1
153
            adc_channel = 5; // set next channel to ADC5 = ACC_Z
153
            adc_channel = 5; // set next channel to ADC5 = ACC_Z
154
            break;
154
            break;
155
       case 9:
155
       case 9:
156
                // get z acceleration
156
                // get z acceleration
157
            AdValueAccTop =  (int16_t) ADC - NeutralAccZ; // get plain acceleration in Z direction
157
            AdValueAccTop =  (int16_t) ADC - NeutralAccZ; // get plain acceleration in Z direction
158
            AdValueAccTop += abs(AdValueAccPitch) / 4 + abs(AdValueAccRoll) / 4;
158
            AdValueAccTop += abs(AdValueAccNick) / 4 + abs(AdValueAccRoll) / 4;
159
            if(AdValueAccTop > 1)
159
            if(AdValueAccTop > 1)
160
             {
160
             {
161
                if(NeutralAccZ < 750)
161
                if(NeutralAccZ < 750)
162
                {
162
                {
163
                                        NeutralAccZ += 0.02;
163
                                        NeutralAccZ += 0.02;