public static void Main(string[] args)
{
Trilateration tri = new Trilateration();
tri.calculateLocation(-84, 44, 92.1791733528, 98, 64, 171.011695506, 116, -108, 171.39719951);
Console.WriteLine("X: {0} Y: {1}", tri.x, tri.y);
}
private Location GetLocation(List <Satellite> satellites)
{
Point p1 = new Point(satellites[0].Location.X, satellites[0].Location.Y, satellites[0].ReceivedMessage.Distance);
Point p2 = new Point(satellites[1].Location.X, satellites[1].Location.Y, satellites[1].ReceivedMessage.Distance);
Point p3 = new Point(satellites[2].Location.X, satellites[2].Location.Y, satellites[2].ReceivedMessage.Distance);
Point a = Trilateration.Compute(p1, p2, p3);
return(new Location(a.X, a.Y));
}
public void TestTrilaterationCompute()
{
Point p1 = new Point(0, 3, 4);
Point p2 = new Point(0, 0, 5);
Point p3 = new Point(4, 0, 3);
Point expected = new Point(4, 3);
Point actual = Trilateration.Compute(p1, p2, p3);
Assert.AreEqual(expected.X, actual.X);
Assert.AreEqual(expected.Y, actual.Y);
}
private static void RangeProcessing(Coord3D[] anchorArray, List <uint> distances)
{
Coord3D bestSolution;
var status = Trilateration.GetLocation(out bestSolution, anchorArray,
distances.ToArray());
if (status != Trilateration.Status.TRIL_3SPHERES &&
status != Trilateration.Status.TRIL_4SPHERES)
{
return;
}
Console.Out.WriteLine($"{status}:\t({bestSolution})");
}
private PositionModel getPosition(TrilaterationModel satelliteKenobi, TrilaterationModel satelliteSkywalker, TrilaterationModel satelliteSato)
{
TrilaterationModel result;
try
{
result = Trilateration.getTrilateration(satelliteKenobi, satelliteSkywalker, satelliteSato);
}
catch (Exception)
{
throw new ErrorException("Ocurrió un error al obtener localización de emisor.");
}
return(new PositionModel()
{
X = Math.Round(result.X, 2),
Y = Math.Round(result.Y, 2)
});
}
public void Trilaterate()
{
bool shouldFindSolution = true;
foreach (Transform sphere in sphereList)
{
//make sure spheres are colliding...
if (!sphere.GetComponent <SphereBehavior> ().isColliding)
{
sphere.GetComponent <SphereBehavior> ().MakeCollide();
return;
}
}
if (shouldFindSolution)
{
foreach (Transform sphere in sphereList)
{
SignalSnapShot snap = new SignalSnapShot {
pos = sphere.position,
radius = sphere.localScale.x / 2
};
signals.Add(snap);
}
Vector3 newPos = Trilateration.Trilaterate(
signals [0],
signals [1],
signals [2]
);
if (newPos != Vector3.one)
{
marker.position = newPos;
//debug positions
textList [2].text = "ROUTER: " + marker.position.ToString();
}
else
{
textList [2].text = "NO Solution";
}
signals.Clear();
}
}
public Position GetLocation(List <AddSatelliteObject> satelliteList)
{
foreach (var satellite in satelliteList)
{
var satInfo = satellitesInformation.FirstOrDefault(x => x.Name == satellite.Name);
if (satInfo != null)
{
pointList.Add(new Point(satInfo.PositionX, satInfo.PositionY, satellite.Distance));
}
}
var position =
Trilateration.Compute(pointList.ElementAt(0), pointList.ElementAt(1), pointList.ElementAt(2));
if (position != null)
{
return(new Position(position[0], position[1]));
}
return(null);
}
private double GetMoveLength(int rp, int pp, int gp, short time)
{
List <Vector3> posStack = new List <Vector3>();
Vector3 startPos = Vector3.zero;
Vector3 endPos = Vector3.zero;
bool ackFail = false;
// Start Pos
uwbManager.AddTagEvent(this.tag_id, delegate(Vector3 pos)
{
if (gp == 0)
{
posStack.Add(new Vector3(pos.x, 0.0, pos.z));
}
else
{
posStack.Add(new Vector3(0.0, pos.y, 0.0));
}
});
while (posStack.Count < POS_COLLECT_COUNT)
{
Thread.Sleep(200);
}
uwbManager.RemoveTagEvent(this.tag_id);
startPos = new Vector3(posStack.Average(item => item.x),
posStack.Average(item => item.y),
posStack.Average(item => item.z));
posStack.Clear();
mainForm.UpdateCalibrationLog(String.Format("Start Pos: {0:00.00}, {1:00.00}, {2:00.00}",
startPos.x, startPos.y, startPos.z));
// Move
drone.PCMD_Move((sbyte)rp, (sbyte)pp, 0, (sbyte)gp, time);
// End Pos
drone.AddACKCheckEvent(delegate()
{
Timer timer = new Timer(delegate(object state)
{
uwbManager.AddTagEvent(this.tag_id, delegate(Vector3 pos)
{
if (gp == 0)
{
posStack.Add(new Vector3(pos.x, 0.0, pos.z));
}
else
{
posStack.Add(new Vector3(0.0, pos.y, 0.0));
}
});
}, null, time, Timeout.Infinite);
}, 2000, "Calibration time out.",
delegate()
{
uwbManager.RemoveTagEvent(this.tag_id);
ackFail = true;
});
while (posStack.Count < POS_COLLECT_COUNT)
{
Thread.Sleep(200);
if (ackFail)
{
return(-1.0);
}
}
uwbManager.RemoveTagEvent(this.tag_id);
endPos = new Vector3(posStack.Average(item => item.x),
posStack.Average(item => item.y),
posStack.Average(item => item.z));
posStack.Clear();
mainForm.UpdateCalibrationLog(String.Format("End Pos: {0:00.00}, {1:00.00}, {2:00.00}",
endPos.x, endPos.y, endPos.z));
return(Trilateration.vdist(startPos, endPos));
}
public async Task <Result <ShipResponse> > Handle(Command request, CancellationToken cancellationToken)
{
List <string> messageItems = new List <string>();
//Iterate for each satellite in order to update data and process the message to decode
foreach (var satellite in request.Satellites)
{
var satelliteFromDb = await context.Satellites.Where(x => x.Name == satellite.Name).FirstOrDefaultAsync(cancellationToken: cancellationToken);
if (satelliteFromDb == null)
{
return(Result <ShipResponse> .Failure(string.Format("Failed to find a satellite named '{0}'", satellite.Name)));
}
satelliteFromDb.Distance = satellite.Distance;
satelliteFromDb.LastUpdatedDate = DateTime.Now;
if (satellite.Message != null && satellite.Message.Count > 0)
{
satelliteFromDb.Message = satellite.Message;
if (messageItems.Count == 0)
{
for (int c = 0; c < satellite.Message.Count; c++)
{
messageItems.Add("");
}
}
for (int c = 0; c < satellite.Message.Count; c++)
{
if (satellite.Message[c] != string.Empty)
{
var itemFound = messageItems.IndexOf(satellite.Message[c]);
if (itemFound == -1)
{
messageItems[c] = satellite.Message[c];
}
}
}
}
}
messageItems.RemoveAll(x => string.IsNullOrWhiteSpace(x));
//Save changes
await context.SaveChangesAsync(cancellationToken);
var satellites = await context.Satellites.ToListAsync(cancellationToken : cancellationToken);
//Process the response
var shipResponse = new ShipResponse();
//Calculate Ship Position by Trilateration
double[] position = Trilateration.Compute(new Point(satellites[0].Y, satellites[0].X, satellites[0].Distance),
new Point(satellites[1].Y, satellites[1].X, satellites[1].Distance),
new Point(satellites[2].Y, satellites[2].X, satellites[2].Distance));
if (position == null || messageItems.Count == 0)
{
return(Result <ShipResponse> .Success(null));
}
shipResponse.Position = new Position {
Y = Math.Round(position[0], 2), X = Math.Round(position[1], 2)
};
shipResponse.Message = string.Join(" ", messageItems);
return(Result <ShipResponse> .Success(shipResponse));
}
