Subversion Repositories FlightCtrl

Rev

Rev 838 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 838 Rev 966
Line 11... Line 11...
11 
are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de
 11 
are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de
12 
unless it is stated otherwise.
 12 
unless it is stated otherwise.
13 
*/
 13 
*/
Line 14... Line 14...
14 
 
 14 
 
15 
#include "main.h"
 15 
#include "main.h"
Line -... Line 16...
- 
 
 16 
#include "FlightControl.h"
16 
#include "KalmanFc.h"
 17 
 
17 
 
 18 
 
18 
int  UBat = 100;
 19 
int UBat = 100; /* Initial Battery Guess */
19 
int  AdWertNick = 0, AdWertRoll = 0, AdWertGier = 0;
 20 
int AdWertNick = 0, AdWertRoll = 0, AdWertGier = 0;
20 
int  AdWertAccRoll = 0,AdWertAccNick = 0,AdWertAccHoch = 0;
 21 
int AdWertAccRoll = 0,AdWertAccNick = 0,AdWertAccHoch = 0;
Line 21... Line 22...
21 
int  AdWertNick_Raw = 0, AdWertRoll_Raw = 0, AdWertGier_Raw = 0;
 22 
int AdWertNick_Raw = 0, AdWertRoll_Raw = 0, AdWertGier_Raw = 0;
22 
int  AdWertAccRoll_Raw = 0,AdWertAccNick_Raw = 0,AdWertAccHoch_Raw = 0;
 23 
int AdWertAccRoll_Raw = 0,AdWertAccNick_Raw = 0,AdWertAccHoch_Raw = 0;
23 
 
 24 
 
24 
int AccumulatedACC_X = 0, AccumulatedACC_Y = 0, AccumulatedACC_Z = 0, AccumulatedAirPressure = 0;
 25 
int AccumulatedACC_X = 0, AccumulatedACC_Y = 0, AccumulatedACC_Z = 0, AccumulatedAirPressure = 0;
Line 25... Line 26...
25 
int AccumulatedACC_X_cnt = 0, AccumulatedACC_Y_cnt = 0, AccumulatedACC_Z_cnt = 0, AccumulatedAirPressure_cnt = 0;
 26 
int AccumulatedACC_X_cnt = 0, AccumulatedACC_Y_cnt = 0, AccumulatedACC_Z_cnt = 0, AccumulatedAirPressure_cnt = 0;
Line 26... Line 27...
26 
int AccumulatedRoll_X = 0, AccumulatedRoll_Y = 0, AccumulatedRoll_Z = 0;
 27 
int AccumulatedRoll_X = 0, AccumulatedRoll_Y = 0, AccumulatedRoll_Z = 0;
27 
int AccumulatedRoll_X_cnt = 0, AccumulatedRoll_Y_cnt = 0, AccumulatedRoll_Z_cnt = 0;
 28 
int AccumulatedRoll_X_cnt = 0, AccumulatedRoll_Y_cnt = 0, AccumulatedRoll_Z_cnt = 0;
28 
 
 29 
 
Line 37... Line 38...
37 
  @post             -
 38 
  @post             -
38 
  @author           H. Buss / I. Busker
 39 
  @author           H. Buss / I. Busker
39 
**************************************************************************** */
 40 
**************************************************************************** */
40 
void ADC_Init(void)
 41 
void ADC_Init(void)
41 
{
 42 
{
- 
 
 43 
#ifdef INTERNAL_REFERENCE
- 
 
 44 
  ADMUX = 64;/* Internal Reference 5V */
- 
 
 45 
#else
42 
    ADMUX = 0;//Referenz ist extern
 46 
  ADMUX = 0; /* External Reference */
- 
 
 47 
#endif
43 
    ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE);
 48 
  ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE);
44 
    //Free Running Mode, Division Factor 128, Interrupt on
 - 
 
45 
}
 49 
}
Line 46... Line 50...
46 
 
 50 
 
47 
/* ****************************************************************************
 51 
/* ****************************************************************************
48 
Functionname:     SucheLuftruckOffset                      */ /*!
 52 
Functionname:     SucheLuftruckOffset                      */ /*!
Line 53... Line 57...
53 
  @post             -
 57 
  @post             -
54 
  @author           H. Buss / I. Busker
 58 
  @author           H. Buss / I. Busker
55 
**************************************************************************** */
 59 
**************************************************************************** */
56 
void SucheLuftruckOffset(void)
 60 
void SucheLuftruckOffset(void)
57 
{
 61 
{
58 
        unsigned int off;
 62 
  unsigned int off;
59 
        off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]);
 63 
  off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]);
60 
        if(off > 20) off -= 10;
 64 
  if(off > 20) off -= 10;
61 
        OCR0A = off;
 65 
  OCR0A = off;
62 
        Delay_ms_Mess(100);
 66 
  Delay_ms_Mess(100);
63 
        if(AdWertAirPressure_Raw < 850) off = 0;
 67 
  if(AdWertAirPressure_Raw < 850) off = 0;
64 
        for(; off < 250;off++)
 68 
  for(; off < 250;off++)
65 
        {
 69 
  {
66 
                OCR0A = off;
 70 
    OCR0A = off;
67 
                Delay_ms_Mess(50);
 71 
    Delay_ms_Mess(50);
68 
                printf(".");  
 72 
    printf(".");  
69 
                if(AdWertAirPressure_Raw < 900) break;
 73 
    if(AdWertAirPressure_Raw < 900) break;
70 
        }
 74 
  }
71 
        eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off);
 75 
  eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off);
72 
        Delay_ms_Mess(300);
 76 
  Delay_ms_Mess(300);
73 
}
 77 
}
Line 74... Line 78...
74 
 
 78 
 
75 
/* ****************************************************************************
 79 
/* ****************************************************************************
76 
Functionname:     SIGNAL                      */ /*!
 80 
Functionname:     SIGNAL                      */ /*!
Line 81... Line 85...
81 
  @post             -
 85 
  @post             -
82 
  @author           Michael Walter
 86 
  @author           Michael Walter
83 
**************************************************************************** */
 87 
**************************************************************************** */
84 
SIGNAL(SIG_ADC)
 88 
SIGNAL(SIG_ADC)
85 
{
 89 
{
86 
    static unsigned char kanal=0,state = 0;
 90 
  static unsigned char kanal=0,state = 0;
87 
    ANALOG_OFF;
 91 
  ANALOG_OFF;
88 
    switch(state++)
 92 
  switch(state++)
89 
        {
 93 
  {
90 
        case 0:
 94 
  case 0:
91 
                AdWertGier = ADC;
 95 
    AdWertGier = ADC;
92 
                AdWertGier_Raw = AdWertGier;
 96 
    AdWertGier_Raw = ADC;
93 
                AccumulatedRoll_Z += (ADC - AdNeutralGier);
 97 
    AccumulatedRoll_Z += (ADC - AdNeutralGier);
94 
                AccumulatedRoll_Z_cnt++;
 98 
    AccumulatedRoll_Z_cnt++;
95 
                kanal = 1;
 99 
    kanal = 1;
96 
                break; 
 100 
    break;     
97 
        case 1:
 101 
  case 1:
98 
                AdWertRoll = ADC;
 102 
    AdWertRoll = ADC;
99 
                AdWertRoll_Raw = AdWertRoll;
 103 
    AdWertRoll_Raw = ADC;
100 
                AccumulatedRoll_X += (ADC - AdNeutralRoll);
 104 
    AccumulatedRoll_X += (ADC - AdNeutralRoll);
101 
                AccumulatedRoll_X_cnt++;
 105 
    AccumulatedRoll_X_cnt++;
102 
                kanal = 2;
 106 
    kanal = 2;
103 
                break;         
 107 
    break;             
104 
        case 2:
 108 
  case 2:
105 
                AdWertNick = ADC;
 109 
    AdWertNick = ADC;
106 
                AdWertNick_Raw = AdWertNick;
 110 
    AdWertNick_Raw = ADC;
107 
                AccumulatedRoll_Y += (ADC - AdNeutralNick);
 111 
    AccumulatedRoll_Y += (ADC - AdNeutralNick);
108 
                AccumulatedRoll_Y_cnt++;
 112 
    AccumulatedRoll_Y_cnt++;
109 
                kanal = 4;
 113 
    kanal = 4;
110 
                break;                 
 114 
    break;                     
111 
        case 3:
 115 
  case 3:
- 
 
 116 
#ifdef INTERNAL_REFERENCE
- 
 
 117 
    UBat = (3 * UBat + (5 * ADC) / 9) / 4; /* The internal Voltage is 5V instesd of 3V */
- 
 
 118 
#else
112 
                UBat = (3 * UBat + ADC / 3) / 4;
 119 
    UBat = (3 * UBat + ADC / 3) / 4;
- 
 
 120 
#endif
113 
                kanal = 6;
 121 
    kanal = 6;
114 
                break;
 122 
    break;
115 
        case 4:
 123 
  case 4:
116 
                AdWertAccRoll = NeutralAccY - ADC;
 124 
    AdWertAccRoll = NeutralAccY - ADC;
117 
                AdWertAccRoll_Raw = ADC;
 125 
    AdWertAccRoll_Raw = ADC;
118 
                AccumulatedACC_Y += (NeutralAccY - ADC);
 126 
    AccumulatedACC_Y += (NeutralAccY - ADC);
119 
                AccumulatedACC_Y_cnt++;
 127 
    AccumulatedACC_Y_cnt++;
120 
                kanal = 7;
 128 
    kanal = 7;
121 
                break;
 129 
    break;
122 
        case 5:
 130 
  case 5:
123 
                AdWertAccNick = ADC - NeutralAccX;
 131 
    AdWertAccNick = ADC - NeutralAccX;
124 
                AdWertAccNick_Raw = ADC;
 132 
    AdWertAccNick_Raw = ADC;
125 
                AccumulatedACC_X += (ADC - NeutralAccX);
 133 
    AccumulatedACC_X += (ADC - NeutralAccX);
126 
                AccumulatedACC_X_cnt++;
 134 
    AccumulatedACC_X_cnt++;
127 
                kanal = 5;
 135 
    kanal = 5;
128 
                break;
 136 
    break;
129 
        case 6:
 137 
  case 6:      
130 
                AdWertAccHoch = (ADC - (NeutralAccX + NeutralAccY) / 2);
 138 
    AdWertAccHoch = (ADC - (NeutralAccX + NeutralAccY) / 2);
131 
                AdWertAccHoch_Raw = ADC;
 139 
    AdWertAccHoch_Raw = ADC;
132 
                AccumulatedACC_Z += (ADC - NeutralAccZ);
 140 
    AccumulatedACC_Z += (ADC - NeutralAccZ);
133 
                AccumulatedACC_Z_cnt++;
 141 
    AccumulatedACC_Z_cnt++;
134 
                kanal = 3;
 142 
    kanal = 3;
135 
                break;
 143 
    break;
136 
        case 7:
 144 
  case 7:
137 
        AdWertAirPressure_Raw = ADC;
 145 
    AdWertAirPressure_Raw = ADC;
138 
                AccumulatedAirPressure += ADC;
 146 
    AccumulatedAirPressure += ADC;
139 
                AccumulatedAirPressure_cnt++;
 147 
    AccumulatedAirPressure_cnt++;
140 
                kanal = 0;
 148 
    kanal = 0;
141 
                state = 0;
 149 
    state = 0;
142 
                break;         
 150 
    break;
143 
        default:
 151 
  default:
144 
                kanal = 0;
 152 
    kanal = 0;
145 
                state = 0;
 153 
    state = 0;
146 
                break;
 154 
    break;
147 
        }
 155 
  }
148 
    ADMUX = kanal;
 156 
  ADMUX = kanal;
- 
 
 157 
#ifdef INTERNAL_REFERENCE
- 
 
 158 
  /* Add 64 in order to use the internal 5v refenrence */
- 
 
 159 
  ADMUX += 64;
- 
 
 160 
#endif
149 
        ANALOG_ON;
 161 
  ANALOG_ON;
150 
}
 162 
}