Rev 1330 | Rev 1334 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1330 | Rev 1332 | ||
|---|---|---|---|
| Line 1219... | Line 1219... | ||
| 1219 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1219 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1220 | if(UBat > BattLowVoltageWarning) GasMischanteil = (GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen |
1220 | if(UBat > BattLowVoltageWarning) GasMischanteil = (GasMischanteil * BattLowVoltageWarning) / UBat; // Gas auf das aktuelle Spannungvieveau beziehen |
| 1221 | GasMischanteil *= STICK_GAIN; |
1221 | GasMischanteil *= STICK_GAIN; |
| Line 1222... | Line 1222... | ||
| 1222 | 1222 | ||
| 1223 | // if height control is activated |
- | |
| 1224 | J4High; |
1223 | // if height control is activated |
| 1225 | 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 |
| 1226 | { |
1225 | { |
| 1227 | // #define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
- | |
| 1228 | #define HOOVER_GAS_AVERAGE 1024L // 1024 * 10ms = 10s averaging |
1226 | #define HOOVER_GAS_AVERAGE 4096L // 4096 * 2ms = 8.2s averaging |
| 1229 | #define HC_GAS_AVERAGE 2 // 2 * 10ms= 20ms averaging |
1227 | #define HC_GAS_AVERAGE 4 // 4 * 2ms= 8ms averaging |
| 1230 | #define OPA_OFFSET_STEP 10 |
1228 | #define OPA_OFFSET_STEP 10 |
| 1231 | int HCGas, HeightDeviation; |
1229 | int HCGas, HeightDeviation; |
| 1232 | static int HeightTrimming = 0; // rate for change of height setpoint |
1230 | static int HeightTrimming = 0; // rate for change of height setpoint |
| 1233 | static int FilterHCGas = 0; |
1231 | static int FilterHCGas = 0; |
| 1234 | static int StickGasHoover = 120, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023, LastHCGas = 0; |
1232 | static int StickGasHoover = 120, HooverGas = 0, HooverGasMin = 0, HooverGasMax = 1023; |
| 1235 | static unsigned long HooverGasFilter = 0; |
1233 | static unsigned long HooverGasFilter = 0; |
| 1236 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
1234 | static unsigned char delay = 100, BaroAtUpperLimit = 0, BaroAtLowerLimit = 0; |
| Line 1237... | Line 1235... | ||
| 1237 | int CosAttitude; // for projection of hoover gas |
1235 | int CosAttitude; // for projection of hoover gas |
| 1238 | 1236 | ||
| 1239 | // get the current hooverpoint |
1237 | // get the current hooverpoint |
| 1240 | if(LoadHandler == 1) |
1238 | // if(LoadHandler == 1) |
| 1241 | { |
1239 | { |
| Line 1242... | Line 1240... | ||
| 1242 | DebugOut.Analog[21] = HooverGas; |
1240 | DebugOut.Analog[21] = HooverGas; |
| Line 1251... | Line 1249... | ||
| 1251 | if(OCR0A < (255 - OPA_OFFSET_STEP)) |
1249 | if(OCR0A < (255 - OPA_OFFSET_STEP)) |
| 1252 | { |
1250 | { |
| 1253 | ExpandBaro -= 1; |
1251 | ExpandBaro -= 1; |
| 1254 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down |
1252 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // increase offset to shift ADC down |
| 1255 | beeptime = 300; |
1253 | beeptime = 300; |
| 1256 | BaroExpandActive = 50; |
1254 | BaroExpandActive = 250; |
| 1257 | } |
1255 | } |
| 1258 | else |
1256 | else |
| 1259 | { |
1257 | { |
| 1260 | BaroAtLowerLimit = 1; |
1258 | BaroAtLowerLimit = 1; |
| 1261 | } |
1259 | } |
| Line 1267... | Line 1265... | ||
| 1267 | if(OCR0A > OPA_OFFSET_STEP) |
1265 | if(OCR0A > OPA_OFFSET_STEP) |
| 1268 | { |
1266 | { |
| 1269 | ExpandBaro += 1; |
1267 | ExpandBaro += 1; |
| 1270 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up |
1268 | OCR0A = DruckOffsetSetting - OPA_OFFSET_STEP * ExpandBaro; // decrease offset to shift ADC up |
| 1271 | beeptime = 300; |
1269 | beeptime = 300; |
| 1272 | BaroExpandActive = 50; |
1270 | BaroExpandActive = 250; |
| 1273 | } |
1271 | } |
| 1274 | else |
1272 | else |
| 1275 | { |
1273 | { |
| 1276 | BaroAtUpperLimit = 1; |
1274 | BaroAtUpperLimit = 1; |
| 1277 | } |
1275 | } |
| Line 1313... | Line 1311... | ||
| 1313 | SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung; |
1311 | SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung; |
| 1314 | HoehenReglerAktiv = 1; |
1312 | HoehenReglerAktiv = 1; |
| 1315 | } |
1313 | } |
| Line 1316... | Line 1314... | ||
| 1316 | 1314 | ||
| 1317 | // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
- | |
| 1318 | J4Low; |
1315 | // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
| 1319 | tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg |
1316 | tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg |
| 1320 | tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
1317 | tmp_int2 = (int)(IntegralRoll/GIER_GRAD_FAKTOR); // roll angle in deg |
| 1321 | CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg |
- | |
| 1322 | DebugOut.Analog[16] = CosAttitude; |
1318 | CosAttitude = (int16_t)ihypot(tmp_int, tmp_int2); // phytagoras gives effective attitude angle in deg |
| 1323 | LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
1319 | LIMIT_MAX(CosAttitude, 60); // limit effective attitude angle |
| 1324 | CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
- | |
| 1325 | J4High; |
1320 | CosAttitude = c_cos_8192(CosAttitude); // cos of actual attitude |
| 1326 | if(HoehenReglerAktiv && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
1321 | if(HoehenReglerAktiv && !(MikroKopterFlags & FLAG_NOTLANDUNG)) |
| 1327 | { |
1322 | { |
| 1328 | #define HEIGHT_TRIM_UP 0x01 |
1323 | #define HEIGHT_TRIM_UP 0x01 |
| 1329 | #define HEIGHT_TRIM_DOWN 0x02 |
1324 | #define HEIGHT_TRIM_DOWN 0x02 |
| Line 1379... | Line 1374... | ||
| 1379 | } |
1374 | } |
| 1380 | } |
1375 | } |
| 1381 | // Trim height set point |
1376 | // Trim height set point |
| 1382 | if(abs(HeightTrimming) > 256) |
1377 | if(abs(HeightTrimming) > 256) |
| 1383 | { |
1378 | { |
| 1384 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(512 / 2); // move setpoint |
1379 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
| 1385 | HeightTrimming = 0; |
1380 | HeightTrimming = 0; |
| 1386 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 75; |
1381 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 75; |
| 1387 | //update hoover gas stick value when setpoint is shifted |
1382 | //update hoover gas stick value when setpoint is shifted |
| - | 1383 | if(!EE_Parameter.Hoehe_StickNeutralPoint) |
|
| - | 1384 | { |
|
| 1388 | // StickGasHoover = HooverGas/STICK_GAIN; // rescale back to stick value |
1385 | StickGasHoover = HooverGas/STICK_GAIN; //rescale back to stick value |
| 1389 | // StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
1386 | StickGasHoover = (StickGasHoover * UBat) / BattLowVoltageWarning; |
| 1390 | // if(StickGasHoover < 70) StickGasHoover = 70; |
1387 | if(StickGasHoover < 70) StickGasHoover = 70; |
| 1391 | // else if(StickGasHoover > 150) StickGasHoover = 150; |
1388 | else if(StickGasHoover > 150) StickGasHoover = 150; |
| - | 1389 | } |
|
| 1392 | } |
1390 | } |
| - | 1391 | ||
| 1393 | } //if MikroKopterFlags & MKFLAG_FLY |
1392 | } //if MikroKopterFlags & MKFLAG_FLY |
| 1394 | else |
1393 | else |
| 1395 | { |
1394 | { |
| 1396 | SollHoehe = HoehenWert - 400; |
1395 | SollHoehe = HoehenWert - 400; |
| 1397 | if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
1396 | if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHoover = EE_Parameter.Hoehe_StickNeutralPoint; |
| Line 1508... | Line 1507... | ||
| 1508 | HooverGasMin = 0; |
1507 | HooverGasMin = 0; |
| 1509 | HooverGasMax = 1023; |
1508 | HooverGasMax = 1023; |
| 1510 | } |
1509 | } |
| 1511 | } |
1510 | } |
| 1512 | } |
1511 | } |
| 1513 | LastHCGas = GasMischanteil; |
- | |
| 1514 | } |
1512 | } |
| 1515 | else |
- | |
| 1516 | { |
- | |
| 1517 | GasMischanteil = LastHCGas; |
- | |
| 1518 | } |
- | |
| 1519 | //DebugOut.Analog[26] = HooverGasMax; |
1513 | //DebugOut.Analog[26] = HooverGasMax; |
| 1520 | }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
1514 | }// EOF ParamSet.GlobalConfig & CFG_HEIGHT_CONTROL |
| 1521 | J4Low; |
- | |
| Line 1522... | Line 1515... | ||
| 1522 | 1515 | ||
| 1523 | // limit gas to parameter setting |
1516 | // limit gas to parameter setting |
| 1524 | LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN); |
1517 | LIMIT_MIN(GasMischanteil, (MIN_GAS + 10) * STICK_GAIN); |