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();
}
/// <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 static HierachyItem GetItembyPath(UAVParameter param, string path)
{
string key = path;
string newpath = "";
MonitoredDictionary <string, UAVParameter> .GetKeyPath(ref key, ref newpath);
if (param is UAVStructure)
{
UAVStructure mystructure = ((UAVStructure)param);
foreach (UAVParameter myparams in mystructure.values.Values)
{
if (myparams.Name == key)
{
GetItembyPath(myparams, newpath);
}
}
}
else
if (param is UAVParameter)
{
if (param.Name == key)
{
return(param);
}
// remotedata.Add(newparam);
}
return(null);
}
private static void NormaliseStructure(UAVSingleParameter param, List <UAVSingleParameter> remotedata)
{
if (param is UAVStructure)
{
UAVStructure mystructure = ((UAVStructure)param);
foreach (UAVSingleParameter myparams in mystructure.values.Values)
{
NormaliseStructure(myparams, remotedata);
UAVSingleParameter structparam = new UAVSingleParameter(param.GetStringPath(), myparams.Value, myparams.Min, myparams.Max);
if (remotedata.Count(m => m.Name.Equals(structparam.Name)) == 0)
{
remotedata.Add(structparam);
}
}
// UAVSingleParameter param1 = new UAVSingleParameter(param.Name, param.Value, param.Min, param.Max);
// remotedata.Add(param1);
}
else
if (param is UAVSingleParameter)
{
UAVSingleParameter newparam = new UAVSingleParameter(param.Name, param.Value);
newparam.Name = param.GetStringPath();
newparam.Min = param.Min;
newparam.Max = param.Max;
newparam.Parent = param.Parent;
newparam.updateRate = param.updateRate;
newparam.Value = param.Value;
if (remotedata.Count(m => m.Name.Equals(newparam.Name)) == 0)
{
remotedata.Add(newparam);
}
}
}
public UAVDataMapping(string name, string port, UAVStructure source)
: base(name, port)
{
this.Source = source;
Source.ValueChanged += new ValueChangedHandler(Source_ValueChanged);
Dictionary<string, string> emptymapping = new Dictionary<string, string>();
foreach (string key in Source.values.Keys) {
mapping.Add(key, key);
}
}
/// <summary>
/// Initialise Navigation
/// Baue Navigations Objekt auf
/// </summary>
/// <param name="name"></param>
/// <param name="ap"></param>
/// <param name="gps"></param>
public BaseNavigation(string name, UAVCommons.UAVStructure gps, UAVCommons.UAVStructure Performancedata)
: base(name, null)
{
this.gps = gps;
this.performanceData = Performancedata;
gps.ValueChanged += new ValueChangedHandler(gps_ValueChanged);
//Config Values
values.Add(new UAVParameter("TargetPsi",0,360,0));
values.Add(new UAVParameter("TargetAltitude", 1000, 999999, 0));
}
/// <summary>
/// Initialise Navigation
/// Baue Navigations Objekt auf
/// </summary>
/// <param name="name"></param>
/// <param name="ap"></param>
/// <param name="gps"></param>
public BaseNavigation(string name, UAVCommons.UAVStructure gps, UAVCommons.UAVStructure Performancedata)
: base(name, null)
{
this.gps = gps;
this.performanceData = Performancedata;
gps.ValueChanged += new ValueChangedHandler(gps_ValueChanged);
//Config Values
values.Add(new UAVParameter("TargetPsi", 0, 360, 0));
values.Add(new UAVParameter("TargetAltitude", 1000, 999999, 0));
}
public UAVDataMapping(string name, string port, UAVStructure source)
: base(name, port)
{
this.Source = source;
Source.ValueChanged += new ValueChangedHandler(Source_ValueChanged);
Dictionary <string, string> emptymapping = new Dictionary <string, string>();
foreach (string key in Source.values.Keys)
{
mapping.Add(key, key);
}
}
public static void NormaliseStructure(UAVSingleParameter param, List <UAVSingleParameter> remotedata)
{
if (param is UAVStructure)
{
UAVStructure mystructure = ((UAVStructure)param);
foreach (UAVSingleParameter myparams in mystructure.values.Values)
{
if (myparams is UAVStructure)
{
NormaliseStructure(myparams, remotedata);
UAVSingleParameter temp = new UAVSingleParameter(myparams.GetStringPath(), myparams.Value);
temp.Max = myparams.Max;
temp.Min = myparams.Min;
temp.updateRate = myparams.updateRate;
temp.Value = myparams.Value;
remotedata.Add(temp);
}
else
{
UAVSingleParameter temp = new UAVSingleParameter(myparams.GetStringPath(), myparams.Value);
temp.Max = myparams.Max;
temp.Min = myparams.Min;
// temp.Value = myparams.Value;
temp.updateRate = myparams.updateRate;
remotedata.Add(temp);
}
}
//Eigen Wert
UAVSingleParameter temp2 = new UAVSingleParameter(param.GetStringPath(), param.Value);
temp2.Max = param.Max;
temp2.Min = param.Min;
// temp.Value = myparams.Value;
temp2.updateRate = param.updateRate;
remotedata.Add(temp2);
}
else
if (param is UAVSingleParameter)
{
UAVSingleParameter newparam = new UAVSingleParameter(param.Name, param.Value);
newparam.Name = param.GetStringPath();
newparam.Min = param.Min;
newparam.Max = param.Max;
// if (!(param.Parent is UAVBase))
// {
// newparam.Parent = param.Parent;
// }
newparam.updateRate = param.updateRate;
newparam.Value = param.Value;
remotedata.Add(newparam);
}
}
/// <summary>
/// Calculates the Distance from the Current Position to the given Waypoint in ?? NM ??
/// </summary>
/// <param name="gps">A object representing the onboard GPS</param>
/// <param name="currentWaypoint">A Waypoint representing the Target for the Distance Calc</param>
/// <returns>The Distance in ?? NM?? as a double value</returns>
public double Distance(UAVStructure gps, UAVCommons.Navigation.WayPoint currentWaypoint)
{
if ((gps.values["lbRMCPositionLongitude"] != null) && (gps.values["lbRMCPositionLatitude"] != null))
{
SharpGis.SharpGps.Coordinate gpslocation = new SharpGis.SharpGps.Coordinate(Convert.ToDouble(gps.values["lbRMCPositionLongitude"].Value), Convert.ToDouble(gps.values["lbRMCPositionLatitude"].Value));
double greatCircleDistance = gpslocation.Distance(new SharpGis.SharpGps.Coordinate(currentWaypoint.Longitude, currentWaypoint.Latitude));
double altDistance = Math.Max(Convert.ToDouble(gps.values["lbGGAAltitude"].Value), currentWaypoint.Altitude) - Math.Min(Convert.ToDouble(gps.values["lbGGAAltitude"].Value), currentWaypoint.Altitude);
return Math.Sqrt(greatCircleDistance * greatCircleDistance + altDistance * altDistance);
}
else {
// Log error
}
return 99999999;
}
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();
}
private void updateTimer_Tick(object sender, EventArgs e)
{
if (GPS == null)
{
try
{
foreach (var item in mycore.currentUAV.uavData)
{
if ((item.Value is UAVCommons.UAVStructure) && (((UAVCommons.UAVStructure)item.Value).values.ContainsKey("lbRMCPositionLatitude")))
{
this.GPS = (UAVCommons.UAVStructure)item.Value;
return;
}
}
}
catch (Exception ex)
{
}
}
if (AHRS == null)
{
try
{
foreach (var item in mycore.currentUAV.uavData)
{
if ((item.Value is UAVCommons.UAVStructure) && (((UAVCommons.UAVStructure)item.Value).values.ContainsKey("phi")))
{
this.AHRS = (UAVCommons.UAVStructure)item.Value;
return;
}
}
}
catch (Exception ex)
{
}
}
if ((AHRS != null) && (GPS != null))
{
LoadDatafromAHRS();
}
}
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 override void LoadValues(System.Xml.XmlElement elem)
{
base.LoadValues(elem);
var mappingelements = elem.SelectNodes("mapping");
Source = (UAVStructure)((UAVBase)this.Parent).uavData.GetFromPath(elem.GetAttribute("source"));
Source.ValueChanged += new ValueChangedHandler(Source_ValueChanged);
lock (Syncobj)
{
var mapping = Mapping;
if (mappingelements.Count > 0)
{
mapping.Clear();
foreach (XmlElement item in mappingelements)
{
mapping.Add(item.GetAttribute("key"), item.GetAttribute("value"));
}
Mapping = mapping;
}
}
}
public override void LoadValues(System.Xml.XmlElement elem)
{
base.LoadValues(elem);
var mappingelements = elem.SelectNodes("mapping");
Source = (UAVStructure)((UAVBase)this.Parent).uavData.GetFromPath(elem.GetAttribute("source"));
Source.ValueChanged += new ValueChangedHandler(Source_ValueChanged);
lock (Syncobj)
{
var mapping = Mapping;
if (mappingelements.Count > 0)
{
mapping.Clear();
foreach (XmlElement item in mappingelements)
{
mapping.Add(item.GetAttribute("key"), item.GetAttribute("value"));
}
Mapping = mapping;
}
}
}
private void updateTimer_Tick(object sender, EventArgs e)
{
if (AHRS == null)
{
try
{
foreach (var item in mycore.uavData)
{
if ((item.Value is UAVCommons.UAVStructure) && (((UAVCommons.UAVStructure)item.Value).values.ContainsKey("phi")))
{
this.AHRS = (UAVCommons.UAVStructure)item.Value;
break;
}
}
}
catch (Exception ex)
{
}
}
if ((AHRS != null))
{
if (ParentForm.Text.Contains("Demo"))
{
ParentForm.Text = ParentForm.Text.Replace("Demo Mode", "");
}
LoadDatafromAHRS();
}
else
{
if (!ParentForm.Text.Contains("Demo"))
{
this.ParentForm.Text += "Demo Mode";
}
_3DModelViewer1.UpdateDemo(true);
}
}
private void updateTimer_Tick(object sender, EventArgs e)
{
if (GPS == null)
{
try
{
foreach (var item in mycore.currentUAV.uavData)
{
if ((item.Value is UAVCommons.UAVStructure) && (((UAVCommons.UAVStructure)item.Value).values.ContainsKey("lbRMCPositionLatitude")))
{
this.GPS = (UAVCommons.UAVStructure)item.Value;
return;
}
}
}
catch (Exception ex)
{
}
}
if (AHRS == null)
{
try
{
foreach (var item in mycore.currentUAV.uavData)
{
if ((item.Value is UAVCommons.UAVStructure) && (((UAVCommons.UAVStructure)item.Value).values.ContainsKey("phi")))
{
this.AHRS = (UAVCommons.UAVStructure)item.Value;
return;
}
}
}
catch (Exception ex)
{
}
}
if ((AHRS != null) && (GPS != null))
{
LoadDatafromAHRS();
}
}
public UAVSingleParameter SilentUpdate(string key, object value, bool isremote)
{
string newpath = "";
GetKeyPath(ref key, ref newpath);
if (uavData.ContainsKey(key))
{
// Cut away current level
return(uavData[key].SilentUpdate(newpath, value, isremote));
}
else
{
UAVSingleParameter parameter;
if (newpath == key)
{
if (owner is UAVParameter)
{
parameter = new UAVSingleParameter(key, value);
}
else
{
if ((key == "Min") || (key == "Max") || (key == "UpdateRate"))
{
parameter = new UAVSingleParameter(key, value);
}
else
{
if (owner.Name != key)
{
parameter = new UAVParameter(key, value);
}
else
{
return((UAVSingleParameter)owner);
}
}
}
}
else
{
parameter = new UAVStructure(key, "");
parameter.Parent = owner;
parameter.ValueChanged += new UAVSingleParameter.ValueChangedHandler(parameter_ValueChanged);
uavData.Add(key, parameter);
UAVSingleParameter child = parameter.SilentUpdate(newpath, value, isremote);
child.Parent = parameter;
child.ValueChanged += new UAVSingleParameter.ValueChangedHandler(parameter_ValueChanged);
return(child);
}
parameter.ValueChanged += new UAVSingleParameter.ValueChangedHandler(parameter_ValueChanged);
parameter.Parent = owner;
uavData.Add(key, parameter);
if (NewValue != null)
{
NewValue(parameter, isremote);
}
return(parameter);
}
}
private void updateTimer_Tick(object sender, EventArgs e)
{
if (AHRS == null)
{
try
{
foreach (var item in mycore.uavData)
{
if ((item.Value is UAVCommons.UAVStructure) && (((UAVCommons.UAVStructure)item.Value).values.ContainsKey("phi")))
{
this.AHRS = (UAVCommons.UAVStructure)item.Value;
break;
}
}
}
catch (Exception ex)
{
}
}
if ((AHRS != null))
{
if (ParentForm.Text.Contains("Demo"))
{
ParentForm.Text = ParentForm.Text.Replace("Demo Mode", "");
}
LoadDatafromAHRS();
}else
{
if (!ParentForm.Text.Contains("Demo"))
{
this.ParentForm.Text += "Demo Mode";
}
_3DModelViewer1.UpdateDemo(true);
}
}
public VTOLFlightStabilizer(string name, UAVStructure myAhrs, FlightControlCommons.components.BaseAutoPilot ap)
: base(name, myAhrs, ap)
{
}
