Rev 1350 | Rev 1374 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1350 | Rev 1352 | ||
---|---|---|---|
Line 56... | Line 56... | ||
56 | #include "main.h" |
56 | #include "main.h" |
57 | #include "eeprom.c" |
57 | #include "eeprom.c" |
58 | #include "mymath.h" |
58 | #include "mymath.h" |
59 | #include "isqrt.h" |
59 | #include "isqrt.h" |
Line -... | Line 60... | ||
- | 60 | ||
60 | 61 | unsigned char h,m,s; |
|
61 | unsigned char h,m,s,BaroExpandActive = 0; |
62 | unsigned int BaroExpandActive = 0; |
62 | volatile unsigned int I2CTimeout = 100; |
63 | volatile unsigned int I2CTimeout = 100; |
63 | int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll; |
64 | int MesswertNick,MesswertRoll,MesswertGier,MesswertGierBias, RohMesswertNick,RohMesswertRoll; |
64 | int TrimNick, TrimRoll; |
65 | int TrimNick, TrimRoll; |
65 | int AdNeutralGierBias; |
66 | int AdNeutralGierBias; |
Line 1120... | Line 1121... | ||
1120 | if(!KompassSignalSchlecht && w < 25) |
1121 | if(!KompassSignalSchlecht && w < 25) |
1121 | { |
1122 | { |
1122 | GierGyroFehler += fehler; |
1123 | GierGyroFehler += fehler; |
1123 | if(NeueKompassRichtungMerken) |
1124 | if(NeueKompassRichtungMerken) |
1124 | { |
1125 | { |
1125 | beeptime = 200; |
1126 | // beeptime = 200; |
1126 | // KompassStartwert = KompassValue; |
1127 | // KompassStartwert = KompassValue; |
1127 | ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
1128 | ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
1128 | KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR); |
1129 | KompassStartwert = (ErsatzKompass/GIER_GRAD_FAKTOR); |
1129 | NeueKompassRichtungMerken = 0; |
1130 | NeueKompassRichtungMerken = 0; |
1130 | } |
1131 | } |
Line 1180... | Line 1181... | ||
1180 | // DebugOut.Analog[25] = MesswertRoll/2; |
1181 | // DebugOut.Analog[25] = MesswertRoll/2; |
1181 | // DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
1182 | // DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
1182 | // DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion; |
1183 | // DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion; |
1183 | // DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K; |
1184 | // DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K; |
1184 | //DebugOut.Analog[28] = I2CError; |
1185 | //DebugOut.Analog[28] = I2CError; |
1185 | DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
1186 | // DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
1186 | DebugOut.Analog[30] = GPS_Nick; |
1187 | DebugOut.Analog[30] = GPS_Nick; |
1187 | DebugOut.Analog[31] = GPS_Roll; |
1188 | DebugOut.Analog[31] = GPS_Roll; |
1188 | } |
1189 | } |
Line 1189... | Line 1190... | ||
1189 | 1190 | ||
Line 1249... | Line 1250... | ||
1249 | if(OCR0A < (255 - OPA_OFFSET_STEP)) |
1250 | if(OCR0A < (255 - OPA_OFFSET_STEP)) |
1250 | { |
1251 | { |
1251 | ExpandBaro -= 1; |
1252 | ExpandBaro -= 1; |
1252 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down |
1253 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down |
1253 | beeptime = 300; |
1254 | beeptime = 300; |
1254 | BaroExpandActive = 250; |
1255 | BaroExpandActive = 350; |
1255 | } |
1256 | } |
1256 | else |
1257 | else |
1257 | { |
1258 | { |
1258 | BaroAtLowerLimit = 1; |
1259 | BaroAtLowerLimit = 1; |
1259 | } |
1260 | } |
Line 1265... | Line 1266... | ||
1265 | if(OCR0A > OPA_OFFSET_STEP) |
1266 | if(OCR0A > OPA_OFFSET_STEP) |
1266 | { |
1267 | { |
1267 | ExpandBaro += 1; |
1268 | ExpandBaro += 1; |
1268 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up |
1269 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up |
1269 | beeptime = 300; |
1270 | beeptime = 300; |
1270 | BaroExpandActive = 250; |
1271 | BaroExpandActive = 350; |
1271 | } |
1272 | } |
1272 | else |
1273 | else |
1273 | { |
1274 | { |
1274 | BaroAtUpperLimit = 1; |
1275 | BaroAtUpperLimit = 1; |
1275 | } |
1276 | } |
Line 1363... | Line 1364... | ||
1363 | HeightTrimmingFlag |= HEIGHT_TRIM_DOWN; |
1364 | HeightTrimmingFlag |= HEIGHT_TRIM_DOWN; |
1364 | HeightTrimming -= abs(StickGas - (StickGasHoover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
1365 | HeightTrimming -= abs(StickGas - (StickGasHoover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
1365 | } |
1366 | } |
1366 | else // Gas Stick in Hoover Range |
1367 | else // Gas Stick in Hoover Range |
1367 | { |
1368 | { |
1368 | if(HeightTrimmingFlag & (HEIGHT_TRIM_UP|HEIGHT_TRIM_DOWN)) |
1369 | if(HeightTrimmingFlag & (HEIGHT_TRIM_UP | HEIGHT_TRIM_DOWN)) |
1369 | { |
1370 | { |
1370 | HeightTrimmingFlag &= ~(HEIGHT_TRIM_UP|HEIGHT_TRIM_DOWN); |
1371 | HeightTrimmingFlag &= ~(HEIGHT_TRIM_UP | HEIGHT_TRIM_DOWN); |
1371 | HeightTrimming = 0; |
1372 | HeightTrimming = 0; |
1372 | SollHoehe = HoehenWert; // update setpoint to current height |
1373 | SollHoehe = HoehenWert; // update setpoint to current height |
1373 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500; |
1374 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500; |
1374 | } |
1375 | } |
1375 | } |
1376 | } |
Line 1386... | Line 1387... | ||
1386 | StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
1387 | StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
1387 | if(StickGasHoover < 70) StickGasHoover = 70; |
1388 | if(StickGasHoover < 70) StickGasHoover = 70; |
1388 | else if(StickGasHoover > 150) StickGasHoover = 150; |
1389 | else if(StickGasHoover > 150) StickGasHoover = 150; |
1389 | } |
1390 | } |
1390 | } |
1391 | } |
1391 | - | ||
- | 1392 | if(BaroExpandActive) SollHoehe = HoehenWert; // update setpoint to current altitude if Expanding is active |
|
1392 | } //if MikroKopterFlags & MKFLAG_FLY |
1393 | } //if MikroKopterFlags & MKFLAG_FLY |
1393 | else |
1394 | else |
1394 | { |
1395 | { |
1395 | SollHoehe = HoehenWert - 400; |
1396 | SollHoehe = HoehenWert - 400; |
1396 | if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
1397 | if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |