public static double distanceFrom(Position p1, Position p2)
{
double xDistance = p2.x - p1.x;
double yDistance = p2.y - p1.y;
double zDistance = p2.z - p1.z;
return calculateRadius(xDistance, yDistance, zDistance);
}
public void Calculate(Position expectedPosition, Position actualPosition)
{
// 5 meters = 0 to 16 ft would be approx +/- 16 ft from your actual
double[] randomPoint = SimRandom.GetRandomPointWithinSphere(maxRadius);
expectedPosition.x = actualPosition.x + randomPoint[0];
expectedPosition.y = actualPosition.y + randomPoint[1];
expectedPosition.z = actualPosition.z + randomPoint[2];
}
public void Calculate(Position actualPosition, Position expectedPosition,
double expectedXTickDistance, double expectedYTickDistance, double expectedZTickDistance,
double theta, double phi, double radius)
{
if (this.theta==null)
{
this.theta = theta;
this.phi = phi;
this.radius = radius;
if (SimRandom.GetRandom(-1,1)>0)
{
posnegTheta *= -1;
}
if (SimRandom.GetRandom(-1, 1) > 0)
{
posnegPhi *= -1;
}
}
double x, y, z;
// Now apply new measrue
++imuTicks;
this.theta += (degreesPerTickTheta * posnegTheta);
this.phi += (degreesPerTickPhi * posnegPhi);
y = this.radius.Value * Math.Sin(getAngle(this.phi.Value)) * Math.Cos(getAngle(this.theta.Value));
x = this.radius.Value * Math.Sin(getAngle(this.phi.Value)) * Math.Sin(getAngle(this.theta.Value));
z = this.radius.Value * Math.Cos(getAngle(this.phi.Value));
// Continue on your wrong path
expectedPosition.x += expectedXTickDistance;
expectedPosition.y += expectedYTickDistance;
expectedPosition.z += expectedZTickDistance;
if (expectedXTickDistance<0)
{
x *= -1;
}
if (expectedYTickDistance < 0)
{
y *= -1;
}
if (expectedZTickDistance < 0)
{
z *= -1;
}
actualPosition.x += x;
actualPosition.y += y;
actualPosition.z += z;
}
public SimulatedObject(Position origin, Position destination, double velocity, double imuGyroAccuracy, double imuAccelAccuracy, int roundTripTime, double gpsLoss)
{
positionLogic = new PositionProtocolLogic(imuGyroAccuracy, imuAccelAccuracy, roundTripTime, gpsLoss);
actualPosition.Clone(origin);
prevActualPosition.Clone(origin);
expectedPosition.Clone(origin);
prevExpectedPosition.Clone(origin);
targetPosition.Clone(destination);
// I want to
speed = velocity;
tickSpeed = speed / SimulatorController.ticksPerSecond;
}
public void incTicks(Position actualPosition)
{
++ticks;
//if ( (gpsLossInTicks>0) && (ticks > gpsLossInTicks) &&(!activeIMU))
//{
// activeGPS = false;
// activePerfectPosition = false;
// activeIMU = true;
//}
if ((gpsLossInTicks > 0) && (ticks > gpsLossInTicks) && (!activeRSSI))
{
activeGPS = false;
activePerfectPosition = false;
activeRSSI = true;
activeIMU = true;
}
}
public void updatePosition(double actualDistance, double tickSpeed,
Position targetPosition, Position actualPosition, Position expectedPosition,
double expectedXTickDistance, double expectedYTickDistance, double expectedZTickDistance,
double theta, double phi, double radius)
{
if (actualDistance > tickSpeed)
{
// Move towards the goal using where we think we are
// and allowing that to affect the actual positioning
if (shouldMeasure())
{
getExpectedPosition(actualPosition, expectedPosition,
expectedXTickDistance, expectedYTickDistance, expectedZTickDistance,
theta, phi, radius);
}
else
{
// Else continue on your wrong path
actualPosition.x += expectedXTickDistance;
actualPosition.y += expectedYTickDistance;
actualPosition.z += expectedZTickDistance;
expectedPosition.x += expectedXTickDistance;
expectedPosition.y += expectedYTickDistance;
expectedPosition.z += expectedZTickDistance;
}
}
else
{
// Last step
if (actualDistance > tickSpeed)
{
actualPosition.x += expectedXTickDistance;
actualPosition.y += expectedYTickDistance;
actualPosition.z += expectedZTickDistance;
expectedPosition.Clone(actualPosition);
}
else
{
actualPosition.Clone(targetPosition);
}
}
}
public Position trilaterate(Position actualP0)
{
Position expectedP1 = nodes[0].expectedPosition;
Position expectedP2 = nodes[1].expectedPosition;
Position expectedP3 = nodes[2].expectedPosition;
Position actualP1 = nodes[0].actualPosition;
Position actualP2 = nodes[1].actualPosition;
Position actualP3 = nodes[2].actualPosition;
// RSSI should be able to get distance measurement to within 3 feet
// For this we use the actual distance
double r1 = Position.distanceFrom(actualP1, actualP0) + SimRandom.GetRandom(accuracyMin, accuracyMax);
double r2 = Position.distanceFrom(actualP2, actualP0) + SimRandom.GetRandom(accuracyMin, accuracyMax);
double r3 = Position.distanceFrom(actualP3, actualP0) + SimRandom.GetRandom(accuracyMin, accuracyMax);
// Now we trilaterate using the expected positions but use the RSSI distance/radius calculations from above.
var rv = Position.trilaterate(expectedP1, expectedP2, expectedP3, r1, r2, r3);
return rv;
}
public static Position trilaterate(Position p1, Position p2, Position p3, double r1, double r2, double r3)
{
var ex = vector_divide(vector_subtract(p2, p1), calculateRadius(vector_subtract(p2, p1)));
var i = dot(ex, vector_subtract(p3, p1));
var a = vector_subtract(vector_subtract(p3, p1), vector_multiply(ex, i));
var ey = vector_divide(a, calculateRadius(a));
var ez = vector_cross(ex, ey);
var d = calculateRadius(vector_subtract(p2, p1));
var j = dot(ey, vector_subtract(p3, p1));
var x = (square(r1) - square(r2) + square(d)) / (2 * d);
var y = (square(r1) - square(r3) + square(i) + square(j)) / (2 * j) - (i / j) * x;
var z = Math.Sqrt(square(r1) - square(x) - square(y));
Position rv = vector_add(p1, vector_add(vector_multiply(ex, x), vector_multiply(ey, y)));
//var p4a = vector_add(a, vector_multiply(ez, z));
//var p4b = vector_subtract(a, vector_multiply(ez, z));
return rv;
}
public static double calculateRadius(Position p)
{
return Math.Sqrt(square(p.x) + square(p.y) + square(p.z));
}
private async void goButton_Click(object sender, EventArgs e)
{
if (!simulationRunning)
{
enableORdisable(false);
origPicBox.Visible = false;
destPicBox.Visible = false;
int repeatCount = 1;
try
{
repeatCount = Convert.ToInt32(Repeat_tb.Text);
}
catch
{
}
tokenSource = new CancellationTokenSource();
double tX = x2 - x1;
double tY = y2 - y1;
double tZ = z2 - z1;
totalSimulationDistance = Math.Sqrt(tX * tX + tY * tY + tZ * tZ);
if (Log_cb.Checked)
{
// Cut it short by GPS length to avoid last minute bad decisions
totalSimulationDistance -= GPS_Module.maxRadius;
}
simulationDuration = Math.Round(totalSimulationDistance / velocity, 4);
double imuGyroAccuracy = Convert.ToDouble(IMU_GyroAccuracy_dd.Text);
double imuAccelAccuracy = Convert.ToDouble(IMU_AccelAccuracy_dd.Text);
double gpsLoss = Convert.ToDouble(gpsLoss_tb.Text);
int roundTripTime = Convert.ToInt32(RTT_tb.Text);
StringBuilder sb = new StringBuilder();
StreamWriter logFile = null;
if (Log_cb.Checked)
{
string logFileName = @"C:\temp\cosc636.csv";
try {
logFile = new StreamWriter(logFileName);
sb.Append("totalSimulationDistance " + totalSimulationDistance.ToString());
sb.Append(",");
sb.Append("simulationDuration " + simulationDuration.ToString());
sb.Append(",");
sb.Append("imuGyroAccuracy " + imuGyroAccuracy.ToString());
sb.Append(",");
sb.Append("imuAccelAccuracy " + imuAccelAccuracy.ToString());
sb.Append(",");
sb.Append("gpsLoss " + gpsLoss.ToString());
logFile.WriteLine(sb.ToString());
}
catch {
MessageBox.Show("Path to " + logFileName + " cannot be written to.");
}
}
for (int simCount = 0; !tokenSource.Token.IsCancellationRequested && simCount < repeatCount; simCount++)
{
resultsListBox.Items.Clear();
clearXY();
paintZ();
simulationRunning = true;
infoLabel.Text = "Distance = " + Math.Round(totalSimulationDistance, 0).ToString() + " feet. Expected Flight Duration in Seconds = " + simulationDuration.ToString();
stopwatch.Reset();
stopwatch.Start();
mainTimer.Interval = 100;
mainTimer.Enabled = true;
goButton.Text = "STOP!";
int formationStyle = Convert.ToInt32(totalSimulatedObjects_dd.SelectedIndex);
Position origin = new Position(x1, y1, z1);
Position destination = new Position(x2, y2, z2);
stv.speedTrackbarValue = speedTrackBar.Value;
controller = new SimulatorController(xyAxisPanel, simulationPictureBox, zAxisPictureBox, stv,
formationStyle, simulationDuration, origin, destination, velocity, imuGyroAccuracy, imuAccelAccuracy, roundTripTime, gpsLoss);
Application.DoEvents();
await Task.Run(() =>
{
controller.Run(tokenSource);
});
sb.Clear();
foreach (var info in controller.getDistances())
{
double distance = info.Item1;
string descript = info.Item2;
if (Log_cb.Checked) {
// We cut it short by GPS length to avoid last minute bad decisions
distance -= GPS_Module.maxRadius;
}
sb.Append(distance.ToString());
sb.Append(",");
// Assume any return value in description is to indicate NOGPS
sb.Append(descript.Length>0 ? "0" : "1");
sb.Append(",");
resultsListBox.Items.Add(Math.Round(distance, 2).ToString() + " " + descript);
}
if (logFile != null)
{
if (sb.Length > 0)
{
sb.Remove(sb.Length - 1, 1);
}
logFile.WriteLine(sb.ToString());
}
}
if (logFile != null)
{
logFile.Flush();
logFile.Close();
}
}
else
{
tokenSource.Cancel();
}
stopwatch.Stop();
mainTimer.Enabled = false;
infoLabel.Text = directions;
goButton.Text = "GO!";
simulationRunning = false;
enableORdisable(true);
}
private void getExpectedPosition(Position actualPosition, Position expectedPosition,
double expectedXTickDistance, double expectedYTickDistance, double expectedZTickDistance,
double theta, double phi, double radius)
{
if ((activeRSSI) && (rssi.remoteMeasurementTransmitRequest(ticks))) {
// We need to request a measurement
// Nothing really to do at the moment but right here's where we could remember "when" the request was made
}
if ((activeRSSI) && (rssi.remoteMeasurementRequestReceived(ticks))) {
// We need to request a measurement
calculatedTrilateratePosition = rssi.trilaterate(actualPosition);
}
if (activePerfectPosition)
{
actualPosition.x += expectedXTickDistance;
actualPosition.y += expectedYTickDistance;
actualPosition.z += expectedZTickDistance;
expectedPosition.Clone(actualPosition);
}
else if ((activeGPS) && (gps.shouldMeasure(ticks)))
{
actualPosition.x += expectedXTickDistance;
actualPosition.y += expectedYTickDistance;
actualPosition.z += expectedZTickDistance;
gps.Calculate(expectedPosition, actualPosition);
}
else if ( (activeRSSI) && (rssi.remoteMeasurementRequestReturned(ticks)) && (calculatedTrilateratePosition!= null) )
{
actualPosition.x += expectedXTickDistance;
actualPosition.y += expectedYTickDistance;
actualPosition.z += expectedZTickDistance;
expectedPosition.Clone(calculatedTrilateratePosition);
// Reset the IMU for new theta
imu.reset();
}
else if (activeIMU)
{
// Constantly apply
imu.Calculate(actualPosition, expectedPosition, expectedXTickDistance, expectedYTickDistance, expectedZTickDistance,
theta, phi, radius);
}
else
{
throw new Exception("ERROR: No other position measuring devices exist!");
}
}
private static Position vector_add(Position p1, Position p2)
{
Position rv = new Position();
rv.x = p1.x + p2.x;
rv.y = p1.y + p2.y;
rv.z = p1.z + p2.z;
return rv;
}
// Credits to: https://github.com/gheja
// based on: https://github.com/gheja/trilateration.js/blob/master/trilateration.js
// which was based on: https://en.wikipedia.org/wiki/Trilateration
private static double dot(Position p1, Position p2)
{
return p1.x * p2.x + p1.y * p2.y + p1.z * p2.z;
}
public double distanceFrom(Position target)
{
double xDistance = target.x - this.x;
double yDistance = target.y - this.y;
double zDistance = target.z - this.z;
return calculateRadius(xDistance, yDistance, zDistance);
}
public void Clone(Position p)
{
this.x = p.x;
this.y = p.y;
this.z = p.z;
}
private static Position vector_cross(Position p1, Position p2)
{
Position rv = new Position();
rv.x = p1.y * p2.z - p1.z * p2.y;
rv.y = p1.z * p2.x - p1.x * p2.z;
rv.z = p1.x * p2.y - p1.y * p2.x;
return rv;
}
private static Position vector_divide(Position p1, double number)
{
Position rv = new Position();
rv.x = p1.x / number;
rv.y = p1.y / number;
rv.z = p1.z / number;
return rv;
}
private static Position vector_subtract(Position p1, Position p2)
{
Position rv = new Position();
rv.x = p1.x - p2.x;
rv.y = p1.y - p2.y;
rv.z = p1.z - p2.z;
return rv;
}
private static Position vector_multiply(Position p1, double number)
{
Position rv = new Position();
rv.x = p1.x * number;
rv.y = p1.y * number;
rv.z = p1.z * number;
return rv;
}
