/// <summary>
/// Call this method to insert a Event
/// </summary>
/// <param name="source"></param>
/// <param name="isremote"></param>
public void EventRecieved(UAVSingleParameter source, bool isremote)
{
if (source.updateRate == Int32.MaxValue) return;
if (source.updateRate == 0){
if (forwarded != null) forwarded(source, false);
return;
}
lock (Syncobj)
{
if (list.ContainsKey(source.GetStringPath()))
{
// timespans[source.Name] = DateTime.Now;
/* if (source.Value is double)
{
list[source.Name].Value = Convert.ToDouble(source.Value.ToString());
}else
{
* */
var toupdate =list[source.GetStringPath()];
toupdate.Value = (UAVSingleParameter)source;
toupdate.Timestamp = DateTime.Now;
// }
}
else {
list.Add(source.GetStringPath(), new EventValue(DateTime.Now,source));
// list.Add(source.GetStringPath(), (UAVParameter)source);//.Clone()
// timespans.TryAdd(source.GetStringPath(), DateTime.Now);
}
}
}
/// <summary>
/// Builds a new PWM Object
/// </summary>
/// <param name="servoname">Name of the Object to use with uavdata["Name of Object"]</param>
/// <param name="servovalue">the Value to set on Initialisation</param>
/// <param name="device">The Hardware Device to use</param>
/// <param name="Channelnr">The Channel Number from 0 to x</param>
public PWM(string servoname, int servovalue, string device, Byte Channelnr)
: base(servoname, "")
{
if (channels == null)
{
channels = new ushort[12];
for (int i = 0; i < channels.Length; i++)
{
channels[i] = 2000;
}
}
values.Add(new UAVParameter("LowLimit", -100, -100, 100));
values.Add(new UAVParameter("HighLimit", 100, -100, 100));
values.Add(new UAVParameter("Neutral", 0, -100, 100));
values.Add(new UAVParameter("Invert", 0, 0, 1));
values.Add(new UAVParameter("Value", servovalue, -100, 100, 100));
internalvalue = values["Value"];
values.Add(new UAVParameter("Output", 0, -100, 100, 100));
values["LowLimit"].ValueChanged += new UAVSingleParameter.ValueChangedHandler(PWM_ValueChanged);
values["HighLimit"].ValueChanged += new UAVSingleParameter.ValueChangedHandler(PWM_ValueChanged);
values["Neutral"].ValueChanged += new UAVSingleParameter.ValueChangedHandler(PWM_ValueChanged);
values["Invert"].ValueChanged += new UAVSingleParameter.ValueChangedHandler(PWM_ValueChanged);
values["Output"].ValueChanged += new UAVSingleParameter.ValueChangedHandler(PWM_ValueChanged);
internalvalue.ValueChanged += new UAVSingleParameter.ValueChangedHandler(PWM_ValueChanged);
//this.Value = servovalue;
channel = Channelnr;
this.device = device;
this.Min = -100;
this.Max = 100;
}
/// <summary>
/// Builds a new PWM Object
/// </summary>
/// <param name="servoname">Name of the Object to use with uavdata["Name of Object"]</param>
/// <param name="servovalue">the Value to set on Initialisation</param>
/// <param name="device">The Hardware Device to use</param>
/// <param name="Channelnr">The Channel Number from 0 to x</param>
public PWM(string servoname, int servovalue, string device, Byte Channelnr)
: base(servoname, "")
{
if (channels == null) {
channels = new ushort[12];
for (int i = 0; i < channels.Length; i++ )
{
channels[i] = 2000;
}
}
values.Add(new UAVParameter("LowLimit", -100, -100, 100));
values.Add(new UAVParameter("HighLimit", 100, -100, 100));
values.Add(new UAVParameter("Neutral", 0, -100, 100));
values.Add(new UAVParameter("Invert", 0, 0, 1));
values.Add(new UAVParameter("Value", servovalue, -100, 100,100));
internalvalue = values["Value"];
values.Add(new UAVParameter("Output", 0, -100, 100,100));
values["LowLimit"].ValueChanged += new UAVSingleParameter.ValueChangedHandler(PWM_ValueChanged);
values["HighLimit"].ValueChanged += new UAVSingleParameter.ValueChangedHandler(PWM_ValueChanged);
values["Neutral"].ValueChanged += new UAVSingleParameter.ValueChangedHandler(PWM_ValueChanged);
values["Invert"].ValueChanged += new UAVSingleParameter.ValueChangedHandler(PWM_ValueChanged);
values["Output"].ValueChanged += new UAVSingleParameter.ValueChangedHandler(PWM_ValueChanged);
internalvalue.ValueChanged += new UAVSingleParameter.ValueChangedHandler(PWM_ValueChanged);
//this.Value = servovalue;
channel = Channelnr;
this.device = device;
this.Min = -100;
this.Max = 100;
}
public DictionaryPropertyDescriptor(MonitoredDictionary<string, UAVSingleParameter> d, string key)
: base(key.ToString(), null)
{
_dictionary = d;
_key = key;
elem = _dictionary.GetFromPath(_key);
if (elem == null) throw new Exception("Element not found Key:"+key);
}
/// <summary>
/// Update Output
/// </summary>
/// <param name="param"></param>
/// <param name="isremote"></param>
void PWM_ValueChanged(UAVSingleParameter param, bool isremote)
{
//Update Servo Value
Console.WriteLine("Changed");
if (param.Name != "Output")
{
Value = Value;
}
}
public DictionaryPropertyDescriptor(MonitoredDictionary <string, UAVSingleParameter> d, string key)
: base(key.ToString(), null)
{
_dictionary = d;
_key = key;
elem = _dictionary.GetFromPath(_key);
if (elem == null)
{
throw new Exception("Element not found Key:" + key);
}
}
//Entkoppelungsglied damit eine Langsame Ausführung von Ausgaberoutine nicht den Lagesensor ausbremst
public void MakeAusgabeAsync(UAVSingleParameter param, bool isremote)
{
//Console.WriteLine(watch.ElapsedMilliseconds+"ms");
if ((AusgabecallResult != null) && (!AusgabecallResult.IsCompleted)) // Wenn noch immer nicht fertig aber bereits ausgeführt
{
Console.WriteLine("Programm zu langsam");
}
else // Noch nicht gestartet oder vom letzten Mal schon fertig dann ausführen
{
AHRS.ValueChangedHandler mehtodenaufruf = new AHRS.ValueChangedHandler(Ausgaberoutine); // Speicher ausgaberoutine in Variable
AusgabecallResult = mehtodenaufruf.BeginInvoke(param, isremote, null, null); //Rufe Ausgabe asyncron auf
}
}
/// <summary>
/// Sends a UAVParameter to all clients
/// </summary>
/// <param name="param"></param>
public void Send(UAVSingleParameter param)
{
foreach (TcpClient myclient in clients)
{
if (myclient.Connected)
{
if (System.Threading.Thread.CurrentThread.CurrentCulture.Name != "en-US")
{
System.Threading.Thread.CurrentThread.CurrentCulture = new
System.Globalization.CultureInfo("en-US");
}
NetworkStream clientStream = myclient.GetStream();
UTF8Encoding encoder = new UTF8Encoding();
byte[] buffer = encoder.GetBytes(param.GetStringPath() + "|" + DateTime.UtcNow.Ticks.ToString() + "|" + param.Value.ToString() + "\n");
clientStream.Write(buffer, 0, buffer.Length);
clientStream.Flush();
}
}
}
/// <summary>
/// Speichere geänderten Wert sobald sich die Trackbar bewegt
/// </summary>
/// <param name="sender"></param>
/// <param name="e"></param>
private void valueTracker_Scroll(object sender, EventArgs e)
{
try
{
UAVSingleParameter p = selectedParameter;
if ((selectedParameter is UAVStructure) && (((UAVStructure)selectedParameter).values.ContainsKey("Value")))
{
p = ((UAVStructure)selectedParameter)["Value"];
}
p.Value = valueTracker.Value;
ValueBar.Value = valueTracker.Value;
this.lbl_value.Text = Convert.ToString(valueTracker.Value);
}
catch (Exception ex) {
}
}
public override void ProcessResults(UAVBase core)
{
base.ProcessResults(core);
try
{
foreach (UAVSingleParameter param in this.uavData)
{
if (param.Value == null)
{
param.Value = "";
}
UAVSingleParameter newparam = core.uavData.SilentUpdate(param.Name, param.Value, true);
if (newparam != null)
{
if (newparam.Parent is UAVStructure)
{
((UAVStructure)newparam.Parent).ValueChanged += new UAVStructure.ValueChangedHandler(core.uavData.Value_ValueChanged);
}
newparam.Min = param.Min;
newparam.Max = param.Max;
newparam.updateRate = param.updateRate;
newparam.Parent = core.uavData.GetFromPath(param.Name.Substring(0, param.Name.LastIndexOf("\\")));
if (newparam.Parent == null)
{
newparam.Parent = core;
}
if (newparam.Parent.Name == newparam.Name)
{
newparam.Parent = core;
}
}
else
{
core.uavData.Add(param.Name, (UAVParameter)param.Clone());
}
}
}
catch (Exception ex)
{
}
core.initialised = true;
}
/// <summary>
/// Sends a UAVParameter to all clients
/// </summary>
/// <param name="param"></param>
public void Send(UAVSingleParameter param)
{
foreach (TcpClient myclient in clients)
{
if (myclient.Connected)
{
if (System.Threading.Thread.CurrentThread.CurrentCulture.Name != "en-US")
{
System.Threading.Thread.CurrentThread.CurrentCulture = new
System.Globalization.CultureInfo("en-US");
}
NetworkStream clientStream = myclient.GetStream();
UTF8Encoding encoder = new UTF8Encoding();
byte[] buffer = encoder.GetBytes(param.GetStringPath() + "|" + DateTime.UtcNow.Ticks.ToString() + "|" + param.Value.ToString() + "\n");
clientStream.Write(buffer, 0, buffer.Length);
clientStream.Flush();
}
}
}
public void Ausgaberoutine(UAVSingleParameter param, bool isremote) //100Hz Schleife //keine Ahnung wie das geht ,...was ist die max aktualisierungsrate von Maestro und PWM-switch ???
{
counter++;
// sp_Quer.DoubleValue = empfängerusb ["Axis0"].DoubleValue/10;//bin mir nicht sicher welche Achse
// sp_Höhe.DoubleValue = empfängerusb ["Axis1"].DoubleValue/10; //bin mir nicht sicher welche Achse
//sp_Quer.DoubleValue = 0;
//sp_Höhe.DoubleValue = 0;
//SP_Seite = nicht definiert ->Richtung wird nur über Fernsteuerung geregelt
// Console.WriteLine(param.Name + ":" + param.Value);
// servo1.DoubleValue = lagesensor["phi"].DoubleValue;
Console.WriteLine("Phi:" + lagesensor["phi"].DoubleValue);
// PID_Höhe.Compute();
// PID_Quer.Compute();
// //PID_Seite.Compute();->Richtung wird nur über Fernsteuerung geregelt
// servo1.DoubleValue = Tools.Limit ((PID_Out_Quer.DoubleValue - PID_Out_Höhe.DoubleValue) , 100, -100);
// servo2.DoubleValue = Tools.Limit ((PID_Out_Quer.DoubleValue + PID_Out_Höhe.DoubleValue) , 100, -100);
// servo3.DoubleValue = Tools.Limit (-empfängerusb ["Axis4"].DoubleValue + lagesensor ["gyrobetastrich"].DoubleValue - (empfängerusb ["Axis2"].DoubleValue+30), 100, -100);
// servo4.DoubleValue = Tools.Limit (-empfängerusb ["Axis4"].DoubleValue - lagesensor ["gyrobetastrich"].DoubleValue + (empfängerusb ["Axis2"].DoubleValue+30), 100, -100);
}
/// <summary>
/// Call this method to insert a Event
/// </summary>
/// <param name="source"></param>
/// <param name="isremote"></param>
public void EventRecieved(UAVSingleParameter source, bool isremote)
{
if (source.updateRate == Int32.MaxValue)
{
return;
}
if (source.updateRate == 0)
{
if (forwarded != null)
{
forwarded(source, false);
}
return;
}
lock (Syncobj)
{
if (list.ContainsKey(source.GetStringPath()))
{
// timespans[source.Name] = DateTime.Now;
/* if (source.Value is double)
* {
* list[source.Name].Value = Convert.ToDouble(source.Value.ToString());
* }else
* {
* */
var toupdate = list[source.GetStringPath()];
toupdate.Value = (UAVSingleParameter)source;
toupdate.Timestamp = DateTime.Now;
// }
}
else
{
list.Add(source.GetStringPath(), new EventValue(DateTime.Now, source));
// list.Add(source.GetStringPath(), (UAVParameter)source);//.Clone()
// timespans.TryAdd(source.GetStringPath(), DateTime.Now);
}
}
}
public abstract void Sendall(UAVSingleParameter[] param);
public override void Sendall(UAVSingleParameter[] param)
{
try
{
lock (SendSync)
{
if (commType == Communicationtype.Send)
{
foreach (UAVParameter value in param)
{
if (client is UDPDataClient) ((UDPDataClient)(client)).Send(value);
}
}
}
}
catch (Exception ex)
{
}
finally
{
}
}
/// <summary>
/// Update Output
/// </summary>
/// <param name="param"></param>
/// <param name="isremote"></param>
void PWM_ValueChanged(UAVSingleParameter param, bool isremote)
{
//Update Servo Value
Value = Value;
}
/// <summary>
/// Update Output
/// </summary>
/// <param name="param"></param>
/// <param name="isremote"></param>
void PWM_ValueChanged(UAVSingleParameter param, bool isremote)
{
//Update Servo Value
Value = Value;
}
public override void SendData(UAVSingleParameter param)
{
if (param.updateRate == Int32.MaxValue) return;
try
{
lock (SendSync)
{
if (commType == Communicationtype.Send)
{
if ((client is UDPDataClient) &&(client != null)) ((UDPDataClient)(client)).Send(param);
}
}
}
catch (Exception ex)
{
}
}
/// <summary>
/// Something has been changed to queue the Event /filter it using the Send Regulator
/// </summary>
/// <param name="param">The Parameter Object which has been changed</param>
/// <param name="isremote">A boolean Variable to determine if the event comes from here or is an remote event</param>
public void uavData_ValueChanged(UAVSingleParameter param, bool isremote)
{
if (!isremote) {
mySendRegulator.EventRecieved(param, isremote);
}
}
public override void SendData(UAVSingleParameter param)
{
if (param.updateRate == Int32.MaxValue) return;
try
{
lock (SendSync)
{
if (commType == Communicationtype.Send)
{
if (server != null)
{
if (server is TCPServer.DataTcpServer)
{
((TCPServer.DataTcpServer)server).Send(param);
}
}
if (client != null)
{
if (client is TcpDataClient) ((TcpDataClient)(client)).Send(param);
}
}
}
}
catch (Exception ex) {
}
}
public void Send(UAVSingleParameter param)
{
if (param.Value == null) return;
lock (SendSync)
{
if (client != null)
{
UTF8Encoding encoder = new UTF8Encoding();
byte[] buffer = encoder.GetBytes(param.GetStringPath() + "|" + DateTime.UtcNow.Ticks.ToString() + "|" + param.Value.ToString() + "\n");
client.Send(buffer, buffer.Length);
}
}
}
//100Hz Schleife //keine Ahnung wie das geht ,...was ist die max aktualisierungsrate von Maestro und PWM-switch ???
public void Ausgaberoutine(UAVSingleParameter param,bool isremote)
{
counter ++;
// sp_Quer.DoubleValue = empfängerusb ["Axis0"].DoubleValue/10;//bin mir nicht sicher welche Achse
// sp_Höhe.DoubleValue = empfängerusb ["Axis1"].DoubleValue/10; //bin mir nicht sicher welche Achse
//sp_Quer.DoubleValue = 0;
//sp_Höhe.DoubleValue = 0;
//SP_Seite = nicht definiert ->Richtung wird nur über Fernsteuerung geregelt
// Console.WriteLine(param.Name + ":" + param.Value);
// servo1.DoubleValue = lagesensor["phi"].DoubleValue;
Console.WriteLine("Phi:"+lagesensor["phi"].DoubleValue);
// PID_Höhe.Compute();
// PID_Quer.Compute();
// //PID_Seite.Compute();->Richtung wird nur über Fernsteuerung geregelt
// servo1.DoubleValue = Tools.Limit ((PID_Out_Quer.DoubleValue - PID_Out_Höhe.DoubleValue) , 100, -100);
// servo2.DoubleValue = Tools.Limit ((PID_Out_Quer.DoubleValue + PID_Out_Höhe.DoubleValue) , 100, -100);
// servo3.DoubleValue = Tools.Limit (-empfängerusb ["Axis4"].DoubleValue + lagesensor ["gyrobetastrich"].DoubleValue - (empfängerusb ["Axis2"].DoubleValue+30), 100, -100);
// servo4.DoubleValue = Tools.Limit (-empfängerusb ["Axis4"].DoubleValue - lagesensor ["gyrobetastrich"].DoubleValue + (empfängerusb ["Axis2"].DoubleValue+30), 100, -100);
}
public void Ausgaberoutine(UAVSingleParameter param, bool isremote) //100Hz Schleife //keine Ahnung wie das geht ,...was ist die max aktualisierungsrate von Maestro und PWM-switch ???
{
System.Diagnostics.Stopwatch w2 = new System.Diagnostics.Stopwatch();
w2.Start();
try{
// Console.WriteLine(watch.ElapsedMilliseconds+" ms Phi "+lagesensor["phi"].DoubleValue+" Theta"+lagesensor["theta"].DoubleValue+" psi"+lagesensor["psi"].DoubleValue);
//counter ++;
// sp_Quer.DoubleValue = empfängerusb ["Axis0"].DoubleValue/10;//bin mir nicht sicher welche Achse
// sp_Höhe.DoubleValue = empfängerusb ["Axis1"].DoubleValue/10; //bin mir nicht sicher welche Achse
sp_Quer.DoubleValue = phi_rolerate.DoubleValue; //(empfängerusb ["Axis0"].DoubleValue+18)/2;
sp_Höhe.DoubleValue = theta_rolerate.DoubleValue; //(empfängerusb ["Axis1"].DoubleValue+03)/3;
NewSeitePV.DoubleValue = ComputePV((UAVParameter)lagesensor["psi"], sp_Seite);
sp_Seite.DoubleValue = 0; // sp_Seite.DoubleValue + psi_rolerate.DoubleValue/100;//(empfängerusb ["Axis2"].DoubleValue+18)/20;
if (sp_Seite.DoubleValue < 0)
{
sp_Seite.DoubleValue = sp_Seite.DoubleValue + 360;
}
if (sp_Seite.DoubleValue > 360)
{
sp_Seite.DoubleValue = sp_Seite.DoubleValue - 360;
}
PIDS_Höhe.PIDS_Calc();
PIDS_Quer.PIDS_Calc();
PIDS_Seite.PIDS_Calc();
// Wie gehe ich sicher dass die PIDS fertig mit der Berechnung sind bevor ich die output-Werte in Folge zusammenzähle?????
// (möchte keine alten Werte zusammenzählen)
servo1.DoubleValue = Tools.Limit((output_Quer.DoubleValue - output_Höhe.DoubleValue), -80, 80);
servo2.DoubleValue = Tools.Limit((output_Quer.DoubleValue + output_Höhe.DoubleValue), -80, 80);
servo3.DoubleValue = Tools.Limit((SteuerMotorLeistung.DoubleValue) * 1.8 - 80 + output_Seite.DoubleValue, -100, 100); //mal 1.1 //(empfängerusb ["Axis2"].DoubleValue+18)/2
servo4.DoubleValue = Tools.Limit((SteuerMotorLeistung.DoubleValue) * 1.8 - 80 - output_Seite.DoubleValue, -100, 100);
//((empfängerusb ["Axis4"].DoubleValue)+90)*-0.8+110
// HauptMotorLeistung.Value = (int)Tools.ScaleValue(throttle.DoubleValue,-100,100,-80,100);
// Console.WriteLine(HauptMotorLeistung.DoubleValue);
servo5.DoubleValue = HauptMotorLeistung.DoubleValue - HauptMotorDiff.DoubleValue;
servo6.DoubleValue = HauptMotorLeistung.DoubleValue + HauptMotorDiff.DoubleValue;
PWM.UpdateServos();
// Console.WriteLine("HauptMotor Left "+(HauptMotorLeistung.DoubleValue - HauptMotorDiff.DoubleValue)+" Right"+(HauptMotorLeistung.DoubleValue + HauptMotorDiff.DoubleValue)+"Differenz: "+HauptMotorDiff.DoubleValue);
//servo5.DoubleValue = (empfängerusb ["Axis4"].DoubleValue)*-1.8-80;
// servo6.DoubleValue = (empfängerusb ["Axis4"].DoubleValue)*-1.8-80;
}catch (Exception ex) {
Console.WriteLine(ex.Message + " " + ex.StackTrace.ToString());
}finally{
if (w2.ElapsedMilliseconds > 10)
{
Console.WriteLine("too long" + w2.ElapsedMilliseconds);
}
}
// servo5.DoubleValue = Tools.Limit((((empfängerusb ["Axis4"].DoubleValue)+90)*-1+100)*1 , 100, -100);
//- (empfängerusb ["Axis2"].DoubleValue+30)/2
// + (empfängerusb ["Axis2"].DoubleValue+30)/2
//+ lagesensor ["gyrobetastrich"].DoubleValue
//- lagesensor ["gyrobetastrich"].DoubleValue
// Warum sollte ich Servo-Werte limitieren ?? -> sind doch bereits limitiert !! -> Limits kommen weg
// Wie gehe ich sicher dass die Umrechnung in PWM genau nach dem Setzen der Servo-Werte passiert????????
//Console.WriteLine("Servo3=" + servo3.DoubleValue +" Servo4=" + servo4.DoubleValue);
//Console.WriteLine("Wert Servo1:"+Tools.Limit ((output_Quer.DoubleValue - output_Höhe.DoubleValue) , 80, -80));
//
// servo3.DoubleValue =(empfängerusb ["Axis4"].DoubleValue +90)*-0.8+110 - output_Quer.DoubleValue ;
// servo4.DoubleValue =(empfängerusb ["Axis4"].DoubleValue +90)*-0.8+110 + output_Quer.DoubleValue ;
}
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();
}
}
}
}
//100Hz Schleife //keine Ahnung wie das geht ,...was ist die max aktualisierungsrate von Maestro und PWM-switch ???
public void Ausgaberoutine(UAVSingleParameter param,bool isremote)
{
System.Diagnostics.Stopwatch w2 = new System.Diagnostics.Stopwatch();
w2.Start();
try{
// Console.WriteLine(watch.ElapsedMilliseconds+" ms Phi "+lagesensor["phi"].DoubleValue+" Theta"+lagesensor["theta"].DoubleValue+" psi"+lagesensor["psi"].DoubleValue);
//counter ++;
// sp_Quer.DoubleValue = empfängerusb ["Axis0"].DoubleValue/10;//bin mir nicht sicher welche Achse
// sp_Höhe.DoubleValue = empfängerusb ["Axis1"].DoubleValue/10; //bin mir nicht sicher welche Achse
sp_Quer.DoubleValue = phi_rolerate.DoubleValue;//(empfängerusb ["Axis0"].DoubleValue+18)/2;
sp_Höhe.DoubleValue = theta_rolerate.DoubleValue;//(empfängerusb ["Axis1"].DoubleValue+03)/3;
NewSeitePV.DoubleValue = ComputePV((UAVParameter)lagesensor["psi"],sp_Seite);
sp_Seite.DoubleValue = 0;// sp_Seite.DoubleValue + psi_rolerate.DoubleValue/100;//(empfängerusb ["Axis2"].DoubleValue+18)/20;
if(sp_Seite.DoubleValue<0)
{sp_Seite.DoubleValue=sp_Seite.DoubleValue + 360;}
if(sp_Seite.DoubleValue>360)
{sp_Seite.DoubleValue = sp_Seite.DoubleValue-360;}
PIDS_Höhe.PIDS_Calc();
PIDS_Quer.PIDS_Calc();
PIDS_Seite.PIDS_Calc();
// Wie gehe ich sicher dass die PIDS fertig mit der Berechnung sind bevor ich die output-Werte in Folge zusammenzähle?????
// (möchte keine alten Werte zusammenzählen)
servo1.DoubleValue = Tools.Limit((output_Quer.DoubleValue - output_Höhe.DoubleValue) , -80, 80);
servo2.DoubleValue = Tools.Limit((output_Quer.DoubleValue + output_Höhe.DoubleValue) , -80, 80);
servo3.DoubleValue = Tools.Limit((SteuerMotorLeistung.DoubleValue)*1.8-80 + output_Seite.DoubleValue, -100, 100); //mal 1.1 //(empfängerusb ["Axis2"].DoubleValue+18)/2
servo4.DoubleValue = Tools.Limit((SteuerMotorLeistung.DoubleValue)*1.8-80 - output_Seite.DoubleValue , -100, 100);
//((empfängerusb ["Axis4"].DoubleValue)+90)*-0.8+110
// HauptMotorLeistung.Value = (int)Tools.ScaleValue(throttle.DoubleValue,-100,100,-80,100);
// Console.WriteLine(HauptMotorLeistung.DoubleValue);
servo5.DoubleValue = HauptMotorLeistung.DoubleValue - HauptMotorDiff.DoubleValue;
servo6.DoubleValue = HauptMotorLeistung.DoubleValue + HauptMotorDiff.DoubleValue;
PWM.UpdateServos();
// Console.WriteLine("HauptMotor Left "+(HauptMotorLeistung.DoubleValue - HauptMotorDiff.DoubleValue)+" Right"+(HauptMotorLeistung.DoubleValue + HauptMotorDiff.DoubleValue)+"Differenz: "+HauptMotorDiff.DoubleValue);
//servo5.DoubleValue = (empfängerusb ["Axis4"].DoubleValue)*-1.8-80;
// servo6.DoubleValue = (empfängerusb ["Axis4"].DoubleValue)*-1.8-80;
}catch(Exception ex){
Console.WriteLine(ex.Message+" "+ex.StackTrace.ToString());
}finally{
if (w2.ElapsedMilliseconds > 10) Console.WriteLine("too long" + w2.ElapsedMilliseconds);
}
// servo5.DoubleValue = Tools.Limit((((empfängerusb ["Axis4"].DoubleValue)+90)*-1+100)*1 , 100, -100);
//- (empfängerusb ["Axis2"].DoubleValue+30)/2
// + (empfängerusb ["Axis2"].DoubleValue+30)/2
//+ lagesensor ["gyrobetastrich"].DoubleValue
//- lagesensor ["gyrobetastrich"].DoubleValue
// Warum sollte ich Servo-Werte limitieren ?? -> sind doch bereits limitiert !! -> Limits kommen weg
// Wie gehe ich sicher dass die Umrechnung in PWM genau nach dem Setzen der Servo-Werte passiert????????
//Console.WriteLine("Servo3=" + servo3.DoubleValue +" Servo4=" + servo4.DoubleValue);
//Console.WriteLine("Wert Servo1:"+Tools.Limit ((output_Quer.DoubleValue - output_Höhe.DoubleValue) , 80, -80));
//
// servo3.DoubleValue =(empfängerusb ["Axis4"].DoubleValue +90)*-0.8+110 - output_Quer.DoubleValue ;
// servo4.DoubleValue =(empfängerusb ["Axis4"].DoubleValue +90)*-0.8+110 + output_Quer.DoubleValue ;
}
//public void FireValueChangedEvent(UAVSingleParameter param, bool isremote)
//{
// if (ValueChanged != null) ValueChanged(param, isremote);
//}
/// <summary>
/// Fires the changed event.
/// </summary>
/// <param name='param'>
/// Parameter.
/// </param>
/// <param name='isremote'>
/// Isremote.
/// </param>
public void FireDataRecievedEvent(UAVSingleParameter param, bool isremote)
{
if (DataRecieved != null) DataRecieved(param,isremote);
}
//Entkoppelungsglied damit eine Langsame Ausführung von Ausgaberoutine nicht den Lagesensor ausbremst
public void MakeAusgabeAsync(UAVSingleParameter param,bool isremote)
{
//Console.WriteLine(watch.ElapsedMilliseconds+"ms");
if ((AusgabecallResult != null)&&(!AusgabecallResult.IsCompleted)){ // Wenn noch immer nicht fertig aber bereits ausgeführt
Console.WriteLine("Programm zu langsam");
}else{ // Noch nicht gestartet oder vom letzten Mal schon fertig dann ausführen
AHRS.ValueChangedHandler mehtodenaufruf = new AHRS.ValueChangedHandler(Ausgaberoutine); // Speicher ausgaberoutine in Variable
AusgabecallResult = mehtodenaufruf.BeginInvoke(param,isremote,null,null); //Rufe Ausgabe asyncron auf
}
}
public abstract void SendData(UAVSingleParameter param);
/// <summary>
/// Update Output
/// </summary>
/// <param name="param"></param>
/// <param name="isremote"></param>
void PWM_ValueChanged(UAVSingleParameter param, bool isremote)
{
//Update Servo Value
Console.WriteLine("Changed");
if (param.Name != "Output"){
Value = Value;
}
}
/// <summary>
/// This Method is used to send the filtered Eventstream to the CommunicationEndpoints
/// </summary>
/// <param name="param"></param>
/// <param name="isremote"></param>
void SendDataForwardNow(UAVSingleParameter param, bool isremote)
{
try{
if (!isremote)
{
foreach (CommunicationEndpoint myendpoint in knownEndpoints)
{
//if (param is UAVStructure)
//{
// // param.ValueChanged+=new UAVParameter.ValueChangedHandler(param_ValueChanged);
// foreach (UAVSingleParameter myparam in ((UAVStructure)param).values.Values)
// {
// myendpoint.SendData(myparam);
// SendEventCounter++;
// }
//}
myendpoint.SendData(param);
SendEventCounter++;
}
}
}catch (Exception ex){
Console.WriteLine("Error on send forward "+ex.Message+ex.StackTrace.ToString());
}
}
/// <summary>
/// Sieht nach welcher Parameter in wie abgelegt werden soll
/// </summary>
/// <param name="param"></param>
/// <param name="isremote"></param>
void Source_ValueChanged(UAVSingleParameter param, bool isremote)
{
if (param.GetStringPath() == GetStringPath()) return;
lock (Syncobj)
{
// Console.WriteLine(param.Name + " " + param.Value);
if (mapping.ContainsKey(param.Name))
{
if (values.ContainsKey(mapping[param.Name])){
if (values[mapping[param.Name]].GetStringPath() != param.GetStringPath()) {
values[mapping[param.Name]].Value = param.Value;
}
}else{
values.Add(mapping[param.Name],new UAVParameter(mapping[param.Name],param.Value,param.Min, param.Max));
values[mapping[param.Name]].updateRate = 40;
}
}
}
}
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);
}
}
public override void Sendall(UAVSingleParameter[] param)
{
try
{
lock (SendSync)
{
if (commType == Communicationtype.Send)
{
if (server != null)
{
if (server is TCPServer.DataTcpServer)
{
foreach (UAVSingleParameter value in param)
{
((TCPServer.DataTcpServer)server).Send(value);
}
}
}
if (client != null)
{
foreach (UAVSingleParameter value in param)
{
if (client is TcpDataClient) ((TcpDataClient)(client)).Send(value);
}
}
}
}
}
catch (Exception ex)
{
}
finally {
}
}
protected void gpsValues_ValueChanged(UAVSingleParameter param, bool isremote)
{
if (ValueChanged != null) ValueChanged(param, isremote);
}
public void Send(UAVSingleParameter param)
{
if (System.Threading.Thread.CurrentThread.CurrentCulture.Name != "en-US")
{
System.Threading.Thread.CurrentThread.CurrentCulture = new
System.Globalization.CultureInfo("en-US");
}
if (param.Value == null) return;
lock (SendSync)
{
if (clientStream != null)
{
UTF8Encoding encoder = new UTF8Encoding();
byte[] buffer = encoder.GetBytes(param.GetStringPath() + "|" + DateTime.UtcNow.Ticks.ToString() + "|" + param.Value.ToString() + "\n");
clientStream.Write(buffer, 0, buffer.Length);
clientStream.Flush();
}
}
}
