Subversion Repositories FlightCtrl

Rev

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

Rev 935 Rev 936
Line 12... Line 12...
12
#include "main.h"
12
#include "main.h"
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"
-
 
17
#include "twimaster.h"
Line 17... Line 18...
17
 
18
 
18
volatile int16_t Current_AccZ = 0;
19
volatile int16_t Current_AccZ = 0;
19
volatile int16_t UBat = 100;
20
volatile int16_t UBat = 100;
-
 
21
volatile int16_t AdValueGyrNick = 0, AdValueGyrRoll = 0,  AdValueGyrYaw = 0;
-
 
22
uint8_t AnalogOffsetNick = 115, AnalogOffsetRoll = 115, AnalogOffsetYaw = 115;
20
volatile int16_t AdValueGyrNick = 0, AdValueGyrRoll = 0,  AdValueGyrYaw = 0;
23
uint8_t GyroDefectNick = 0, GyroDefectRoll = 0, GyroDefectYaw = 0;
21
volatile int16_t AdValueAccRoll = 0,  AdValueAccNick = 0, AdValueAccTop = 0;
24
volatile int16_t AdValueAccRoll = 0,  AdValueAccNick = 0, AdValueAccTop = 0;
-
 
25
volatile int32_t AirPressure = 32000;
22
volatile int32_t AirPressure = 32000;
26
volatile uint8_t average_pressure = 0;
23
volatile int16_t StartAirPressure;
27
volatile int16_t StartAirPressure;
24
volatile uint16_t ReadingAirPressure = 1023;
28
volatile uint16_t ReadingAirPressure = 1023;
25
uint8_t PressureSensorOffset;
29
uint8_t PressureSensorOffset;
26
volatile int16_t HeightD = 0;
30
volatile int16_t HeightD = 0;
Line 75... Line 79...
75
        PressureSensorOffset = off;
79
        PressureSensorOffset = off;
76
        Delay_ms_Mess(300);
80
        Delay_ms_Mess(300);
77
}
81
}
Line -... Line 82...
-
 
82
 
-
 
83
 
-
 
84
void SearchGyroOffset(void)
-
 
85
{
-
 
86
        uint8_t i, ready = 0;
-
 
87
 
-
 
88
        GyroDefectNick = 0; GyroDefectRoll = 0; GyroDefectYaw = 0;
-
 
89
        for(i = 140; i != 0; i--)
-
 
90
        {
-
 
91
                if(ready == 3 && i > 10) i = 9;
-
 
92
                ready = 0;
-
 
93
                if(AdValueGyrNick < 1020) AnalogOffsetNick--; else if(AdValueGyrNick > 1030) AnalogOffsetNick++; else ready++;
-
 
94
                if(AdValueGyrRoll < 1020) AnalogOffsetRoll--; else if(AdValueGyrRoll > 1030) AnalogOffsetRoll++; else ready++;
-
 
95
                if(AdValueGyrYaw  < 1020) AnalogOffsetYaw-- ; else if(AdValueGyrYaw  > 1030) AnalogOffsetYaw++ ; else ready++;
-
 
96
                twi_state = TWI_STATE_GYRO_OFFSET_TX; // set twi_state in TWI ISR to start of Gyro Offset
-
 
97
                I2C_Start();   // initiate data transmission
-
 
98
                if(AnalogOffsetNick < 10)  { GyroDefectNick = 1; AnalogOffsetNick = 10;}; if(AnalogOffsetNick > 245) { GyroDefectNick = 1; AnalogOffsetNick = 245;};
-
 
99
                if(AnalogOffsetRoll < 10)  { GyroDefectRoll = 1; AnalogOffsetRoll = 10;}; if(AnalogOffsetRoll > 245) { GyroDefectRoll = 1; AnalogOffsetRoll = 245;};
-
 
100
                if(AnalogOffsetYaw  < 10)  { GyroDefectYaw  = 1; AnalogOffsetYaw  = 10;}; if(AnalogOffsetYaw  > 245) { GyroDefectYaw  = 1; AnalogOffsetYaw  = 245;};
-
 
101
                while(twi_state); // wait for end of data transmission
-
 
102
                average_pressure = 0;
-
 
103
                ADC_Enable();
-
 
104
                while(average_pressure == 0);
-
 
105
                if(i < 10) Delay_ms_Mess(10);
-
 
106
        }
-
 
107
        Delay_ms_Mess(70);
-
 
108
}
-
 
109
 
-
 
110
 
78
 
111
 
79
 
112
 
80
/*****************************************************/
113
/*****************************************************/
81
/*     Interrupt Service Routine for ADC             */
114
/*     Interrupt Service Routine for ADC             */
82
/*****************************************************/
115
/*****************************************************/
Line 99... Line 132...
99
 
132
 
100
ISR(ADC_vect)
133
ISR(ADC_vect)
101
{
134
{
102
    static uint8_t adc_channel = 0, state = 0;
135
    static uint8_t adc_channel = 0, state = 0;
103
    static uint16_t yaw1, roll1, nick1;
-
 
104
    static uint8_t average_pressure = 0;
136
    static uint16_t yaw1, roll1, nick1;
105
    static int16_t tmpAirPressure = 0;
137
    static int16_t tmpAirPressure = 0;
106
    // disable further AD conversion
138
    // disable further AD conversion
107
    ADC_Disable();
139
    ADC_Disable();
108
    // state machine
140
    // state machine