/// <summary>
/// Initialise Values and Config Settings
///
/// </summary>
public WayPointUAV()
{
initialised = true;
// Verbinde mit Hardware
ConnectPeripherals();
// theta = The pitch in degrees relative to a plane(mathematical) normal to the earth radius line at the point the plane(aircraft) is at
// phi = The roll of the aircraft in degrees
// psi = The true heading of the aircraft in degrees
// Direkt Steuern in dem Wertänderungen soffort weitergeben werden
UAVStructure performanceData = new UAVStructure("PerformanceData", "");
performanceData.values.Add(new UAVParameter("MaxBankLeft", 10));
performanceData.values.Add(new UAVParameter("MaxBankRight", 10));
performanceData.values.Add(new UAVParameter("MaxDescendAngle", 10));
performanceData.values.Add(new UAVParameter("MaxClimbAngle", 10));
uavData.Add(performanceData);
BaseNavigation navi = new BaseNavigation("navi", (UAVCommons.UAVStructure)uavData["GPS"], performanceData);
WaypointMissionControl waypointMissionControl = new WaypointMissionControl(navi, "Mission Control");
pilot = new BaseAutoPilot("AP", navi, (UAVStructure)uavData["GPS"], (UAVStructure)uavData["AHRS"], performanceData);
navi.PassingWayPoint += new BaseNavigation.PassingWayPointHandler(navi_PassingWayPoint);
navi.ap = ((BaseAutoPilot)pilot);
BaseFlightStabilizer flightstabi = new FlightControlCommons.components.BaseFlightStabilizer("Flugstabilisierung", (UAVStructure)uavData["AHRS"], pilot, uavData["Querruder"], uavData["Seitenruder"], uavData["Höhenruder"]);
uavData.Add(waypointMissionControl);
uavData.Add(navi);
uavData.Add(flightstabi);
uavData.Add(pilot);
uavData.ValueChanged += new MonitoredDictionary<string, UAVParameter>.ValueChangedHandler(uavData_ValueChanged);
this.DataArrived += new DataArrivedHandler(RcUAV_DataArrived);
}
public BaseAutoPilot(string name, BaseNavigation navigation, UAVCommons.UAVStructure mygps, UAVCommons.UAVStructure myahrs, UAVCommons.UAVStructure performancedata)
: base(name)
{
this.navigation = navigation;
this.gps = mygps;
this.ahrs = myahrs;
this.PerformanceData = performancedata;
//Pid Config
// this.values.Add(new UAVCommons.UAVParameter("phi_P_gain", 1));
this.values.Add(new UAVCommons.UAVParameter("psi_P_gain", 1));
this.values.Add(new UAVCommons.UAVParameter("theta_P_gain", 1));
// this.values.Add(new UAVCommons.UAVParameter("phi_I_gain", 1));
this.values.Add(new UAVCommons.UAVParameter("psi_I_gain", 1));
this.values.Add(new UAVCommons.UAVParameter("theta_I_gain", 1));
// this.values.Add(new UAVCommons.UAVParameter("phi_D_gain", 1));
this.values.Add(new UAVCommons.UAVParameter("psi_D_gain", 1));
this.values.Add(new UAVCommons.UAVParameter("theta_D_gain", 1));
Console.WriteLine("AutoPilot: Set Performance DataPoints");
//TargetVector from Navigation
this.values.Add(new UAVCommons.UAVParameter("bankangle", 1, PerformanceData["MaxBankRight"].Value, PerformanceData["MaxBankLeft"].Value));
this.values.Add(new UAVCommons.UAVParameter("theta_out", 1, PerformanceData["MaxDescendAngle"].Value, PerformanceData["MaxClimbAngle"].Value));
PerformanceData.ValueChanged += new UAVStructure.ValueChangedHandler(PerformanceData_ValueChanged);
UAVParameter center = new UAVParameter("Center", 0, 180, -180);
//Achtung muss noch unterscheiden zwischen drehung nach link oder rechts ahrs.values["psi"], values["bankangle"],navigation.values["psi"]
// vs speed muss noch berechnet werden und als eingangswert statt altitutude ned so wichtig
Console.WriteLine("AutoPilot: Init Bank PID");
psiPid = new PIDLibrary.ParameterPID(values["psi_P_gain"], values["psi_I_gain"], values["psi_D_gain"], center, values["bankangle"], new PIDLibrary.GetDouble(psiPV), new PIDLibrary.GetDouble(psiSP), new PIDLibrary.SetDouble(Setpsi_out));
Console.WriteLine("AutoPilot: Init ClimbDescend PID");
if (((UAVStructure)gps).values.ContainsKey("lbGGAAltitude"))
{
Console.WriteLine("Autopilot: Gps Altitude found");
}
thetaPid = new PIDLibrary.ParameterPID(values["theta_P_gain"], values["theta_I_gain"], values["theta_D_gain"], gps["lbGGAAltitude"], values["theta_out"], navigation.values["TargetAltitude"]);
// PID QuerachsenPID = new PID(1, 1, 1, 90, -90, 2000, 1000, new GetDouble(GetPV), new GetDouble(GetSP),new SetDouble( SetResult));
Console.WriteLine("AutoPilot: PID Enable");
// phiPid.Enable();
}
public BaseAutoPilot(string name, BaseNavigation navigation, UAVCommons.UAVStructure mygps, UAVCommons.UAVStructure myahrs, UAVCommons.UAVStructure performancedata)
: base(name)
{
this.navigation = navigation;
this.gps = mygps;
this.ahrs = myahrs;
this.PerformanceData = performancedata;
//Pid Config
// this.values.Add(new UAVCommons.UAVParameter("phi_P_gain", 1));
this.values.Add(new UAVCommons.UAVParameter("psi_P_gain", 1));
this.values.Add(new UAVCommons.UAVParameter("theta_P_gain", 1));
// this.values.Add(new UAVCommons.UAVParameter("phi_I_gain", 1));
this.values.Add(new UAVCommons.UAVParameter("psi_I_gain", 1));
this.values.Add(new UAVCommons.UAVParameter("theta_I_gain", 1));
// this.values.Add(new UAVCommons.UAVParameter("phi_D_gain", 1));
this.values.Add(new UAVCommons.UAVParameter("psi_D_gain", 1));
this.values.Add(new UAVCommons.UAVParameter("theta_D_gain", 1));
Console.WriteLine("AutoPilot: Set Performance DataPoints");
//TargetVector from Navigation
this.values.Add(new UAVCommons.UAVParameter("bankangle", 1, PerformanceData["MaxBankRight"].Value, PerformanceData["MaxBankLeft"].Value));
this.values.Add(new UAVCommons.UAVParameter("theta_out", 1, PerformanceData["MaxDescendAngle"].Value, PerformanceData["MaxClimbAngle"].Value));
PerformanceData.ValueChanged += new UAVStructure.ValueChangedHandler(PerformanceData_ValueChanged);
UAVParameter center = new UAVParameter("Center", 0, 180, -180);
//Achtung muss noch unterscheiden zwischen drehung nach link oder rechts ahrs.values["psi"], values["bankangle"],navigation.values["psi"]
// vs speed muss noch berechnet werden und als eingangswert statt altitutude ned so wichtig
Console.WriteLine("AutoPilot: Init Bank PID");
psiPid = new PIDLibrary.ParameterPID(values["psi_P_gain"], values["psi_I_gain"], values["psi_D_gain"], center, values["bankangle"], new PIDLibrary.GetDouble(psiPV), new PIDLibrary.GetDouble(psiSP), new PIDLibrary.SetDouble(Setpsi_out));
Console.WriteLine("AutoPilot: Init ClimbDescend PID");
if (((UAVStructure)gps).values.ContainsKey("lbGGAAltitude")) Console.WriteLine("Autopilot: Gps Altitude found");
thetaPid = new PIDLibrary.ParameterPID(values["theta_P_gain"], values["theta_I_gain"], values["theta_D_gain"], gps["lbGGAAltitude"], values["theta_out"], navigation.values["TargetAltitude"]);
// PID QuerachsenPID = new PID(1, 1, 1, 90, -90, 2000, 1000, new GetDouble(GetPV), new GetDouble(GetSP),new SetDouble( SetResult));
Console.WriteLine("AutoPilot: PID Enable");
// phiPid.Enable();
}
public BaseMissionControl(BaseNavigation navi, string name)
: base(name, null)
{
this.navigation = navi;
}
public WaypointMissionControl(BaseNavigation navi, string name)
: base(navi, name)
{
this.values.Add(new UAVCommons.UAVParameter("WaypointIndex", 1)); // Active Waypoint
navi.PassingWayPoint += new BaseNavigation.PassingWayPointHandler(navi_PassingWayPoint);
}
public BaseMissionControl(BaseNavigation navi, string name) : base(name, null)
{
this.navigation = navi;
}
public WaypointMissionControl(BaseNavigation navi, string name)
: base(navi, name)
{
this.values.Add(new UAVCommons.UAVParameter("WaypointIndex", 1)); // Active Waypoint
navi.PassingWayPoint += new BaseNavigation.PassingWayPointHandler(navi_PassingWayPoint);
}
