Rev 1862 | Rev 1876 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1862 | Rev 1865 | ||
---|---|---|---|
Line 193... | Line 193... | ||
193 | DebugOut.Analog[14] = Motor[2].SetPoint; |
193 | DebugOut.Analog[14] = Motor[2].SetPoint; |
194 | DebugOut.Analog[15] = Motor[3].SetPoint; |
194 | DebugOut.Analog[15] = Motor[3].SetPoint; |
195 | DebugOut.Analog[20] = ServoNickValue; |
195 | DebugOut.Analog[20] = ServoNickValue; |
196 | DebugOut.Analog[22] = Capacity.ActualCurrent; |
196 | DebugOut.Analog[22] = Capacity.ActualCurrent; |
197 | DebugOut.Analog[23] = Capacity.UsedCapacity; |
197 | DebugOut.Analog[23] = Capacity.UsedCapacity; |
- | 198 | DebugOut.Analog[24] = SollHoehe/5; |
|
198 | // DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
199 | // DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
199 | // DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
200 | // DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
200 | DebugOut.Analog[27] = KompassSollWert; |
201 | DebugOut.Analog[27] = KompassSollWert; |
201 | DebugOut.Analog[29] = Capacity.MinOfMaxPWM; |
202 | DebugOut.Analog[29] = Capacity.MinOfMaxPWM; |
202 | DebugOut.Analog[30] = GPS_Nick; |
203 | DebugOut.Analog[30] = GPS_Nick; |
Line 852... | Line 853... | ||
852 | stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
853 | stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
Line 853... | Line 854... | ||
853 | 854 | ||
854 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
855 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
855 | // CareFree und freie Wahl der vorderen Richtung |
856 | // CareFree und freie Wahl der vorderen Richtung |
856 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
857 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
857 | if(CareFree) |
858 | if(CareFree) |
858 | { |
859 | { |
859 | signed int nick, roll; |
860 | signed int nick, roll; |
860 | nick = stick_nick / 4; |
861 | nick = stick_nick / 4; |
861 | roll = stick_roll / 4; |
862 | roll = stick_roll / 4; |
862 | StickNick = ((FromNC_Rotate_C * nick) + (FromNC_Rotate_S * roll)) / (32 / 4); |
863 | StickNick = ((FromNC_Rotate_C * nick) + (FromNC_Rotate_S * roll)) / (32 / 4); |
863 | StickRoll = ((FromNC_Rotate_C * roll) - (FromNC_Rotate_S * nick)) / (32 / 4); |
864 | StickRoll = ((FromNC_Rotate_C * roll) - (FromNC_Rotate_S * nick)) / (32 / 4); |
864 | } |
865 | } |
865 | else |
866 | else |
866 | { |
867 | { |
867 | FromNC_Rotate_C = sintab[EE_Parameter.OrientationAngle + 6]; |
868 | FromNC_Rotate_C = sintab[EE_Parameter.OrientationAngle + 6]; |
868 | FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle]; |
869 | FromNC_Rotate_S = sintab[EE_Parameter.OrientationAngle]; |
869 | StickNick = ((FromNC_Rotate_C * stick_nick) + (FromNC_Rotate_S * stick_roll)) / 8; |
870 | StickNick = ((FromNC_Rotate_C * stick_nick) + (FromNC_Rotate_S * stick_roll)) / 8; |
870 | StickRoll = ((FromNC_Rotate_C * stick_roll) - (FromNC_Rotate_S * stick_nick)) / 8; |
871 | StickRoll = ((FromNC_Rotate_C * stick_roll) - (FromNC_Rotate_S * stick_nick)) / 8; |
Line 871... | Line 872... | ||
871 | } |
872 | } |
872 | 873 | ||
873 | StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
874 | StickGier = -PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]; |
Line 1239... | Line 1240... | ||
1239 | if(v > w) w = v; // grösste Neigung ermitteln |
1240 | if(v > w) w = v; // grösste Neigung ermitteln |
1240 | korrektur = w / 4 + 1; |
1241 | korrektur = w / 4 + 1; |
1241 | // Kompassfehlerwert bestimmen |
1242 | // Kompassfehlerwert bestimmen |
1242 | fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
1243 | fehler = ((540 + KompassValue - (ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
1243 | // GIER_GRAD_FAKTOR ist ca. 1200 |
1244 | // GIER_GRAD_FAKTOR ist ca. 1200 |
1244 | 1245 | ||
1245 | // Kompasswert einloggen |
1246 | // Kompasswert einloggen |
1246 | if(KompassSignalSchlecht) KompassSignalSchlecht--; |
1247 | if(KompassSignalSchlecht) KompassSignalSchlecht--; |
1247 | else |
1248 | else |
1248 | if(w < 25) |
1249 | if(w < 25) |
1249 | { |
1250 | { |
Line 1262... | Line 1263... | ||
1262 | 1263 | ||
1263 | // MK Gieren |
1264 | // MK Gieren |
1264 | if(!NeueKompassRichtungMerken) |
1265 | if(!NeueKompassRichtungMerken) |
1265 | { |
1266 | { |
1266 | r = ((540 + (KompassSollWert - ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
- | |
1267 | DebugOut.Analog[19] = r; |
1267 | r = ((540 + (KompassSollWert - ErsatzKompass/GIER_GRAD_FAKTOR)) % 360) - 180; |
1268 | v = r * (Parameter_KompassWirkung/2); // nach Kompass ausrichten |
- | |
1269 | // if(abs(r) > 20) v *= 2; // über 20° -> doppelt so schnell |
1268 | v = r * (Parameter_KompassWirkung/2); // nach Kompass ausrichten |
1270 | CompassGierSetpoint = v / 16; |
1269 | CompassGierSetpoint = v / 16; |
1271 | } |
1270 | } |
1272 | else CompassGierSetpoint = 0; |
- | |
1273 | - | ||
1274 | /* |
- | |
1275 | DebugOut.Analog[16] = fehler; |
- | |
1276 | DebugOut.Analog[17] = korrektur; |
- | |
1277 | */ |
- | |
1278 | DebugOut.Analog[25] = CompassGierSetpoint; |
- | |
1279 | DebugOut.Analog[18] = KompassFusion; |
- | |
1280 | - | ||
1281 | /* |
- | |
1282 | w = (w * Parameter_KompassWirkung) / 32; // auf die Wirkung normieren |
- | |
1283 | w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
- | |
1284 | if(w >= 0) |
- | |
1285 | { |
- | |
1286 | if(!KompassSignalSchlecht) |
- | |
1287 | { |
- | |
1288 | v = 64 + ((MaxStickNick + MaxStickRoll)) / 8; |
- | |
1289 | DebugOut.Analog[16] = korrektur; |
- | |
1290 | r = ((540 + (ErsatzKompass/GIER_GRAD_FAKTOR) - KompassSollWert) % 360) - 180; |
- | |
1291 | v = (r * w) / v; // nach Kompass ausrichten |
- | |
1292 | w = 3 * Parameter_KompassWirkung; |
- | |
1293 | if(v > w) v = w; // Begrenzen |
- | |
1294 | else |
- | |
1295 | if(v < -w) v = -w; |
- | |
1296 | // Mess_Integral_Gier += v; |
- | |
1297 | } |
- | |
1298 | if(KompassSignalSchlecht) KompassSignalSchlecht--; |
- | |
1299 | } |
- | |
1300 | else KompassSignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
- | |
1301 | */ |
1271 | else CompassGierSetpoint = 0; |
1302 | } // CalculateCompassTimer |
1272 | } // CalculateCompassTimer |
1303 | } |
1273 | } |
1304 | else CompassGierSetpoint = 0; |
1274 | else CompassGierSetpoint = 0; |
1305 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1275 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 1468... | Line 1438... | ||
1468 | SollHoehe = HoehenWert; // update setpoint to current heigth |
1438 | SollHoehe = HoehenWert; // update setpoint to current heigth |
1469 | } |
1439 | } |
1470 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP; |
1440 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP; |
1471 | HeightTrimming += abs(StickGas - (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD)); |
1441 | HeightTrimming += abs(StickGas - (StickGasHover + HEIGHT_CONTROL_STICKTHRESHOLD)); |
1472 | VarioCharacter = '+'; |
1442 | VarioCharacter = '+'; |
- | 1443 | WaypointTrimming = 0; |
|
1473 | } // gas stick is below hoover point |
1444 | } // gas stick is below hoover point |
1474 | else if(StickGas < (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit ) |
1445 | else if(StickGas < (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD) && !BaroAtLowerLimit ) |
1475 | { |
1446 | { |
1476 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP) |
1447 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP) |
1477 | { |
1448 | { |
Line 1479... | Line 1450... | ||
1479 | SollHoehe = HoehenWert; // update setpoint to current heigth |
1450 | SollHoehe = HoehenWert; // update setpoint to current heigth |
1480 | } |
1451 | } |
1481 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN; |
1452 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN; |
1482 | HeightTrimming -= abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
1453 | HeightTrimming -= abs(StickGas - (StickGasHover - HEIGHT_CONTROL_STICKTHRESHOLD)); |
1483 | VarioCharacter = '-'; |
1454 | VarioCharacter = '-'; |
- | 1455 | WaypointTrimming = 0; |
|
1484 | } |
1456 | } |
1485 | else // Gas Stick in Hover Range |
1457 | else // Gas Stick in Hover Range |
1486 | { |
1458 | { |
1487 | VarioCharacter = '='; |
1459 | VarioCharacter = '='; |
1488 | if(FromNC_AltitudeSpeed && FromNC_AltitudeSetpoint > SollHoehe) // von NC gesteuert -> Steigen |
1460 | if(FromNC_AltitudeSpeed && FromNC_AltitudeSetpoint > SollHoehe) // von NC gesteuert -> Steigen |
1489 | { |
1461 | { |
1490 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP; |
1462 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_UP; |
1491 | HeightTrimming += FromNC_AltitudeSpeed; |
1463 | HeightTrimming += FromNC_AltitudeSpeed; |
1492 | WaypointTrimming = 10; |
1464 | WaypointTrimming = 10; |
1493 | VarioCharacter = '^'; |
1465 | VarioCharacter = '^'; |
- | 1466 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_DOWN) // changed from sinking to rising |
|
- | 1467 | { |
|
- | 1468 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_DOWN; |
|
- | 1469 | SollHoehe = HoehenWert; // update setpoint to current heigth |
|
- | 1470 | } |
|
1494 | } |
1471 | } |
1495 | else |
1472 | else |
1496 | if(FromNC_AltitudeSpeed && FromNC_AltitudeSetpoint < SollHoehe) // von NC gesteuert -> sinken |
1473 | if(FromNC_AltitudeSpeed && FromNC_AltitudeSetpoint < SollHoehe) // von NC gesteuert -> sinken |
1497 | { |
1474 | { |
1498 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN; |
1475 | FC_StatusFlags |= FC_STATUS_VARIO_TRIM_DOWN; |
1499 | HeightTrimming -= FromNC_AltitudeSpeed; |
1476 | HeightTrimming -= FromNC_AltitudeSpeed; |
1500 | WaypointTrimming = -10; |
1477 | WaypointTrimming = -10; |
1501 | VarioCharacter = 'v'; |
1478 | VarioCharacter = 'v'; |
- | 1479 | if(FC_StatusFlags & FC_STATUS_VARIO_TRIM_UP) // changed from rising to sinking |
|
- | 1480 | { |
|
- | 1481 | FC_StatusFlags &= ~FC_STATUS_VARIO_TRIM_UP; |
|
- | 1482 | SollHoehe = HoehenWert; // update setpoint to current heigth |
|
- | 1483 | } |
|
1502 | } |
1484 | } |
1503 | else |
1485 | else |
1504 | if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) |
1486 | if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) |
1505 | { |
1487 | { |
1506 | if(!WaypointTrimming) SollHoehe = HoehenWert; // update setpoint to current height |
1488 | if(!WaypointTrimming) SollHoehe = HoehenWert; // update setpoint to current height |
Line 1666... | Line 1648... | ||
1666 | { // reduce the time constant of averaging by factor of 2 to get much faster a stable value |
1648 | { // reduce the time constant of averaging by factor of 2 to get much faster a stable value |
1667 | HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/4L); |
1649 | HoverGasFilter -= HoverGasFilter/(HOVER_GAS_AVERAGE/4L); |
1668 | HoverGasFilter += 4L * tmp_long2; |
1650 | HoverGasFilter += 4L * tmp_long2; |
1669 | } |
1651 | } |
1670 | else //later |
1652 | else //later |
1671 | if(abs(VarioMeter) < 100) // only on small vertical speed |
1653 | if(abs(VarioMeter) < 100 && abs(HoehenWert - SollHoehe) < 256) // only on small vertical speed & difference is small (only descending) |
1672 | { |
1654 | { |
1673 | HoverGasFilter -= HoverGasFilter/HOVER_GAS_AVERAGE; |
1655 | HoverGasFilter -= HoverGasFilter/HOVER_GAS_AVERAGE; |
1674 | HoverGasFilter += tmp_long2; |
1656 | HoverGasFilter += tmp_long2; |
1675 | } |
1657 | } |
1676 | HoverGas = (int16_t)(HoverGasFilter/HOVER_GAS_AVERAGE); |
1658 | HoverGas = (int16_t)(HoverGasFilter/HOVER_GAS_AVERAGE); |