public SetParameterOptionsForm(UAVCommons.UAVParameter myparam)
{
param = myparam;
InitializeComponent();
errorProvider1.Clear();
}
/// <summary>
/// Initialises all inputs and outputs
/// </summary>
/// <param name="uavdata"></param>
/// <param name="powerlever"></param>
/// <param name="throttle"></param>
/// <param name="brakes"></param>
public ThrottleSpeedBrakesMixer(string name, UAVCommons.MonitoredDictionary<string, object> uavdata,UAVParameter powerlever,UAVParameter throttle,UAVParameter brakes)
: base(name,uavdata)
{
this.throttle = null;
this.brakes = null;
this.powerlever = powerlever;
}
public ParameterControl(GroundControlCore.GroundControlCore core, UAVCommons.UAVSingleParameter parameter)
: base()
{
InitializeComponent();
this.core = core;
core.currentUAV.uavData.NewValue += new UAVCommons.MonitoredDictionary<string, UAVCommons.UAVSingleParameter>.NewValueHandler(uavData_NewValue);
if (parameter != null) selectedParameter = parameter;
Update_Combobox();
if (parameter != null)
{
cb_Parameter.SelectedItem = parameter.GetStringPath();
}
btn_output.ForeColor = Color.Blue;
btn_output.Text = "Input";
output = false;
if (parameter != null)
{
ValueBar.Step = 1;
valueTracker.TickFrequency = 10;
valueTracker.TickStyle = TickStyle.Both;
valueTracker.Maximum = Convert.ToDecimal(parameter.Max);
valueTracker.Minimum = Convert.ToDecimal(parameter.Min);
ValueBar.Minimum = valueTracker.Minimum;
ValueBar.Maximum = valueTracker.Maximum;
if (parameter.Max != null) tb_max.Text = parameter.Max.ToString();
if (parameter.Min != null) tb_min.Text = parameter.Min.ToString();
if ((parameter.Value != null) && (parameter.Value.ToString() != ""))
{
if ((parameter.DoubleValue >= parameter.MinDoubleValue) &&
(parameter.DoubleValue <= parameter.MaxDoubleValue))
{
valueTracker.Value = Convert.ToDecimal(parameter.Value);
ValueBar.Value = Convert.ToDecimal(valueTracker.Value);
lbl_value.Text = parameter.Value.ToString();
}
else
{
lbl_value.Text = "Value out of Bounds " + parameter.Value.ToString();
}
}
}
else {
ValueBar.Minimum = 0;
valueTracker.Minimum = 0;
ValueBar.Maximum = 100;
valueTracker.Maximum = 100;
ValueBar.Value = 50;
valueTracker.Value = 50;
lbl_value.Text = "50";
}
this.valueTracker.Factor = 0.0001m;
}
/// <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>
/// Returns the Direction from the Current Position to the given Waypoint
/// </summary>
/// <param name="gps">The Current GPS</param>
/// <param name="myWaypoint">A Waypoint Object containing the position of the Target Waypoint</param>
/// <returns>The Bearing from the Current Position to the Waypoint in degrees (0..360)</returns>
public double GetDirection(GPS gps, UAVCommons.Navigation.WayPoint myWaypoint)
{
double lat1 = Convert.ToDouble(gps["lbRMCPositionLatitude"].Value);
double lat2 = myWaypoint.Latitude;
double lon1 = Convert.ToDouble(gps["lbRMCPositionLongitude"].Value);
double lon2 = myWaypoint.Longitude;
// Calculate Bearing in Radians
double resultRad = (Math.Atan2((Math.Cos(lat1)*Math.Sin(lat2)) - (Math.Sin(lat1)*Math.Cos(lat2)*Math.Cos(lon2 - lon1)),
Math.Sin(lon2 - lon1)*Math.Cos(lat2)) % (2*Math.PI));
return resultRad*(180/Math.PI);
}
public JoystickSettings(UAVCommons.UAVStructure joystick, UAVCommons.UAVDataMapping mapping)
{
InitializeComponent();
this.mapping = mapping;
this.joystick = joystick;
string[] keys = new string[mapping.Mapping.Keys.Count];
mapping.Mapping.Keys.CopyTo(keys, 0);
List<string> stearingFunctions = new List<string>();
stearingFunctions.Add("phi_rollrate");
stearingFunctions.Add("theta_rollrate");
stearingFunctions.Add("psi_rollrate");
stearingFunctions.Add("throttle");
stearingFunctions.Add("");
comboBox1.Items.AddRange(stearingFunctions.ToArray());
comboBox2.Items.AddRange(stearingFunctions.ToArray());
comboBox3.Items.AddRange(stearingFunctions.ToArray());
comboBox4.Items.AddRange(stearingFunctions.ToArray());
comboBox5.Items.AddRange(stearingFunctions.ToArray());
comboBox6.Items.AddRange(stearingFunctions.ToArray());
if (mapping.Mapping.ContainsKey("Axis0"))
{
comboBox1.SelectedItem = mapping.Mapping["Axis0"];
}
if (mapping.Mapping.ContainsKey("Axis1"))
{
comboBox2.SelectedItem = mapping.Mapping["Axis1"];
}
if (mapping.Mapping.ContainsKey("Axis2"))
{
comboBox3.SelectedItem = mapping.Mapping["Axis2"];
}
if (mapping.Mapping.ContainsKey("Axis3"))
{
comboBox4.SelectedItem = mapping.Mapping["Axis3"];
}
if (mapping.Mapping.ContainsKey("Axis4"))
{
comboBox5.SelectedItem = mapping.Mapping["Axis4"];
}
if (mapping.Mapping.ContainsKey("Axis5"))
{
comboBox6.SelectedItem = mapping.Mapping["Axis5"];
}
// mapping.ValueChanged += new UAVCommons.UAVStructure.ValueChangedHandler(mapping_ValueChanged);
joystick.ValueChanged += new UAVCommons.UAVStructure.ValueChangedHandler(joystick_ValueChanged);
if (((FlightControlCommons.UAVJoystick)joystick).errorOnCreate) MessageBox.Show(this, "Es ist ein Fehler beim Verbinden mit dem Joystick aufgetrehten. Bitte prüfen Sie die Verbindung und die Treiber und starten Sie das Programm neu", "GroundControl", MessageBoxButtons.OK, MessageBoxIcon.Error, MessageBoxDefaultButton.Button1);
}
/// <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;
}
/// <summary>
/// Something has Changed in our Connections update the Pad!!!
/// </summary>
/// <param name="source"></param>
/// <param name="arg"></param>
/// <param name="state"></param>
void currentUAV_CommunicationStatusChanged(UAVCommons.UAVBase source, UAVCommons.CommunicationEndpoint arg,string state)
{
try
{
if (this.dataGridView1.InvokeRequired)
{
this.dataGridView1.Invoke((MethodInvoker)delegate {
UpdateGrid(source.knownEndpoints);
});
}
else
{
UpdateGrid(source.knownEndpoints);
}
}
catch (Exception ex) { }
}
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();
}
void mapping_ValueChanged(UAVCommons.UAVSingleParameter param, bool isremote)
{
if (this.InvokeRequired)
{
this.Invoke(new MethodInvoker(UpdateBars));
}
else
{
UpdateBars();
}
}
void joystick_ValueChanged(UAVCommons.UAVSingleParameter param, bool isremote)
{
try
{
mapping_ValueChanged(param, isremote);
}
catch (Exception ex) { }
}
void uavData_NewValue(UAVCommons.UAVSingleParameter param, bool isremote)
{
if (cb_Parameter.InvokeRequired) {
cb_Parameter.Invoke(new MethodInvoker(Update_Combobox));
}
// param.ValueChanged += new UAVCommons.UAVParameter.ValueChangedHandler(parameter_ValueChanged);
}
private void UpdateValue(UAVCommons.UAVSingleParameter param)
{
if (param != null)
{
double min = 0;
double max = 0;
UAVSingleParameter p = selectedParameter;
// Check if is Servo
if ((selectedParameter is UAVStructure) && (((UAVStructure)selectedParameter).values.ContainsKey("Value")))
{
p = ((UAVStructure)selectedParameter)["Value"];
}
if ((p is UAVStructure) && (((UAVStructure)p)["Min"] != null))
{
min = Convert.ToDouble(((UAVStructure)p)["Min"].Value.ToString());
}
else
{
min = Convert.ToDouble(p.Min.ToString());
}
if ((p is UAVStructure) && (((UAVStructure)p)["Max"] != null))
{
max = Convert.ToDouble(((UAVStructure)p)["Max"].Value);
}
else
{
max = Convert.ToDouble(p.Max.ToString());
}
double result = 0;
if (Double.TryParse(Convert.ToString(param.Value), out result))
{
if (output)
{
if (ValueBar.InvokeRequired)
{
ValueBar.Invoke((MethodInvoker)delegate
{
ValueBar.Minimum = Convert.ToDecimal(min);
ValueBar.Maximum = Convert.ToDecimal(max);
if ((Convert.ToDouble(param.Value) > min) && (Convert.ToDouble(param.Value) < max))
{
ValueBar.Value = Convert.ToDecimal(Convert.ToDouble(param.Value));
}
ValueBar.Refresh();
});
}
else
{
ValueBar.Minimum = Convert.ToDecimal(min);
ValueBar.Maximum = Convert.ToDecimal(max);
if ((Convert.ToDouble(param.Value) > min) && (Convert.ToDouble(param.Value) < max))
{
ValueBar.Value = Convert.ToDecimal(Convert.ToDouble(param.Value));
}
ValueBar.Invalidate();
}
}
else
{
if (valueTracker.InvokeRequired)
{
valueTracker.Invoke((MethodInvoker)delegate
{
valueTracker.Maximum = Convert.ToDecimal(max);
valueTracker.Minimum = Convert.ToDecimal(min);
if ((Convert.ToDouble(param.Value) > min) && (Convert.ToDouble(param.Value) < max))
{
valueTracker.Value = Convert.ToDecimal(Convert.ToDouble(param.Value));
}
valueTracker.TickFrequency = 10;
valueTracker.TickStyle = TickStyle.Both;
});
valueTracker.Invalidate();
}
else
{
// valueTracker.Value = Convert.ToDecimal(param.Value);
valueTracker.TickFrequency = 10;
valueTracker.TickStyle = TickStyle.Both;
valueTracker.Maximum = Convert.ToDecimal(max);
valueTracker.Minimum = Convert.ToDecimal(min);
if ((param.DoubleValue > min) && (param.DoubleValue < max))
{
valueTracker.Value = Convert.ToDecimal(param.Value);
}
valueTracker.Invalidate();
}
}
if (lbl_value.InvokeRequired)
{
// lbl_value.Invoke((MethodInvoker)(() => lbl_value.Text = param.Value.ToString()));
tb_max.Invoke((MethodInvoker)(() => tb_max.Text = max.ToString()));
tb_min.Invoke((MethodInvoker)(() => tb_min.Text = min.ToString()));
// lbl_value.Invalidate();
}
else
{
tb_max.Text = max.ToString();
tb_min.Text = min.ToString();
lbl_value.Text = param.Value.ToString();
lbl_value.Invalidate();
}
}
}
}
public _3DViewer(UAVCommons.UAVBase uAVBase)
{
this.mycore = uAVBase;
InitializeComponent();
}
/// <summary>
/// When Passing Waypoint, report to Ground Control
/// </summary>
/// <param name="waypoint"></param>
void navi_PassingWayPoint(UAVCommons.Navigation.WayPoint waypoint)
{
UAVCommons.Commands.WriteToFlightLog logcmd = new WriteToFlightLog("Passing WayPoint: " + waypoint.Latitude + "," + waypoint.Longitude, DateTime.Now);
this.SendCommandAsync(logcmd);
}
/// <summary>
/// Calculate Distance to current Waypoint and if overhead call event
/// </summary>
/// <param name="param"></param>
/// <param name="isremote"></param>
void gps_ValueChanged(UAVCommons.UAVParameter param, bool isremote)
{
if (currentWaypoint != null)
{
if (Distance((UAVStructure)this.gps, currentWaypoint) < Convert.ToDouble(performanceData["WayPointThreshold"].Value))
{
if (PassingWayPoint != null) PassingWayPoint(currentWaypoint);
}
values["TargetPsi"].Value = this.GetDirection((FlightControlCommons.GPS)gps, currentWaypoint);
values["TargetAltitude"].Value = currentWaypoint.Altitude;
}
else {
// What todo if there is no current WayPoint Set?
}
}
void currentUAV_CommunicationStatusChanged(UAVCommons.UAVBase source, UAVCommons.CommunicationEndpoint arg,string state)
{
if (ShowConnectionsStateChanges)
{
string connectedstring = "";
if (arg.Connected())
{
connectedstring = " established";
}
else
{
connectedstring = " lost";
}
if (logbox.InvokeRequired)
{
logbox.Invoke((MethodInvoker)delegate { logbox.Items.Insert(0, "Verbindung mit uav:" + arg.commType.ToString() + connectedstring); });
}
}
}
public ExpoMixer(string name,UAVCommons.MonitoredDictionary<string,object> uavdata)
: base(name,uavdata)
{
}
/// <summary>
/// UAV Position hat sich Verändert aktualisiere Markierung auf der Karte
/// </summary>
/// <param name="param"></param>
/// <param name="isremote"></param>
public void MapView_ValueChanged(UAVCommons.CommunicationEndpoint source, UAVCommons.UAVSingleParameter arg)
{
//if (arg.Name == "lbRMCPositionLatitude")
//{
// if (gMapControl1.InvokeRequired)
// {
// gMapControl1.Invoke(new MethodInvoker(UpdateUAVPosition));
// }
// else
// {
// UpdateUAVPosition();
// }
//}
}
void uavData_ValueChanged(UAVCommons.UAVParameter param, bool isremote)
{
//if (Lastupdate.AddMilliseconds(-100) < DateTime.Now)
//{
// Lastupdate = DateTime.Now;
// LoadDatafromAHRS();
//}
}
private void UpdateBar(UAVCommons.UAVSingleParameter value, ColorProgressBar.ColorProgressBar bar)
{
bar.Minimum = Convert.ToInt32(value.Min) * 100;
bar.Maximum = Convert.ToInt32(value.Max) * 100;
bar.Value = Convert.ToInt32(Convert.ToDouble(value.Value) * 100);
bar.Refresh();
}
void endpoint_LatencyUpdated(UAVCommons.CommunicationEndpoint source, int ms)
{
if (this.dataGridView1.InvokeRequired)
{
this.dataGridView1.Invoke((MethodInvoker)delegate
{
UpdateGrid(this.core.currentUAV.knownEndpoints);
});
}
else
{
UpdateGrid(this.core.currentUAV.knownEndpoints);
}
}
public virtual void ahrs_ValueChanged(UAVCommons.UAVParameter param, bool isremote)
{
//lastphi = Convert.ToDouble(ahrs.values["phi"]);
//lastpsi = Convert.ToDouble(ahrs.values["psi"]);
//lasttheta = Convert.ToDouble(ahrs.values["theta"]);
}
public BaseMixer(string name,UAVCommons.MonitoredDictionary<string, object> uavdata)
: base(name,null)
{
this.uavdata = uavdata;
}
public virtual void currentUAV_DataArrived(UAVCommons.CommunicationEndpoint source, UAVCommons.UAVSingleParameter arg)
{
}
/// <summary>
/// UAV Position hat sich Verändert aktualisiere Markierung auf der Karte
/// </summary>
/// <param name="param"></param>
/// <param name="isremote"></param>
public virtual void MapView_ValueChanged(UAVCommons.CommunicationEndpoint source, UAVCommons.UAVSingleParameter arg)
{
}
void currentUAV_DataArrived(UAVCommons.CommunicationEndpoint source, UAVCommons.UAVParameter arg)
{
}
/// <summary>
/// Passing old Waypoint select new Waypoint
/// </summary>
/// <param name="waypoint"></param>
void navi_PassingWayPoint(UAVCommons.Navigation.WayPoint waypoint)
{
if (values["WaypointIndex"].IntValue < Waypoints.Count) values["WaypointIndex"].IntValue++;
navigation.currentWaypoint = Waypoints[values["WaypointIndex"].IntValue];
}
