public static Vector3D Transform(Matrix3D m, Vector3D v)
{
return new Vector3D(v.X * m.M11 + v.Y * m.M12 + v.Z * m.M13,
v.X * m.M21 + v.Y * m.M22 + v.Z * m.M23,
v.X * m.M31 + v.Y * m.M32 + v.Z * m.M33);
}
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 static Vector3D Subtract(Vector3D a, Vector3D b)
{
return new Vector3D(a.X - b.X, a.Y - b.Y, a.Z - b.Z);
}
public static Vector3D Scale(Vector3D v, double s)
{
return new Vector3D(v.X * s, v.Y * s, v.Z * s);
}
public static double Dot(Vector3D a, Vector3D b)
{
return (double) (a.X * b.X + a.Y * b.Y + a.Z * b.Z);
}
public static Vector3D Add(Vector3D a, Vector3D b)
{
return new Vector3D(a.X + b.X, a.Y + b.Y, a.Z + b.Z);
}
public bool Equals(Vector3D that)
{
return this.X == that.X && this.Y == that.Y && this.Z == that.Z;
}
