private void LoadFly(List <PlaybackInfo> listOfPlay)
{
if (File.Exists("FlyHxModel.bin"))
{
using (BinaryReader br = new BinaryReader(File.Open("FlyHxModel.bin", FileMode.Open, FileAccess.Read)))
{
var lenght = br.BaseStream.Length;
double pos = 0;
Console.WriteLine("");
while (br.BaseStream.Position != br.BaseStream.Length)
{
DataOut dataOut = new DataOut();
KinematicsState kState = new KinematicsState();
var dOutByte = br.ReadBytes(Marshal.SizeOf(dataOut));
var ksByte = br.ReadBytes(Marshal.SizeOf(kState));
ConvertHelper.ByteToObject(dOutByte, dataOut);
kState = (KinematicsState)ConvertHelper.ByteToObject(ksByte, kState, typeof(KinematicsState));
listOfPlay.Add(new PlaybackInfo()
{
DataOut = dataOut, KinematicsState = kState
});
pos += 838;
decimal percent = (decimal)Math.Round(((pos / lenght) * 100.0), 2);
PrintProgress((int)percent, _processLoad, _finishLoad);
}
}
}
}
private void InitModel(StartPosition startPosition)
{
Hel.FCSState.TurnCoordSpd = 80;
Hel.FCSState.TurnCoordination = 1;
Hel.FCSState.PitchChnl.StickTrimmedPos = 0.5;
Hel.FCSState.PitchChnl.StickAngleSpd = 9;
Hel.FCSState.PitchChnl.StickDeadzone = 0.02;
Hel.FCSState.PitchChnl.MinAngle = -12;
Hel.FCSState.PitchChnl.MaxAngle = 16;
Hel.FCSState.RollChnl.StickTrimmedPos = 0.5;
Hel.FCSState.RollChnl.StickAngleSpd = 22;
Hel.FCSState.RollChnl.StickDeadzone = 0.02;
Hel.FCSState.RollChnl.MinAngle = -24;
Hel.FCSState.RollChnl.MaxAngle = 24;
KinematicsState initialState = default;
initialState.AbsSpeed = new XVECTOR3()
{
X = 0, Y = 0, Z = 0
};
initialState.Angs.Psi = startPosition.in_Kurs0;
initialState.Pos.Elevation = startPosition.in_Hgeom + 3;
initialState.Pos.Latitude = startPosition.StartX; //= 43.44794255;
initialState.Pos.Longitude = startPosition.StartY; // = 39.94518977;
IntPtr ksPtr = GetIntPtr(initialState);
Init(ksPtr);
Marshal.FreeHGlobal(ksPtr);
}
public void Step(ControlElement controlElement)
{
ContactEnv.Elevation = _gmTerrainH;
ContactEnv.Normal = _normal;
Hel.WindSpeed = _windSpeed;
Hel.Mass = 10800.0;
Hel.InertialMoments = _inertialMoments;
Hel.PosCG = _posCg;
Hel.vehicleCtrl.CyclicPitch = controlElement._cyclicStepHandleLeft.Elevator;
Hel.vehicleCtrl.CyclicRoll = controlElement._cyclicStepHandleLeft.Aileron;
Hel.vehicleCtrl.Direction = controlElement._pedalsLeft.Pedal;
Hel.vehicleCtrl.Collective = controlElement._generalStepHandleLeft.GeneralStep;
IntPtr ptrHel = GetIntPtr(Hel);
IntPtr ptrCe = GetIntPtr(ContactEnv);
IntPtr ptrRs = GetIntPtr(ResState);
IntPtr ptrRh = GetIntPtr(ResHel);
Step(0.02, ptrHel, ptrCe, ptrRs, ptrRh);
ResState = (KinematicsState)Marshal.PtrToStructure(ptrRs, typeof(KinematicsState));
ResHel = (VhclOutp)Marshal.PtrToStructure(ptrRh, typeof(VhclOutp));
_dataOut.KinematicsState = ResState;
_dataOut.VhclOutp = ResHel;
_udpHelper.Send(GetByte(ResState), "127.0.0.1", 6100);
_udpHelper.Send(GetByte(_dataOut), "127.0.0.1", 20020);
_udpHelper.Send(GetByte(ResState), "127.0.0.1", 6105);
Marshal.FreeHGlobal(ptrHel);
Marshal.FreeHGlobal(ptrCe);
Marshal.FreeHGlobal(ptrRs);
Marshal.FreeHGlobal(ptrRh);
}
public void RecordData(DataOut dataOut, KinematicsState kinematicsState)
{
if (!_fs.CanWrite)
{
return;
}
_bw.Write(ConvertHelper.ObjectToByte(dataOut));
_bw.Write(ConvertHelper.ObjectToByte(kinematicsState));
}
private void InitModel(StartPosition startPosition)
{
KinematicsState initialState = default;
initialState.AbsSpeed = new XVECTOR3()
{
X = 1, Y = 0, Z = 0
};
initialState.Angs.Psi = startPosition.in_Kurs0;
initialState.Pos.Elevation = startPosition.in_Hbar = 50;
initialState.Pos.Latitude = startPosition.StartX = 43.44794255;
initialState.Pos.Longitude = startPosition.StartY = 39.94518977;
IntPtr ksPtr = GetIntPtr(initialState);
Init(ksPtr);
Marshal.FreeHGlobal(ksPtr);
}
private byte[] GetByte(KinematicsState state)
{
_svvo.Preamble.wSync = 0xCDEF;
_svvo.Preamble.ProtVers = 0x04;
_svvo.Preamble.wLength = (ushort)Marshal.SizeOf(_svvo);
_svvo.Header._type = 3;
_svvo.Header.Number = 0;
_svvo.Id = 1;
_svvo.Size = (ushort)(Marshal.SizeOf(_svvo.Packetcam) + 4);
_svvo.Packetcam.x = state.Pos.Latitude * Math.PI / 180D;
_svvo.Packetcam.z = state.Pos.Longitude * Math.PI / 180D;
_svvo.Packetcam.ht = (float)state.Pos.Elevation;
_svvo.Packetcam.tet = (float)(state.Angs.Fi * Math.PI / 180D);
_svvo.Packetcam.gam = (float)(state.Angs.Gam * Math.PI / 180D);
_svvo.Packetcam.psi = (float)(state.Angs.Psi * Math.PI / 180D);
_svvo.Packetcam.flag = 1;
return(ConvertHelper.ObjectToByte(_svvo));
}
public void Step(ControlElement controlElement)
{
Hel.VehicleCtrl.AltimeterBaroPressure = 761.2;
ContactEnv.Elevation = _gmTerrainH;
ContactEnv.Normal = _normal;
Hel.AirState.WindSpeed = _windSpeed;
Hel.Mass = 10800.0;
Hel.InertialMoments = _inertialMoments;
Hel.PosCG = _posCg;
if (controlElement.Channel == 1)
{
Hel.VehicleCtrl.CyclicPitch = controlElement._cyclicStepHandleLeft.Elevator;
Hel.VehicleCtrl.CyclicRoll = controlElement._cyclicStepHandleLeft.Aileron;
Hel.VehicleCtrl.Direction = controlElement._pedalsLeft.Pedal;
Hel.VehicleCtrl.Collective = controlElement._generalStepHandleLeft.GeneralStep;
Hel.VehicleCtrl.Trimmer = (byte)controlElement._cyclicStepHandleLeft.BtnTrim;
Hel.VehicleCtrl.Friction = (byte)controlElement._generalStepHandleLeft.BtnStabilizer;
Hel.VehicleCtrl.NoseGear.Brake = controlElement._cyclicStepHandleLeft.BtnWheelBrake;
Hel.VehicleCtrl.MainGearLeft.Brake = controlElement._cyclicStepHandleLeft.BtnWheelBrake;
Hel.VehicleCtrl.MainGearRight.Brake = controlElement._cyclicStepHandleLeft.BtnWheelBrake;
}
else if (controlElement.Channel == 2)
{
Hel.VehicleCtrl.CyclicPitch = controlElement._cyclicStepHandleRight.Elevator;
Hel.VehicleCtrl.CyclicRoll = controlElement._cyclicStepHandleRight.Aileron;
Hel.VehicleCtrl.Direction = controlElement._pedalsLeft.Pedal;
Hel.VehicleCtrl.Collective = controlElement._generalStepHandleRight.GeneralStep;
Hel.VehicleCtrl.Trimmer = (byte)controlElement._cyclicStepHandleRight.BtnTrim;
Hel.VehicleCtrl.Friction = (byte)controlElement._generalStepHandleRight.BtnStabilizer;
Hel.VehicleCtrl.NoseGear.Brake = controlElement._cyclicStepHandleRight.BtnWheelBrake;
Hel.VehicleCtrl.MainGearLeft.Brake = controlElement._cyclicStepHandleRight.BtnWheelBrake;
Hel.VehicleCtrl.MainGearRight.Brake = controlElement._cyclicStepHandleRight.BtnWheelBrake;
}
//Hel.VehicleCtrl.CyclicPitch = 0.5;
//Hel.VehicleCtrl.CyclicRoll = 0.5;
//Hel.VehicleCtrl.Direction = 0.5; ;
//Hel.VehicleCtrl.Collective = 0.0;
//Hel.VehicleCtrl.Trimmer = 0;
//Hel.VehicleCtrl.Friction = 0;
//Hel.VehicleCtrl.NoseGear.Brake = 1;
//Hel.VehicleCtrl.MainGearLeft.Brake = 1;
//Hel.VehicleCtrl.MainGearRight.Brake = 1;
IntPtr ptrHel = GetIntPtr(Hel);
IntPtr ptrCe = GetIntPtr(ContactEnv);
IntPtr ptrRs = GetIntPtr(ResState);
IntPtr ptrRh = GetIntPtr(ResHel);
Step(0.01, ptrHel, ptrCe, ptrRs, ptrRh);
ResState = (KinematicsState)Marshal.PtrToStructure(ptrRs, typeof(KinematicsState));
ResHel = (VhclOutp)Marshal.PtrToStructure(ptrRh, typeof(VhclOutp));
_dataOut.KinematicsState = ResState;
_dataOut.VhclOutp = ResHel;
_dataOut.VhclInp = Hel;
foreach (var ippoint in _config.NetworkSettings.Svvo.Position.IPPoint)
{
_udpHelper.Send(GetByte(ResState), ippoint.Ip, ippoint.Port);
}
foreach (var ippoint in _config.NetworkSettings.DataTransfer.DynamicModel.IPPoint)
{
_udpHelper.Send(GetByte(_dataOut), ippoint.Ip, ippoint.Port);
}
if (_isRecord)
{
_recordFlight.RecordData(_dataOut, ResState);
}
Marshal.FreeHGlobal(ptrHel);
Marshal.FreeHGlobal(ptrCe);
Marshal.FreeHGlobal(ptrRs);
Marshal.FreeHGlobal(ptrRh);
}
public ModelHx()
{
KinematicsState = new KinematicsState();
VhclOutp = new VhclOutp();
VhclInp = new VhclInp();
}
