//i need to take the transpose of this matrix.
internal Orientation Copy()
{
Orientation o = new Orientation();
o.phi_rollDegrees = phi_rollDegrees;
o.sai_yawDegrees = sai_yawDegrees;
o.theta_pitchDegrees = theta_pitchDegrees;
return o;
}
//static void Main(string[] args)
//{
// Receiver receiver = new Receiver();
// receiver.Start();
// Measurement measurement = new Measurement();
// Position positionInertialFrame = new Position();
// positionInertialFrame.XLatitudePosition = 32.0;
// positionInertialFrame.YLongitudePosition = 44.0;
// positionInertialFrame.ZAltitudePosition = 22.0;
// measurement.Position = positionInertialFrame;
// Orientation orientation = new Orientation();
// orientation.Phi_rollDegrees = 10.0;
// orientation.Sai_yawDegrees = 10.0;
// orientation.Theta_pitchDegrees = 10.0;
// measurement.Orientation = orientation;
// Measurement referenceMeasurement = new Measurement();
// positionInertialFrame = new Position();
// positionInertialFrame.XLatitudePosition = 42.0;
// positionInertialFrame.YLongitudePosition = 54.0;
// positionInertialFrame.ZAltitudePosition = 32.0;
// referenceMeasurement.Position = positionInertialFrame;
// Orientation referenceOrientation = new Orientation();
// referenceOrientation.Phi_rollDegrees = 20.0;
// referenceOrientation.Sai_yawDegrees = 25.0;
// referenceOrientation.Theta_pitchDegrees = 20.0;
// referenceMeasurement.Orientation = referenceOrientation;
// Velocity referenceVelocity = new Velocity();
// referenceVelocity.XVelocityBodyFrame = 4.4;
// referenceVelocity.YVelocityBodyFrame = 5.5;
// referenceVelocity.ZVelocityBodyFrame = 1.2;
// referenceMeasurement.Velocity = referenceVelocity;
// referenceMeasurement.ReferenceYaw = 0.0;
// ControlLaw controllerLaw = new ControlLaw();
// controllerLaw.setReferenceMeasurement(referenceMeasurement);
// controllerLaw.initializationMeasurment(measurement);
// measurement = new Measurement();
// positionInertialFrame = new Position();
// positionInertialFrame.XLatitudePosition = 34.0;
// positionInertialFrame.YLongitudePosition = 46.0;
// positionInertialFrame.ZAltitudePosition = 24.0;
// measurement.Position = positionInertialFrame;
// orientation = new Orientation();
// orientation.Phi_rollDegrees = 13.0;
// orientation.Sai_yawDegrees = 15.0;
// orientation.Theta_pitchDegrees = 13.0;
// measurement.Orientation = orientation;
// double ulonold = 0.0;
// double ulatold = 0.0;
// double ucolold = 0.0;
// double upedold = 0.0;
// for (int i = 0; i < 4; i++)
// {
// controllerLaw.processMeasurement(measurement);
// double ulon = controllerLaw.Getulon_MainRotorLongitudinalCyclicControlCommand_Pitch();
// double ulat = controllerLaw.Getulat_MainRotorLateralCyclicControlCommand_Roll();
// double ucol = controllerLaw.Getucol_MainRotorCollectiveControlCommand();
// double uped = controllerLaw.Getuped_TailRotorPedalControlCommand();
// ulonold = ulon;
// ulatold = ulat;
// ucolold = ucol;
// upedold = uped;
// }
// Measurement measurementcloser = new Measurement();
// Position closerPosition = new Position();
// Orientation closerOrientation = new Orientation();
// closerPosition.XLatitudePosition = 34.0;
// closerPosition.YLongitudePosition = 46.0;
// closerPosition.ZAltitudePosition = 24.0;
// measurementcloser.Position = positionInertialFrame;
// /******************************* im setting the orientation the same so i just change 1 thing at a time*/
// closerOrientation = new Orientation();
// closerOrientation.Phi_rollDegrees = 13.0;
// closerOrientation.Sai_yawDegrees = 15.0;
// closerOrientation.Theta_pitchDegrees = 13.0;
// //closerOrientation = referenceOrientation.Copy();
// measurementcloser.Orientation = orientation;
// //List<Tuple<double, double, double, double>> diffs = new List<Tuple<double, double, double, double>>();
// List<double[]> diffs = new List<double[]>();
// bool NotDone = true;
// while (NotDone)
// {
// /*closerPosition = new Position();
// measurementcloser = new Measurement();
// closerOrientation = new Orientation();*/
// NotDone = false;
// if (closerOrientation.Phi_rollDegrees < referenceOrientation.Phi_rollDegrees)
// {
// closerOrientation.Phi_rollDegrees += .2;
// NotDone = true;
// }
// if (closerOrientation.Sai_yawDegrees < referenceOrientation.Sai_yawDegrees)
// {
// closerOrientation.Sai_yawDegrees += .1;
// NotDone = true;
// }
// if (closerOrientation.Theta_pitchDegrees < referenceOrientation.Theta_pitchDegrees)
// {
// closerOrientation.Theta_pitchDegrees += .3;
// NotDone = true;
// }
// if (closerPosition.XLatitudePosition < referenceMeasurement.Position.XLatitudePosition)
// {
// closerPosition.XLatitudePosition += .1;
// NotDone = true;
// }
// if (closerPosition.YLongitudePosition < referenceMeasurement.Position.YLongitudePosition)
// {
// closerPosition.YLongitudePosition += .1;
// NotDone = true;
// }
// if (closerPosition.ZAltitudePosition < referenceMeasurement.Position.ZAltitudePosition)
// {
// closerPosition.ZAltitudePosition += .1;
// NotDone = true;
// }
// measurementcloser.Orientation = closerOrientation;
// measurementcloser.Position = closerPosition;
// controllerLaw.processMeasurement(measurementcloser);
// double ulon = controllerLaw.Getulon_MainRotorLongitudinalCyclicControlCommand_Pitch();
// double ulat = controllerLaw.Getulat_MainRotorLateralCyclicControlCommand_Roll();
// double ucol = controllerLaw.Getucol_MainRotorCollectiveControlCommand();
// double uped = controllerLaw.Getuped_TailRotorPedalControlCommand();
// double diff1 = ulon-ulonold;
// double diff2 = ulat-ulatold;
// double diff3 = ucol-ucolold;
// double diff4 = uped-upedold;
// diffs.Add(new double[]{diff1, diff2, diff3, diff4});
// ulonold = ulon;
// ulatold = ulat;
// ucolold = ucol;
// upedold = uped;
// }
// Console.Out.WriteLine("Done");
// //if I process the same measurements, i expect the new values to be greater because it isn't moving.
// //if i process measurements that are closer to the reference, i expect values to be less since it's closer.
//}
static void Main(string[] args)
{
Receiver2 receiver = new Receiver2();
receiver.Start();
Measurement measurement = new Measurement();
Position positionInertialFrame = new Position();
positionInertialFrame.XLatitudePosition = 32.0;
positionInertialFrame.YLongitudePosition = 44.0;
positionInertialFrame.ZAltitudePosition = 22.0;
measurement.Position = positionInertialFrame;
Orientation orientation = new Orientation();
orientation.Phi_rollDegrees = 10.0;
orientation.Sai_yawDegrees = 10.0;
orientation.Theta_pitchDegrees = 10.0;
measurement.Orientation = orientation;
Measurement referenceMeasurement = new Measurement();
positionInertialFrame = new Position();
positionInertialFrame.XLatitudePosition = 42.0;
positionInertialFrame.YLongitudePosition = 54.0;
positionInertialFrame.ZAltitudePosition = 32.0;
referenceMeasurement.Position = positionInertialFrame;
Orientation referenceOrientation = new Orientation();
referenceOrientation.Phi_rollDegrees = 20.0;
referenceOrientation.Sai_yawDegrees = 25.0;
referenceOrientation.Theta_pitchDegrees = 20.0;
referenceMeasurement.Orientation = referenceOrientation;
Velocity referenceVelocity = new Velocity();
referenceVelocity.XVelocityBodyFrame = 4.4;
referenceVelocity.YVelocityBodyFrame = 5.5;
referenceVelocity.ZVelocityBodyFrame = 1.2;
referenceMeasurement.Velocity = referenceVelocity;
referenceMeasurement.ReferenceYaw = 0.0;
ControlLaw controllerLaw = new ControlLaw();
controllerLaw.setReferenceMeasurement(referenceMeasurement);
measurement = new Measurement();
positionInertialFrame = new Position();
positionInertialFrame.XLatitudePosition = 34.0;
positionInertialFrame.YLongitudePosition = 46.0;
positionInertialFrame.ZAltitudePosition = 24.0;
measurement.Position = positionInertialFrame;
orientation = new Orientation();
orientation.Phi_rollDegrees = 13.0;
orientation.Sai_yawDegrees = 15.0;
orientation.Theta_pitchDegrees = 13.0;
measurement.Orientation = orientation;
double ulonold = 0.0;
double ulatold = 0.0;
double ucolold = 0.0;
double upedold = 0.0;
for (int i = 0; i < 4; i++)
{
//controllerLaw.processMeasurement(measurement, file);
double ulon = controllerLaw.Getulon_MainRotorLongitudinalCyclicControlCommand_Pitch();
double ulat = controllerLaw.Getulat_MainRotorLateralCyclicControlCommand_Roll();
double ucol = controllerLaw.Getucol_MainRotorCollectiveControlCommand();
double uped = controllerLaw.Getuped_TailRotorPedalControlCommand();
ulonold = ulon;
ulatold = ulat;
ucolold = ucol;
upedold = uped;
}
Measurement measurementcloser = new Measurement();
Position closerPosition = new Position();
Orientation closerOrientation = new Orientation();
closerPosition.XLatitudePosition = 34.0;
closerPosition.YLongitudePosition = 46.0;
closerPosition.ZAltitudePosition = 24.0;
measurementcloser.Position = positionInertialFrame;
/******************************* im setting the orientation the same so i just change 1 thing at a time*/
closerOrientation = new Orientation();
closerOrientation.Phi_rollDegrees = 13.0;
closerOrientation.Sai_yawDegrees = 15.0;
closerOrientation.Theta_pitchDegrees = 13.0;
//closerOrientation = referenceOrientation.Copy();
measurementcloser.Orientation = orientation;
//List<Tuple<double, double, double, double>> diffs = new List<Tuple<double, double, double, double>>();
List<double[]> diffs = new List<double[]>();
bool NotDone = true;
while (NotDone)
{
/*closerPosition = new Position();
measurementcloser = new Measurement();
closerOrientation = new Orientation();*/
NotDone = false;
if (closerOrientation.Phi_rollDegrees < referenceOrientation.Phi_rollDegrees)
{
closerOrientation.Phi_rollDegrees += .2;
NotDone = true;
}
if (closerOrientation.Sai_yawDegrees < referenceOrientation.Sai_yawDegrees)
{
closerOrientation.Sai_yawDegrees += .1;
NotDone = true;
}
if (closerOrientation.Theta_pitchDegrees < referenceOrientation.Theta_pitchDegrees)
{
closerOrientation.Theta_pitchDegrees += .3;
NotDone = true;
}
if (closerPosition.XLatitudePosition < referenceMeasurement.Position.XLatitudePosition)
{
closerPosition.XLatitudePosition += .1;
NotDone = true;
}
if (closerPosition.YLongitudePosition < referenceMeasurement.Position.YLongitudePosition)
{
closerPosition.YLongitudePosition += .1;
NotDone = true;
}
if (closerPosition.ZAltitudePosition < referenceMeasurement.Position.ZAltitudePosition)
{
closerPosition.ZAltitudePosition += .1;
NotDone = true;
}
measurementcloser.Orientation = closerOrientation;
measurementcloser.Position = closerPosition;
// controllerLaw.processMeasurement(measurementcloser);
double ulon = controllerLaw.Getulon_MainRotorLongitudinalCyclicControlCommand_Pitch();
double ulat = controllerLaw.Getulat_MainRotorLateralCyclicControlCommand_Roll();
double ucol = controllerLaw.Getucol_MainRotorCollectiveControlCommand();
double uped = controllerLaw.Getuped_TailRotorPedalControlCommand();
double diff1 = ulon - ulonold;
double diff2 = ulat - ulatold;
double diff3 = ucol - ucolold;
double diff4 = uped - upedold;
diffs.Add(new double[] { diff1, diff2, diff3, diff4 });
ulonold = ulon;
ulatold = ulat;
ucolold = ucol;
upedold = uped;
}
Console.Out.WriteLine("Done");
//if I process the same measurements, i expect the new values to be greater because it isn't moving.
//if i process measurements that are closer to the reference, i expect values to be less since it's closer.
}
public static Position RotatePositionFromInertialFrameToBodyFrame(Orientation orientation, Position currentPosition)
{
Position newPosition = new Position();
double[,] rotationMatrix = GetRotationMatrixFromInertialToBodyFrame(orientation);
double[,] positionMatrix = currentPosition.GetPositionMatrix();
double[,] rotatedPosition = new double[,] { { 0 }, { 0 }, { 0 } };
/*rotatedPosition[0,0] = rotationMatrix[0,0]*positionMatrix[0,0] + rotationMatrix[0,1]*positionMatrix[1,0] + rotationMatrix[0,2]*positionMatrix[2,0];
rotatedPosition[1,0] = rotationMatrix[1,0]*positionMatrix[0,0] + rotationMatrix[1,1]*positionMatrix[1,0] + rotationMatrix[1,2]*positionMatrix[2,0];
rotatedPosition[2,0] = rotationMatrix[2,0]*positionMatrix[0,0] + rotationMatrix[2,1]*positionMatrix[1,0] + rotationMatrix[2,2]*positionMatrix[2,0];
*/
//for each column in rotation matrix through columns
for (int i = 0; i < rotationMatrix.GetLength(1); i++)
{
for (int j = 0; j < positionMatrix.GetLength(0); j++)
{
rotatedPosition[i, 0] += rotationMatrix[i, j] * positionMatrix[j, 0];
}
}
newPosition.XLatitudePosition = rotatedPosition[0, 0];
newPosition.YLongitudePosition = rotatedPosition[1, 0];
newPosition.ZAltitudePosition = rotatedPosition[2, 0];
return newPosition;
}
//public static double[] ConvertFromECEFToLocalNED(double[] ecefReferencePosition, double[] ecefCurrentPosition)
//{
// double[,] rotationMatrix = GetRotationMatrixFromInertialToBodyFrame(orientation);
// double[,] positionMatrix = currentPosition.GetPositionMatrix();
// double[,] rotatedPosition = new double[,] { { 0 }, { 0 }, { 0 } };
// /*rotatedPosition[0,0] = rotationMatrix[0,0]*positionMatrix[0,0] + rotationMatrix[0,1]*positionMatrix[1,0] + rotationMatrix[0,2]*positionMatrix[2,0];
// rotatedPosition[1,0] = rotationMatrix[1,0]*positionMatrix[0,0] + rotationMatrix[1,1]*positionMatrix[1,0] + rotationMatrix[1,2]*positionMatrix[2,0];
// rotatedPosition[2,0] = rotationMatrix[2,0]*positionMatrix[0,0] + rotationMatrix[2,1]*positionMatrix[1,0] + rotationMatrix[2,2]*positionMatrix[2,0];
// */
// //for each column in rotation matrix through columns
// for (int i = 0; i < rotationMatrix.GetLength(1); i++)
// {
// for (int j = 0; j < positionMatrix.GetLength(0); j++)
// {
// rotatedPosition[i, 0] += rotationMatrix[i, j] * positionMatrix[j, 0];
// }
// }
// newPosition.XLatitudePosition = rotatedPosition[0, 0];
// newPosition.YLongitudePosition = rotatedPosition[1, 0];
// newPosition.ZAltitudePosition = rotatedPosition[2, 0];
// return newPosition;
//}
public static Position RotatePositionFromGeodedicFrameToBodyFrame(Orientation orientation, Position currentPosition, Position originalReferencePosition)
{
//Rotate from Geodedic to earth centered earth fixed
/* Position currentEcefPosition = new Position();
double[] ecef = ConvertFromGeodeticToECEF(currentPosition.YLongitudePosition, currentPosition.XLatitudePosition, currentPosition.ZAltitudePosition);
currentEcefPosition.XLatitudePosition = ecef[0];
currentEcefPosition.YLongitudePosition = ecef[1];
currentEcefPosition.ZAltitudePosition = ecef[2];*/
/*
Position referenceEcefPosition = new Position();
ecef = ConvertFromGeodeticToECEF(originalReferencePosition.YLongitudePosition, originalReferencePosition.XLatitudePosition, originalReferencePosition.ZAltitudePosition);
referenceEcefPosition.XLatitudePosition = ecef[0];
referenceEcefPosition.YLongitudePosition = ecef[1];
referenceEcefPosition.ZAltitudePosition = ecef[2];
*/
//Rotate from earth centered earth fixed to local NED (which is the same as vehicle carried NED)
double[,] ecefToNEDRotationMatrix = calculateEcefToNEDMatrix(originalReferencePosition);
Position currentMinusRef = new Position();
currentMinusRef.XLatitudePosition = currentPosition.XLatitudePosition - originalReferencePosition.XLatitudePosition;
currentMinusRef.YLongitudePosition = currentPosition.YLongitudePosition - originalReferencePosition.YLongitudePosition;
currentMinusRef.ZAltitudePosition = currentPosition.ZAltitudePosition - originalReferencePosition.ZAltitudePosition;
double[,] positionMatrix = currentMinusRef.GetPositionMatrix();
double[,] rotatedPosition = new double[,] { { 0 }, { 0 }, { 0 } };
Position currentPositionLocalNED = new Position();
//for each column in rotation matrix through columns
for (int i = 0; i < ecefToNEDRotationMatrix.GetLength(1); i++)
{
for (int j = 0; j < positionMatrix.GetLength(0); j++)
{
rotatedPosition[i, 0] += ecefToNEDRotationMatrix[i, j] * positionMatrix[j, 0];
}
}
currentPositionLocalNED.XLatitudePosition = rotatedPosition[0, 0];
currentPositionLocalNED.YLongitudePosition = rotatedPosition[1, 0];
currentPositionLocalNED.ZAltitudePosition = rotatedPosition[2, 0];
//Rotate from vehicle carried NED to body NED
double[,] NEDToBodyRotationMatrix = calculateNEDToBodyMatrix(orientation);
positionMatrix = currentPositionLocalNED.GetPositionMatrix();
rotatedPosition = new double[,] { { 0 }, { 0 }, { 0 } };
Position currentPositionBody = new Position();
//for each column in rotation matrix through columns
for (int i = 0; i < NEDToBodyRotationMatrix.GetLength(1); i++)
{
for (int j = 0; j < positionMatrix.GetLength(0); j++)
{
rotatedPosition[i, 0] += NEDToBodyRotationMatrix[i, j] * positionMatrix[j, 0];
}
}
currentPositionBody.XLatitudePosition = rotatedPosition[0, 0];
currentPositionBody.YLongitudePosition = rotatedPosition[1, 0];
currentPositionBody.ZAltitudePosition = rotatedPosition[2, 0];
return currentPositionBody;
}
public static double[,] GetRotationMatrixFromInertialToBodyFrame(Orientation orientation)
{
//TODO Handle for the case where you hit a singularity at +-Pi/2
double[,] rotationMatrixBodyToInertial = GetRotationMatrixFromBodyToInertialFrame(orientation);
double[,] rotationMatrixInertialToBody = new double[,]{{0,0,0},{0,0,0},{0,0,0}};
for (int i = 0; i < rotationMatrixBodyToInertial.GetLength(0); i++)
{
for (int j = 0; j < rotationMatrixBodyToInertial.GetLength(0); j++)
{
rotationMatrixInertialToBody[j, i] = rotationMatrixBodyToInertial[i, j];
}
}
return rotationMatrixInertialToBody;
}
public static double[,] GetRotationMatrixFromBodyToInertialFrame(Orientation orientation)
{
//TODO Handle for the case where you hit a singularity at +-Pi/2
double[,] rotationMatrixBodyToInertial = new double[,] {
{ Sin(orientation.Theta_pitchDegrees) * Cos(orientation.Sai_yawDegrees), Sin(orientation.Phi_rollDegrees) * Sin(orientation.Theta_pitchDegrees) * Cos(orientation.Sai_yawDegrees) - Cos(orientation.Phi_rollDegrees) * Sin(orientation.Sai_yawDegrees), Cos(orientation.Phi_rollDegrees) * Sin(orientation.Theta_pitchDegrees) * Cos(orientation.Sai_yawDegrees) + Sin(orientation.Phi_rollDegrees) * Sin(orientation.Sai_yawDegrees) },
{ Cos(orientation.Theta_pitchDegrees) * Sin(orientation.Sai_yawDegrees), Sin(orientation.Phi_rollDegrees) * Sin(orientation.Theta_pitchDegrees) * Sin(orientation.Sai_yawDegrees) + Cos(orientation.Phi_rollDegrees) * Cos(orientation.Sai_yawDegrees), Cos(orientation.Phi_rollDegrees) * Sin(orientation.Theta_pitchDegrees) * Sin(orientation.Sai_yawDegrees) - Sin(orientation.Phi_rollDegrees) * Cos(orientation.Sai_yawDegrees) },
{ Sin(orientation.Theta_pitchDegrees) * -1, Sin(orientation.Phi_rollDegrees) * Cos(orientation.Theta_pitchDegrees), Cos(orientation.Phi_rollDegrees) * Cos(orientation.Theta_pitchDegrees) } };
return rotationMatrixBodyToInertial;
}
public static double[,] calculateNEDToBodyMatrix(Orientation currentOrientation)
{
double[,] rotationMatrix = new double[,] {
{ Cos(currentOrientation.Theta_pitchDegrees)*Cos(currentOrientation.Sai_yawDegrees), Cos(currentOrientation.Theta_pitchDegrees)*Sin(currentOrientation.Sai_yawDegrees), -1 * Sin(currentOrientation.Theta_pitchDegrees) },
{ Sin(currentOrientation.Phi_rollDegrees)*Sin(currentOrientation.Theta_pitchDegrees)*Cos(currentOrientation.Sai_yawDegrees) - Cos(currentOrientation.Phi_rollDegrees)*Sin(currentOrientation.Sai_yawDegrees), Sin(currentOrientation.Phi_rollDegrees)*Sin(currentOrientation.Theta_pitchDegrees)*Sin(currentOrientation.Sai_yawDegrees)+Cos(currentOrientation.Phi_rollDegrees)*Cos(currentOrientation.Sai_yawDegrees), Sin(currentOrientation.Phi_rollDegrees) * Cos(currentOrientation.Theta_pitchDegrees)},
{ Cos(currentOrientation.Phi_rollDegrees)*Sin(currentOrientation.Theta_pitchDegrees)*Cos(currentOrientation.Sai_yawDegrees)+Sin(currentOrientation.Phi_rollDegrees)*Sin(currentOrientation.Sai_yawDegrees), Cos(currentOrientation.Phi_rollDegrees)*Sin(currentOrientation.Theta_pitchDegrees)*Sin(currentOrientation.Sai_yawDegrees) - Sin(currentOrientation.Phi_rollDegrees)*Cos(currentOrientation.Sai_yawDegrees), Cos(currentOrientation.Phi_rollDegrees)*Cos(currentOrientation.Theta_pitchDegrees)} };
return rotationMatrix;
}
internal void Start()
{
// Form1.trackBar1.Value = (int) (Util.K_SAI * 10.0);
Form1.trackBar1.Value = (int)(Util.K_Z * 1000.0);
Form1.trackBar2.Value = (int)(Util.K_W * 1000.0);
Form1.trackBar3.Value = (int)(Util.Ti_Z * 1000.0);
Form1.trackBar4.Value = (int)(Util.K_ETA_Z * 1000.0);
using (System.IO.StreamWriter file = new System.IO.StreamWriter(@"C:\Heli\logs\log1.csv"))
{
using (System.IO.StreamWriter file2 = new System.IO.StreamWriter(@"C:\Heli\logs\log1pid.csv"))
{
Measurement measurement = new Measurement();
Position positionInertialFrame = new Position();
positionInertialFrame.XLatitudePosition = 0;
positionInertialFrame.YLongitudePosition = 0;
positionInertialFrame.ZAltitudePosition = 0;
measurement.Position = positionInertialFrame;
Orientation orientation = new Orientation();
orientation.Phi_rollDegrees = 0.0;
orientation.Sai_yawDegrees = 0.0;
orientation.Theta_pitchDegrees = 0.0;
measurement.Orientation = orientation;
positionInertialFrame = new Position();
Measurement referenceMeasurement = new Measurement();
Orientation referenceOrientation = new Orientation();
/*
* Stand still
* 3 feet
* 0 deg
*/
//positionInertialFrame.XLatitudePosition = 47.500312805175781;
//positionInertialFrame.YLongitudePosition = -122.21683502197266;
//referenceOrientation.Sai_yawDegrees = 0.0;
//positionInertialFrame.ZAltitudePosition = 3.0;
/*
* Stand still
* 10 feet
* 0 deg
*/
//positionInertialFrame.XLatitudePosition = 47.500312805175781;
//positionInertialFrame.YLongitudePosition = -122.21683502197266;
//referenceOrientation.Sai_yawDegrees = 0.0;
//positionInertialFrame.ZAltitudePosition = 10.0;
/*
* Stand still
* 110 feet
* 0 deg
*/
positionInertialFrame.XLatitudePosition = 47.500312805175781;
positionInertialFrame.YLongitudePosition = -122.21683502197266;
referenceOrientation.Sai_yawDegrees = 0.0;
positionInertialFrame.ZAltitudePosition = 110.0;
/*
* Stand still
* 3 feet
* 160 deg
*/
//positionInertialFrame.XLatitudePosition = 47.500312805175781;
//positionInertialFrame.YLongitudePosition = -122.21683502197266;
//referenceOrientation.Sai_yawDegrees = 160.0;
//positionInertialFrame.ZAltitudePosition = 3.0;
/*
* Stand still
* 10 feet
* 160 deg
*/
//positionInertialFrame.XLatitudePosition = 47.500312805175781;
//positionInertialFrame.YLongitudePosition = -122.21683502197266;
//referenceOrientation.Sai_yawDegrees = 160.0;
//positionInertialFrame.ZAltitudePosition = 10.0;
/*
* Stand still
* 110 feet
* 160 deg
*/
//positionInertialFrame.XLatitudePosition = 47.500312805175781;
//positionInertialFrame.YLongitudePosition = -122.21683502197266;
//referenceOrientation.Sai_yawDegrees = 160.0;
//positionInertialFrame.ZAltitudePosition = 110.0;
/*
* Goto
* 3 feet
* 160 deg
*/
//positionInertialFrame.XLatitudePosition = 47.48675900142983;
//positionInertialFrame.YLongitudePosition = -122.21457481384277;
//referenceOrientation.Sai_yawDegrees = 160.0;
//positionInertialFrame.ZAltitudePosition = 3.0;
/*
* Goto
* 10 feet
* 160 deg
*/
//positionInertialFrame.XLatitudePosition = 47.48675900142983;
//positionInertialFrame.YLongitudePosition = -122.21457481384277;
//referenceOrientation.Sai_yawDegrees = 160.0;
//positionInertialFrame.ZAltitudePosition = 10.0;
/*
* Goto
* 110 feet
* 160 deg
// */
//positionInertialFrame.XLatitudePosition = 47.48675900142983;
//positionInertialFrame.YLongitudePosition = -122.21457481384277;
//referenceOrientation.Sai_yawDegrees = 160.0;
//positionInertialFrame.ZAltitudePosition = 110.0;
//stand still
/*positionInertialFrame.XLatitudePosition = 47.500312805175781;
positionInertialFrame.YLongitudePosition = -122.21683502197266;
referenceOrientation.Sai_yawDegrees = 0.0;
positionInertialFrame.ZAltitudePosition = 3.0;*/
/*positionInertialFrame.XLatitudePosition = 47.48675900142983;
positionInertialFrame.YLongitudePosition = -122.21457481384277;*/
// positionInertialFrame.ZAltitudePosition = 50.0;
// positionInertialFrame.ZAltitudePosition = 110.0;
// positionInertialFrame.ZAltitudePosition = 3.0;
referenceMeasurement.Position = positionInertialFrame;
referenceOrientation.Phi_rollDegrees = 0.0;
// referenceOrientation.Sai_yawDegrees = 160.0;
//referenceOrientation.Sai_yawDegrees = 90.0;
// referenceOrientation.Sai_yawDegrees = 0.0;
// referenceOrientation.Sai_yawDegrees = 180.0;
//referenceOrientation.Sai_yawDegrees = 170.0;
referenceOrientation.Theta_pitchDegrees = 0.0;
referenceMeasurement.Orientation = referenceOrientation;
Velocity referenceVelocity = new Velocity();
referenceVelocity.XVelocityBodyFrame = 0.0;
referenceVelocity.YVelocityBodyFrame = 0.0;
referenceVelocity.ZVelocityBodyFrame = 0.0;
referenceVelocity.YawVelocity = 0.0;
referenceMeasurement.Velocity = referenceVelocity;
referenceMeasurement.ReferenceYaw = 0.0;
ControlLaw controllerLaw = new ControlLaw();
Position intialStartingReferencePosition = null;
DateTime starTime = DateTime.Now;
try
{
while (!done)
{
byte[] bytes = listener.Receive(ref groupEP);
starTime = DateTime.Now;
measurement = new Measurement();
positionInertialFrame = new Position();
Velocity newVelocity = new Velocity();
newVelocity.YawVelocity = System.BitConverter.ToSingle(bytes.Skip(53).Take(4).ToArray(), 0) * 57.2957795;
newVelocity.PitchVelocityDeg = System.BitConverter.ToSingle(bytes.Skip(45).Take(4).ToArray(), 0) * 57.2957795;
newVelocity.RollVelocityDeg = System.BitConverter.ToSingle(bytes.Skip(49).Take(4).ToArray(), 0) * 57.2957795;
newVelocity.XVelocityNEDFrame = System.BitConverter.ToSingle(bytes.Skip(173).Take(4).ToArray(), 0) * -1; // * -1 to convert to north
newVelocity.YVelocityNEDFrame = System.BitConverter.ToSingle(bytes.Skip(165).Take(4).ToArray(), 0);
newVelocity.ZVelocityNEDFrame = System.BitConverter.ToSingle(bytes.Skip(169).Take(4).ToArray(), 0) * -1; // * -1 to convert to down
positionInertialFrame.XLatitudePosition = System.BitConverter.ToSingle(bytes.Skip(117).Take(4).ToArray(), 0);
positionInertialFrame.YLongitudePosition = System.BitConverter.ToSingle(bytes.Skip(121).Take(4).ToArray(), 0);
positionInertialFrame.ZAltitudePosition = System.BitConverter.ToSingle(bytes.Skip(129).Take(4).ToArray(), 0);
measurement.Position = positionInertialFrame;
measurement.Velocity = newVelocity;
if (!firstValue)
{
intialStartingReferencePosition = positionInertialFrame;
firstValue = true;
controllerLaw.SetInitialReferenceStartingPosition(intialStartingReferencePosition);
controllerLaw.setReferenceMeasurement(referenceMeasurement);
}
orientation = new Orientation();
orientation.Phi_rollDegrees = System.BitConverter.ToSingle(bytes.Skip(85).Take(4).ToArray(), 0);
orientation.Sai_yawDegrees = System.BitConverter.ToSingle(bytes.Skip(89).Take(4).ToArray(), 0);
orientation.Theta_pitchDegrees = System.BitConverter.ToSingle(bytes.Skip(81).Take(4).ToArray(), 0);
measurement.Orientation = orientation;
controllerLaw.processMeasurement(measurement, file, file2);
double ulon = controllerLaw.Getulon_MainRotorLongitudinalCyclicControlCommand_Pitch();
double ulat = controllerLaw.Getulat_MainRotorLateralCyclicControlCommand_Roll();
double ucol = controllerLaw.Getucol_MainRotorCollectiveControlCommand();
double uped = controllerLaw.Getuped_TailRotorPedalControlCommand();
// Console.Write(ulon + ", " + ulat + ", " + ucol +", " + uped+ "\r");
// ulon = ulon / 100;
if (ulon > .8) ulon = .8;
if (ulon < -.8) ulon = -.8;
byte[] negativeonebytes = System.BitConverter.GetBytes((float)ulon);
Array.Copy(negativeonebytes, 0, bytes, 9, negativeonebytes.Length);
// ulat = ulat / 100;
if (ulat > .8) ulat = .8;
if (ulat < -.8) ulat = -.8;
negativeonebytes = System.BitConverter.GetBytes((float)ulat);
Array.Copy(negativeonebytes, 0, bytes, 13, negativeonebytes.Length);
// uped = -1 * uped / 100.0;
// Util.K_SAI = Form1.trackBar1.Value / 100.0;
// Form1.textBox1.Text = Util.K_SAI.ToString();
Util.K_Z = Form1.trackBar1.Value / 1000.0;
Form1.textBox1.Text = Util.K_Z.ToString();
Util.K_W = Form1.trackBar2.Value / 1000.0;
Form1.textBox2.Text = Util.K_W.ToString();
Util.K_ETA_Z = Form1.trackBar4.Value / 1000.0;
Form1.textBox4.Text = Util.K_ETA_Z.ToString();
/*ControlLaw.referenceMeasurementNED.Orientation.Sai_yawDegrees = Form1.trackBar3.Value / 1000.0;
Form1.textBox3.Text = ControlLaw.referenceMeasurementNED.Orientation.Sai_yawDegrees.ToString();*/
Util.Ti_Z = Form1.trackBar3.Value / 1000.0;
Form1.textBox3.Text = Util.Ti_Z.ToString();
//uped = Form1.trackBar1.Value / 10.0;
//Form1.textBox1.Text = uped.ToString() ;
// if (uped < 0) uped = uped * -1;
// if (uped < 0) uped = 0;
if (uped > Util.UPEDMAXCONTROLVALUE)
{
negativeonebytes = System.BitConverter.GetBytes((float)Util.UPEDMAXCONTROLVALUE);
olduped = (float) Util.UPEDMAXCONTROLVALUE;
}
else if (uped < Util.UPEDMINCONTROLVALUE)
{
negativeonebytes = System.BitConverter.GetBytes((float)Util.UPEDMINCONTROLVALUE);
olduped = (float) Util.UPEDMINCONTROLVALUE;
}
else
{
negativeonebytes = System.BitConverter.GetBytes((float)uped);
}
Array.Copy(negativeonebytes, 0, bytes, 193, negativeonebytes.Length);
// ucol = -1 * ucol / 10.0;
// if (ucol < 0) ucol = 0;
if (ucol > Util.UCOLMAXCONTROLVALUE)
{
negativeonebytes = System.BitConverter.GetBytes((float)Util.UCOLMAXCONTROLVALUE);
Array.Copy(negativeonebytes, 0, bytes, 189, negativeonebytes.Length);
}
else if (ucol < Util.UCOLMINCONTROLVALUE)
{
negativeonebytes = System.BitConverter.GetBytes((float)Util.UCOLMINCONTROLVALUE); // it was -5
Array.Copy(negativeonebytes, 0, bytes, 189, negativeonebytes.Length);
}
else
{
negativeonebytes = System.BitConverter.GetBytes((float)ucol);
Array.Copy(negativeonebytes, 0, bytes, 189, negativeonebytes.Length);
}
IPEndPoint ep = new IPEndPoint(broadcast, 49000);
s.SendTo(bytes, ep);
// Form1.textBox1.Text = starTime.Subtract(DateTime.Now).Milliseconds.ToString();
}
}
catch (Exception e)
{
Console.WriteLine(e.ToString());
}
finally
{
listener.Close();
}
}
}
}
