/// <summary>
/// Receives telemetry data from the helicopter and parses it
/// into telemetry messages
/// </summary>
/// <returns>An object containing the telemetry data</returns>
/// <exception cref="SystemException">Thrown when there is an issue parsing the received telemetry data</exception>
public override Message Receive()
{
FlightComputerTelemetryMessage data = new FlightComputerTelemetryMessage();
// data.MagX = (short)testValue;
return data;
}
public static FlightComputerTelemetryMessage CreateFromModel(GroundControlStationModel model)
{
FlightComputerTelemetryMessage msg = new FlightComputerTelemetryMessage();
msg.YawIntegralGain = model.YawIntegralGain;
msg.YawDerivativeGain = model.YawDerivativeGain;
msg.YawProportionalGain = model.YawProportionalGain;
msg.YawAntiWindupGain = model.YawAntiWindupGain;
msg.XIntegralGain = model.XIntegralGain;
msg.XDerivativeGain = model.XDerivativeGain;
msg.XProportionalGain = model.XProportionalGain;
msg.XAntiWindupGain = model.XAntiWindupGain;
msg.LongitudeInnerLoopGain = model.LongitudeInnerLoopGain;
msg.PitchAngularVelocityRadsPerSecond = model.PitchAngularVelocityRadsPerSecond;
msg.PitchAngularVelocityGain = model.PitchAngularVelocityGain;
msg.YIntegralGain = model.YIntegralGain;
msg.YDerivativeGain = model.YDerivativeGain;
msg.YProportionalGain = model.YProportionalGain;
msg.YAntiWindupGain = model.YAntiWindupGain;
msg.LateralInnerLoopGain = model.LateralInnerLoopGain;
msg.RollAngularVelocityRadsPerSecond = model.RollAngularVelocityRadsPerSecond;
msg.RollAngularVelocityGain = model.RollAngularVelocityGain;
msg.ZIntegralGain = model.ZIntegralGain;
msg.ZDerivativeGain = model.ZDerivativeGain;
msg.ZProportionalGain = model.ZProportionalGain;
msg.ZAntiWindupGain = model.ZAntiWindupGain;
msg.LatitudeDegrees = model.SimTelm.LatitudeDegrees;
msg.LongitudeDegrees = model.SimTelm.LongitudeDegrees;
// msg.AltitudeMetersAgl = model.SimTelm.ZAltitudeFtAgl * .3048f; //Removed since it should be calculated from pressure
//http://stackoverflow.com/questions/1568568/how-to-convert-euler-angles-to-directional-vector
float yawRads = Util.Util.ConvertToRads(model.SimTelm.MagHeadingDegrees);
float pitchRads = Util.Util.ConvertToRads(model.SimTelm.PitchDegrees);
float rollRads = Util.Util.ConvertToRads(model.SimTelm.RollDegrees);
//Multiply by 100 to convert to centimeters.
float[] velocitiesNEDCms = { model.SimTelm.XVelocityNEDMs * 100, model.SimTelm.YVelocityNEDMs * 100, model.SimTelm.ZVelocityNEDMs * 100};
float[,] nedToFRDRotationMatrix = Util.Util.CreateRotationMatrixTransposed(rollRads, pitchRads, yawRads);
float[] velocityFRDBodyFrameCms = Util.Util.RotateMatrix(nedToFRDRotationMatrix, velocitiesNEDCms);
/*
data.YVelocityFRDBodyFrameMs = BitConverter.ToSingle(byteReader.ReadBytes(4), 0); //x value from simulator is y value.
data.ZVelocityFRDBodyFrameMs = BitConverter.ToSingle(byteReader.ReadBytes(4), 0) * -1; //Multiply by -1 to convert to down positive. Y value from simulator *-1 is Z
data.XVelocityFRDBodyFrameMs = BitConverter.ToSingle(byteReader.ReadBytes(4), 0) * -1; //Multiply by -1 to convert to north positive. Z value from simualtor * -1 is X
*/
//model.SimTelm.XVelocityFRDBodyFrameMs = velocityFRDBodyFrameMs[0]; //x value from simulator is y value.
//model.SimTelm.YVelocityFRDBodyFrameMs = velocityFRDBodyFrameMs[1]; //Multiply by -1 to convert to down positive. Y value from simulator *-1 is Z
//model.SimTelm.ZVelocityFRDBodyFrameMs = velocityFRDBodyFrameMs[2]; //Multiply by -1 to convert to north positive. Z value from simualtor * -1 is X
msg.XVelocityFRDCms = velocityFRDBodyFrameCms[0];
msg.YVelocityFRDCms = velocityFRDBodyFrameCms[1];
msg.ZVelocityFRDCms = velocityFRDBodyFrameCms[2];
//****** Remove these since these should be calculated.
//msg.PitchRads = Util.Util.ConvertToRads(model.SimTelm.PitchDegrees);
//msg.RollRads = Util.Util.ConvertToRads(model.SimTelm.RollDegrees);
/**
* Calculate fake sensor readings from data from simulator.
*/
//Take the yaw, pitch, and roll, to create a gravitational vector
//then add linear acceleration to it
//float[,] rotationMatrix = Util.Util.CreateRotationMatrixTransposed(rollRads,pitchRads,yawRads);
//float[] rotatedUnitVector = Util.Util.RotateMatrix(nedToFRDRotationMatrix, new float[3] { 0, 0, 1 });
//float[] rotatedUnitVector = Util.Util.RotateMatrix(nedToFRDRotationMatrix, new float[3] { 0, 0, -1 });//mostrecent 11/10/2014
//I changed this back to be 1 because now the gravitational vector is in FRD frame, not the acceleration vector.
float[] rotatedUnitVector = Util.Util.RotateMatrix(nedToFRDRotationMatrix, new float[3] { 0, 0, 1 });
float xGrav = rotatedUnitVector[0] * Util.Util.GRAVITY;
float yGrav = rotatedUnitVector[1] * Util.Util.GRAVITY;
float zGrav = rotatedUnitVector[2] * Util.Util.GRAVITY;
/**
* Add linear acceleration to grav vectors
*/
float linearAccelXNEDMss = (velocitiesNEDCms[0]/100f - model.PreviousXVelocityNEDCms/100f) / Util.Util.INTERVAL_BETWEEN_SIM_DATA;
float linearAccelYNEDMss = (velocitiesNEDCms[1]/100f - model.PreviousYVelocityNEDCms/100f) / Util.Util.INTERVAL_BETWEEN_SIM_DATA;
float linearAccelZNEDMss = (velocitiesNEDCms[2]/100f - model.PreviousZVelocityNEDCms/100f) / Util.Util.INTERVAL_BETWEEN_SIM_DATA;
//Rotate linear acceleration from NED into body frame in meters per second per second (because gravity is ms^2
float[] linearAccelerationFRDBodyFrameMss = Util.Util.RotateMatrix(nedToFRDRotationMatrix, new float[] { linearAccelXNEDMss, linearAccelYNEDMss, linearAccelZNEDMss });
float[] bodyFrameVelocities = Util.Util.RotateMatrix(nedToFRDRotationMatrix, velocitiesNEDCms);
/*
System.Diagnostics.Debug.WriteLine("XLinearAccel: " + linearAccelerationFRDBodyFrameMss[0]);
System.Diagnostics.Debug.WriteLine("YLinearAccel: " + linearAccelerationFRDBodyFrameMss[1]);
System.Diagnostics.Debug.WriteLine("ZLinearAccel: " + linearAccelerationFRDBodyFrameMss[2]);
*/
//System.Diagnostics.Debug.WriteLine("ygrav before: " + yGrav + ", adjustment " + linearAccelerationFRDBodyFrameMss[1]);
//Subtract out linear acceleration because in FRD the gravitational vector is what is FRD, not acceleration due to vehicle motion.
xGrav -= linearAccelerationFRDBodyFrameMss[0];
yGrav -= linearAccelerationFRDBodyFrameMss[1];
zGrav -= linearAccelerationFRDBodyFrameMss[2];
/*xGrav += linearAccelerationFRDBodyFrameMss[0];
//yGrav += linearAccelerationFRDBodyFrameMss[1];
yGrav += linearAccelerationFRDBodyFrameMss[1];
zGrav += linearAccelerationFRDBodyFrameMss[2];*/
//System.Diagnostics.Debug.WriteLine("XLinearAccel: " + xGrav + ", YLinearAccel: " + yGrav + ", ZLinearAccel: " + zGrav + ", Xvel " + bodyFrameVelocities[0] + ", yvel " + bodyFrameVelocities[1] + ", zvel " + bodyFrameVelocities[2]);
//System.Diagnostics.Debug.WriteLine("YLinearAccel: " + yGrav);
//System.Diagnostics.Debug.WriteLine("ZLinearAccel: " + zGrav);
/*System.Diagnostics.Debug.WriteLine("XLinearV: " + msg.XVelocityFRDCms);
System.Diagnostics.Debug.WriteLine("YLinearV: " + msg.YVelocityFRDCms);
System.Diagnostics.Debug.WriteLine("ZLinearV: " + msg.ZVelocityFRDCms);*/
/*
xGrav += (velocitiesNEDCms[0] - model.PreviousXVelocityNEDCms) * (1 / Util.Util.INTERVAL_BETWEEN_SIM_DATA);
yGrav += (velocitiesNEDCms[1] - model.PreviousYVelocityNEDCms) * (1 / Util.Util.INTERVAL_BETWEEN_SIM_DATA);
zGrav += (velocitiesNEDCms[2] - model.PreviousZVelocityNEDCms) * (1 / Util.Util.INTERVAL_BETWEEN_SIM_DATA);
* */
model.PreviousXVelocityNEDCms = velocitiesNEDCms[0];
model.PreviousYVelocityNEDCms = velocitiesNEDCms[1];
model.PreviousZVelocityNEDCms = velocitiesNEDCms[2];
/**
* Calculate magnetic vector including the angle of inclination
*/
float[,] magFrameRotationMatrix = Util.Util.CreateRotationMatrix(0, Util.Util.ConvertToRads(70), 0);
float[] magFrameRotatedUnitVector = Util.Util.RotateMatrix(magFrameRotationMatrix, new float[3] { 1, 0, 0 });
//multiply the Z axis by -1 because the rotation rotates it 'down' correctly, but 'down' is technically positive in NED frame.
magFrameRotatedUnitVector[2] *= -1;
float[,] magRotationMatrix = Util.Util.CreateRotationMatrixTransposed(rollRads, pitchRads, yawRads);
float[] magRotatedUnitVector = Util.Util.RotateMatrix(magRotationMatrix, magFrameRotatedUnitVector);
/**
* Calculate position in ECEF based on lat
*/
float ecefX = 0;
float ecefY = 0;
float ecefZ = 0;
//Convert lat/long to earth centered earth fixed coordinates. Convert altitude to meters.
Util.Util.ConvertFromGeodeticToECEF(model.SimTelm.LatitudeDegrees, model.SimTelm.LongitudeDegrees, model.SimTelm.ZAltitudeFtAgl * 0.3048f, out ecefX, out ecefY, out ecefZ);
/**
* Convert velocity from Body frame to NED to ECEF
*/
//float[,] rotationMatrixVelocity = Util.Util.CreateRotationMatrix(rollRads, pitchRads, yawRads);
//float[] velocityRotatedVectorNED = Util.Util.RotateMatrix(rotationMatrixVelocity, new float[] { model.SimTelm.XVelocityFRDBodyFrameMs, model.SimTelm.YVelocityFRDBodyFrameMs, model.SimTelm.ZVelocityFRDBodyFrameMs});
float[,] rotationMatrixVelocityECEF = Util.Util.CalculateECEFToLocalNEDRotationMatrixTransposed(model.SimTelm.LatitudeDegrees, model.SimTelm.LongitudeDegrees);
float[] velocityRotatedVectorECEFCms = Util.Util.RotateMatrix(rotationMatrixVelocityECEF, velocitiesNEDCms);
/*
float[] velocityRotatedVectorECEF2 = velocityRotatedVectorECEF;
float[,] rotationMatrixVelocityNED2 = Util.Util.CalculateECEFToLocalNEDRotationMatrix(model.SimTelm.LatitudeDegrees, model.SimTelm.LongitudeDegrees);
float[] velocityRotatedVectorNED2 = Util.Util.RotateMatrix(rotationMatrixVelocityNED2, velocityRotatedVectorECEF2);
float[,] rotationMatrixVelocity2 = Util.Util.CreateRotationMatrixTransposed(rollRads, pitchRads, yawRads);
float[] velocityRotatedVectorBody = Util.Util.RotateMatrix(rotationMatrixVelocity2, new float[] { velocityRotatedVectorNED2[0], velocityRotatedVectorNED2[1], velocityRotatedVectorNED2[2]});
System.Diagnostics.Debug.WriteLine("FRD x from sim: " + model.SimTelm.XVelocityFRDBodyFrameMs);
System.Diagnostics.Debug.WriteLine("FRD x from sim recalced: " + velocityRotatedVectorBody[0]);
*/
/**
* Convert altitude to pressure
*/
//equation from http://www.engineeringtoolbox.com/air-altitude-pressure-d_462.html
//Divide by 100 to convert from Pa to Mb
float pressureMb = (101325 * (float) Math.Pow(1 - .0000225577 * (model.SimTelm.ZAltitudeFtMsl * .3048), 5.25588)) / 100;
//equation from http://www.regentsprep.org/Regents/math/algtrig/ATP8b/exponentialResource.htm
// float altitudeKmMsl = (float) model.SimTelm.ZAltitudeFtMsl / 3280.84f;
// float pressureMb = (float)(1.0 * Math.Exp((float)-1 * ((altitudeKmMsl) / 7))) * 1000;
//root (((altitude / (288.15 / (6.5 / 1000.0))) - 1), (6.5 / 1000.0) * (287.052 / 9.78)))
msg.XAccelFrdMss = xGrav;
msg.YAccelFrdMss = yGrav;
msg.ZAccelFrdMss = zGrav;
msg.YawAngularVelocityRadsPerSecond = model.SimTelm.YawVelocityRadsPerS;
msg.PitchAngularVelocityRadsPerSecond = model.SimTelm.PitchVelocityRadsPerS;
msg.RollAngularVelocityRadsPerSecond = model.SimTelm.RollVelocityRadsPerS;
msg.XMagFrd = magRotatedUnitVector[0];
msg.YMagFrd = magRotatedUnitVector[1];
msg.ZMagFrd = magRotatedUnitVector[2];
/*
msg.XEcefCm = (int)(ecefX * 100.0f);
msg.YEcefCm = (int)(ecefY * 100.0f);
msg.ZEcefCm = (int)(ecefZ * 100.0f);
msg.XVEcefCms = (int)(velocityRotatedVectorECEFCms[0]);
msg.YVEcefCms = (int)(velocityRotatedVectorECEFCms[1]);
msg.ZVEcefCms = (int)(velocityRotatedVectorECEFCms[2]);
*/
msg.PressureMillibars = pressureMb;
//four times a second, fully update the position and velocity values.
//techincally this is based on how frequently the FC reads data, so if that changes, intervalbetweensimdata would need to change to something other than that constant. but for now, they are the same.
if (counter == 0)
{
xvel = (int)(velocityRotatedVectorECEFCms[0]);
yvel = (int)(velocityRotatedVectorECEFCms[1]);
zvel = (int)(velocityRotatedVectorECEFCms[2]);
xecef = (int)(ecefX * 100.0f);
yecef = (int)(ecefY * 100.0f);
zecef = (int)(ecefZ * 100.0f);
counter++;
counter2++;
//System.Diagnostics.Debug.WriteLine("here :");
}
//else if (counter > ((1 / Util.Util.INTERVAL_BETWEEN_SIM_DATA) / 4))
else if (counter > 5)
{
counter = 0;
}
else
{
counter++;
}
TimeSpan seconds = DateTime.Now.Subtract(startTime);
if (seconds.Seconds >= 1)
{
// System.Diagnostics.Debug.WriteLine("counter2 : " + counter2 + " Seconds, " + seconds.TotalMilliseconds);
counter2 = 0;
startTime = DateTime.Now;
}
msg.XEcefCm = xecef;
msg.YEcefCm = yecef;
msg.ZEcefCm = zecef;
msg.XVEcefCms = xvel;
msg.YVEcefCms = yvel;
msg.ZVEcefCms = zvel;
msg.XRefSetpoint = model.XRefSetpoint;
msg.YRefSetpoint = model.YRefSetpoint;
msg.ZRefSetpoint = model.ZRefSetpoint;
msg.YawRefSetpoint = model.YawRefSetpoint * ((float)Math.PI / (float)180);
return msg;
}
public static FlightComputerTelemetryMessage BuildMessageSt(byte[] byteBuffer)
{
FlightComputerTelemetryMessage fct = new FlightComputerTelemetryMessage();
fct.BuildMessage(byteBuffer);
return fct;
}