Subversion Repositories FlightCtrl

Rev

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

Rev 711 Rev 750
Line 13... Line 13...
13
#include "timer0.h"
13
#include "timer0.h"
14
#include "fc.h"
14
#include "fc.h"
15
#include "printf_P.h"
15
#include "printf_P.h"
16
#include "eeprom.h"
16
#include "eeprom.h"
Line 17... Line -...
17
 
-
 
18
volatile int16_t Current_Pitch = 0, Current_Roll = 0, Current_Yaw = 0;
17
 
19
volatile int16_t Current_AccX = 0, Current_AccY = 0, Current_AccZ = 0;
18
volatile int16_t Current_AccZ = 0;
20
volatile int16_t UBat = 100;
19
volatile int16_t UBat = 100;
21
volatile int16_t AdValueGyrPitch = 0, AdValueGyrRoll = 0, AdValueGyrYaw = 0;
20
volatile int16_t AdValueGyrPitch = 0, AdValueGyrRoll = 0,  AdValueGyrYaw = 0;
22
volatile int16_t AdValueAccRoll = 0, AdValueAccPitch = 0, AdValueAccTop = 0;
21
volatile int16_t AdValueAccRoll = 0,  AdValueAccPitch = 0, AdValueAccTop = 0;
23
volatile int32_t AirPressure = 32000;
22
volatile int32_t AirPressure = 32000;
24
volatile int16_t StartAirPressure;
23
volatile int16_t StartAirPressure;
25
volatile uint16_t ReadingAirPressure = 1023;
24
volatile uint16_t ReadingAirPressure = 1023;
26
uint8_t PressureSensorOffset;
25
uint8_t PressureSensorOffset;
27
volatile int16_t HightD = 0;
-
 
28
volatile int16_t tmpAirPressure;
26
volatile int16_t HightD = 0;
Line 29... Line 27...
29
volatile uint16_t MeasurementCounter = 0;
27
volatile uint16_t MeasurementCounter = 0;
30
 
28
 
31
/*****************************************************/
29
/*****************************************************/
Line 58... Line 56...
58
    SREG = sreg;
56
    SREG = sreg;
59
}
57
}
Line 60... Line 58...
60
 
58
 
61
void SearchAirPressureOffset(void)
59
void SearchAirPressureOffset(void)
62
{
60
{
63
 uint8_t off;
61
        uint8_t off;
64
 off = GetParamByte(PID_LAST_OFFSET);
62
        off = GetParamByte(PID_PRESSURE_OFFSET);
65
 if(off > 20) off -= 10;
63
        if(off > 20) off -= 10;
66
 OCR0A = off;
64
        OCR0A = off;
67
 Delay_ms_Mess(100);
65
        Delay_ms_Mess(100);
68
 if(ReadingAirPressure < 850) off = 0;
66
        if(ReadingAirPressure < 850) off = 0;
69
 for(; off < 250;off++)
67
        for(; off < 250;off++)
70
  {
68
        {
71
  OCR0A = off;
69
                OCR0A = off;
72
  Delay_ms_Mess(50);
70
                Delay_ms_Mess(50);
73
  printf(".");
71
                printf(".");
74
  if(ReadingAirPressure < 900) break;
72
                if(ReadingAirPressure < 900) break;
75
  }
73
        }
76
 SetParamByte(PID_LAST_OFFSET, off);
74
        SetParamByte(PID_PRESSURE_OFFSET, off);
77
 PressureSensorOffset = off;
75
        PressureSensorOffset = off;
78
 Delay_ms_Mess(300);
76
        Delay_ms_Mess(300);
Line 79... Line 77...
79
}
77
}
80
 
78
 
81
 
79
 
-
 
80
/*****************************************************/
-
 
81
/*     Interrupt Service Routine for ADC             */
-
 
82
/*****************************************************/
-
 
83
// The routine changes the ADC input muxer running
-
 
84
// thru the state machine by the following order.
-
 
85
// state 0: ch0 (yaw gyro)
-
 
86
// state 1: ch1 (roll gyro)
-
 
87
// state 2: ch2 (pitch gyro)
-
 
88
// state 3: ch4 (battery voltage -> UBat)
-
 
89
// state 4: ch6 (acc y -> Current_AccY)
-
 
90
// state 5: ch7 (acc x -> Current_AccX)
-
 
91
// state 6: ch0 (yaw gyro average with first reading   -> AdValueGyrYaw)
-
 
92
// state 7: ch1 (roll gyro average with first reading  -> AdValueGyrRoll)
-
 
93
// state 8: ch2 (pitch gyro average with first reading -> AdValueGyrPitch)
82
/*****************************************************/
94
// state 9: ch5 (acc z add also 4th part of acc x and acc y to reading)
83
/*     Interrupt Service Routine for ADC             */
95
// state10: ch3 (air pressure averaging over 5 single readings -> tmpAirPressure)
84
/*****************************************************/
96
 
85
ISR(ADC_vect)
97
ISR(ADC_vect)
86
{
98
{
-
 
99
    static uint8_t adc_channel = 0, state = 0;
87
    static uint8_t adc_channel = 0, state = 0;
100
    static uint16_t yaw1, roll1, pitch1;
88
    static uint16_t yaw1, roll1, pitch1;
101
    static uint8_t average_pressure = 0;
89
    static uint8_t average_pressure = 0;
102
    static int16_t tmpAirPressure = 0;
90
    // disable further AD conversion
103
    // disable further AD conversion
91
    ADC_Disable();
104
    ADC_Disable();
Line 109... Line 122...
109
                // get actual UBat (Volts*10) is ADC*30V/1024*10 = ADC/3
122
                // get actual UBat (Volts*10) is ADC*30V/1024*10 = ADC/3
110
            UBat = (3 * UBat + ADC / 3) / 4; // low pass filter updates UBat only to 1 quater with actual ADC value
123
            UBat = (3 * UBat + ADC / 3) / 4; // low pass filter updates UBat only to 1 quater with actual ADC value
111
            adc_channel = 6; // set next channel to ADC6 = ACC_Y
124
            adc_channel = 6; // set next channel to ADC6 = ACC_Y
112
            break;
125
            break;
113
        case 4:
126
        case 4:
114
            Current_AccY = NeutralAccY - ADC; // get acceleration in Y direction
127
            AdValueAccRoll = NeutralAccY - ADC; // get acceleration in Y direction
115
            AdValueAccRoll = Current_AccY;
-
 
116
            adc_channel = 7; // set next channel to ADC7 = ACC_X
128
            adc_channel = 7; // set next channel to ADC7 = ACC_X
117
            break;
129
            break;
118
        case 5:
130
        case 5:
119
            Current_AccX = ADC - NeutralAccX; // get acceleration in X direction
131
            AdValueAccPitch = ADC - NeutralAccX; // get acceleration in X direction
120
            AdValueAccPitch =  Current_AccX;
-
 
121
                    adc_channel = 0; // set next channel to ADC7 = YAW GYRO
132
                    adc_channel = 0; // set next channel to ADC7 = YAW GYRO
122
            break;
133
            break;
123
        case 6:
134
        case 6:
124
                // average over two samples to create current AdValueGyrYaw
135
                // average over two samples to create current AdValueGyrYaw
125
            if(BoardRelease == 10)  AdValueGyrYaw = (ADC + yaw1) / 2;
136
            if(BoardRelease == 10)  AdValueGyrYaw = (ADC + yaw1) / 2;
126
                        else                                       AdValueGyrYaw = ADC + yaw1; // gain is 2 times lower on FC 1.1
137
                        else                                    AdValueGyrYaw = ADC + yaw1; // gain is 2 times lower on FC 1.1
127
            adc_channel = 1; // set next channel to ADC7 = ROLL GYRO
138
            adc_channel = 1; // set next channel to ADC7 = ROLL GYRO
128
            break;
139
            break;
129
        case 7:
140
        case 7:
130
                // average over two samples to create current ADValueGyrRoll
141
                // average over two samples to create current ADValueGyrRoll
131
            if(BoardRelease == 10)  AdValueGyrRoll = (ADC + roll1) / 2;
142
            if(BoardRelease == 10)  AdValueGyrRoll = (ADC + roll1) / 2;
132
                        else                                       AdValueGyrRoll = ADC + roll1; // gain is 2 times lower on FC 1.1
143
                        else                                    AdValueGyrRoll = ADC + roll1; // gain is 2 times lower on FC 1.1
133
            adc_channel = 2; // set next channel to ADC2 = PITCH GYRO
144
            adc_channel = 2; // set next channel to ADC2 = PITCH GYRO
134
            break;
145
            break;
135
        case 8:
146
        case 8:
136
                // average over two samples to create current ADValuePitch
147
                // average over two samples to create current ADValuePitch
137
            if(BoardRelease == 10)  AdValueGyrPitch = (ADC + pitch1) / 2;
148
            if(BoardRelease == 10)  AdValueGyrPitch = (ADC + pitch1) / 2;
138
                        else                                       AdValueGyrPitch = ADC + pitch1; // gain is 2 times lower on FC 1.1
149
                        else                                    AdValueGyrPitch = ADC + pitch1; // gain is 2 times lower on FC 1.1
139
            adc_channel = 5; // set next channel to ADC5 = ACC_Z
150
            adc_channel = 5; // set next channel to ADC5 = ACC_Z
140
            break;
151
            break;
141
       case 9:
152
       case 9:
142
                // get z acceleration
153
                // get z acceleration
143
            AdValueAccTop =  (int16_t) ADC - NeutralAccZ; // get plain acceleration in Z direction
154
            AdValueAccTop =  (int16_t) ADC - NeutralAccZ; // get plain acceleration in Z direction
144
            AdValueAccTop += abs(Current_AccY) / 4 + abs(Current_AccX) / 4;
155
            AdValueAccTop += abs(AdValueAccPitch) / 4 + abs(AdValueAccRoll) / 4;
145
            if(AdValueAccTop > 1)
156
            if(AdValueAccTop > 1)
146
             {
157
             {
147
              if(NeutralAccZ < 800) NeutralAccZ+= 0.02;
158
                if(NeutralAccZ < 800) NeutralAccZ+= 0.02;
148
             }
159
             }
149
             else if(AdValueAccTop < -1)
160
             else if(AdValueAccTop < -1)
150
             {
161
             {
151
              if(NeutralAccZ > 600) NeutralAccZ-= 0.02;
162
                if(NeutralAccZ > 600) NeutralAccZ-= 0.02;
152
             }
163
             }
153
            Current_AccZ = ADC;
164
            Current_AccZ = ADC;
154
            Reading_Integral_Top += AdValueAccTop;      // Integrieren
165
            Reading_Integral_Top += AdValueAccTop;      // Integrieren
155
            Reading_Integral_Top -= Reading_Integral_Top / 1024; // dämfen
166
            Reading_Integral_Top -= Reading_Integral_Top / 1024; // dämfen
156
                adc_channel = 3; // set next channel to ADC3 = air pressure
167
                adc_channel = 3; // set next channel to ADC3 = air pressure
157
            break;
168
            break;
158
        case 10:
169
        case 10:
159
            tmpAirPressure += ADC; // sum vadc values
170
            tmpAirPressure += ADC; // sum vadc values
160
            if(++average_pressure >= 5) // if 5 values are summerized for averaging
171
            if(++average_pressure >= 5) // if 5 values are summerized for averaging
161
                {
172
            {
162
                ReadingAirPressure = ADC; // update measured air pressure
173
                ReadingAirPressure = ADC; // update measured air pressure
163
                average_pressure = 0; // reset air pressure measurement counter
-
 
164
                                HightD = (int16_t)(StartAirPressure - tmpAirPressure - ReadingHight);  // D-Anteil = neuerWert - AlterWert
174
                                HightD = (int16_t)(StartAirPressure - tmpAirPressure - ReadingHight);  // D-Anteil = neuerWert - AlterWert
165
                AirPressure = (tmpAirPressure + 3 * AirPressure) / 4; // averaging using history
175
                AirPressure = (tmpAirPressure + 3 * AirPressure) / 4; // averaging using history
166
                ReadingHight = StartAirPressure - AirPressure;
176
                ReadingHight = StartAirPressure - AirPressure;
-
 
177
                average_pressure = 0; // reset air pressure measurement counter
167
                tmpAirPressure = 0;
178
                tmpAirPressure = 0;
168
                }
179
            }
169
            adc_channel = 0; // set next channel to ADC0 = GIER GYRO
180
            adc_channel = 0; // set next channel to ADC0 = GIER GYRO
170
            state = 0; // reset state
181
            state = 0; // reset state machine
171
            break;
182
            break;
172
        default:
183
        default:
173
            adc_channel = 0;
184
            adc_channel = 0;
174
            state = 0;
185
            state = 0;
175
            break;
186
            break;
176
        }
187
        }
177
    // set adc muxer to next adc_channel
188
    // set adc muxer to next adc_channel
178
    ADMUX = (ADMUX & 0xE0) | adc_channel;
189
    ADMUX = (ADMUX & 0xE0) | adc_channel;
179
    // ??
190
    // after full cycle stop further interrupts
180
    if(state != 0) ADC_Enable();
191
    if(state != 0) ADC_Enable();
181
}
192
}