Subversion Repositories FlightCtrl

Rev

Rev 935 | Go to most recent revision | 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