static int FlushAsync(IntPtr L)
{
try
{
ToLua.CheckArgsCount(L, 1);
System.IO.TextWriter obj = (System.IO.TextWriter)ToLua.CheckObject <System.IO.TextWriter>(L, 1);
System.Threading.Tasks.Task o = obj.FlushAsync();
ToLua.PushObject(L, o);
return(1);
}
catch (Exception e)
{
return(LuaDLL.toluaL_exception(L, e));
}
}
private static async Task InternalWriteAllLinesAsync(TextWriter writer, IEnumerable <string> contents, CancellationToken cancellationToken)
{
Debug.Assert(writer != null);
Debug.Assert(contents != null);
using (writer)
{
foreach (string line in contents)
{
cancellationToken.ThrowIfCancellationRequested();
// Note that this working depends on the fix to #14563, and cannot be ported without
// either also porting that fix, or explicitly checking for line being null.
await writer.WriteLineAsync(line).ConfigureAwait(false);
}
cancellationToken.ThrowIfCancellationRequested();
await writer.FlushAsync().ConfigureAwait(false);
}
}
static void Drive(Func<Tuple<FlightData, double>> source, TextWriter recordWriter, Action<String> transmit, Action<Stats> reportStats)
{
// How long to sleep between polling, in ms
int interval = 10;
int history = 3;
double[] ts = new double[history];
Vector3D[] pos = new Vector3D[history];
// Yaw, pitch, roll
Vector3D[] seats = new Vector3D[history];
Vector3D[] vAirframe = new Vector3D[history];
Vector3D[] vSeats = new Vector3D[history];
Vector3D[] fs = new Vector3D[history];
long counter = 0;
double deg20 = ToRadians(20);
double deg6 = ToRadians(6);
double G = 32.174F;
Vector3D gravity = new Vector3D(0, 0, G);
// Where the seat is relative to the center of rotation,
// in feet, in aircraft coords.
Vector3D seatPos = new Vector3D(10, 0, -3);
// The G force on the various parts of the seat when at rest.
double backNeutralG = sin(deg20);
double seatNeutralG = cos(deg6);
Vector3D blNormal = Transform(YPR(deg20, deg20, 0),
new Vector3D(-1, 0, 0));
Vector3D brNormal = Transform(YPR(-deg20, deg20, 0),
new Vector3D(-1, 0, 0));
Vector3D slNormal = Transform(YPR(0, deg6, deg20),
new Vector3D(0, 0, 1));
Vector3D srNormal = Transform(YPR(0, deg6, -deg20),
new Vector3D(0, 0, 1));
while (true)
{
long index = counter % history;
var sourceData = source();
FlightData data = sourceData.Item1;
double t = sourceData.Item2;
double x = data.x;
double y = data.y;
double z = data.z;
double yaw = data.yaw;
double pitch = data.pitch;
double roll = data.roll;
// Rotating index into the various history arrays
int[] ago = new int[history];
for (int n = 0; n < history; ++n)
{
ago[n] = (int) ((index - n + history) % history);
}
int now = ago[0];
ts[now] = t;
pos[now] = new Vector3D(x, y, z);
// If position hasn't changed, we've probably read the
// same sampled data twice - skip updating, since it's
// going to look like we've instantly gone to zero
// speed.
if (!pos[now].Equals(pos[ago[1]]))
{
Matrix3D xform = YPR(yaw, pitch, roll);
seats[now] = Transform(xform, seatPos);
vAirframe[now] = new Vector3D(data.xDot, data.yDot, data.zDot);
//vAirframe[now] = Subtract(pos[now], pos[ago[1]]);
vSeats[now] = Scale(Subtract(seats[now], seats[ago[1]]), ts[now] - ts[ago[1]]);
Vector3D bln = Transform(xform, blNormal);
Vector3D brn = Transform(xform, brNormal);
Vector3D sln = Transform(xform, slNormal);
Vector3D srn = Transform(xform, srNormal);
// ft/sec/sec
double deltaT = ts[now] - ts[ago[1]];
Vector3D aAirframe = Scale(Subtract(vAirframe[now], vAirframe[ago[1]]), 1.0 / deltaT);
Vector3D aSeat = Scale(Subtract(vSeats[now], vSeats[ago[1]]), 1.0 / deltaT);
Vector3D acc = Add(aAirframe, aSeat);
// Turns out the position and velocity data coming
// from Falcon is pretty noisy. We smooth things out
// to help with this.
double smoothing = 0.9;
fs[now] = Add(Scale(fs[ago[1]], smoothing),
Scale(Add(gravity, Scale(acc, -1.0)), 1.0 - smoothing));
Vector3D f = fs[now];
double bl = Dot(f, bln);
double br = Dot(f, brn);
double sl = Dot(f, sln);
double sr = Dot(f, srn);
double blG = bl / G;
double brG = br / G;
double slG = sl / G;
double srG = sr / G;
// TODO: Map the neutral (1G) position as .25
long commandBL = Position(blG, backNeutralG);
long commandBR = Position(brG, backNeutralG);
long commandSL = Position(slG, seatNeutralG);
long commandSR = Position(srG, seatNeutralG);
Stats stats;
stats.Forces.BL = bl;
stats.Forces.BR = br;
stats.Forces.SL = sl;
stats.Forces.SR = sr;
stats.G.BL = blG;
stats.G.BR = brG;
stats.G.SL = slG;
stats.G.SR = srG;
stats.Commands.BL = commandBL;
stats.Commands.BR = commandBR;
stats.Commands.SL = commandSL;
stats.Commands.SR = commandSR;
stats.Yaw = yaw;
stats.Pitch = pitch;
stats.Roll = roll;
stats.VAC = vAirframe[now];
stats.DVAC = aAirframe;
stats.DVSeat = aSeat;
stats.DVTot = f;
stats.T = ts[now];
stats.DeltaT = ts[now] - ts[ago[1]];
// We skip the first few frames until we have enough
// history to do calculation
if (counter > history)
{
reportStats(stats);
//ts[now] - ts[ago[1]], brG, blG, srG, slG);
if (recordWriter != null)
{
if (_recordFlushOp != null)
{
_recordFlushOp.Wait();
}
recordWriter.WriteLine("{0}, {1}, {2}, {3}, {4}, {5}, {6}, {7}, {8}, {9}, {10}, {11}, {12}, {13}, {14}, {15}, {16}, {17}",
t,
data.x, data.y, data.z,
data.xDot, data.yDot, data.zDot,
yaw, pitch, roll,
commandBL, commandBR,
commandSL, commandSR,
acc.X, acc.Y, acc.Z,
data.alpha);
_recordFlushOp = recordWriter.FlushAsync();
}
transmit(String.Format("M BL {0}", commandBL));
transmit(String.Format("M BR {0}", commandBR));
transmit(String.Format("M SL {0}", commandSL));
transmit(String.Format("M SR {0}", commandSR));
}
++counter;
}
Sleep(interval);
}
}
public override Task FlushAsync()
{
lock (this) {
return(writer.FlushAsync());
}
}
