public double ComputePV(UAVParameter sensor, UAVParameter sp)
{
double PV = 0;
if (sensor.DoubleValue < 0) //=WENN(A6<0; 360 + A6;A6)
{
PV = 360 + sensor.DoubleValue;
}
else
{
PV = sensor.DoubleValue;
}
double delta = sp.DoubleValue - PV;
//=WENN(D6<-180;360+D6;WENN(D6>180;D6-360;D6))
double corrdelta = 0;
if (delta < -180)
{
corrdelta = 360 + delta;
}
else
{
if (delta > 180)
{
corrdelta = delta - 360;
}
else
{
corrdelta = delta;
}
}
//Console.WriteLine("Psi:"+(int)sensor.DoubleValue+" PV: "+(int)PV+" delta SP-PV "+(int)delta+" delta"+(int)corrdelta+" NeuerPV"+(int)(sp.DoubleValue-corrdelta)+" Setpoint: "+(int)sp.DoubleValue+" Gyrobstrich:"+(int)lagesensor["gyrobetastrich"].DoubleValue);
return(sp.DoubleValue - corrdelta);
}
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();
}
void PerformanceData_ValueChanged(UAVParameter param, bool isremote)
{
values["bankangle"].Min = PerformanceData["MaxBankLeft"].Value;
values["bankangle"].Max = PerformanceData["MaxBankRight"].Value;
values["theta_out"].Max = PerformanceData["MaxClimbAngle"].Value;
values["theta_out"].Min = PerformanceData["MaxDescendAngle"].Value;
}
/// <summary>
/// Daten über das Netzwerk empfangen schreibe in datei.
/// </summary>
/// <param name="source"></param>
/// <param name="arg"></param>
void RcUAV_DataArrived(CommunicationEndpoint source, UAVParameter arg)
{
UAVCommons.Logging.ParameterLogEvent logevent = new UAVCommons.Logging.ParameterLogEvent();
logevent.name = arg.Name;
logevent.value = arg.Value.ToString();
Netlog.Info(logevent);
}
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>
/// 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 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 UAVParameter totalgain; // Gewichtung für alle gains gemeinsam
#endregion Fields
#region Constructors
public PID(UAVParameter kp, UAVParameter ki, UAVParameter kd, UAVParameter pv, UAVParameter diff, UAVParameter output,UAVParameter sp,UAVParameter totalgain)
{
this.pv = pv;
this.diff = diff;
this.output = output;
this.totalgain = totalgain;
this.sp = sp;
this.kp = kp;
this.ki = ki;
this.kd = kd;
}
public PID(UAVParameter kp, UAVParameter ki, UAVParameter kd, UAVParameter pv, UAVParameter diff, UAVParameter output, UAVParameter sp, UAVParameter totalgain)
{
this.pv = pv;
this.diff = diff;
this.output = output;
this.totalgain = totalgain;
this.sp = sp;
this.kp = kp;
this.ki = ki;
this.kd = kd;
}
public void EventRecieved(UAVParameter source, bool isremote)
{
lock (Syncobj)
{
if (list.ContainsKey(source.Name))
{
list[source.Name] = (UAVParameter)source.Clone();
}
else {
list.Add(source.Name, (UAVParameter)source.Clone());
}
}
}
public void EventRecieved(UAVParameter source, bool isremote)
{
lock (Syncobj)
{
if (list.ContainsKey(source.Name))
{
list[source.Name] = (UAVParameter)source.Clone();
}
else
{
list.Add(source.Name, (UAVParameter)source.Clone());
}
}
}
/// <summary>
/// Neuer Sensorwert schreibe in Datei
/// </summary>
/// <param name="param"></param>
/// <param name="isremote"></param>
void uavData_ValueChanged(UAVParameter param, bool isremote)
{
// if (!isremote)
{
UAVCommons.Logging.ParameterLogEvent logevent = new UAVCommons.Logging.ParameterLogEvent();
if ((param != null) && (param.Value != null))
{
logevent.name = param.Name;
logevent.value = param.Value.ToString();
Sensorlog.Info(logevent);
}
// Motor_ValueChanged(param, isremote);
}
}
private void LoadDatafromAHRS()
{
updateTimer.Enabled = false;
try
{
System.Diagnostics.Stopwatch watch = new System.Diagnostics.Stopwatch();
watch.Start();
UAVParameter speedparam = (UAVParameter)GPS["lbRMCSpeed"];
UAVParameter altparam = (UAVParameter)GPS["lbGGAAltitude"];
UAVParameter phiparam = (UAVParameter)AHRS["phi"];
UAVParameter thetaparam = (UAVParameter)AHRS["theta"];
UAVParameter psiparam = (UAVParameter)AHRS["psi"];
if (speedparam != null)
{
if (!speedparam.Value.Equals("N/A"))
{
airSpeedIndicator.SetAirSpeedIndicatorParameters(Convert.ToInt32(speedparam.Value));
}
}
if ((thetaparam != null) && (phiparam != null))
{
attitudeIndicator.SetAttitudeIndicatorParameters(Convert.ToDouble(thetaparam.Value), Convert.ToDouble(phiparam.Value));
}
// Änderungsrate berechnen
// Turn Quality berechnen
// this.vspeed = vspeed + Convert.ToInt32(mycore.currentUAV.uavData["lbGGAAltitude"].Value)*0.9;
if ((psiparam != null) && (psiparam != null))
{
this.Compass.SetHeadingIndicatorParameters(Convert.ToInt32(Convert.ToDouble(psiparam.Value)));
}
// if (mycore.currentUAV.uavData.ContainsKey("yaw")) Compass.SetHeadingIndicatorParameters(Convert.ToInt32(mycore.currentUAV.uavData["yaw"].Value));
// if (mycore.currentUAV.uavData.ContainsKey("vspeed")) verticalSpeedIndicator.SetVerticalSpeedIndicatorParameters(Convert.ToInt32(mycore.currentUAV.uavData["vspeed"].Value));
if (altparam != null)
{
altimeter.SetAlimeterParameters(Convert.ToInt32(Convert.ToDouble(altparam.Value)));
}
// if (mycore.currentUAV.uavData.ContainsKey("turnrate") && mycore.currentUAV.uavData.ContainsKey("turnquality")) turnCoordinator.SetTurnCoordinatorParameters(Convert.ToSingle(mycore.currentUAV.uavData["turnrate"].Value), Convert.ToSingle(mycore.currentUAV.uavData["turnquality"].Value));
this.Invalidate();
Console.WriteLine("time update:" + watch.ElapsedMilliseconds);
watch.Stop();
}
catch (Exception ex) {
}
updateTimer.Enabled = true;
}
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 UAVParameter totalgain; // Gewichtung für alle gains gemeinsam
#endregion Fields
#region Constructors
public PIDS(UAVParameter kp, UAVParameter ki, UAVParameter kd, UAVParameter ks,
UAVParameter lp, UAVParameter li, UAVParameter ld, UAVParameter ls, UAVParameter pv, UAVParameter diff, UAVParameter output,UAVParameter sp,UAVParameter totalgain)
{
this.pv = pv;
this.diff = diff;
this.output = output;
this.sp = sp;
this.totalgain = totalgain;
this.kp = kp;
this.ki = ki;
this.kd = kd;
this.ks = ks;
this.lp = lp;
this.li = li;
this.ld = ld;
this.ls = ls;
}
public PIDS(UAVParameter kp, UAVParameter ki, UAVParameter kd, UAVParameter ks,
UAVParameter lp, UAVParameter li, UAVParameter ld, UAVParameter ls, UAVParameter pv, UAVParameter diff, UAVParameter output, UAVParameter sp, UAVParameter totalgain)
{
this.pv = pv;
this.diff = diff;
this.output = output;
this.sp = sp;
this.totalgain = totalgain;
this.kp = kp;
this.ki = ki;
this.kd = kd;
this.ks = ks;
this.lp = lp;
this.li = li;
this.ld = ld;
this.ls = ls;
}
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();
}
/// <summary>
/// To be Implemented
/// Schlussrechnung
/// </summary>
/// <param name="oldValueandRange"></param>
/// <param name="newValueandRange"></param>
/// <returns></returns>
public static Double TransformToRange(UAVParameter oldValueandRange, UAVParameter newValueandRange)
{
return 0;
}
/// <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 void EventRecieved(CommunicationEndpoint source, UAVParameter arg)
{
this.source = source;
this.arg = arg;
}
public double ComputePV(UAVParameter sensor, UAVParameter sp)
{
double PV = 0;
if (sensor.DoubleValue < 0){ //=WENN(A6<0; 360 + A6;A6)
PV = 360+sensor.DoubleValue;
}else{
PV = sensor.DoubleValue;
}
double delta = sp.DoubleValue-PV;
//=WENN(D6<-180;360+D6;WENN(D6>180;D6-360;D6))
double corrdelta = 0;
if (delta < -180){
corrdelta = 360+delta;
}else{
if (delta > 180){
corrdelta = delta-360;
}else{
corrdelta = delta;
}
}
//Console.WriteLine("Psi:"+(int)sensor.DoubleValue+" PV: "+(int)PV+" delta SP-PV "+(int)delta+" delta"+(int)corrdelta+" NeuerPV"+(int)(sp.DoubleValue-corrdelta)+" Setpoint: "+(int)sp.DoubleValue+" Gyrobstrich:"+(int)lagesensor["gyrobetastrich"].DoubleValue);
return sp.DoubleValue-corrdelta;
}
/// <summary>
/// Motor wird nicht vom FlightStabiliser gesetzt setze bei Änderung selber
/// </summary>
/// <param name="param"></param>
/// <param name="isremote"></param>
void Motor_ValueChanged(UAVParameter param, bool isremote)
{
// Console.WriteLine(param.Name);
// dval = param.Value;
if ((param.GetStringPath().ToLower().Contains("gps")) || (param.GetStringPath().ToLower().Contains("gps"))) return;
// Console.WriteLine("MotorWerte:"+param.Name + ": " + param.Value);
// Console.WriteLine("Wert:"+param.Name+": "+param.Value.ToString());
/* if (param.Name == "throttleout")
{
uavData["Motor"].Value = param.Value;
}
*/
/*
* Peter: bitte ignorieren
*
*
*/
if (!stabilise)
{
try
{
string ValueStr = param.Value.ToString().Replace(",", "."); //
Double dval = 0;
if (ValueStr.Contains("."))
{
string komma = ValueStr.Split('.')[1];
dval = Convert.ToDouble(komma) / Math.Pow(10, komma.Length);
// Console.WriteLine("ValueStr: " + Convert.ToDouble(komma) + dval);
}
if (ValueStr == "1")
{
dval = 1;
}
// Console.WriteLine("preAusgabe");
/*
* Max: Setze Ausgänge
*
*/
if (param.Name == "theta_rollrateout") //"phi_rollrateout"
{
uavData["Höhenruder"].Value = dval * 1000 + 1000;
// Console.WriteLine("Ausgabe");
}
else if (param.Name == "phi_rollrateout")//"psi_rollrateout"
{
uavData["Querruder"].Value = dval * 1000 + 1000;
// Console.WriteLine("Ausgabe");
}
else if (param.Name == "psi_rollrateout")//""
{
uavData["Seitenruder"].Value = dval * 1000 + 1000;
// Console.WriteLine("Ausgabe");
}
else if (param.Name == "throttleout")//"theta_rollrateout"
{
uavData["Motor"].Value = dval * 1000 + 1000;
// Console.WriteLine("Ausgabe");
}
}
catch (Exception ex)
{
Console.WriteLine(ex.Message + ex.StackTrace.ToString());
}
}
}
/// <summary>
/// Daten über das Netzwerk empfangen schreibe in datei.
/// </summary>
/// <param name="source"></param>
/// <param name="arg"></param>
void RcUAV_DataArrived(CommunicationEndpoint source, UAVParameter arg)
{
UAVCommons.Logging.ParameterLogEvent logevent = new UAVCommons.Logging.ParameterLogEvent();
logevent.name = arg.Name;
logevent.value = arg.Value.ToString();
Netlog.Info(logevent);
}
/// <summary>
/// Neuer Sensorwert schreibe in Datei
/// </summary>
/// <param name="param"></param>
/// <param name="isremote"></param>
void uavData_ValueChanged(UAVParameter param, bool isremote)
{
// if (!isremote)
{
UAVCommons.Logging.ParameterLogEvent logevent = new UAVCommons.Logging.ParameterLogEvent();
if ((param != null) && (param.Value != null))
{
logevent.name = param.Name;
logevent.value = param.Value.ToString();
Sensorlog.Info(logevent);
}
// Motor_ValueChanged(param, isremote);
}
}
// Constructor -> Is Called when the Object is created
public VTOLUAV()
{
// When the Object is loaded from the sd card this method will not be called
joystick.values.Add(phi_rolerate);
joystick.values.Add(theta_rolerate);
joystick.values.Add(psi_rolerate);
joystick.values.Add(throttle);
uavData.Add(joystick);
//Factor = new UAVParameter ("Factory", 10, 0, 1000, 0); // Factor for Mixers
//uavData.Add (Factor);
lagesensor = new AHRS ("lagesensor", UsbHelper.GetDevicPathebyClass ("AHRS"));
uavData.Add (lagesensor);
////gpsempfänger = new GPS ("gpsempfänger", UsbHelper.GetDevicPathebyClass ("GPS"), 38400);
////uavData.Add (gpsempfänger);
////empfängerusb = new UAVJoystick ("empfängerusb", "", new IntPtr ());
////uavData.Add (empfängerusb);
// Servo Initialisierung
servo1 = new PWM("servo1", 0, null, 1); //("Name", startwert, "null"=ausgabegerät_maestro, Kanal am Maestro)
uavData.Add(servo1);
servo1["LowLimit"].DoubleValue = -80;
servo1["Neutral"].DoubleValue = 0;
servo1["HighLimit"].DoubleValue = 80;
servo1["Invert"].IntValue = 0;
servo2 = new PWM ("servo2", 0, null, 2);
uavData.Add (servo2);
servo2 ["LowLimit"].DoubleValue = -80;
servo2 ["Neutral"].DoubleValue = 0;
servo2 ["HighLimit"].DoubleValue = 80;
servo2 ["Invert"].IntValue = 0;
servo3 = new PWM ("servo3", 90, null, 3);
uavData.Add (servo3);
servo3 ["LowLimit"].DoubleValue = -100;
servo3 ["Neutral"].DoubleValue = 0;
servo3 ["HighLimit"].DoubleValue = 100;
servo3 ["Invert"].IntValue = 1;
servo4 = new PWM ("servo4", 90, null, 4);
uavData.Add (servo4);
servo4 ["LowLimit"].DoubleValue = -100;
servo4 ["Neutral"].DoubleValue = 0;
servo4 ["HighLimit"].DoubleValue = 100;
servo4 ["Invert"].IntValue = 1;
servo5 = new PWM ("servo5", 0, null, 5);
uavData.Add (servo5);
servo5 ["LowLimit"].DoubleValue = -100;
servo5 ["Neutral"].DoubleValue = 0;
servo5 ["HighLimit"].DoubleValue = 100;
servo5 ["Invert"].IntValue = 0;
//// PID CONFIG
kp_Höhe = new UAVParameter("kp_Höhe", 5, 0, 10, 0);
kd_Höhe = new UAVParameter("kd_Höhe", 0, 0, 10, 0);
ki_Höhe = new UAVParameter("ki_Höhe", 0, 0, 10, 0);
uavData.Add(kp_Höhe);
uavData.Add(kd_Höhe);
uavData.Add(ki_Höhe);
kp_Quer = new UAVParameter("kp_Quer", 5, 1, 10, 0);
kd_Quer = new UAVParameter("kd_Quer", 0, 1, 10, 0);
ki_Quer = new UAVParameter("ki_Quer", 0, 1, 10, 0);
uavData.Add(kp_Quer);
uavData.Add(kd_Quer);
uavData.Add(ki_Quer);
kp_Seite = new UAVParameter("kp_Seite", 5, 0, 10, 0);
kd_Seite = new UAVParameter("kd_Seite", 0, 0, 10, 0);
ki_Seite = new UAVParameter("ki_Seite", 0, 0, 10, 0);
uavData.Add(kp_Seite);
uavData.Add(kd_Seite);
uavData.Add(ki_Seite);
sp_Höhe = new UAVParameter("Höhe_SP", 0, -90, 90, 10);
sp_Quer = new UAVParameter("Quer_SP", 0, -180, 180, 10);
sp_Seite = new UAVParameter("Seite_SP", 0, -180, 180, 10);
uavData.Add(sp_Höhe);
uavData.Add(sp_Quer);
uavData.Add(sp_Seite);
PID_Out_Höhe = new UAVParameter("PID_Out_Höhe", 0, -100, 100, 0);
PID_Out_Quer = new UAVParameter("PID_Out_Quer", 0, -100, 100, 0);
PID_Out_Seite = new UAVParameter("PID_Out_Seite", 0, -100, 100, 0);
uavData.Add(PID_Out_Höhe);
uavData.Add(PID_Out_Quer);
uavData.Add(PID_Out_Seite);
}
void PerformanceData_ValueChanged(UAVParameter param, bool isremote)
{
values["bankangle"].Min = PerformanceData["MaxBankLeft"].Value;
values["bankangle"].Max = PerformanceData["MaxBankRight"].Value;
values["theta_out"].Max = PerformanceData["MaxClimbAngle"].Value;
values["theta_out"].Min = PerformanceData["MaxDescendAngle"].Value;
}
public VTOLUAV() // Constructor -> Is Called when the Object is created
{ // When the Object is loaded from the sd card this method will not be called
joystick.values.Add(phi_rolerate);
joystick.values.Add(theta_rolerate);
joystick.values.Add(psi_rolerate);
joystick.values.Add(throttle);
uavData.Add(joystick);
//Factor = new UAVParameter ("Factory", 10, 0, 1000, 0); // Factor for Mixers
//uavData.Add (Factor);
lagesensor = new AHRS("lagesensor", UsbHelper.GetDevicPathebyClass("AHRS"));
uavData.Add(lagesensor);
////gpsempfänger = new GPS ("gpsempfänger", UsbHelper.GetDevicPathebyClass ("GPS"), 38400);
////uavData.Add (gpsempfänger);
////empfängerusb = new UAVJoystick ("empfängerusb", "", new IntPtr ());
////uavData.Add (empfängerusb);
// Servo Initialisierung
servo1 = new PWM("servo1", 0, null, 1); //("Name", startwert, "null"=ausgabegerät_maestro, Kanal am Maestro)
uavData.Add(servo1);
servo1["LowLimit"].DoubleValue = -80;
servo1["Neutral"].DoubleValue = 0;
servo1["HighLimit"].DoubleValue = 80;
servo1["Invert"].IntValue = 0;
servo2 = new PWM("servo2", 0, null, 2);
uavData.Add(servo2);
servo2 ["LowLimit"].DoubleValue = -80;
servo2 ["Neutral"].DoubleValue = 0;
servo2 ["HighLimit"].DoubleValue = 80;
servo2 ["Invert"].IntValue = 0;
servo3 = new PWM("servo3", 90, null, 3);
uavData.Add(servo3);
servo3 ["LowLimit"].DoubleValue = -100;
servo3 ["Neutral"].DoubleValue = 0;
servo3 ["HighLimit"].DoubleValue = 100;
servo3 ["Invert"].IntValue = 1;
servo4 = new PWM("servo4", 90, null, 4);
uavData.Add(servo4);
servo4 ["LowLimit"].DoubleValue = -100;
servo4 ["Neutral"].DoubleValue = 0;
servo4 ["HighLimit"].DoubleValue = 100;
servo4 ["Invert"].IntValue = 1;
servo5 = new PWM("servo5", 0, null, 5);
uavData.Add(servo5);
servo5 ["LowLimit"].DoubleValue = -100;
servo5 ["Neutral"].DoubleValue = 0;
servo5 ["HighLimit"].DoubleValue = 100;
servo5 ["Invert"].IntValue = 0;
//// PID CONFIG
kp_Höhe = new UAVParameter("kp_Höhe", 5, 0, 10, 0);
kd_Höhe = new UAVParameter("kd_Höhe", 0, 0, 10, 0);
ki_Höhe = new UAVParameter("ki_Höhe", 0, 0, 10, 0);
uavData.Add(kp_Höhe);
uavData.Add(kd_Höhe);
uavData.Add(ki_Höhe);
kp_Quer = new UAVParameter("kp_Quer", 5, 1, 10, 0);
kd_Quer = new UAVParameter("kd_Quer", 0, 1, 10, 0);
ki_Quer = new UAVParameter("ki_Quer", 0, 1, 10, 0);
uavData.Add(kp_Quer);
uavData.Add(kd_Quer);
uavData.Add(ki_Quer);
kp_Seite = new UAVParameter("kp_Seite", 5, 0, 10, 0);
kd_Seite = new UAVParameter("kd_Seite", 0, 0, 10, 0);
ki_Seite = new UAVParameter("ki_Seite", 0, 0, 10, 0);
uavData.Add(kp_Seite);
uavData.Add(kd_Seite);
uavData.Add(ki_Seite);
sp_Höhe = new UAVParameter("Höhe_SP", 0, -90, 90, 10);
sp_Quer = new UAVParameter("Quer_SP", 0, -180, 180, 10);
sp_Seite = new UAVParameter("Seite_SP", 0, -180, 180, 10);
uavData.Add(sp_Höhe);
uavData.Add(sp_Quer);
uavData.Add(sp_Seite);
PID_Out_Höhe = new UAVParameter("PID_Out_Höhe", 0, -100, 100, 0);
PID_Out_Quer = new UAVParameter("PID_Out_Quer", 0, -100, 100, 0);
PID_Out_Seite = new UAVParameter("PID_Out_Seite", 0, -100, 100, 0);
uavData.Add(PID_Out_Höhe);
uavData.Add(PID_Out_Quer);
uavData.Add(PID_Out_Seite);
}
public SetParameterOptions(UAVParameter param)
{
parameter = param;
}
public VTOLUAV() // Constructor -> Is Called when the Object is created
{ // When the Object is loaded from the sd card this method will not be called
uavData.Add(totalpidgain);
joystick.values.Add(phi_rolerate);
joystick.values.Add(theta_rolerate);
joystick.values.Add(psi_rolerate);
joystick.values.Add(throttle);
uavData.Add(joystick);
lagesensor = new AHRS("lagesensor", UsbHelper.GetDevicPathebyClass("AHRS"));
lagesensor["cursettings"].Value = "371,376,379,17,-127,-87";
uavData.Add(lagesensor);
gpsempfänger = new GPS("gpsempfänger", UsbHelper.GetDevicPathebyClass("GPS"), 38400);
uavData.Add(gpsempfänger);
// empfängerusb = new UAVJoystick ("empfängerusb", "", new IntPtr ());
//uavData.Add (empfängerusb);
watch.Start();
// Servo Initialisierung
servo1 = new PWM("servo1", 0, null, 1); //("Name", startwert, "null"=ausgabegerät_maestro, Kanal am Maestro)
uavData.Add(servo1);
servo1 ["LowLimit"].DoubleValue = -100;
servo1 ["Neutral"].DoubleValue = 0;
servo1 ["HighLimit"].DoubleValue = 100;
servo1 ["Invert"].IntValue = 0;
servo1.SetHomePosition(0);
servo2 = new PWM("servo2", 0, null, 2);
uavData.Add(servo2);
servo2 ["LowLimit"].DoubleValue = -100;
servo2 ["Neutral"].DoubleValue = 0;
servo2 ["HighLimit"].DoubleValue = 100;
servo2 ["Invert"].IntValue = 0;
servo2.SetHomePosition(0);
servo3 = new PWM("servo3", 100, null, 3);
uavData.Add(servo3);
servo3 ["LowLimit"].DoubleValue = -80;
servo3 ["Neutral"].DoubleValue = 0;
servo3 ["HighLimit"].DoubleValue = 80;
servo3 ["Invert"].IntValue = 1;
servo3.SetHomePosition(100);
servo4 = new PWM("servo4", 100, null, 4);
uavData.Add(servo4);
servo4 ["LowLimit"].DoubleValue = -80;
servo4 ["Neutral"].DoubleValue = 0;
servo4 ["HighLimit"].DoubleValue = 80;
servo4 ["Invert"].IntValue = 1;
servo4.SetHomePosition(100);
servo5 = new PWM("servo5", -80, null, 5);
uavData.Add(servo5);
servo5 ["LowLimit"].DoubleValue = -100;
servo5 ["Neutral"].DoubleValue = 0;
servo5 ["HighLimit"].DoubleValue = 100;
servo5 ["Invert"].IntValue = 0;
servo5.SetHomePosition(-80);
servo6 = new PWM("servo6", -80, null, 6);
uavData.Add(servo6);
servo6 ["LowLimit"].DoubleValue = -100;
servo6 ["Neutral"].DoubleValue = 0;
servo6 ["HighLimit"].DoubleValue = 100;
servo6 ["Invert"].IntValue = 0;
servo6.SetHomePosition(-80);
// PID CONFIG --------------------------------------
kp_Höhe = new UAVParameter("kp_Höhe", 7, 0, 10, 0); //Sinnvoll=1Höhe = new UAVParameter ("kp_Höhe", 1, max, min, updaterate)
kd_Höhe = new UAVParameter("kd_Höhe", 10, -10, 10, 0); //Sinnvoll=-3 Achtung aus irgend einem Grund negativ !!!
ki_Höhe = new UAVParameter("ki_Höhe", 0, 0, 10, 0); //Sinnvoll=1
ks_Höhe = new UAVParameter("ks_Höhe", 0.0, 0, 10, 0); //Sinnvoll=0.2
lp_Höhe = new UAVParameter("lp_Höhe", 1, 0, 10, 0);
ld_Höhe = new UAVParameter("ld_Höhe", 1, 0, 10, 0);
li_Höhe = new UAVParameter("li_Höhe", 1, 0, 10, 0);
ls_Höhe = new UAVParameter("ls_Höhe", 0.2, 0, 10, 0);
uavData.Add(kp_Höhe);
uavData.Add(kd_Höhe);
uavData.Add(ki_Höhe);
uavData.Add(ks_Höhe);
uavData.Add(lp_Höhe);
uavData.Add(ld_Höhe);
uavData.Add(li_Höhe);
uavData.Add(ls_Höhe);
//----------------------------------------------------
kp_Quer = new UAVParameter("kp_Quer", 5, 0, 10, 0); //Sinnvoll=1
kd_Quer = new UAVParameter("kd_Quer", 5, 0, 10, 0); //Sinnvoll=3
ki_Quer = new UAVParameter("ki_Quer", 0, 0, 10, 0); //Sinnvoll=1
ks_Quer = new UAVParameter("ks_Quer", 0.0, 0, 10, 0); //Sinnvoll=0.2
lp_Quer = new UAVParameter("lp_Quer", 1, 0, 10, 0);
ld_Quer = new UAVParameter("ld_Quer", 1, 0, 10, 0);
li_Quer = new UAVParameter("li_Quer", 1, 0, 10, 0);
ls_Quer = new UAVParameter("ls_Quer", 0.2, 0, 10, 0);
uavData.Add(kp_Quer);
uavData.Add(kd_Quer);
uavData.Add(ki_Quer);
uavData.Add(ks_Quer);
uavData.Add(lp_Quer);
uavData.Add(ld_Quer);
uavData.Add(li_Quer);
uavData.Add(ls_Quer);
//----------------------------------------------------
kp_Seite = new UAVParameter("kp_Seite", -3, -10, 10, 0);
kd_Seite = new UAVParameter("kd_Seite", 6, -10, 10, 0);
ki_Seite = new UAVParameter("ki_Seite", 0, -10, 10, 0);
ks_Seite = new UAVParameter("ks_Seite", 0, -10, 10, 0);
lp_Seite = new UAVParameter("lp_Seite", 0.5, 0, 10, 0);
ld_Seite = new UAVParameter("ld_Seite", 0.5, 0, 10, 0);
li_Seite = new UAVParameter("li_Seite", 1, 0, 10, 0);
ls_Seite = new UAVParameter("ls_Seite", 0, 0, 10, 0);
uavData.Add(kp_Seite);
uavData.Add(kd_Seite);
uavData.Add(ki_Seite);
uavData.Add(ks_Seite);
uavData.Add(lp_Seite);
uavData.Add(ld_Seite);
uavData.Add(li_Seite);
uavData.Add(ls_Seite);
//----------------------------------------------------
sp_Höhe = new UAVParameter("Höhe_SP", 0, -90, 90, 10);
sp_Quer = new UAVParameter("Quer_SP", 0, -180, 180, 10);
sp_Seite = new UAVParameter("Seite_SP", 0, -180, 180, 10);
uavData.Add(sp_Höhe);
uavData.Add(sp_Quer);
uavData.Add(sp_Seite);
output_Höhe = new UAVParameter("PID_Out_Höhe", 0, -100, 100, 0);
output_Quer = new UAVParameter("PID_Out_Quer", 0, -100, 100, 0);
output_Seite = new UAVParameter("PID_Out_Seite", 0, -100, 100, 0);
uavData.Add(output_Höhe);
uavData.Add(output_Quer);
uavData.Add(output_Seite);
SteuerMotorLeistung = new UAVParameter("SteuerMotorLeistung", 0, 0, 100, 0);
HauptMotorLeistung = new UAVParameter("HauptMotorLeistung", -80, -100, 100, 0);
HauptMotorDiff = new UAVParameter("HauptMotorDiff", 0, -20, 20, 0);
uavData.Add(SteuerMotorLeistung);
uavData.Add(HauptMotorLeistung);
uavData.Add(HauptMotorDiff);
NewSeitePV = new UAVParameter("NewSeitePV", 0, -180, 180, 0);
uavData.Add(NewSeitePV);
uavData.Add(output_Höhe);
}
// Constructor -> Is Called when the Object is created
public VTOLUAV()
{
// When the Object is loaded from the sd card this method will not be called
uavData.Add(totalpidgain);
joystick.values.Add(phi_rolerate);
joystick.values.Add(theta_rolerate);
joystick.values.Add(psi_rolerate);
joystick.values.Add(throttle);
uavData.Add(joystick);
lagesensor = new AHRS ("lagesensor", UsbHelper.GetDevicPathebyClass ("AHRS"));
lagesensor["cursettings"].Value = "371,376,379,17,-127,-87";
uavData.Add (lagesensor);
gpsempfänger = new GPS ("gpsempfänger", UsbHelper.GetDevicPathebyClass ("GPS"), 38400);
uavData.Add (gpsempfänger);
// empfängerusb = new UAVJoystick ("empfängerusb", "", new IntPtr ());
//uavData.Add (empfängerusb);
watch.Start();
// Servo Initialisierung
servo1 = new PWM ("servo1", 0, null, 1); //("Name", startwert, "null"=ausgabegerät_maestro, Kanal am Maestro)
uavData.Add (servo1);
servo1 ["LowLimit"].DoubleValue = -100;
servo1 ["Neutral"].DoubleValue = 0;
servo1 ["HighLimit"].DoubleValue = 100;
servo1 ["Invert"].IntValue = 0;
servo1.SetHomePosition(0);
servo2 = new PWM ("servo2", 0, null, 2);
uavData.Add (servo2);
servo2 ["LowLimit"].DoubleValue = -100;
servo2 ["Neutral"].DoubleValue = 0;
servo2 ["HighLimit"].DoubleValue = 100;
servo2 ["Invert"].IntValue = 0;
servo2.SetHomePosition(0);
servo3 = new PWM ("servo3", 100, null, 3);
uavData.Add (servo3);
servo3 ["LowLimit"].DoubleValue = -80;
servo3 ["Neutral"].DoubleValue = 0;
servo3 ["HighLimit"].DoubleValue = 80;
servo3 ["Invert"].IntValue = 1;
servo3.SetHomePosition(100);
servo4 = new PWM ("servo4", 100, null, 4);
uavData.Add (servo4);
servo4 ["LowLimit"].DoubleValue = -80;
servo4 ["Neutral"].DoubleValue = 0;
servo4 ["HighLimit"].DoubleValue = 80;
servo4 ["Invert"].IntValue = 1;
servo4.SetHomePosition(100);
servo5 = new PWM ("servo5", -80, null, 5);
uavData.Add (servo5);
servo5 ["LowLimit"].DoubleValue = -100;
servo5 ["Neutral"].DoubleValue = 0;
servo5 ["HighLimit"].DoubleValue = 100;
servo5 ["Invert"].IntValue = 0;
servo5.SetHomePosition(-80);
servo6 = new PWM ("servo6", -80, null, 6);
uavData.Add (servo6);
servo6 ["LowLimit"].DoubleValue = -100;
servo6 ["Neutral"].DoubleValue = 0;
servo6 ["HighLimit"].DoubleValue = 100;
servo6 ["Invert"].IntValue = 0;
servo6.SetHomePosition(-80);
// PID CONFIG --------------------------------------
kp_Höhe = new UAVParameter ("kp_Höhe", 7 , 0, 10, 0);//Sinnvoll=1Höhe = new UAVParameter ("kp_Höhe", 1, max, min, updaterate)
kd_Höhe = new UAVParameter ("kd_Höhe", 10 , -10, 10, 0);//Sinnvoll=-3 Achtung aus irgend einem Grund negativ !!!
ki_Höhe = new UAVParameter ("ki_Höhe", 0 , 0, 10, 0);//Sinnvoll=1
ks_Höhe = new UAVParameter ("ks_Höhe", 0.0, 0, 10, 0);//Sinnvoll=0.2
lp_Höhe = new UAVParameter ("lp_Höhe", 1 , 0, 10, 0);
ld_Höhe = new UAVParameter ("ld_Höhe", 1 , 0, 10, 0);
li_Höhe = new UAVParameter ("li_Höhe", 1 , 0, 10, 0);
ls_Höhe = new UAVParameter ("ls_Höhe", 0.2 , 0, 10, 0);
uavData.Add (kp_Höhe);
uavData.Add (kd_Höhe);
uavData.Add (ki_Höhe);
uavData.Add (ks_Höhe);
uavData.Add (lp_Höhe);
uavData.Add (ld_Höhe);
uavData.Add (li_Höhe);
uavData.Add (ls_Höhe);
//----------------------------------------------------
kp_Quer = new UAVParameter ("kp_Quer", 5 , 0, 10, 0);//Sinnvoll=1
kd_Quer = new UAVParameter ("kd_Quer", 5 , 0, 10, 0);//Sinnvoll=3
ki_Quer = new UAVParameter ("ki_Quer", 0 , 0, 10, 0);//Sinnvoll=1
ks_Quer = new UAVParameter ("ks_Quer", 0.0, 0, 10, 0);//Sinnvoll=0.2
lp_Quer = new UAVParameter ("lp_Quer", 1 , 0, 10, 0);
ld_Quer = new UAVParameter ("ld_Quer", 1 , 0, 10, 0);
li_Quer = new UAVParameter ("li_Quer", 1 , 0, 10, 0);
ls_Quer = new UAVParameter ("ls_Quer", 0.2 , 0, 10, 0);
uavData.Add (kp_Quer);
uavData.Add (kd_Quer);
uavData.Add (ki_Quer);
uavData.Add (ks_Quer);
uavData.Add (lp_Quer);
uavData.Add (ld_Quer);
uavData.Add (li_Quer);
uavData.Add (ls_Quer);
//----------------------------------------------------
kp_Seite = new UAVParameter ("kp_Seite", -3 , -10, 10, 0);
kd_Seite = new UAVParameter ("kd_Seite", 6 , -10, 10, 0);
ki_Seite = new UAVParameter ("ki_Seite", 0 , -10, 10, 0);
ks_Seite = new UAVParameter ("ks_Seite", 0 , -10, 10, 0);
lp_Seite = new UAVParameter ("lp_Seite", 0.5 , 0, 10, 0);
ld_Seite = new UAVParameter ("ld_Seite", 0.5 , 0, 10, 0);
li_Seite = new UAVParameter ("li_Seite", 1 , 0, 10, 0);
ls_Seite = new UAVParameter ("ls_Seite", 0 , 0, 10, 0);
uavData.Add (kp_Seite);
uavData.Add (kd_Seite);
uavData.Add (ki_Seite);
uavData.Add (ks_Seite);
uavData.Add (lp_Seite);
uavData.Add (ld_Seite);
uavData.Add (li_Seite);
uavData.Add (ls_Seite);
//----------------------------------------------------
sp_Höhe = new UAVParameter ("Höhe_SP" , 0, -90, 90, 10);
sp_Quer = new UAVParameter ("Quer_SP" , 0, -180, 180, 10);
sp_Seite = new UAVParameter ("Seite_SP", 0, -180, 180, 10);
uavData.Add (sp_Höhe);
uavData.Add (sp_Quer);
uavData.Add (sp_Seite);
output_Höhe = new UAVParameter("PID_Out_Höhe" ,0,-100,100,0);
output_Quer = new UAVParameter("PID_Out_Quer" ,0,-100,100,0);
output_Seite = new UAVParameter("PID_Out_Seite",0,-100,100,0);
uavData.Add(output_Höhe);
uavData.Add(output_Quer);
uavData.Add(output_Seite);
SteuerMotorLeistung = new UAVParameter("SteuerMotorLeistung",0,0,100,0);
HauptMotorLeistung = new UAVParameter("HauptMotorLeistung",-80,-100,100,0);
HauptMotorDiff = new UAVParameter("HauptMotorDiff",0,-20,20,0);
uavData.Add(SteuerMotorLeistung);
uavData.Add(HauptMotorLeistung);
uavData.Add(HauptMotorDiff);
NewSeitePV = new UAVParameter("NewSeitePV",0,-180,180,0);
uavData.Add(NewSeitePV);
uavData.Add(output_Höhe);
}
public void EventRecieved(CommunicationEndpoint source, UAVParameter arg)
{
this.source = source;
this.arg = arg;
}
/// <summary>
/// Motor wird nicht vom FlightStabiliser gesetzt setze bei Änderung selber
/// </summary>
/// <param name="param"></param>
/// <param name="isremote"></param>
void Motor_ValueChanged(UAVParameter param, bool isremote)
{
// Console.WriteLine(param.Name);
// dval = param.Value;
if ((param.GetStringPath().ToLower().Contains("gps")) || (param.GetStringPath().ToLower().Contains("gps")))
{
return;
}
// Console.WriteLine("MotorWerte:"+param.Name + ": " + param.Value);
// Console.WriteLine("Wert:"+param.Name+": "+param.Value.ToString());
/* if (param.Name == "throttleout")
* {
* uavData["Motor"].Value = param.Value;
* }
*/
/*
* Peter: bitte ignorieren
*
*
*/
if (!stabilise)
{
try
{
string ValueStr = param.Value.ToString().Replace(",", "."); //
Double dval = 0;
if (ValueStr.Contains("."))
{
string komma = ValueStr.Split('.')[1];
dval = Convert.ToDouble(komma) / Math.Pow(10, komma.Length);
// Console.WriteLine("ValueStr: " + Convert.ToDouble(komma) + dval);
}
if (ValueStr == "1")
{
dval = 1;
}
// Console.WriteLine("preAusgabe");
/*
* Max: Setze Ausgänge
*
*/
if (param.Name == "theta_rollrateout") //"phi_rollrateout"
{
uavData["Höhenruder"].Value = dval * 1000 + 1000;
// Console.WriteLine("Ausgabe");
}
else if (param.Name == "phi_rollrateout")//"psi_rollrateout"
{
uavData["Querruder"].Value = dval * 1000 + 1000;
// Console.WriteLine("Ausgabe");
}
else if (param.Name == "psi_rollrateout")//""
{
uavData["Seitenruder"].Value = dval * 1000 + 1000;
// Console.WriteLine("Ausgabe");
}
else if (param.Name == "throttleout")//"theta_rollrateout"
{
uavData["Motor"].Value = dval * 1000 + 1000;
// Console.WriteLine("Ausgabe");
}
}
catch (Exception ex)
{
Console.WriteLine(ex.Message + ex.StackTrace.ToString());
}
}
}
public SetParameterOptions(UAVParameter param)
{
parameter = param;
}