Rev 304 | Rev 401 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 304 | Rev 395 | ||
|---|---|---|---|
| Line 52... | Line 52... | ||
| 52 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
52 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
| 53 | // + POSSIBILITY OF SUCH DAMAGE. |
53 | // + POSSIBILITY OF SUCH DAMAGE. |
| 54 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
54 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| Line 55... | Line 55... | ||
| 55 | 55 | ||
| - | 56 | #include "main.h" |
|
| Line 56... | Line 57... | ||
| 56 | #include "main.h" |
57 | #include "eeprom.c" |
| 57 | 58 | ||
| 58 | unsigned char h,m,s; |
59 | unsigned char h,m,s; |
| 59 | volatile unsigned int I2CTimeout = 100; |
60 | volatile unsigned int I2CTimeout = 100; |
| 60 | volatile int MesswertNick,MesswertRoll,MesswertGier; |
61 | volatile int MesswertNick,MesswertRoll,MesswertGier; |
| 61 | volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0; |
62 | volatile int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0; |
| 62 | volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0; |
63 | volatile int Mittelwert_AccNick, Mittelwert_AccRoll,Mittelwert_AccHoch, NeutralAccX=0, NeutralAccY=0; |
| 63 | volatile float NeutralAccZ = 0; |
64 | volatile float NeutralAccZ = 0; |
| 64 | unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0; |
65 | unsigned char CosinusNickWinkel = 0, CosinusRollWinkel = 0; |
| - | 66 | volatile long IntegralNick = 0,IntegralNick2 = 0; |
|
| 65 | volatile long IntegralNick = 0,IntegralNick2 = 0; |
67 | volatile long IntegralRoll = 0,IntegralRoll2 = 0; |
| 66 | volatile long IntegralRoll = 0,IntegralRoll2 = 0; |
68 | volatile long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
| 67 | volatile long Integral_Gier = 0; |
69 | volatile long Integral_Gier = 0; |
| 68 | volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0; |
70 | volatile long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0; |
| - | 71 | volatile long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0; |
|
| 69 | volatile long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0; |
72 | volatile long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0; |
| 70 | volatile long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0; |
73 | volatile long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2; |
| 71 | volatile long Mess_Integral_Hoch = 0; |
74 | volatile long Mess_Integral_Hoch = 0; |
| 72 | volatile int KompassValue = 0; |
75 | volatile int KompassValue = 0; |
| 73 | volatile int KompassStartwert = 0; |
76 | volatile int KompassStartwert = 0; |
| 74 | volatile int KompassRichtung = 0; |
77 | volatile int KompassRichtung = 0; |
| 75 | unsigned char MAX_GAS,MIN_GAS; |
78 | unsigned char MAX_GAS,MIN_GAS; |
| - | 79 | unsigned char Notlandung = 0; |
|
| Line 76... | Line 80... | ||
| 76 | unsigned char Notlandung = 0; |
80 | unsigned char HoehenReglerAktiv = 0; |
| 77 | unsigned char HoehenReglerAktiv = 0; |
81 | long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L; |
| Line 78... | Line 82... | ||
| 78 | 82 | ||
| Line 86... | Line 90... | ||
| 86 | volatile unsigned char SenderOkay = 0; |
90 | volatile unsigned char SenderOkay = 0; |
| 87 | int StickNick = 0,StickRoll = 0,StickGier = 0; |
91 | int StickNick = 0,StickRoll = 0,StickGier = 0; |
| 88 | char MotorenEin = 0; |
92 | char MotorenEin = 0; |
| 89 | int HoehenWert = 0; |
93 | int HoehenWert = 0; |
| 90 | int SollHoehe = 0; |
94 | int SollHoehe = 0; |
| 91 | unsigned char Looping_Nick = 0,Looping_Roll = 0; |
95 | int I_LageRoll = 0,I_LageNick = 0; |
| 92 | - | ||
| 93 | float Kp = FAKTOR_P; |
96 | float Kp = FAKTOR_P; |
| 94 | float Ki = FAKTOR_I; |
97 | float Ki = FAKTOR_I; |
| - | 98 | unsigned char Looping_Nick = 0,Looping_Roll = 0; |
|
| - | 99 | unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0; |
|
| Line 95... | Line 100... | ||
| 95 | 100 | ||
| 96 | unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250 |
101 | unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250 |
| 97 | unsigned char Parameter_MaxHoehe = 251; // Wert : 0-250 |
102 | unsigned char Parameter_MaxHoehe = 251; // Wert : 0-250 |
| 98 | unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32 |
103 | unsigned char Parameter_Hoehe_P = 16; // Wert : 0-32 |
| Line 106... | Line 111... | ||
| 106 | unsigned char Parameter_UserParam2 = 0; |
111 | unsigned char Parameter_UserParam2 = 0; |
| 107 | unsigned char Parameter_UserParam3 = 0; |
112 | unsigned char Parameter_UserParam3 = 0; |
| 108 | unsigned char Parameter_UserParam4 = 0; |
113 | unsigned char Parameter_UserParam4 = 0; |
| 109 | unsigned char Parameter_ServoNickControl = 100; |
114 | unsigned char Parameter_ServoNickControl = 100; |
| 110 | unsigned char Parameter_LoopGasLimit = 70; |
115 | unsigned char Parameter_LoopGasLimit = 70; |
| - | 116 | unsigned char Parameter_AchsKopplung1 = 0; |
|
| - | 117 | unsigned char Parameter_AchsGegenKopplung1 = 0; |
|
| 111 | struct mk_param_struct EE_Parameter; |
118 | struct mk_param_struct EE_Parameter; |
| Line 112... | Line 119... | ||
| 112 | 119 | ||
| 113 | void Piep(unsigned char Anzahl) |
120 | void Piep(unsigned char Anzahl) |
| 114 | { |
121 | { |
| Line 123... | Line 130... | ||
| 123 | //############################################################################ |
130 | //############################################################################ |
| 124 | // Nullwerte ermitteln |
131 | // Nullwerte ermitteln |
| 125 | void SetNeutral(void) |
132 | void SetNeutral(void) |
| 126 | //############################################################################ |
133 | //############################################################################ |
| 127 | { |
134 | { |
| 128 | unsigned int timer; |
- | |
| 129 | NeutralAccX = 0; |
135 | NeutralAccX = 0; |
| 130 | NeutralAccY = 0; |
136 | NeutralAccY = 0; |
| 131 | NeutralAccZ = 0; |
137 | NeutralAccZ = 0; |
| 132 | AdNeutralNick = 0; |
138 | AdNeutralNick = 0; |
| 133 | AdNeutralRoll = 0; |
139 | AdNeutralRoll = 0; |
| 134 | AdNeutralGier = 0; |
140 | AdNeutralGier = 0; |
| - | 141 | Parameter_AchsKopplung1 = 0; |
|
| - | 142 | Parameter_AchsGegenKopplung1 = 0; |
|
| 135 | CalibrierMittelwert(); |
143 | CalibrierMittelwert(); |
| 136 | timer = SetDelay(5); |
144 | Delay_ms_Mess(100); |
| 137 | while (!CheckDelay(timer)); |
- | |
| 138 | CalibrierMittelwert(); |
145 | CalibrierMittelwert(); |
| 139 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
146 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
| 140 | { |
147 | { |
| 141 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
148 | if((MessLuftdruck > 950) || (MessLuftdruck < 750)) SucheLuftruckOffset(); |
| 142 | } |
149 | } |
| Line 143... | Line -... | ||
| 143 | - | ||
| 144 | if(PlatinenVersion == 10) |
- | |
| 145 | { |
150 | |
| 146 | AdNeutralNick= abs(MesswertNick); |
151 | AdNeutralNick= AdWertNick; |
| 147 | AdNeutralRoll= abs(MesswertRoll); |
152 | AdNeutralRoll= AdWertRoll; |
| 148 | AdNeutralGier= abs(MesswertGier); |
- | |
| 149 | } |
- | |
| 150 | else |
- | |
| 151 | { |
- | |
| 152 | AdNeutralNick= abs(MesswertNick) / 2; |
- | |
| 153 | AdNeutralRoll= abs(MesswertRoll) / 2; |
- | |
| 154 | AdNeutralGier= abs(MesswertGier) / 2; |
153 | AdNeutralGier= AdWertGier; |
| 155 | } |
154 | |
| 156 | NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY; |
155 | NeutralAccY = abs(Mittelwert_AccRoll) / ACC_AMPLIFY; |
| 157 | NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY; |
156 | NeutralAccX = abs(Mittelwert_AccNick) / ACC_AMPLIFY; |
| Line 158... | Line 157... | ||
| 158 | NeutralAccZ = Aktuell_az; |
157 | NeutralAccZ = Aktuell_az; |
| Line 169... | Line 168... | ||
| 169 | HoeheD = 0; |
168 | HoeheD = 0; |
| 170 | Mess_Integral_Hoch = 0; |
169 | Mess_Integral_Hoch = 0; |
| 171 | KompassStartwert = KompassValue; |
170 | KompassStartwert = KompassValue; |
| 172 | GPS_Neutral(); |
171 | GPS_Neutral(); |
| 173 | beeptime = 50; |
172 | beeptime = 50; |
| - | 173 | Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
|
| - | 174 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
|
| 174 | } |
175 | } |
| Line 175... | Line 176... | ||
| 175 | 176 | ||
| 176 | //############################################################################ |
177 | //############################################################################ |
| 177 | // Bildet den Mittelwert aus den Messwerten |
178 | // Bearbeitet die Messwerte |
| 178 | void Mittelwert(void) |
179 | void Mittelwert(void) |
| 179 | //############################################################################ |
180 | //############################################################################ |
| - | 181 | { |
|
| 180 | { |
182 | static signed long tmpl,tmpl2; |
| 181 | // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern |
183 | MesswertGier = (signed int) AdNeutralGier - AdWertGier; |
| 182 | ANALOG_OFF; |
184 | MesswertRoll = (signed int) AdWertRoll - AdNeutralRoll; |
| - | 185 | MesswertNick = (signed int) AdWertNick - AdNeutralNick; |
|
| - | 186 | if(PlatinenVersion != 10) |
|
| - | 187 | { |
|
| 183 | if(MessanzahlNick) (MesswertNick = AccumulateNick / MessanzahlNick); |
188 | MesswertGier *= 2; |
| - | 189 | MesswertRoll *= 2; |
|
| - | 190 | MesswertNick *= 2; |
|
| 184 | if(MessanzahlRoll) (MesswertRoll = AccumulateRoll / MessanzahlRoll); |
191 | } |
| 185 | if(MessanzahlGier) (MesswertGier = AccumulateGier / MessanzahlGier); |
192 | // Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
| 186 | if(messanzahl_AccNick) Mittelwert_AccNick = ((long)Mittelwert_AccNick * 7 + ((ACC_AMPLIFY * (long)accumulate_AccNick) / messanzahl_AccNick)) / 8L; |
193 | Mittelwert_AccNick = ((long)Mittelwert_AccNick * 1 + ((ACC_AMPLIFY * (long)AdWertAccNick))) / 2L; |
| 187 | if(messanzahl_AccRoll) Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 7 + ((ACC_AMPLIFY * (long)accumulate_AccRoll) / messanzahl_AccRoll)) / 8L; |
194 | Mittelwert_AccRoll = ((long)Mittelwert_AccRoll * 1 + ((ACC_AMPLIFY * (long)AdWertAccRoll))) / 2L; |
| - | 195 | Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 1 + ((long)AdWertAccHoch)) / 2L; |
|
| - | 196 | IntegralAccNick += ACC_AMPLIFY * AdWertAccNick; |
|
| - | 197 | IntegralAccRoll += ACC_AMPLIFY * AdWertAccRoll; |
|
| - | 198 | IntegralAccZ += Aktuell_az - 704;//NeutralAccZ; |
|
| - | 199 | // Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 200 | Mess_Integral_Gier += MesswertGier; |
|
| - | 201 | Mess_Integral_Gier2 += MesswertGier; |
|
| - | 202 | // Kopplungsanteil +++++++++++++++++++++++++++++++++++++ |
|
| - | 203 | if(!Looping_Nick && !Looping_Roll && (EE_Parameter.GlobalConfig & CFG_ACHSENKOPPLUNG_AKTIV)) |
|
| 188 | if(messanzahl_AccHoch) Mittelwert_AccHoch = ((long)Mittelwert_AccHoch * 7 + ((long)accumulate_AccHoch) / messanzahl_AccHoch) / 8L; |
204 | { |
| - | 205 | tmpl = Mess_IntegralNick / 4096L; |
|
| - | 206 | tmpl *= MesswertGier; |
|
| - | 207 | tmpl *= Parameter_AchsKopplung1; //125 |
|
| - | 208 | tmpl /= 2048L; |
|
| - | 209 | tmpl2 = Mess_IntegralRoll / 4096L; |
|
| - | 210 | tmpl2 *= MesswertGier; |
|
| - | 211 | tmpl2 *= Parameter_AchsKopplung1; |
|
| - | 212 | tmpl2 /= 2048L; |
|
| - | 213 | } |
|
| - | 214 | else tmpl = tmpl2 = 0; |
|
| - | 215 | // Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 216 | MesswertRoll += tmpl; |
|
| - | 217 | MesswertRoll += (tmpl2*Parameter_AchsGegenKopplung1)/512L; //109 |
|
| - | 218 | Mess_IntegralRoll2 += MesswertRoll; |
|
| - | 219 | Mess_IntegralRoll += MesswertRoll - I_LageRoll; |
|
| - | 220 | if(Mess_IntegralRoll > Umschlag180Roll) |
|
| - | 221 | { |
|
| - | 222 | Mess_IntegralRoll = -(Umschlag180Roll - 10000L); |
|
| - | 223 | Mess_IntegralRoll2 = Mess_IntegralRoll; |
|
| - | 224 | } |
|
| - | 225 | if(Mess_IntegralRoll <-Umschlag180Roll) |
|
| - | 226 | { |
|
| - | 227 | Mess_IntegralRoll = (Umschlag180Roll - 10000L); |
|
| - | 228 | Mess_IntegralRoll2 = Mess_IntegralRoll; |
|
| 189 | AccumulateNick = 0; MessanzahlNick = 0; |
229 | } |
| 190 | AccumulateRoll = 0; MessanzahlRoll = 0; |
230 | if(AdWertRoll < 15) MesswertRoll = -1000; |
| - | 231 | if(AdWertRoll < 7) MesswertRoll = -2000; |
|
| 191 | AccumulateGier = 0; MessanzahlGier = 0; |
232 | if(AdWertRoll > 1010) MesswertRoll = +1000; |
| - | 233 | if(AdWertRoll > 1017) MesswertRoll = +2000; |
|
| - | 234 | // Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 235 | MesswertNick -= tmpl2; |
|
| - | 236 | MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L; |
|
| - | 237 | Mess_IntegralNick2 += MesswertNick; |
|
| - | 238 | Mess_IntegralNick += MesswertNick - I_LageNick; |
|
| - | 239 | if(Mess_IntegralNick > Umschlag180Nick) |
|
| - | 240 | { |
|
| - | 241 | Mess_IntegralNick = -(Umschlag180Nick - 10000L); |
|
| - | 242 | Mess_IntegralNick2 = Mess_IntegralNick; |
|
| - | 243 | } |
|
| - | 244 | if(Mess_IntegralNick <-Umschlag180Nick) |
|
| - | 245 | { |
|
| - | 246 | Mess_IntegralNick = (Umschlag180Nick - 10000L); |
|
| - | 247 | Mess_IntegralNick2 = Mess_IntegralNick; |
|
| - | 248 | } |
|
| - | 249 | if(AdWertNick < 15) MesswertNick = -1000; |
|
| 192 | accumulate_AccRoll = 0;messanzahl_AccRoll = 0; |
250 | if(AdWertNick < 7) MesswertNick = -2000; |
| 193 | accumulate_AccNick = 0;messanzahl_AccNick = 0; |
251 | if(AdWertNick > 1010) MesswertNick = +1000; |
| - | 252 | if(AdWertNick > 1017) MesswertNick = +2000; |
|
| - | 253 | //++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 254 | // ADC einschalten |
|
| - | 255 | ANALOG_ON; |
|
| - | 256 | //++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| 194 | accumulate_AccHoch = 0;messanzahl_AccHoch = 0; |
257 | |
| 195 | Integral_Gier = Mess_Integral_Gier; |
- | |
| 196 | // Integral_Gier2 = Mess_Integral_Gier2; |
258 | Integral_Gier = Mess_Integral_Gier; |
| 197 | IntegralNick = Mess_IntegralNick; |
259 | IntegralNick = Mess_IntegralNick; |
| 198 | IntegralRoll = Mess_IntegralRoll; |
260 | IntegralRoll = Mess_IntegralRoll; |
| 199 | IntegralNick2 = Mess_IntegralNick2; |
261 | IntegralNick2 = Mess_IntegralNick2; |
| 200 | IntegralRoll2 = Mess_IntegralRoll2; |
- | |
| 201 | // ADC einschalten |
- | |
| Line -... | Line 262... | ||
| - | 262 | IntegralRoll2 = Mess_IntegralRoll2; |
|
| 202 | ANALOG_ON; |
263 | |
| 203 | - | ||
| 204 | /* |
264 | if(EE_Parameter.GlobalConfig & CFG_DREHRATEN_BEGRENZER && !Looping_Nick && !Looping_Roll) |
| 205 | //------------------------------------------------------------------------------ |
- | |
| 206 | if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200); |
265 | { |
| 207 | else |
266 | if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200); |
| 208 | if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200); |
- | |
| 209 | if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200); |
267 | else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200); |
| 210 | else |
- | |
| 211 | if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200); |
268 | if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200); |
| 212 | //------------------------------------------------------------------------------ |
269 | else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200); |
| 213 | */ |
270 | } |
| 214 | if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
271 | if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
| 215 | if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
272 | if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
| 216 | if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
273 | if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
| Line 226... | Line 283... | ||
| 226 | void CalibrierMittelwert(void) |
283 | void CalibrierMittelwert(void) |
| 227 | //############################################################################ |
284 | //############################################################################ |
| 228 | { |
285 | { |
| 229 | // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern |
286 | // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern |
| 230 | ANALOG_OFF; |
287 | ANALOG_OFF; |
| 231 | if(MessanzahlNick) (MesswertNick = AccumulateNick / MessanzahlNick); |
- | |
| 232 | if(MessanzahlRoll) (MesswertRoll = AccumulateRoll / MessanzahlRoll); |
- | |
| 233 | if(MessanzahlGier) (MesswertGier = AccumulateGier / MessanzahlGier); |
- | |
| 234 | if(messanzahl_AccNick) Mittelwert_AccNick = ((ACC_AMPLIFY * (long)accumulate_AccNick) / messanzahl_AccNick); |
- | |
| 235 | if(messanzahl_AccRoll) Mittelwert_AccRoll = (ACC_AMPLIFY * (long)accumulate_AccRoll) / messanzahl_AccRoll; |
- | |
| 236 | if(messanzahl_AccHoch) Mittelwert_AccHoch = ((long)accumulate_AccHoch) / messanzahl_AccHoch; |
- | |
| 237 | AccumulateNick = 0; MessanzahlNick = 0; |
288 | MesswertNick = AdWertNick; |
| 238 | AccumulateRoll = 0; MessanzahlRoll = 0; |
289 | MesswertRoll = AdWertRoll; |
| 239 | AccumulateGier = 0; MessanzahlGier = 0; |
290 | MesswertGier = AdWertGier; |
| 240 | accumulate_AccRoll = 0;messanzahl_AccRoll = 0; |
291 | Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick; |
| 241 | accumulate_AccNick = 0;messanzahl_AccNick = 0; |
292 | Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll; |
| 242 | accumulate_AccHoch = 0;messanzahl_AccHoch = 0; |
293 | Mittelwert_AccHoch = (long)AdWertAccHoch; |
| 243 | // ADC einschalten |
294 | // ADC einschalten |
| 244 | ANALOG_ON; |
295 | ANALOG_ON; |
| 245 | if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
296 | if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
| 246 | if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
297 | if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
| 247 | if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
298 | if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
| 248 | if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--; |
299 | if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--; |
| 249 | if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
300 | if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
| 250 | if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
301 | if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
| 251 | if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
302 | if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
| 252 | if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
303 | if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
| - | 304 | ||
| - | 305 | Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
|
| - | 306 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
|
| 253 | } |
307 | } |
| Line 254... | Line 308... | ||
| 254 | 308 | ||
| 255 | //############################################################################ |
309 | //############################################################################ |
| 256 | // Senden der Motorwerte per I2C-Bus |
310 | // Senden der Motorwerte per I2C-Bus |
| Line 278... | Line 332... | ||
| 278 | twi_state = 0; |
332 | twi_state = 0; |
| 279 | motor = 0; |
333 | motor = 0; |
| 280 | i2c_start(); |
334 | i2c_start(); |
| 281 | } |
335 | } |
| Line 282... | Line -... | ||
| 282 | - | ||
| 283 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
| 284 | // + Konstanten |
- | |
| 285 | // + 0-250 -> normale Werte |
- | |
| 286 | // + 251 -> Poti1 |
- | |
| 287 | // + 252 -> Poti2 |
- | |
| 288 | // + 253 -> Poti3 |
- | |
| 289 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
| 290 | void DefaultKonstanten1(void) |
- | |
| 291 | { |
- | |
| 292 | EE_Parameter.Kanalbelegung[K_NICK] = 1; |
- | |
| 293 | EE_Parameter.Kanalbelegung[K_ROLL] = 2; |
- | |
| 294 | EE_Parameter.Kanalbelegung[K_GAS] = 3; |
- | |
| 295 | EE_Parameter.Kanalbelegung[K_GIER] = 4; |
- | |
| 296 | EE_Parameter.Kanalbelegung[K_POTI1] = 5; |
- | |
| 297 | EE_Parameter.Kanalbelegung[K_POTI2] = 6; |
- | |
| 298 | EE_Parameter.Kanalbelegung[K_POTI3] = 7; |
- | |
| 299 | EE_Parameter.Kanalbelegung[K_POTI4] = 8; |
- | |
| 300 | EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV | CFG_KOMPASS_FIX;//0x01; |
- | |
| 301 | EE_Parameter.Hoehe_MinGas = 30; |
- | |
| 302 | EE_Parameter.MaxHoehe = 251; // Wert : 0-32 251 -> Poti1 |
- | |
| 303 | EE_Parameter.Hoehe_P = 10; // Wert : 0-32 |
- | |
| 304 | EE_Parameter.Luftdruck_D = 50; // Wert : 0-250 |
- | |
| 305 | EE_Parameter.Hoehe_ACC_Wirkung = 50; // Wert : 0-250 |
- | |
| 306 | EE_Parameter.Hoehe_Verstaerkung = 4; // Wert : 0-50 |
- | |
| 307 | EE_Parameter.Stick_P = 4; //2 // Wert : 1-6 |
- | |
| 308 | EE_Parameter.Stick_D = 8; //8 // Wert : 0-64 |
- | |
| 309 | EE_Parameter.Gier_P = 16; // Wert : 1-20 |
- | |
| 310 | EE_Parameter.Gas_Min = 15; // Wert : 0-32 |
- | |
| 311 | EE_Parameter.Gas_Max = 250; // Wert : 33-250 |
- | |
| 312 | EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64 |
- | |
| 313 | EE_Parameter.KompassWirkung = 128; // Wert : 0-250 |
- | |
| 314 | EE_Parameter.Gyro_P = 120; //80 // Wert : 0-250 |
- | |
| 315 | EE_Parameter.Gyro_I = 150; // Wert : 0-250 |
- | |
| 316 | EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250 |
- | |
| 317 | EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust |
- | |
| 318 | EE_Parameter.NotGasZeit = 20; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
- | |
| 319 | EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
- | |
| 320 | EE_Parameter.I_Faktor = 0; |
- | |
| 321 | EE_Parameter.UserParam1 = 0; //zur freien Verwendung |
- | |
| 322 | EE_Parameter.UserParam2 = 0; //zur freien Verwendung |
- | |
| 323 | EE_Parameter.UserParam3 = 0; //zur freien Verwendung |
- | |
| 324 | EE_Parameter.UserParam4 = 0; //zur freien Verwendung |
- | |
| 325 | EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
- | |
| 326 | EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
- | |
| 327 | EE_Parameter.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
- | |
| 328 | EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag |
- | |
| 329 | EE_Parameter.ServoNickMax = 150; // Wert : 0-250 // Anschlag |
- | |
| 330 | EE_Parameter.ServoNickRefresh = 5; |
- | |
| 331 | EE_Parameter.LoopGasLimit = 50; |
- | |
| 332 | EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
- | |
| 333 | EE_Parameter.LoopConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts / wird getrennt behandelt |
- | |
| 334 | memcpy(EE_Parameter.Name, "Sport\0", 12); |
- | |
| 335 | } |
- | |
| 336 | - | ||
| 337 | void DefaultKonstanten2(void) |
- | |
| 338 | { |
- | |
| 339 | EE_Parameter.Kanalbelegung[K_NICK] = 1; |
- | |
| 340 | EE_Parameter.Kanalbelegung[K_ROLL] = 2; |
- | |
| 341 | EE_Parameter.Kanalbelegung[K_GAS] = 3; |
- | |
| 342 | EE_Parameter.Kanalbelegung[K_GIER] = 4; |
- | |
| 343 | EE_Parameter.Kanalbelegung[K_POTI1] = 5; |
- | |
| 344 | EE_Parameter.Kanalbelegung[K_POTI2] = 6; |
- | |
| 345 | EE_Parameter.Kanalbelegung[K_POTI3] = 7; |
- | |
| 346 | EE_Parameter.Kanalbelegung[K_POTI4] = 8; |
- | |
| 347 | EE_Parameter.GlobalConfig = 0;//CFG_HOEHENREGELUNG | /*CFG_HOEHEN_SCHALTER |*/ CFG_KOMPASS_AKTIV;//0x01; |
- | |
| 348 | EE_Parameter.Hoehe_MinGas = 30; |
- | |
| 349 | EE_Parameter.MaxHoehe = 251; // Wert : 0-32 251 -> Poti1 |
- | |
| 350 | EE_Parameter.Hoehe_P = 10; // Wert : 0-32 |
- | |
| 351 | EE_Parameter.Luftdruck_D = 50; // Wert : 0-250 |
- | |
| 352 | EE_Parameter.Hoehe_ACC_Wirkung = 50; // Wert : 0-250 |
- | |
| 353 | EE_Parameter.Hoehe_Verstaerkung = 2; // Wert : 0-50 |
- | |
| 354 | EE_Parameter.Stick_P = 4; //2 // Wert : 1-6 |
- | |
| 355 | EE_Parameter.Stick_D = 0; //8 // Wert : 0-64 |
- | |
| 356 | EE_Parameter.Gier_P = 16; // Wert : 1-20 |
- | |
| 357 | EE_Parameter.Gas_Min = 15; // Wert : 0-32 |
- | |
| 358 | EE_Parameter.Gas_Max = 250; // Wert : 33-250 |
- | |
| 359 | EE_Parameter.GyroAccFaktor = 26; // Wert : 1-64 |
- | |
| 360 | EE_Parameter.KompassWirkung = 128; // Wert : 0-250 |
- | |
| 361 | EE_Parameter.Gyro_P = 175; //80 // Wert : 0-250 |
- | |
| 362 | EE_Parameter.Gyro_I = 175; // Wert : 0-250 |
- | |
| 363 | EE_Parameter.UnterspannungsWarnung = 94; // Wert : 0-250 |
- | |
| 364 | EE_Parameter.NotGas = 35; // Wert : 0-250 // Gaswert bei Empangsverlust |
- | |
| 365 | EE_Parameter.NotGasZeit = 20; // Wert : 0-250 // Zeit bis auf NotGas geschaltet wird, wg. Rx-Problemen |
- | |
| 366 | EE_Parameter.UfoAusrichtung = 0; // X oder + Formation |
- | |
| 367 | EE_Parameter.I_Faktor = 0; |
- | |
| 368 | EE_Parameter.UserParam1 = 0; //zur freien Verwendung |
- | |
| 369 | EE_Parameter.UserParam2 = 0; //zur freien Verwendung |
- | |
| 370 | EE_Parameter.UserParam3 = 0; //zur freien Verwendung |
- | |
| 371 | EE_Parameter.UserParam4 = 0; //zur freien Verwendung |
- | |
| 372 | EE_Parameter.ServoNickControl = 100; // Wert : 0-250 // Stellung des Servos |
- | |
| 373 | EE_Parameter.ServoNickComp = 40; // Wert : 0-250 // Einfluss Gyro/Servo |
- | |
| 374 | EE_Parameter.ServoNickCompInvert = 0; // Wert : 0-250 // Richtung Einfluss Gyro/Servo |
- | |
| 375 | EE_Parameter.ServoNickMin = 50; // Wert : 0-250 // Anschlag |
- | |
| 376 | EE_Parameter.ServoNickMax = 150; // Wert : 0-250 // Anschlag |
- | |
| 377 | EE_Parameter.ServoNickRefresh = 5; |
- | |
| 378 | EE_Parameter.LoopGasLimit = 50; |
- | |
| 379 | EE_Parameter.LoopThreshold = 90; // Wert: 0-250 Schwelle für Stickausschlag |
- | |
| 380 | EE_Parameter.LoopConfig = 0; // Bitcodiert: 0x01=oben, 0x02=unten, 0x04=links, 0x08=rechts |
- | |
| 381 | memcpy(EE_Parameter.Name, "Kamera\0", 12); |
- | |
| Line 382... | Line 336... | ||
| 382 | } |
336 | |
| 383 | 337 | ||
| 384 | 338 | ||
| Line 400... | Line 354... | ||
| 400 | CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255); |
354 | CHK_POTI(Parameter_UserParam1,EE_Parameter.UserParam1,0,255); |
| 401 | CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255); |
355 | CHK_POTI(Parameter_UserParam2,EE_Parameter.UserParam2,0,255); |
| 402 | CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255); |
356 | CHK_POTI(Parameter_UserParam3,EE_Parameter.UserParam3,0,255); |
| 403 | CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255); |
357 | CHK_POTI(Parameter_UserParam4,EE_Parameter.UserParam4,0,255); |
| 404 | CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255); |
358 | CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255); |
| 405 | CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255); |
- | |
| 406 | CHK_POTI(Parameter_ServoNickControl,EE_Parameter.ServoNickControl,0,255); |
- | |
| 407 | CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255); |
359 | CHK_POTI(Parameter_LoopGasLimit,EE_Parameter.LoopGasLimit,0,255); |
| - | 360 | CHK_POTI(Parameter_AchsKopplung1, EE_Parameter.AchsKopplung1,0,255); |
|
| - | 361 | CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
|
| Line 408... | Line 362... | ||
| 408 | 362 | ||
| 409 | Ki = (float) Parameter_I_Faktor * 0.0001; |
363 | Ki = (float) Parameter_I_Faktor * 0.0001; |
| 410 | MAX_GAS = EE_Parameter.Gas_Max; |
364 | MAX_GAS = EE_Parameter.Gas_Max; |
| 411 | MIN_GAS = EE_Parameter.Gas_Min; |
365 | MIN_GAS = EE_Parameter.Gas_Min; |
| Line 419... | Line 373... | ||
| 419 | { |
373 | { |
| 420 | int motorwert,pd_ergebnis,h,tmp_int; |
374 | int motorwert,pd_ergebnis,h,tmp_int; |
| 421 | int GierMischanteil,GasMischanteil; |
375 | int GierMischanteil,GasMischanteil; |
| 422 | static long SummeNick=0,SummeRoll=0; |
376 | static long SummeNick=0,SummeRoll=0; |
| 423 | static long sollGier = 0,tmp_long,tmp_long2; |
377 | static long sollGier = 0,tmp_long,tmp_long2; |
| 424 | long IntegralFehlerNick = 0; |
378 | static long IntegralFehlerNick = 0; |
| 425 | long IntegralFehlerRoll = 0; |
379 | static long IntegralFehlerRoll = 0; |
| 426 | static unsigned int RcLostTimer; |
380 | static unsigned int RcLostTimer; |
| 427 | static unsigned char delay_neutral = 0; |
381 | static unsigned char delay_neutral = 0; |
| 428 | static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
382 | static unsigned char delay_einschalten = 0,delay_ausschalten = 0; |
| 429 | static unsigned int modell_fliegt = 0; |
383 | static unsigned int modell_fliegt = 0; |
| 430 | static int hoehenregler = 0; |
384 | static int hoehenregler = 0; |
| 431 | static char TimerWerteausgabe = 0; |
385 | static char TimerWerteausgabe = 0; |
| 432 | static char NeueKompassRichtungMerken = 0; |
386 | static char NeueKompassRichtungMerken = 0; |
| - | 387 | static long ausgleichNick, ausgleichRoll; |
|
| - | 388 | ||
| 433 | Mittelwert(); |
389 | Mittelwert(); |
| Line 434... | Line 390... | ||
| 434 | 390 | ||
| 435 | GRN_ON; |
- | |
| 436 | 391 | GRN_ON; |
|
| 437 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
392 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 438 | // Gaswert ermitteln |
393 | // Gaswert ermitteln |
| 439 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
394 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 440 | GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
395 | GasMischanteil = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
| Line 493... | Line 448... | ||
| 493 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
448 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 494 | // auf Nullwerte kalibrieren |
449 | // auf Nullwerte kalibrieren |
| 495 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
450 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 496 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte |
451 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] > 75) // Neutralwerte |
| 497 | { |
452 | { |
| - | 453 | unsigned char setting = 2; |
|
| 498 | if(++delay_neutral > 200) // nicht sofort |
454 | if(++delay_neutral > 200) // nicht sofort |
| 499 | { |
455 | { |
| 500 | GRN_OFF; |
456 | GRN_OFF; |
| 501 | MotorenEin = 0; |
457 | MotorenEin = 0; |
| 502 | delay_neutral = 0; |
458 | delay_neutral = 0; |
| Line 608... | Line 564... | ||
| 608 | { |
564 | { |
| 609 | StickRoll += 8 * EE_Parameter.Stick_P; |
565 | StickRoll += 8 * EE_Parameter.Stick_P; |
| 610 | if(IntegralRoll > 80000) StickRoll -= 16 * EE_Parameter.Stick_P; |
566 | if(IntegralRoll > 80000) StickRoll -= 16 * EE_Parameter.Stick_P; |
| 611 | } |
567 | } |
| 612 | } |
568 | } |
| 613 | - | ||
| 614 | } |
- | |
| 615 | - | ||
| 616 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
569 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 617 | // Looping? |
570 | // Looping? |
| 618 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
571 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 619 | if(((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) || |
572 | if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1; |
| 620 | ((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS)) |
- | |
| - | 573 | else |
|
| 621 | { |
574 | { |
| 622 | Looping_Roll = 1; |
575 | { |
| 623 | if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit; |
576 | if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Links = 0; |
| - | 577 | } |
|
| 624 | } |
578 | } |
| - | 579 | if((PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_RECHTS) Looping_Rechts = 1; |
|
| - | 580 | else |
|
| - | 581 | { |
|
| 625 | else Looping_Roll = 0; |
582 | if(Looping_Rechts) // Hysterese |
| - | 583 | { |
|
| - | 584 | if(PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Rechts = 0; |
|
| - | 585 | } |
|
| - | 586 | } |
|
| Line 626... | Line 587... | ||
| 626 | 587 | ||
| 627 | if(((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) || |
- | |
| - | 588 | if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_OBEN) Looping_Oben = 1; |
|
| 628 | ((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN)) |
589 | else |
| 629 | { |
590 | { |
| - | 591 | if(Looping_Oben) // Hysterese |
|
| - | 592 | { |
|
| - | 593 | if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < (EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese))) Looping_Oben = 0; |
|
| - | 594 | } |
|
| - | 595 | } |
|
| - | 596 | if((PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] < -EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_UNTEN) Looping_Unten = 1; |
|
| - | 597 | else |
|
| - | 598 | { |
|
| - | 599 | if(Looping_Unten) // Hysterese |
|
| - | 600 | { |
|
| - | 601 | if(PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] > -(EE_Parameter.LoopThreshold - EE_Parameter.LoopHysterese)) Looping_Unten = 0; |
|
| - | 602 | } |
|
| - | 603 | } |
|
| - | 604 | ||
| - | 605 | if(Looping_Links || Looping_Rechts) Looping_Roll = 1; else Looping_Roll = 0; |
|
| - | 606 | if(Looping_Oben || Looping_Unten) {Looping_Nick = 1; Looping_Roll = 0; Looping_Links = 0; Looping_Rechts = 0;} else Looping_Nick = 0; |
|
| - | 607 | } // Ende neue Funken-Werte |
|
| 630 | Looping_Nick = 1; |
608 | |
| - | 609 | if(Looping_Roll) beeptime = 100; |
|
| - | 610 | if(Looping_Roll || Looping_Nick) |
|
| 631 | Looping_Roll = 0; |
611 | { |
| 632 | if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit; |
612 | if(GasMischanteil > EE_Parameter.LoopGasLimit) GasMischanteil = EE_Parameter.LoopGasLimit; |
| 633 | } |
- | |
| Line 634... | Line 613... | ||
| 634 | else Looping_Nick = 0; |
613 | } |
| 635 | 614 | ||
| 636 | 615 | ||
| Line 645... | Line 624... | ||
| 645 | GyroFaktor = 0.1; |
624 | GyroFaktor = 0.1; |
| 646 | IntegralFaktor = 0.005; |
625 | IntegralFaktor = 0.005; |
| 647 | Looping_Roll = 0; |
626 | Looping_Roll = 0; |
| 648 | Looping_Nick = 0; |
627 | Looping_Nick = 0; |
| 649 | } |
628 | } |
| 650 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
| 651 | // Gyro-Drift kompensieren |
- | |
| 652 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
| 653 | #define DRIFT_FAKTOR 3 |
- | |
| 654 | if(ZaehlMessungen >= 1000 / DRIFT_FAKTOR) |
- | |
| 655 | { |
629 | |
| 656 | IntegralFehlerNick = IntegralNick2 - IntegralNick; |
- | |
| 657 | IntegralFehlerRoll = IntegralRoll2 - IntegralRoll; |
- | |
| 658 | ZaehlMessungen = 0; |
- | |
| 659 | if(IntegralFehlerNick > 800/DRIFT_FAKTOR) AdNeutralNick++; |
- | |
| 660 | if(IntegralFehlerNick < -800/DRIFT_FAKTOR) AdNeutralNick--; |
- | |
| 661 | if(IntegralFehlerRoll > 800/DRIFT_FAKTOR) AdNeutralRoll++; |
- | |
| 662 | if(IntegralFehlerRoll < -800/DRIFT_FAKTOR) AdNeutralRoll--; |
- | |
| 663 | // if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--; // macht nur mit Referenz (Kompass Sinn) |
- | |
| 664 | // if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++; // macht nur mit Referenz (Kompass Sinn) |
- | |
| 665 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
- | |
| 666 | Mess_IntegralNick2 = IntegralNick; |
- | |
| 667 | Mess_IntegralRoll2 = IntegralRoll; |
- | |
| 668 | Mess_Integral_Gier2 = Integral_Gier; |
- | |
| 669 | ANALOG_ON; // ADC einschalten |
- | |
| 670 | } |
630 | |
| 671 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
631 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 672 | // Integrale auf ACC-Signal abgleichen |
632 | // Integrale auf ACC-Signal abgleichen |
| 673 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
633 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| - | 634 | #define ABGLEICH_ANZAHL 256L |
|
| - | 635 | ||
| - | 636 | MittelIntegralNick += IntegralNick; // Für die Mittelwertbildung aufsummieren |
|
| - | 637 | MittelIntegralRoll += IntegralRoll; |
|
| - | 638 | MittelIntegralNick2 += IntegralNick2; |
|
| - | 639 | MittelIntegralRoll2 += IntegralRoll2; |
|
| - | 640 | ||
| 674 | if(!Looping_Nick && !Looping_Roll) |
641 | if(Looping_Nick || Looping_Roll) |
| 675 | { |
642 | { |
| 676 | tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
643 | IntegralAccNick = 0; |
| 677 | tmp_long /= 32; |
644 | IntegralAccRoll = 0; |
| 678 | tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
645 | MittelIntegralNick = 0; |
| 679 | tmp_long2 /= 32; |
646 | MittelIntegralRoll = 0; |
| 680 | - | ||
| 681 | #define AUSGLEICH 100 |
647 | MittelIntegralNick2 = 0; |
| 682 | if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
648 | MittelIntegralRoll2 = 0; |
| 683 | if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
649 | Mess_IntegralNick2 = Mess_IntegralNick; |
| 684 | if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
650 | Mess_IntegralRoll2 = Mess_IntegralRoll; |
| 685 | if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
651 | ZaehlMessungen = 0; |
| - | 652 | I_LageNick = 0; |
|
| - | 653 | I_LageRoll = 0; |
|
| 686 | } |
654 | } |
| - | 655 | ||
| - | 656 | if(ZaehlMessungen >= ABGLEICH_ANZAHL) |
|
| 687 | else |
657 | { |
| - | 658 | static int cnt = 0; |
|
| - | 659 | static char last_n_p,last_n_n,last_r_p,last_r_n; |
|
| - | 660 | static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt; |
|
| - | 661 | if(!Looping_Nick && !Looping_Roll) |
|
| 688 | { |
662 | { |
| - | 663 | MittelIntegralNick /= ABGLEICH_ANZAHL; |
|
| - | 664 | MittelIntegralRoll /= ABGLEICH_ANZAHL; |
|
| - | 665 | IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL; |
|
| - | 666 | IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL; |
|
| - | 667 | IntegralAccZ = IntegralAccZ / ABGLEICH_ANZAHL; |
|
| - | 668 | ||
| - | 669 | //DebugOut.Analog[6] = labs(MittelIntegralNick_Alt - MittelIntegralNick) / 10; |
|
| - | 670 | // Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 671 | IntegralFehlerNick = (long)(MittelIntegralNick - (long)IntegralAccNick); |
|
| - | 672 | ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich; |
|
| - | 673 | if(labs(MittelIntegralNick_Alt - MittelIntegralNick) < 5000) |
|
| - | 674 | { |
|
| - | 675 | tmp_long = IntegralFehlerNick / 700L; |
|
| - | 676 | if(tmp_long > 40) tmp_long = 40; |
|
| - | 677 | if(tmp_long <-40) tmp_long =-40; |
|
| - | 678 | I_LageNick = tmp_long; |
|
| - | 679 | } |
|
| - | 680 | else |
|
| - | 681 | { |
|
| - | 682 | I_LageNick /= 2; |
|
| - | 683 | } |
|
| - | 684 | // Roll ++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 685 | IntegralFehlerRoll = (long)(MittelIntegralRoll - (long)IntegralAccRoll); |
|
| - | 686 | ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich; |
|
| - | 687 | if(labs(MittelIntegralRoll_Alt - MittelIntegralRoll) < 5000) |
|
| - | 688 | { |
|
| - | 689 | tmp_long2 = IntegralFehlerRoll / 700L; |
|
| - | 690 | if(tmp_long2 > 40) tmp_long2 = 40; |
|
| - | 691 | if(tmp_long2 <-40) tmp_long2 =-40; |
|
| - | 692 | I_LageRoll = tmp_long2; |
|
| - | 693 | } |
|
| - | 694 | else |
|
| - | 695 | { |
|
| - | 696 | I_LageRoll /=2; |
|
| - | 697 | } |
|
| - | 698 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 699 | MittelIntegralNick_Alt = MittelIntegralNick; |
|
| - | 700 | MittelIntegralRoll_Alt = MittelIntegralRoll; |
|
| - | 701 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 702 | Mess_IntegralNick -= ausgleichNick; |
|
| - | 703 | Mess_IntegralRoll -= ausgleichRoll; |
|
| - | 704 | ||
| - | 705 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 706 | // Gyro-Drift ermitteln |
|
| - | 707 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 708 | MittelIntegralNick2 /= ABGLEICH_ANZAHL; |
|
| - | 709 | MittelIntegralRoll2 /= ABGLEICH_ANZAHL; |
|
| - | 710 | tmp_long = (long)(MittelIntegralNick2 - (long)IntegralAccNick); |
|
| - | 711 | tmp_long2 = (long)(MittelIntegralRoll2 - (long)IntegralAccRoll); |
|
| - | 712 | //DebugOut.Analog[25] = MittelIntegralRoll2 / 26; |
|
| - | 713 | ||
| - | 714 | IntegralFehlerNick = tmp_long; |
|
| - | 715 | IntegralFehlerRoll = tmp_long2; |
|
| - | 716 | Mess_IntegralNick2 -= IntegralFehlerNick; |
|
| - | 717 | Mess_IntegralRoll2 -= IntegralFehlerRoll; |
|
| - | 718 | ||
| - | 719 | if(PlatinenVersion != 10) |
|
| - | 720 | { |
|
| 689 | tmp_long = 0; |
721 | tmp_long /= 2; |
| 690 | tmp_long2 = 0; |
722 | tmp_long2 /= 2; |
| - | 723 | } |
|
| - | 724 | ||
| - | 725 | // IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2; |
|
| - | 726 | // IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2; |
|
| - | 727 | ||
| - | 728 | ||
| - | 729 | DebugOut.Analog[30] = I_LageRoll; |
|
| - | 730 | DebugOut.Analog[17] = IntegralAccNick / 26; |
|
| - | 731 | DebugOut.Analog[18] = IntegralAccRoll / 26; |
|
| - | 732 | DebugOut.Analog[19] = IntegralFehlerNick;// / 26; |
|
| - | 733 | DebugOut.Analog[20] = IntegralFehlerRoll;// / 26; |
|
| - | 734 | DebugOut.Analog[21] = MittelIntegralNick / 26; |
|
| - | 735 | DebugOut.Analog[22] = MittelIntegralRoll / 26; |
|
| - | 736 | DebugOut.Analog[28] = ausgleichNick; |
|
| - | 737 | DebugOut.Analog[29] = ausgleichRoll; |
|
| - | 738 | ||
| - | 739 | #define FEHLER_LIMIT (ABGLEICH_ANZAHL/2) |
|
| - | 740 | #define FEHLER_LIMIT2 4096 |
|
| - | 741 | ||
| - | 742 | // Nick +++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 743 | cnt = 1;// + labs(IntegralFehlerNick) / 4096; |
|
| - | 744 | if(IntegralFehlerNick > FEHLER_LIMIT2) |
|
| - | 745 | { |
|
| - | 746 | if(last_n_p) |
|
| - | 747 | { |
|
| - | 748 | cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2; |
|
| - | 749 | ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich; |
|
| - | 750 | if(ausgleichNick > 10000) ausgleichNick = 10000; |
|
| - | 751 | Mess_IntegralNick += ausgleichNick; |
|
| - | 752 | } |
|
| - | 753 | else last_n_p = 1; |
|
| - | 754 | } else last_n_p = 0; |
|
| - | 755 | if(IntegralFehlerNick < -FEHLER_LIMIT2) |
|
| - | 756 | { |
|
| - | 757 | if(last_n_n) |
|
| - | 758 | { |
|
| - | 759 | cnt += labs(IntegralFehlerNick) / FEHLER_LIMIT2; |
|
| - | 760 | ausgleichNick = IntegralFehlerNick / EE_Parameter.GyroAccAbgleich; |
|
| - | 761 | if(ausgleichNick < -10000) ausgleichNick = -10000; |
|
| - | 762 | Mess_IntegralNick += ausgleichNick; |
|
| - | 763 | } |
|
| - | 764 | else last_n_n = 1; |
|
| - | 765 | } else last_n_n = 0; |
|
| - | 766 | ||
| - | 767 | if(cnt > 2) cnt = 2; |
|
| - | 768 | if(IntegralFehlerNick > FEHLER_LIMIT) AdNeutralNick += cnt; |
|
| - | 769 | if(IntegralFehlerNick < -FEHLER_LIMIT) AdNeutralNick -= cnt; |
|
| - | 770 | // if(IntegralFehlerNick > FEHLER_LIMIT) { AdNeutralNick += (cnt + last_n_p/20); if(last_n_p++ == 4) {last_n_p = 4;}} else last_n_p = 0; |
|
| - | 771 | // if(IntegralFehlerNick < -FEHLER_LIMIT) { AdNeutralNick -= (cnt + last_n_n/20); if(last_n_n++ == 4) {last_n_n = 4;}} else last_n_n = 0; |
|
| - | 772 | // Roll +++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| - | 773 | cnt = 1;// + labs(IntegralFehlerRoll) / 4096; |
|
| - | 774 | if(IntegralFehlerRoll > FEHLER_LIMIT2) |
|
| - | 775 | { |
|
| - | 776 | if(last_r_p) |
|
| - | 777 | { |
|
| - | 778 | cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2; |
|
| - | 779 | ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich; |
|
| - | 780 | if(ausgleichRoll > 10000) ausgleichRoll = 10000; |
|
| - | 781 | Mess_IntegralRoll += ausgleichRoll; |
|
| - | 782 | } |
|
| - | 783 | else last_r_p = 1; |
|
| - | 784 | } else last_r_p = 0; |
|
| - | 785 | if(IntegralFehlerRoll < -FEHLER_LIMIT2) |
|
| - | 786 | { |
|
| - | 787 | if(last_r_n) |
|
| - | 788 | { |
|
| - | 789 | cnt += labs(IntegralFehlerRoll) / FEHLER_LIMIT2; |
|
| - | 790 | ausgleichRoll = IntegralFehlerRoll / EE_Parameter.GyroAccAbgleich; |
|
| - | 791 | if(ausgleichRoll < -10000) ausgleichRoll = -10000; |
|
| - | 792 | Mess_IntegralRoll += ausgleichRoll; |
|
| - | 793 | } |
|
| - | 794 | else last_r_n = 1; |
|
| - | 795 | } else last_r_n = 0; |
|
| - | 796 | ||
| - | 797 | if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt; |
|
| - | 798 | if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt; |
|
| - | 799 | if(cnt > 2) cnt = 2; |
|
| - | 800 | // if(IntegralFehlerRoll > FEHLER_LIMIT) { AdNeutralRoll += (cnt + last_r_p/20); if(last_r_p++ == 4) {beeptime =500;last_r_p = 4;}} else last_r_p = 0; |
|
| - | 801 | // if(IntegralFehlerRoll < -FEHLER_LIMIT) { AdNeutralRoll -= (cnt + last_r_n/20); if(last_r_n++ == 4) {beeptime = 60;last_r_n = 4;}} else last_r_n = 0; |
|
| - | 802 | ||
| - | 803 | // if(Mess_Integral_Gier2 > 500/DRIFT_FAKTOR) AdNeutralGier--; // macht nur mit Referenz (Kompass Sinn) |
|
| - | 804 | // if(Mess_Integral_Gier2 <-500/DRIFT_FAKTOR) AdNeutralGier++; // macht nur mit Referenz (Kompass Sinn) |
|
| - | 805 | cnt = 2; |
|
| - | 806 | //DebugOut.Analog[24] = 10*(AdNeutralRoll - 550); |
|
| 691 | } |
807 | } |
| - | 808 | IntegralAccNick = 0; |
|
| - | 809 | IntegralAccRoll = 0; |
|
| - | 810 | IntegralAccZ = 0; |
|
| 692 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
811 | MittelIntegralNick = 0; |
| - | 812 | MittelIntegralRoll = 0; |
|
| 693 | Mess_IntegralNick -= tmp_long; |
813 | MittelIntegralNick2 = 0; |
| 694 | Mess_IntegralRoll -= tmp_long2; |
814 | MittelIntegralRoll2 = 0; |
| - | 815 | ZaehlMessungen = 0; |
|
| - | 816 | } |
|
| - | 817 | ||
| 695 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
818 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 696 | // Gieren |
819 | // Gieren |
| 697 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
820 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 698 | if(abs(StickGier) > 20) // war 35 |
821 | if(abs(StickGier) > 20) // war 35 |
| 699 | { |
822 | { |
| 700 | if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
823 | if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
| 701 | } |
824 | } |
| 702 | tmp_int = EE_Parameter.Gier_P * (StickGier * abs(StickGier)) / 512; // expo y = ax + bx² |
825 | tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
| 703 | tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
826 | tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
| 704 | sollGier = tmp_int; |
827 | sollGier = tmp_int; |
| 705 | Mess_Integral_Gier -= tmp_int; |
828 | Mess_Integral_Gier -= tmp_int; |
| 706 | if(Mess_Integral_Gier > 25000) Mess_Integral_Gier = 25000; // begrenzen |
829 | if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen |
| 707 | if(Mess_Integral_Gier <-25000) Mess_Integral_Gier =-25000; |
830 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
| 708 | - | ||
| 709 | ANALOG_ON; // ADC einschalten |
- | |
| 710 | 831 | ||
| 711 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
832 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 712 | // Kompass |
833 | // Kompass |
| 713 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
834 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 714 | if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
835 | if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
| 715 | { |
836 | { |
| Line 725... | Line 846... | ||
| 725 | } |
846 | } |
| 726 | w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
847 | w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
| 727 | w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
848 | w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
| 728 | if(w > 0) |
849 | if(w > 0) |
| 729 | { |
850 | { |
| 730 | ANALOG_OFF; // ADC ausschalten, damit die Werte sich nicht während der Berechnung ändern |
- | |
| 731 | if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
851 | if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
| 732 | ANALOG_ON; // ADC einschalten |
- | |
| 733 | if(SignalSchlecht) SignalSchlecht--; |
852 | if(SignalSchlecht) SignalSchlecht--; |
| 734 | } |
853 | } |
| 735 | else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
854 | else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
| 736 | } |
855 | } |
| 737 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
856 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| Line 739... | Line 858... | ||
| 739 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
858 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 740 | // Debugwerte zuordnen |
859 | // Debugwerte zuordnen |
| 741 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
860 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 742 | if(!TimerWerteausgabe--) |
861 | if(!TimerWerteausgabe--) |
| 743 | { |
862 | { |
| 744 | TimerWerteausgabe = 49; |
863 | TimerWerteausgabe = 24; |
| 745 | DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
864 | DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
| 746 | DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
865 | DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
| 747 | DebugOut.Analog[2] = Mittelwert_AccNick; |
866 | DebugOut.Analog[2] = Mittelwert_AccNick; |
| 748 | DebugOut.Analog[3] = Mittelwert_AccRoll; |
867 | DebugOut.Analog[3] = Mittelwert_AccRoll; |
| 749 | DebugOut.Analog[4] = MesswertGier; |
868 | DebugOut.Analog[4] = MesswertGier; |
| 750 | DebugOut.Analog[5] = HoehenWert; |
869 | DebugOut.Analog[5] = HoehenWert; |
| 751 | DebugOut.Analog[6] = (Mess_Integral_Hoch / 512); |
870 | DebugOut.Analog[6] =(Mess_Integral_Hoch / 512); |
| 752 | DebugOut.Analog[8] = KompassValue; |
871 | DebugOut.Analog[8] = KompassValue; |
| 753 | DebugOut.Analog[9] = UBat; |
872 | DebugOut.Analog[9] = UBat; |
| 754 | DebugOut.Analog[10] = SenderOkay; |
873 | DebugOut.Analog[10] = SenderOkay; |
| - | 874 | DebugOut.Analog[16] = Mittelwert_AccHoch; |
|
| Line 755... | Line 875... | ||
| 755 | 875 | ||
| 756 | /* DebugOut.Analog[16] = motor_rx[0]; |
876 | /* DebugOut.Analog[16] = motor_rx[0]; |
| 757 | DebugOut.Analog[17] = motor_rx[1]; |
877 | DebugOut.Analog[17] = motor_rx[1]; |
| 758 | DebugOut.Analog[18] = motor_rx[2]; |
878 | DebugOut.Analog[18] = motor_rx[2]; |
| Line 859... | Line 979... | ||
| 859 | 979 | ||
| 860 | if(GasMischanteil < 20) GierMischanteil = 0; |
980 | if(GasMischanteil < 20) GierMischanteil = 0; |
| 861 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
981 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 862 | // Nick-Achse |
982 | // Nick-Achse |
| 863 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
| 864 | 983 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| 865 | DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick)); // Differenz bestimmen |
984 | DiffNick = Kp * (MesswertNick - (StickNick - GPS_Nick)); // Differenz bestimmen |
| 866 | SummeNick += DiffNick; // I-Anteil |
985 | SummeNick += DiffNick; // I-Anteil |
| 867 | if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1; |
986 | if(SummeNick > 0) SummeNick-= (abs(SummeNick)/256 + 1); else SummeNick += abs(SummeNick)/256 + 1; |
| 868 | if(SummeNick > 16000) SummeNick = 16000; |
987 | if(SummeNick > 16000) SummeNick = 16000; |