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()); } } }
/// <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()); } } }
// 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); }
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); }
/// <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()); } } }