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(); }
private void LoadDatafromAHRS() { updateTimer.Enabled = false; try { // System.Diagnostics.Stopwatch watch = new System.Diagnostics.Stopwatch(); // watch.Start(); UAVStructure phiparam = (UAVStructure)AHRS["phi"]; UAVStructure thetaparam = (UAVStructure)AHRS["theta"]; UAVStructure psiparam = (UAVStructure)AHRS["psi"]; // if (speedparam != null) if (!speedparam.Value.Equals("N/A")) airSpeedIndicator.SetAirSpeedIndicatorParameters(Convert.ToInt32(speedparam.Value)); if ((thetaparam != null) && (phiparam != null) && (psiparam != null)) { this.glefis1.ahrs.heading = Convert.ToInt32(psiparam.DoubleValue); this.glefis1.ahrs.pitch = Convert.ToInt32(thetaparam.DoubleValue); this.glefis1.ahrs.bank = Convert.ToInt32(phiparam.DoubleValue) * -1; // 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))); this.glefis1.Refresh(); // Console.WriteLine("time update:"+watch.ElapsedMilliseconds); // watch.Stop(); } catch (Exception ex) { } updateTimer.Enabled = true; }
/// <summary> /// Calculates the Distance from the Current Position to the given Waypoint in ?? NM ?? /// </summary> /// <param name="gps">A object representing the onboard GPS</param> /// <param name="currentWaypoint">A Waypoint representing the Target for the Distance Calc</param> /// <returns>The Distance in ?? NM?? as a double value</returns> public double Distance(UAVStructure gps, UAVCommons.Navigation.WayPoint currentWaypoint) { if ((gps.values["lbRMCPositionLongitude"] != null) && (gps.values["lbRMCPositionLatitude"] != null)) { SharpGis.SharpGps.Coordinate gpslocation = new SharpGis.SharpGps.Coordinate(Convert.ToDouble(gps.values["lbRMCPositionLongitude"].Value), Convert.ToDouble(gps.values["lbRMCPositionLatitude"].Value)); double greatCircleDistance = gpslocation.Distance(new SharpGis.SharpGps.Coordinate(currentWaypoint.Longitude, currentWaypoint.Latitude)); double altDistance = Math.Max(Convert.ToDouble(gps.values["lbGGAAltitude"].Value), currentWaypoint.Altitude) - Math.Min(Convert.ToDouble(gps.values["lbGGAAltitude"].Value), currentWaypoint.Altitude); return(Math.Sqrt(greatCircleDistance * greatCircleDistance + altDistance * altDistance)); } else { // Log error } return(99999999); }
/// <summary> /// Initialise Values and Config Settings /// /// </summary> public WayPointUAV() { initialised = true; // Verbinde mit Hardware ConnectPeripherals(); // theta = The pitch in degrees relative to a plane(mathematical) normal to the earth radius line at the point the plane(aircraft) is at // phi = The roll of the aircraft in degrees // psi = The true heading of the aircraft in degrees // Direkt Steuern in dem Wertänderungen soffort weitergeben werden UAVStructure performanceData = new UAVStructure("PerformanceData", ""); performanceData.values.Add(new UAVParameter("MaxBankLeft", 10)); performanceData.values.Add(new UAVParameter("MaxBankRight", 10)); performanceData.values.Add(new UAVParameter("MaxDescendAngle", 10)); performanceData.values.Add(new UAVParameter("MaxClimbAngle", 10)); uavData.Add(performanceData); BaseNavigation navi = new BaseNavigation("navi", (UAVCommons.UAVStructure)uavData["GPS"], performanceData); WaypointMissionControl waypointMissionControl = new WaypointMissionControl(navi, "Mission Control"); pilot = new BaseAutoPilot("AP", navi, (UAVStructure)uavData["GPS"], (UAVStructure)uavData["AHRS"], performanceData); navi.PassingWayPoint += new BaseNavigation.PassingWayPointHandler(navi_PassingWayPoint); navi.ap = ((BaseAutoPilot)pilot); BaseFlightStabilizer flightstabi = new FlightControlCommons.components.BaseFlightStabilizer("Flugstabilisierung", (UAVStructure)uavData["AHRS"], pilot, uavData["Querruder"], uavData["Seitenruder"], uavData["Höhenruder"]); uavData.Add(waypointMissionControl); uavData.Add(navi); uavData.Add(flightstabi); uavData.Add(pilot); uavData.ValueChanged += new MonitoredDictionary <string, UAVParameter> .ValueChangedHandler(uavData_ValueChanged); this.DataArrived += new DataArrivedHandler(RcUAV_DataArrived); }
public VTOLFlightStabilizer(string name, UAVStructure myAhrs, FlightControlCommons.components.BaseAutoPilot ap) : base(name, myAhrs, ap) { }