public override void Contribute(Output aApparatusOutput, ref Input aApparatusInput)
{
float heading = CorrectRotation(aApparatusOutput.Navigation.Yaw);
aApparatusInput.Yaw = Arithmetics.KeepInRange(CorrectRotation((Value) - (heading))*Agression, Input.Limits.Yaw.Min,
Input.Limits.Yaw.Max);
if (CanBeObtained)
{
if (Math.Abs(aApparatusInput.Yaw) < 0.05) _obtainCandidates++;
else _obtainCandidates = 0;
if (_obtainCandidates > 3) Obtained = true;
}
}
/// <summary>
/// Thread looping method resposible for Autopilot to queued new objectives.
/// </summary>
protected override void Loop(CancellationToken token)
{
while (!token.IsCancellationRequested)
{
if (!Active)
{
ClearOutputQueue();
Thread.Sleep(10);
}
else
{
Apparatus.Output output;
if (ApparatusOutputQueue.TryDequeue(out output))
{
Objective currentIntent = GetCurrentObjective();
var input = new Apparatus.Input();
input.Reset();
currentIntent.Contribute(output, ref input);
input.Send(DroneClient);
LastInput = input;
if (OnCommandSend != null)
{
OnCommandSend.Invoke(input);
}
}
else
{
Thread.Sleep(3);
}
}
}
}
public override void Contribute(Output aApparatusOutput, ref Input aApparatusInput)
{
aApparatusInput.Command = Input.Type.Land;
}
public override void Contribute(Output aApparatusOutput, ref Input aApparatusInput)
{
aApparatusInput.Gaz = Arithmetics.KeepInRange(Value, Input.Limits.Gaz.Min, Input.Limits.Gaz.Max);
}
public override void Contribute(Output aApparatusOutput, ref Input aApparatusInput)
{
aApparatusInput.Command = Input.Type.ResetEmergency;
Obtained = true;
}
public override void Contribute(Output aApparatusOutput, ref Input aApparatusInput)
{
aApparatusInput.Command = Input.Type.FlatTrim;
Obtained = true;
}
public override void Contribute(Output aApparatusOutput, ref Input aApparatusInput)
{
float altitude = Arithmetics.KeepInRange(aApparatusOutput.Navigation.Altitude, Min, Max);
aApparatusInput.Gaz = Arithmetics.KeepInRange(Diff(altitude)*Agression, Input.Limits.Gaz.Min, Input.Limits.Gaz.Max);
}
public override void Contribute(Output aApparatusOutput, ref Input aApparatusInput)
{
aApparatusInput.Pitch = -CalculateVelocityManeuver(aApparatusOutput.Navigation.Velocity.X);
}
public abstract void Contribute(Output aApparatusOutput, ref Input aApparatusInput);
/// <summary>
/// Thread looping method resposible for Autopilot to queued new objectives.
/// </summary>
protected override void Loop(CancellationToken token)
{
while (!token.IsCancellationRequested)
{
if (!Active)
{
ClearOutputQueue();
Thread.Sleep(10);
}
else
{
Apparatus.Output output;
if (ApparatusOutputQueue.TryDequeue(out output))
{
Objective currentIntent = GetCurrentObjective();
var input = new Apparatus.Input();
input.Reset();
currentIntent.Contribute(output, ref input);
input.Send(DroneClient);
LastInput = input;
if (OnCommandSend != null) OnCommandSend.Invoke(input);
}
else Thread.Sleep(3);
}
}
}