Rev 1322 | Rev 1330 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1322 | Rev 1328 | ||
---|---|---|---|
Line 154... | Line 154... | ||
154 | long GIER_GRAD_FAKTOR = 1291; |
154 | long GIER_GRAD_FAKTOR = 1291; |
155 | signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
155 | signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
156 | unsigned char RequiredMotors = 4; |
156 | unsigned char RequiredMotors = 4; |
157 | unsigned char Motor[MAX_MOTORS]; |
157 | unsigned char Motor[MAX_MOTORS]; |
158 | signed int tmp_motorwert[MAX_MOTORS]; |
158 | signed int tmp_motorwert[MAX_MOTORS]; |
- | 159 | unsigned char LoadHandler = 0; |
|
159 | #define LIMIT_MIN(value, min) {if(value < min) value = min;} |
160 | #define LIMIT_MIN(value, min) {if(value < min) value = min;} |
160 | #define LIMIT_MAX(value, max) {if(value > max) value = max;} |
161 | #define LIMIT_MAX(value, max) {if(value > max) value = max;} |
161 | #define LIMIT_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
162 | #define LIMIT_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
Line 162... | Line 163... | ||
162 | 163 | ||
Line 553... | Line 554... | ||
553 | static char TimerWerteausgabe = 0; |
554 | static char TimerWerteausgabe = 0; |
554 | static char NeueKompassRichtungMerken = 0; |
555 | static char NeueKompassRichtungMerken = 0; |
555 | static long ausgleichNick, ausgleichRoll; |
556 | static long ausgleichNick, ausgleichRoll; |
556 | int IntegralNickMalFaktor,IntegralRollMalFaktor; |
557 | int IntegralNickMalFaktor,IntegralRollMalFaktor; |
557 | unsigned char i; |
558 | unsigned char i; |
- | 559 | if(--LoadHandler == 0) LoadHandler = 5; // verteilt die Prozessorlast |
|
558 | Mittelwert(); |
560 | Mittelwert(); |
559 | - | ||
560 | GRN_ON; |
561 | GRN_ON; |
561 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
562 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
562 | // Gaswert ermitteln |
563 | // Gaswert ermitteln |
563 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
564 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
564 | GasMischanteil = StickGas; |
565 | GasMischanteil = StickGas; |
Line 722... | Line 723... | ||
722 | } |
723 | } |
723 | } |
724 | } |
724 | else delay_ausschalten = 0; |
725 | else delay_ausschalten = 0; |
725 | } |
726 | } |
726 | } |
727 | } |
727 | - | ||
728 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
728 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
729 | // neue Werte von der Funke |
729 | // neue Werte von der Funke |
730 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
730 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 731... | Line 731... | ||
731 | 731 | ||
Line 967... | Line 967... | ||
967 | IntegralFehlerNick = tmp_long; |
967 | IntegralFehlerNick = tmp_long; |
968 | IntegralFehlerRoll = tmp_long2; |
968 | IntegralFehlerRoll = tmp_long2; |
969 | Mess_IntegralNick2 -= IntegralFehlerNick; |
969 | Mess_IntegralNick2 -= IntegralFehlerNick; |
970 | Mess_IntegralRoll2 -= IntegralFehlerRoll; |
970 | Mess_IntegralRoll2 -= IntegralFehlerRoll; |
Line 971... | Line -... | ||
971 | - | ||
972 | // IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2; |
- | |
973 | // IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2; |
971 | |
974 | if(EE_Parameter.Driftkomp) |
972 | if(EE_Parameter.Driftkomp) |
975 | { |
973 | { |
976 | if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; } |
974 | if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; } |
977 | if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; } |
975 | if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; } |
978 | } |
- | |
979 | //DebugOut.Analog[22] = MittelIntegralRoll / 26; |
- | |
980 | //DebugOut.Analog[24] = GierGyroFehler; |
976 | } |
Line 981... | Line -... | ||
981 | GierGyroFehler = 0; |
- | |
982 | - | ||
983 | - | ||
984 | /*DebugOut.Analog[17] = IntegralAccNick / 26; |
- | |
985 | DebugOut.Analog[18] = IntegralAccRoll / 26; |
- | |
986 | DebugOut.Analog[19] = IntegralFehlerNick;// / 26; |
- | |
987 | DebugOut.Analog[20] = IntegralFehlerRoll;// / 26; |
- | |
988 | */ |
- | |
989 | //DebugOut.Analog[21] = MittelIntegralNick / 26; |
- | |
990 | //MittelIntegralRoll = MittelIntegralRoll; |
- | |
991 | //DebugOut.Analog[28] = ausgleichNick; |
- | |
992 | /* |
- | |
993 | DebugOut.Analog[29] = ausgleichRoll; |
- | |
994 | DebugOut.Analog[30] = LageKorrekturRoll * 10; |
977 | GierGyroFehler = 0; |
995 | */ |
978 | |
996 | 979 | ||
997 | #define FEHLER_LIMIT (ABGLEICH_ANZAHL / 2) |
980 | #define FEHLER_LIMIT (ABGLEICH_ANZAHL / 2) |
998 | #define FEHLER_LIMIT1 (ABGLEICH_ANZAHL * 2) //4 |
981 | #define FEHLER_LIMIT1 (ABGLEICH_ANZAHL * 2) //4 |
Line 1188... | Line 1171... | ||
1188 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
1171 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
1189 | //DebugOut.Analog[16] = Mittelwert_AccHoch; |
1172 | //DebugOut.Analog[16] = Mittelwert_AccHoch; |
1190 | //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
1173 | //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
1191 | DebugOut.Analog[18] = VarioMeter; |
1174 | DebugOut.Analog[18] = VarioMeter; |
1192 | DebugOut.Analog[19] = WinkelOut.CalcState; |
1175 | DebugOut.Analog[19] = WinkelOut.CalcState; |
1193 | DebugOut.Analog[20] = ServoValue; |
1176 | DebugOut.Analog[20] = ServoNickValue; |
1194 | // DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
1177 | // DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
1195 | // DebugOut.Analog[24] = MesswertNick/2; |
1178 | // DebugOut.Analog[24] = MesswertNick/2; |
1196 | // DebugOut.Analog[25] = MesswertRoll/2; |
1179 | // DebugOut.Analog[25] = MesswertRoll/2; |
1197 | // DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
1180 | // DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
1198 | // DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion; |
1181 | // DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion; |
1199 | // DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K; |
1182 | // DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K; |
1200 | //DebugOut.Analog[28] = I2CError; |
1183 | //DebugOut.Analog[28] = I2CError; |
1201 | DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
1184 | DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
1202 | DebugOut.Analog[30] = GPS_Nick; |
1185 | DebugOut.Analog[30] = GPS_Nick; |
1203 | DebugOut.Analog[31] = GPS_Roll; |
1186 | DebugOut.Analog[31] = GPS_Roll; |
1204 | - | ||
1205 | } |
1187 | } |
Line 1206... | Line 1188... | ||
1206 | 1188 | ||
1207 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1189 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1208 | // Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen |
1190 | // Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen |
Line 1235... | Line 1217... | ||
1235 | // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht |
1217 | // Die Höhenregelung schwächt lediglich das Gas ab, erhöht es allerdings nicht |
1236 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1218 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1237 | if(UBat > BattLowVoltageWarning) GasMischanteil = (GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen |
1219 | if(UBat > BattLowVoltageWarning) GasMischanteil = (GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen |
1238 | GasMischanteil *= STICK_GAIN; |
1220 | GasMischanteil *= STICK_GAIN; |
Line 1239... | Line -... | ||
1239 | - | ||
1240 | DebugOut.Analog[16] = BaroExpandActive; |
1221 | |
- | 1222 | // if height control is activated |
|
1241 | // if height control is activated |
1223 | J4High; |
1242 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick)) // Höhenregelung |
1224 | if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && !(Looping_Roll || Looping_Nick)) // Höhenregelung |
1243 | { |
1225 | { |
- | 1226 | // #define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
|
1244 | #define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
1227 | #define HOOVER_GAS_AVERAGE 1024L // 1024 * 10ms = 10s averaging |
1245 | #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8 ms averaging |
1228 | #define HC_GAS_AVERAGE 2 // 2 * 10ms= 20ms averaging |
1246 | #define OPA_OFFSET_STEP 10 |
1229 | #define OPA_OFFSET_STEP 10 |
1247 | int HCGas, HeightDeviation; |
1230 | int HCGas, HeightDeviation; |
1248 | static int HeightTrimming = 0; // rate for change of height setpoint |
1231 | static int HeightTrimming = 0; // rate for change of height setpoint |
1249 | static int FilterHCGas = 0; |
1232 | static int FilterHCGas = 0; |
1250 | static int StickGasHoover = 120, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023; |
1233 | static int StickGasHoover = 120, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023, LastHCGas = 0; |
1251 | static unsigned long HooverGasFilter = 0; |
1234 | static unsigned long HooverGasFilter = 0; |
1252 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
1235 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
1253 | int CosAttitude; // for projection of hoover gas |
1236 | int CosAttitude; // for projection of hoover gas |
1254 | long tmp_long3; |
1237 | long tmp_long3; |
1255 | // const unsigned char GAS_TAB[31] = {128,128,128,129,129,130,131,132,133,135,136,138,140,142,145,148,151,154,158,162,167,172,178,184,191,199,208,218,229,241,255}; |
1238 | // const unsigned char GAS_TAB[31] = {128,128,128,129,129,130,131,132,133,135,136,138,140,142,145,148,151,154,158,162,167,172,178,184,191,199,208,218,229,241,255}; |
- | 1239 | // get the current hooverpoint |
|
- | 1240 | if(LoadHandler == 1) |
|
1256 | // get the current hooverpoint |
1241 | { |
1257 | DebugOut.Analog[21] = HooverGas; |
1242 | DebugOut.Analog[21] = HooverGas; |
Line 1258... | Line -... | ||
1258 | DebugOut.Analog[18] = VarioMeter; |
- | |
1259 | - | ||
1260 | // if height control is activated by an rc channel |
- | |
1261 | if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
- | |
1262 | { // check if parameter is less than activation threshold |
- | |
1263 | if(Parameter_MaxHoehe < 50) // for 3 or 2-state switch height control is disabled in lowest position |
- | |
1264 | { //height control not active |
- | |
1265 | if(!delay--) |
- | |
1266 | { |
- | |
1267 | HoehenReglerAktiv = 0; // disable height control |
- | |
1268 | SollHoehe = HoehenWert; // update SetPoint with current reading |
- | |
1269 | delay = 1; |
- | |
1270 | } |
- | |
1271 | } |
- | |
1272 | else |
- | |
1273 | { //height control is activated |
- | |
1274 | HoehenReglerAktiv = 1; // enable height control |
- | |
1275 | delay = 200; |
1243 | DebugOut.Analog[18] = VarioMeter; |
1276 | } |
1244 | |
1277 | // Expand the measurement |
1245 | // Expand the measurement |
1278 | // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs |
1246 | // measurement of air pressure close to upper limit and no overflow in correction of the new OCR0A value occurs |
1279 | if(!BaroExpandActive) |
1247 | if(!BaroExpandActive) |
Line 1283... | Line 1251... | ||
1283 | if(OCR0A < (255 - OPA_OFFSET_STEP)) |
1251 | if(OCR0A < (255 - OPA_OFFSET_STEP)) |
1284 | { |
1252 | { |
1285 | ExpandBaro -= 1; |
1253 | ExpandBaro -= 1; |
1286 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down |
1254 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down |
1287 | beeptime = 300; |
1255 | beeptime = 300; |
1288 | delay = 250; |
- | |
1289 | BaroExpandActive = 250; |
1256 | BaroExpandActive = 50; |
1290 | } |
1257 | } |
1291 | else |
1258 | else |
1292 | { |
1259 | { |
1293 | BaroAtLowerLimit = 1; |
1260 | BaroAtLowerLimit = 1; |
1294 | } |
1261 | } |
Line 1300... | Line 1267... | ||
1300 | if(OCR0A > OPA_OFFSET_STEP) |
1267 | if(OCR0A > OPA_OFFSET_STEP) |
1301 | { |
1268 | { |
1302 | ExpandBaro += 1; |
1269 | ExpandBaro += 1; |
1303 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up |
1270 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up |
1304 | beeptime = 300; |
1271 | beeptime = 300; |
1305 | delay = 250; |
- | |
1306 | BaroExpandActive = 250; |
1272 | BaroExpandActive = 50; |
1307 | } |
1273 | } |
1308 | else |
1274 | else |
1309 | { |
1275 | { |
1310 | BaroAtUpperLimit = 1; |
1276 | BaroAtUpperLimit = 1; |
1311 | } |
1277 | } |
Line 1321... | Line 1287... | ||
1321 | // now clear the D-values |
1287 | // now clear the D-values |
1322 | SummenHoehe = HoehenWert * SM_FILTER; |
1288 | SummenHoehe = HoehenWert * SM_FILTER; |
1323 | VarioMeter = 0; |
1289 | VarioMeter = 0; |
1324 | BaroExpandActive--; |
1290 | BaroExpandActive--; |
1325 | } |
1291 | } |
- | 1292 | ||
- | 1293 | // if height control is activated by an rc channel |
|
- | 1294 | if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
|
- | 1295 | { // check if parameter is less than activation threshold |
|
- | 1296 | if(Parameter_MaxHoehe < 50) // for 3 or 2-state switch height control is disabled in lowest position |
|
- | 1297 | { //height control not active |
|
- | 1298 | if(!delay--) |
|
- | 1299 | { |
|
- | 1300 | HoehenReglerAktiv = 0; // disable height control |
|
- | 1301 | SollHoehe = HoehenWert; // update SetPoint with current reading |
|
- | 1302 | delay = 1; |
|
- | 1303 | } |
|
- | 1304 | } |
|
- | 1305 | else |
|
- | 1306 | { //height control is activated |
|
- | 1307 | HoehenReglerAktiv = 1; // enable height control |
|
- | 1308 | delay = 40; |
|
- | 1309 | } |
|
1326 | } |
1310 | } |
1327 | else // no switchable height control |
1311 | else // no switchable height control |
1328 | { |
1312 | { |
1329 | SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung; |
1313 | SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung; |
1330 | HoehenReglerAktiv = 1; |
1314 | HoehenReglerAktiv = 1; |
1331 | } |
1315 | } |
Line 1332... | Line 1316... | ||
1332 | 1316 | ||
- | 1317 | // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
|
1333 | // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
1318 | J4Low; |
1334 | tmp_long = IntegralNick/GIER_GRAD_FAKTOR; // nick angle in deg |
1319 | tmp_long = IntegralNick/GIER_GRAD_FAKTOR; // nick angle in deg |
1335 | tmp_long *= tmp_long; |
1320 | tmp_long *= tmp_long; |
1336 | tmp_long2 = IntegralRoll/GIER_GRAD_FAKTOR; // roll angle in deg |
1321 | tmp_long2 = IntegralRoll/GIER_GRAD_FAKTOR; // roll angle in deg |
1337 | tmp_long2 *= tmp_long2; |
- | |
1338 | 1322 | tmp_long2 *= tmp_long2; |
|
- | 1323 | CosAttitude = (int16_t)c_sqrt(tmp_long + tmp_long2); // phytagoras gives effective attitude angle in deg |
|
1339 | CosAttitude = (int16_t)c_sqrt(tmp_long + tmp_long2); // phytagoras gives effective attitude angle in deg |
1324 | DebugOut.Analog[16] = CosAttitude; |
1340 | LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
1325 | LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
1341 | CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
1326 | CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
1342 | 1327 | J4High; |
|
1343 | if(HoehenReglerAktiv && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
1328 | if(HoehenReglerAktiv && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
1344 | { |
1329 | { |
1345 | #define HEIGHT_TRIM_UP 0x01 |
1330 | #define HEIGHT_TRIM_UP 0x01 |
1346 | #define HEIGHT_TRIM_DOWN 0x02 |
1331 | #define HEIGHT_TRIM_DOWN 0x02 |
Line 1394... | Line 1379... | ||
1394 | SollHoehe = HoehenWert; // update setpoint to current height |
1379 | SollHoehe = HoehenWert; // update setpoint to current height |
1395 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500; |
1380 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500; |
1396 | } |
1381 | } |
1397 | } |
1382 | } |
1398 | // Trim height set point |
1383 | // Trim height set point |
1399 | if(abs(HeightTrimming) > 512) |
1384 | if(abs(HeightTrimming) > 256) |
1400 | { |
1385 | { |
1401 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
1386 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(512 / 2); // move setpoint |
1402 | HeightTrimming = 0; |
1387 | HeightTrimming = 0; |
1403 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 75; |
1388 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 75; |
1404 | //update hoover gas stick value when setpoint is shifted |
1389 | //update hoover gas stick value when setpoint is shifted |
1405 | // StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
1390 | // StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
1406 | // StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
1391 | // StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
Line 1409... | Line 1394... | ||
1409 | } |
1394 | } |
1410 | } //if MikroKopterFlags & MKFLAG_FLY |
1395 | } //if MikroKopterFlags & MKFLAG_FLY |
1411 | else |
1396 | else |
1412 | { |
1397 | { |
1413 | SollHoehe = HoehenWert - 400; |
1398 | SollHoehe = HoehenWert - 400; |
- | 1399 | if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
|
1414 | StickGasHoover = 120; |
1400 | else StickGasHoover = 120; |
1415 | } |
1401 | } |
1416 | HCGas = HooverGas; // take hoover gas (neutral point) |
1402 | HCGas = HooverGas; // take hoover gas (neutral point) |
1417 | } |
1403 | } |
Line 1418... | Line 1404... | ||
1418 | 1404 | ||
Line 1432... | Line 1418... | ||
1432 | HCGas -= tmp_int; |
1418 | HCGas -= tmp_int; |
1433 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1419 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1434 | tmp_int = ((Mess_Integral_Hoch / 128) * (long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
1420 | tmp_int = ((Mess_Integral_Hoch / 128) * (long) Parameter_Hoehe_ACC_Wirkung) / (128 / STICK_GAIN); |
1435 | LIMIT_MIN_MAX(tmp_int, -127, 255); |
1421 | LIMIT_MIN_MAX(tmp_int, -127, 255); |
1436 | HCGas -= tmp_int; |
1422 | HCGas -= tmp_int; |
1437 | 1423 | ||
1438 | // limit deviation from hoover point within the target region |
1424 | // limit deviation from hoover point within the target region |
1439 | if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero |
1425 | if( (abs(HeightDeviation) < 150) && (!HeightTrimming) && (HooverGas > 0)) // height setpoint is not changed and hoover gas not zero |
1440 | { |
1426 | { |
1441 | LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point |
1427 | LIMIT_MIN_MAX(HCGas, HooverGasMin, HooverGasMax); // limit gas around the hoover point |
1442 | } |
1428 | } |
Line 1474... | Line 1460... | ||
1474 | StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
1460 | StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
1475 | } |
1461 | } |
1476 | else StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
1462 | else StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
1477 | if(StickGasHoover < 70) StickGasHoover = 70; |
1463 | if(StickGasHoover < 70) StickGasHoover = 70; |
1478 | else if(StickGasHoover > 150) StickGasHoover = 150; |
1464 | else if(StickGasHoover > 150) StickGasHoover = 150; |
- | 1465 | FilterHCGas = GasMischanteil; |
|
1479 | } |
1466 | } |
Line 1480... | Line 1467... | ||
1480 | 1467 | ||
1481 | // Hoover gas estimation by averaging gas control output on small z-velocities |
1468 | // Hoover gas estimation by averaging gas control output on small z-velocities |
1482 | // this is done only if height contol option is selected in global config and aircraft is flying |
1469 | // this is done only if height contol option is selected in global config and aircraft is flying |
Line 1523... | Line 1510... | ||
1523 | HooverGasMin = 0; |
1510 | HooverGasMin = 0; |
1524 | HooverGasMax = 1023; |
1511 | HooverGasMax = 1023; |
1525 | } |
1512 | } |
1526 | } |
1513 | } |
1527 | } |
1514 | } |
- | 1515 | LastHCGas = GasMischanteil; |
|
- | 1516 | } |
|
- | 1517 | else |
|
- | 1518 | { |
|
1528 | DebugOut.Analog[25] = HooverGas; |
1519 | GasMischanteil = LastHCGas; |
- | 1520 | } |
|
1529 | //DebugOut.Analog[26] = HooverGasMax; |
1521 | //DebugOut.Analog[26] = HooverGasMax; |
1530 | }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
1522 | }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
- | 1523 | J4Low; |
|
Line 1531... | Line 1524... | ||
1531 | 1524 | ||
1532 | // limit gas to parameter setting |
1525 | // limit gas to parameter setting |
1533 | LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN); |
1526 | LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN); |