public bool CalTargetByMatrix(out Locate3D position)
{
double x, y = 0;
Matrix m = new Matrix();
Input i1 = new Input()
{
x = x1,
y = y1,
z = z1,
r = range1,
};
Input i2 = new Input()
{
x = x2,
y = y2,
z = z2,
r = range2,
};
Input i3 = new Input()
{
x = x3,
y = y3,
z = z3,
r = range3,
};
double[] D = new double[3];
MatrixLocate.InitMatrix(ref m, i1, i2, i3, ref D);
position = new Locate3D(DateTime.UtcNow);
if (MatrixLocate.locate(m, D, out x, out y) != 1)
{
return(false);
}
var diff = range1 * range1 - (y - y1) * (y - y1) - (x - x1) * (x - x1);
if (diff < 0)
{
return(false);
}
double z = Math.Sqrt(diff);
position = new Locate3D(DateTime.UtcNow, x, y, z);
return(true);
}
private string Locate(int index)
{
string log = "";
TriangleLocate locate = null;
Locate3D targetpos = new Locate3D(DateTime.UtcNow);
if (index == 0)
{
locate = UnitCore.Instance.Locate1;
}
if (index == 1)
{
locate = UnitCore.Instance.Locate2;
}
var itor = locate.Buoys.GetEnumerator();
float presure = 0.0f;
while (itor.MoveNext())
{
log += index.ToString() + " - " + "(" + itor.Current.Key.ToString() + ":" + "Lat" + itor.Current.Value.Lat.ToString() + " Long" + itor.Current.Value.Lng.ToString() + " Distance" + itor.Current.Value.Range.ToString() + ")";
}
for (int i = 0; i < 4; i++)
{
if (index == 0)
{
if (((Buoy)UnitCore.Instance.Buoy[i]).teleRange1.Crc == 0)
{
float p;
var ret = float.TryParse(((Buoy)UnitCore.Instance.Buoy[i]).teleRange1.Presure, out p);
if (ret)
{
presure = p;
}
}
}
if (index == 1)
{
if (((Buoy)UnitCore.Instance.Buoy[i]).teleRange2.Crc == 0)
{
float p;
var ret = float.TryParse(((Buoy)UnitCore.Instance.Buoy[i]).teleRange2.Presure, out p);
if (ret)
{
presure = p;
}
}
}
}
if (MonProtocol.TriangleLocate.UseMatrix)
{
Dxy1 = 0;
Dxy2 = 0;
if (locate.CalTargetByMatrix(out targetpos))
{
targetpos.centerLat = center.Lat;
targetpos.centerLng = center.Lng;
if (index == 0)
{
UnitCore.Instance.TargetObj1 = new Target("AUV1")
{
ID = (byte)UnitCore.Instance.MonConfigueService.GetSetup().AUVID1,
Status = "已定位",
UTCTime = targetpos.Time,
Longitude = (float)Util.LongOffset(targetpos.centerLng, targetpos.centerLat, targetpos.X),
Latitude = (float)Util.LatOffset(targetpos.centerLat, targetpos.Y),
Depth = presure,// (float)(targetpos.Z + UnitCore.Instance.MonConfigueService.GetSetup().SonarDepth),
};
log += "定位结果:" + "long:" + UnitCore.Instance.TargetObj1.Longitude + " lat:" + UnitCore.Instance.TargetObj1.Latitude;
RefreshTarget1();
}
if (index == 1)
{
UnitCore.Instance.TargetObj2 = new Target("AUV2")
{
ID = (byte)UnitCore.Instance.MonConfigueService.GetSetup().AUVID2,
Status = "已定位",
UTCTime = targetpos.Time,
Longitude = (float)Util.LongOffset(targetpos.centerLng, targetpos.centerLat, targetpos.X),
Latitude = (float)Util.LatOffset(targetpos.centerLat, targetpos.Y),
Depth = presure,// (float)(targetpos.Z + UnitCore.Instance.MonConfigueService.GetSetup().SonarDepth),
};
log += "定位结果:" + "long:" + UnitCore.Instance.TargetObj2.Longitude + " lat:" + UnitCore.Instance.TargetObj2.Latitude;
RefreshTarget2();
}
}
else
{
Dxy1 = 0;
Dxy2 = 0;
log += "定位结果:未成功定位";
}
}
else
{
var sonardepth = (float)UnitCore.Instance.MonConfigueService.GetSetup().SonarDepth;
MonProtocol.TriangleLocate.SonarDepth = sonardepth / 1000;
var result = locate.CalTargetByApproach();
if (index == 0)
{
UnitCore.Instance.TargetObj1 = new Target("AUV1")
{
ID = (byte)UnitCore.Instance.MonConfigueService.GetSetup().AUVID1,
Status = "已定位",
UTCTime = DateTime.UtcNow,
Longitude = (float)result[1],
Latitude = (float)result[0],
Depth = presure,//(float)result[2],
};
Dxy1 = (float)result[3];
log += "定位结果:" + "long:" + UnitCore.Instance.TargetObj1.Longitude + " lat:" + UnitCore.Instance.TargetObj1.Latitude + " D=" + Dxy1.ToString("F06");
RefreshTarget1();
}
if (index == 1)
{
UnitCore.Instance.TargetObj2 = new Target("AUV2")
{
ID = (byte)UnitCore.Instance.MonConfigueService.GetSetup().AUVID2,
Status = "已定位",
UTCTime = DateTime.UtcNow,
Longitude = (float)result[1],
Latitude = (float)result[0],
Depth = presure,//(float)result[2],
};
Dxy2 = (float)result[3];
log += "定位结果:" + "long:" + UnitCore.Instance.TargetObj2.Longitude + " lat:" + UnitCore.Instance.TargetObj2.Latitude + " D=" + Dxy2.ToString("F06");
RefreshTarget2();
}
}
UnitCore.Instance.MonTraceService.Save("Position", log);
return(log);
}
