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();
}
// Constructor. CameraProperties remain constant during the mission
public CAMFrame(CAMProperties camera)
{
zeroCamDistance = RTDPSettings.geolocationSettings.zeroCamDistance;
zeroCamPanRoll = RTDPSettings.geolocationSettings.zeroCamPanRoll;
camPosition = camera.camPosition;
scaleFactor = camera.pixelPitch;
offsetX = (camera.imageHeight - 1) * scaleFactor / 2;
offsetY = (camera.imageWidth - 1) * scaleFactor / 2;
focalLength = camera.focalLength;
camAngles = camera.camAngles;
cosPitch = Math.Cos(camAngles.Pitch);
sinPitch = Math.Sin(camAngles.Pitch);
if (zeroCamPanRoll)
{
cosYaw = cosRoll = 1;
sinYaw = sinRoll = 0;
}
else
{
cosYaw = Math.Cos(camAngles.Yaw);
sinYaw = Math.Sin(camAngles.Yaw);
cosRoll = Math.Cos(camAngles.Roll);
sinRoll = Math.Sin(camAngles.Roll);
}
R_BODY_CAM = null;
R_CAM_BODY = null;
}
public Angles camAngles; // pan/yaw/azimut: 0-to the nose, 90-to the right wing
// tilt/pitch/elevation: 0-vertical down, 90-horizontal
// roll
public CAMProperties()
{
camPosition = new Vector3();
camAngles = new Angles();
}