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(); }