public Position(Position position)
{
this.Latitude = position.Latitude;
this.Longitude = position.Longitude;
this.Altitude = position.Altitude;
this.Press_Altitude = position.Press_Altitude;
}
public void Init(Telemetry tel)
{
// Do not move N computation below, N is used by LLAtoECEF method!
gpsLLA = tel.uavPosition;
gpsLLA.Press_Altitude = gpsLLA.Altitude - localGeoidHeight;
N = a / Math.Sqrt(1 - e2 * Math.Pow(Math.Sin(gpsLLA.Latitude), 2));
cosLat = Math.Cos(gpsLLA.Latitude);
sinLat = Math.Sin(gpsLLA.Latitude);
cosLon = Math.Cos(gpsLLA.Longitude);
sinLon = Math.Sin(gpsLLA.Longitude);
gpsECEF = LLAtoECEF(gpsLLA);
uavAngles = tel.uavAngles;
cosYaw = Math.Cos(uavAngles.Yaw);
sinYaw = Math.Sin(uavAngles.Yaw);
if (zeroUavPitchRoll)
{
cosPitch = cosRoll = 1;
sinPitch = sinRoll = 0;
}
else
{
cosPitch = Math.Cos(uavAngles.Pitch);
sinPitch = Math.Sin(uavAngles.Pitch);
cosRoll = Math.Cos(uavAngles.Roll);
sinRoll = Math.Sin(uavAngles.Roll);
}
R_ECEF_LNED = null;
R_LNED_ECEF = null;
R_LNED_BODY = null;
R_BODY_LNED = null;
}
public Telemetry()
{
uavAngles = new Angles();
uavPosition = new Position();
uavAcceleration = new Acceleration();
uavSpeed = new Speed();
}
public Position RoundEarthNE_2_LatLon(double posNorth_ft, double posEast_ft, double OrLat_rad, double OrLon_rad)
{
// Formula: lat = posNorth_ft / EarthRadius_ft + lat_origin
// Formula: lon = (posEast_ft / (EarthRadius_ft * cos ( lat ))) + lon_origin
Position ECEFWp = new Position();
ECEFWp.Latitude = posNorth_ft / EARTH_RADIUS_FT + OrLat_rad;
ECEFWp.Longitude = (posEast_ft / (EARTH_RADIUS_FT * Math.Cos(ECEFWp.Latitude)));
return ECEFWp;
}
//public ServiceDescription GetDescription()
//{
// ServiceDescription s = new ServiceDescription();
// s.Name = "DEM Manager Service";
// s.Description = "This service provides a set of DEM support functions.";
// s.Functions = new FunctionDescription[3];
// s.Functions[0] = new FunctionDescription();
// s.Functions[0].Name = "GetAltitude";
// s.Functions[0].parameters = new ParameterDescription[1];
// s.Functions[0].parameters[0] = new ParameterDescription();
// s.Functions[0].parameters[0].Name = "point";
// s.Functions[0].parameters[0].Type = "USAL.Position";
// s.Functions[0].ReturnType = "double";
// s.Functions[1] = new FunctionDescription();
// s.Functions[1].Name = "SetPrecision";
// s.Functions[1].parameters = new ParameterDescription[1];
// s.Functions[1].parameters[0] = new ParameterDescription();
// s.Functions[1].parameters[0].Name = "precision";
// s.Functions[1].parameters[0].Type = "Dem.Precison";
// s.Functions[1].ReturnType = "void";
// s.Functions[2] = new FunctionDescription();
// s.Functions[2].Name = "SetWorkingArea";
// s.Functions[2].parameters = new ParameterDescription[2];
// s.Functions[2].parameters[0] = new ParameterDescription();
// s.Functions[2].parameters[0].Name = "bottomLeft";
// s.Functions[2].parameters[0].Type = "USAL.Position";
// s.Functions[2].parameters = new ParameterDescription[2];
// s.Functions[2].parameters[1] = new ParameterDescription();
// s.Functions[2].parameters[1].Name = "topRight";
// s.Functions[2].parameters[1].Type = "USAL.Position";
// s.Functions[2].ReturnType = "void";
// return s;
//}
private WgsPoint convertUsalPosToWgsPoint(USAL.Position usal_wp)
{
return(new WgsPoint(usal_wp.Latitude * 180 / Math.PI, usal_wp.Longitude * 180 / Math.PI, usal_wp.Altitude));
}
// LLA to LNED
public Vector3 LLAtoLNED(Position LLA)
{
return ECEFtoLNED(LLAtoECEF(LLA));
}
// ECEF to LLA
public Position ECEFtoLLA(Vector3 ECEF)
{
double p = Math.Sqrt(ECEF.x * ECEF.x + ECEF.y * ECEF.y);
double g = Math.Atan2(ECEF.z * a, p * b);
Position LLA = new Position();
LLA.Latitude = Math.Atan2(ECEF.z + ep2 * b * Math.Pow(Math.Sin(g), 3), p - e2 * a * Math.Pow(Math.Cos(g), 3));
LLA.Longitude = Math.Atan2(ECEF.y, ECEF.x);
LLA.Altitude = Convert.ToSingle(p / Math.Cos(LLA.Latitude) - N);
LLA.Press_Altitude = LLA.Altitude - localGeoidHeight;
return (LLA);
}
// LLA to ECEF
public Vector3 LLAtoECEF(Position LLA)
{
return new Vector3 ((N + LLA.Altitude) * Math.Cos(LLA.Latitude) * Math.Cos(LLA.Longitude),
(N + LLA.Altitude) * Math.Cos(LLA.Latitude) * Math.Sin(LLA.Longitude),
(N * (1 - e2) + LLA.Altitude) * Math.Sin(LLA.Latitude));
}
// Get LLA coordinates of pixel [m,n] (m:row, n:column)
public Position GetPixelGeolocation(int m, int n)
{
Position targetLLA = new Position(nedFrame.GpsLLA);
Vector3 targetNED = new Vector3();
ArrayList historic_dem = new ArrayList();
int index, i = 0;
if (RTDPSettings.geolocationSettings.zeroCamDistance)
{
Vector3 pixelNED = nedFrame.BODYtoLNED(camFrame.PIXELtoBODY(m, n));
double aux_h = nedFrame.GpsLLA.Press_Altitude;
double aux_x = pixelNED.x / pixelNED.z;
double aux_y = pixelNED.y / pixelNED.z;
double dem_alt, target_alt= 0;
do
{
dem_alt = GetDemAltitude(targetLLA);
index = historic_dem.IndexOf(dem_alt);
if (index == -1)
{
target_alt = dem_alt;
targetNED.z = aux_h - target_alt;
targetNED.x = targetNED.z * aux_x;
targetNED.y = targetNED.z * aux_y;
targetLLA = nedFrame.LNEDtoLLA(targetNED);
historic_dem.Add(dem_alt);
i++;
}
else if (index != historic_dem.Count - 1)
{
double sum_dem = 0;
for (int j = index; j < historic_dem.Count; j++)
sum_dem += (double)historic_dem[j];
target_alt = sum_dem / (historic_dem.Count - index);
targetNED.z = aux_h - target_alt;
targetNED.x = targetNED.z * aux_x;
targetNED.y = targetNED.z * aux_y;
targetLLA = nedFrame.LNEDtoLLA(targetNED);
i++;
}
}
while ((index == -1) && (i < RTDPSettings.geolocationSettings.maxIterations));
}
else
{
Vector3 pixelNED = nedFrame.BODYtoLNED(camFrame.PIXELtoBODY(m, n));
double aux_h = nedFrame.GpsLLA.Press_Altitude;
double aux_x = (pixelNED.x - cameraNED.x) / (pixelNED.z - cameraNED.z);
double aux_y = (pixelNED.y - cameraNED.y) / (pixelNED.z - cameraNED.z);
double dem_alt, target_alt= 0;
do
{
dem_alt = GetDemAltitude(targetLLA);
index = historic_dem.IndexOf(dem_alt);
if (index == -1)
{
target_alt = dem_alt;
targetNED.z = aux_h - target_alt;
targetNED.x = cameraNED.x + (targetNED.z - cameraNED.z) * aux_x;
targetNED.y = cameraNED.y + (targetNED.z - cameraNED.z) * aux_y;
targetLLA = nedFrame.LNEDtoLLA(targetNED);
historic_dem.Add(dem_alt);
i++;
}
else if (index != historic_dem.Count - 1)
{
double sum_dem = 0;
for (int j = index; j < historic_dem.Count; j++)
sum_dem += (double)historic_dem[j];
target_alt = sum_dem / (historic_dem.Count - index);
targetNED.z = aux_h - target_alt;
targetNED.x = cameraNED.x + (targetNED.z - cameraNED.z) * aux_x;
targetNED.y = cameraNED.y + (targetNED.z - cameraNED.z) * aux_y;
targetLLA = nedFrame.LNEDtoLLA(targetNED);
i++;
}
}
while ((index == -1) && (i < RTDPSettings.geolocationSettings.maxIterations));
}
return targetLLA;
}
public double GetDemAltitude(Position lla)
{
return (double)DemManager.DemManagerService.GetInstance().FunctionCall("GetAltitude", new object[] { lla });
}
