Rev 2427 | Rev 2439 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 2427 | Rev 2437 | ||
|---|---|---|---|
| Line 88... | Line 88... | ||
| 88 | unsigned char MAX_GAS,MIN_GAS; |
88 | unsigned char MAX_GAS,MIN_GAS; |
| 89 | unsigned char HoehenReglerAktiv = 0; |
89 | unsigned char HoehenReglerAktiv = 0; |
| 90 | unsigned char TrichterFlug = 0; |
90 | unsigned char TrichterFlug = 0; |
| 91 | long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L; |
91 | long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L; |
| 92 | long ErsatzKompass; |
92 | long ErsatzKompass; |
| 93 | int ErsatzKompassInGrad; // Kompasswert in Grad |
93 | int ErsatzKompassInGrad, CompassCorrected; // Kompasswert in Grad |
| 94 | int GierGyroFehler = 0; |
94 | int GierGyroFehler = 0; |
| 95 | char GyroFaktor,GyroFaktorGier; |
95 | char GyroFaktor,GyroFaktorGier; |
| 96 | char IntegralFaktor,IntegralFaktorGier; |
96 | char IntegralFaktor,IntegralFaktorGier; |
| 97 | int DiffNick,DiffRoll; |
97 | int DiffNick,DiffRoll; |
| 98 | unsigned int StickGasHover = 127; |
98 | unsigned int StickGasHover = 127; |
| Line 215... | Line 215... | ||
| 215 | DebugOut.Analog[20] = ServoNickValue; |
215 | DebugOut.Analog[20] = ServoNickValue; |
| 216 | DebugOut.Analog[21] = HoverGas; |
216 | DebugOut.Analog[21] = HoverGas; |
| 217 | DebugOut.Analog[22] = Capacity.ActualCurrent; |
217 | DebugOut.Analog[22] = Capacity.ActualCurrent; |
| 218 | DebugOut.Analog[23] = Capacity.UsedCapacity; |
218 | DebugOut.Analog[23] = Capacity.UsedCapacity; |
| 219 | DebugOut.Analog[24] = SollHoehe/10; |
219 | DebugOut.Analog[24] = SollHoehe/10; |
| 220 | DebugOut.Analog[27] = KompassSollWert; |
220 | // DebugOut.Analog[27] = KompassSollWert; |
| 221 | DebugOut.Analog[29] = Capacity.MinOfMaxPWM; |
221 | DebugOut.Analog[29] = Capacity.MinOfMaxPWM; |
| 222 | DebugOut.Analog[30] = GPS_Nick; |
222 | DebugOut.Analog[30] = GPS_Nick; |
| 223 | DebugOut.Analog[31] = GPS_Roll; |
223 | DebugOut.Analog[31] = GPS_Roll; |
| 224 | if(VersionInfo.HardwareError[0] || VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 1; else DebugOut.Status[1] &= 0xfe; |
224 | if(VersionInfo.HardwareError[0] || VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 1; else DebugOut.Status[1] &= 0xfe; |
| 225 | //DebugOut.Analog[16] = MinBlTemperture; |
225 | //DebugOut.Analog[16] = MinBlTemperture; |
| Line 258... | Line 258... | ||
| 258 | //############################################################################ |
258 | //############################################################################ |
| 259 | // Messwerte beim Ermitteln der Nullage |
259 | // Messwerte beim Ermitteln der Nullage |
| 260 | void CalibrierMittelwert(void) |
260 | void CalibrierMittelwert(void) |
| 261 | //############################################################################ |
261 | //############################################################################ |
| 262 | { |
262 | { |
| 263 | unsigned char i; |
- | |
| 264 | // if(PlatinenVersion == 13) SucheGyroOffset(); |
263 | // if(PlatinenVersion == 13) SucheGyroOffset(); |
| 265 | // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern |
264 | // ADC auschalten, damit die Werte sich nicht während der Berechnung ändern |
| 266 | ANALOG_OFF; |
265 | ANALOG_OFF; |
| 267 | MesswertNick = AdWertNick; |
266 | MesswertNick = AdWertNick; |
| 268 | MesswertRoll = AdWertRoll; |
267 | MesswertRoll = AdWertRoll; |
| 269 | MesswertGier = AdWertGier; |
268 | MesswertGier = AdWertGier; |
| 270 | Mittelwert_AccNick = ACC_AMPLIFY * AdWertAccNick; |
269 | Mittelwert_AccNick = ACC_AMPLIFY * AdWertAccNick; |
| 271 | Mittelwert_AccRoll = ACC_AMPLIFY * AdWertAccRoll; |
270 | Mittelwert_AccRoll = ACC_AMPLIFY * AdWertAccRoll; |
| 272 | // ADC einschalten |
271 | // ADC einschalten |
| 273 | ANALOG_ON; |
272 | ANALOG_ON; |
| 274 | for(i=0;i<8;i++) |
- | |
| 275 | { |
- | |
| 276 | int tmp; |
- | |
| 277 | tmp = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1 + i]] + 127; |
- | |
| 278 | LIMIT_MIN_MAX(tmp, 0, 255); |
- | |
| 279 | if(Poti[i] > tmp) Poti[i]--; else if(Poti[i] < tmp) Poti[i]++; |
- | |
| 280 | } |
- | |
| 281 | Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
273 | Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
| 282 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
274 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
| 283 | } |
275 | } |
| Line 284... | Line 276... | ||
| 284 | 276 | ||
| Line 308... | Line 300... | ||
| 308 | 300 | ||
| Line 309... | Line 301... | ||
| 309 | ExpandBaro = 0; |
301 | ExpandBaro = 0; |
| 310 | 302 | ||
| 311 | CalibrierMittelwert(); |
- | |
| 312 | Delay_ms_Mess(100); |
303 | CalibrierMittelwert(); |
| Line -... | Line 304... | ||
| - | 304 | Delay_ms_Mess(100); |
|
| - | 305 | CalibrierMittelwert(); |
|
| - | 306 | ||
| - | 307 | DebugOut.Analog[16] = 0; |
|
| 313 | 308 | DebugOut.Analog[17] = 0; |
|
| 314 | CalibrierMittelwert(); |
309 | |
| 315 | 310 | ||
| 316 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
311 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)) // Höhenregelung aktiviert? |
| Line 387... | Line 382... | ||
| 387 | LED_Init(); |
382 | LED_Init(); |
| 388 | if(AdjustmentMode != 0) FC_StatusFlags |= FC_STATUS_CALIBRATE; |
383 | if(AdjustmentMode != 0) FC_StatusFlags |= FC_STATUS_CALIBRATE; |
| 389 | FromNaviCtrl_Value.Kalman_K = -1; |
384 | FromNaviCtrl_Value.Kalman_K = -1; |
| 390 | FromNaviCtrl_Value.Kalman_MaxDrift = 0; |
385 | FromNaviCtrl_Value.Kalman_MaxDrift = 0; |
| 391 | FromNaviCtrl_Value.Kalman_MaxFusion = 32; |
386 | FromNaviCtrl_Value.Kalman_MaxFusion = 32; |
| 392 | for(i=0;i<8;i++) |
- | |
| 393 | { |
- | |
| 394 | Poti[i] = PPM_in[EE_Parameter.Kanalbelegung[K_POTI1 + i]] + 127; |
- | |
| 395 | } |
- | |
| 396 | SenderOkay = 100; |
387 | SenderOkay = 100; |
| Line 397... | Line 388... | ||
| 397 | 388 | ||
| 398 | if(ServoActive) DDRD |=0x80; // enable J7 -> Servo signal |
389 | if(ServoActive) DDRD |=0x80; // enable J7 -> Servo signal |
| 399 | else |
390 | else |
| Line 1380... | Line 1371... | ||
| 1380 | if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
1371 | if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
| 1381 | } |
1372 | } |
| Line 1382... | Line 1373... | ||
| 1382 | 1373 | ||
| 1383 | Mess_IntegralNick -= tmp_long; |
1374 | Mess_IntegralNick -= tmp_long; |
| - | 1375 | Mess_IntegralRoll -= tmp_long2; |
|
| - | 1376 | ||
| - | 1377 | DebugOut.Analog[16] += tmp_long; |
|
| - | 1378 | DebugOut.Analog[17] += tmp_long2; |
|
| 1384 | Mess_IntegralRoll -= tmp_long2; |
1379 | |
| 1385 | } |
1380 | } |
| 1386 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1381 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1387 | if(ZaehlMessungen >= ABGLEICH_ANZAHL) |
1382 | if(ZaehlMessungen >= ABGLEICH_ANZAHL) |
| 1388 | { |
1383 | { |
| Line 2271... | Line 2266... | ||
| 2271 | Motor[i].SetPoint = 0; |
2266 | Motor[i].SetPoint = 0; |
| 2272 | Motor[i].SetPointLowerBits = 0; |
2267 | Motor[i].SetPointLowerBits = 0; |
| 2273 | } |
2268 | } |
| 2274 | } |
2269 | } |
| 2275 | #ifdef REDUNDANT_FC_MASTER |
2270 | #ifdef REDUNDANT_FC_MASTER |
| 2276 | if(Parameter_UserParam6 > 200 && RedundanceBlOperation) Motor[0].SetPoint = 0; |
2271 | if(Parameter_UserParam6 > 230) Motor[0].SetPoint = 0; |
| 2277 | #endif |
2272 | #endif |
| 2278 | } |
2273 | } |
| 2279 | //DebugOut.Analog[16] |
2274 | //DebugOut.Analog[16] |