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)
{
}
