public BaseFlightStabilizer(string name, UAVStructure myAhrs, UAVStructure ap, UAVParameter phi_out, UAVParameter psi_out, UAVParameter theta_out)
: base(name, null)
{
this.ahrs = myAhrs;
this.ap = ap;
this.phi_out = phi_out;
this.psi_out = psi_out;
this.theta_out = theta_out;
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));
phiPid = new PIDLibrary.ParameterPID(values["phi_P_gain"], values["phi_I_gain"], values["phi_D_gain"], this.ahrs["phi"], phi_out, ap.values["bankangle"]);
// psiPid = new PIDLibrary.ParameterPID(values["psi_P_gain"], values["psi_I_gain"], values["psi_D_gain"], this.ahrs["psi"], psi_out, ap.values["psi_out"]);//should only correct drift
thetaPid = new PIDLibrary.ParameterPID(values["theta_P_gain"], values["theta_I_gain"], values["theta_D_gain"], this.ahrs["theta"], theta_out, ap.values["theta_out"]);
// PID QuerachsenPID = new PID(1, 1, 1, 90, -90, 2000, 1000, new GetDouble(GetPV), new GetDouble(GetSP),new SetDouble( SetResult));
phiPid.Enable();
// psiPid.Enable();
thetaPid.Enable();
}
public BaseFlightStabilizer(string name, UAVStructure myAhrs, UAVStructure ap, UAVParameter phi_out,UAVParameter psi_out,UAVParameter theta_out)
: base(name, null)
{
this.ahrs = myAhrs;
this.ap = ap;
this.phi_out = phi_out;
this.psi_out = psi_out;
this.theta_out = theta_out;
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));
phiPid = new PIDLibrary.ParameterPID(values["phi_P_gain"], values["phi_I_gain"], values["phi_D_gain"], this.ahrs["phi"], phi_out, ap.values["bankangle"]);
// psiPid = new PIDLibrary.ParameterPID(values["psi_P_gain"], values["psi_I_gain"], values["psi_D_gain"], this.ahrs["psi"], psi_out, ap.values["psi_out"]);//should only correct drift
thetaPid = new PIDLibrary.ParameterPID(values["theta_P_gain"], values["theta_I_gain"], values["theta_D_gain"], this.ahrs["theta"], theta_out, ap.values["theta_out"]);
// PID QuerachsenPID = new PID(1, 1, 1, 90, -90, 2000, 1000, new GetDouble(GetPV), new GetDouble(GetSP),new SetDouble( SetResult));
phiPid.Enable();
// psiPid.Enable();
thetaPid.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 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();
}
