From 2913e40df6fb970886846b6af0f11c374d12f4d5 Mon Sep 17 00:00:00 2001 From: eric Date: Mon, 13 May 2024 22:24:29 -0500 Subject: [PATCH 1/3] started implementing jsbsim support --- Assets/Scenes/ImageGenerator.unity | 14 + Assets/Scripts/JSBSim.meta | 8 + .../Scripts/JSBSim/JSBSimDroneController.cs | 145 ++++++++++ .../JSBSim/JSBSimDroneController.cs.meta | 11 + Assets/Scripts/input_output.meta | 8 + Assets/Scripts/input_output/FGNetFDM.cs | 247 ++++++++++++++++++ Assets/Scripts/input_output/FGNetFDM.cs.meta | 11 + Assets/Scripts/input_output/TCPReceiver.cs | 51 ++++ .../Scripts/input_output/TCPReceiver.cs.meta | 11 + Assets/Scripts/input_output/UDPReceiver.cs | 47 ++++ .../Scripts/input_output/UDPReceiver.cs.meta | 11 + .../RenderStreamingProjectSettings.asset | 2 +- 12 files changed, 565 insertions(+), 1 deletion(-) create mode 100644 Assets/Scripts/JSBSim.meta create mode 100644 Assets/Scripts/JSBSim/JSBSimDroneController.cs create mode 100644 Assets/Scripts/JSBSim/JSBSimDroneController.cs.meta create mode 100644 Assets/Scripts/input_output.meta create mode 100644 Assets/Scripts/input_output/FGNetFDM.cs create mode 100644 Assets/Scripts/input_output/FGNetFDM.cs.meta create mode 100644 Assets/Scripts/input_output/TCPReceiver.cs create mode 100644 Assets/Scripts/input_output/TCPReceiver.cs.meta create mode 100644 Assets/Scripts/input_output/UDPReceiver.cs create mode 100644 Assets/Scripts/input_output/UDPReceiver.cs.meta diff --git a/Assets/Scenes/ImageGenerator.unity b/Assets/Scenes/ImageGenerator.unity index 75719cd..df8eec9 100644 --- a/Assets/Scenes/ImageGenerator.unity +++ b/Assets/Scenes/ImageGenerator.unity @@ -170,6 +170,7 @@ GameObject: - component: {fileID: 110319848} - component: {fileID: 110319849} - component: {fileID: 110319850} + - component: {fileID: 110319851} m_Layer: 0 m_Name: IGcontroller m_TagString: Untagged @@ -11041,6 +11042,19 @@ MonoBehaviour: m_Name: m_EditorClassIdentifier: cameras: [] +--- !u!114 &110319851 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 110319842} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 45d4fc40122054c4b9d035bf9999e32f, type: 3} + m_Name: + m_EditorClassIdentifier: + port: 12345 --- !u!1 &134658867 GameObject: m_ObjectHideFlags: 0 diff --git a/Assets/Scripts/JSBSim.meta b/Assets/Scripts/JSBSim.meta new file mode 100644 index 0000000..7d43e04 --- /dev/null +++ b/Assets/Scripts/JSBSim.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 25fc380d182958e4c99c633bdc4b75a6 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/JSBSim/JSBSimDroneController.cs b/Assets/Scripts/JSBSim/JSBSimDroneController.cs new file mode 100644 index 0000000..34fd435 --- /dev/null +++ b/Assets/Scripts/JSBSim/JSBSimDroneController.cs @@ -0,0 +1,145 @@ +using System; +using UnityEngine; +using Unity.Mathematics; + +public class JSBSimDroneController : MonoBehaviour +{ + public CesiumForUnity.CesiumGeoreference georeference; + public MavlinkMessageProcessor mavlinkMessageProcessor; + public GameObject drone; + + public int systemId = 1; + public Vector3 latLonAlt = new Vector3(0, 0, 0); + + public float alpha = 0.98f; + public float positionAlpha = 0.98f; + private string aircraftType = "NONE"; + private Vector3 nedPos = new Vector3(0, 0, 0); + + private Quaternion lastOrientation = Quaternion.identity; + + private void updatellaPos() + { + latLonAlt.x = mavlinkMessageProcessor.globalPositionIntArray[systemId].message.lat / 1e7f; + latLonAlt.y = mavlinkMessageProcessor.globalPositionIntArray[systemId].message.lon / 1e7f; + latLonAlt.z = mavlinkMessageProcessor.globalPositionIntArray[systemId].message.alt / 1e3f; + } + public Vector3 ConvertGeoToUnityCoordinates(double longitude, double latitude, double altitude) + { + // Step 1: Convert to ECEF + double3 longitudeLatitudeHeight = new double3(longitude, latitude, altitude); + double3 ecef = CesiumForUnity.CesiumWgs84Ellipsoid.LongitudeLatitudeHeightToEarthCenteredEarthFixed(longitudeLatitudeHeight); + + // subtrace + + // Step 2: Transform ECEF to Unity coordinates + double3 unityCoords = georeference.TransformEarthCenteredEarthFixedPositionToUnity(ecef); + + // Return as Vector3 for Unity + return new Vector3((float)unityCoords.x, (float)unityCoords.y, (float)unityCoords.z); + } + private void calculateNedPos() + { + nedPos = ConvertGeoToUnityCoordinates(latLonAlt.y, latLonAlt.x, latLonAlt.z); + } + + // TODO: make the dynamic loading of the aircraft mesh better + private void UpdateAircraftType() + { + // Check if mavlinkMessageProcessor or heartbeatArray[systemId] is null + if (mavlinkMessageProcessor == null || mavlinkMessageProcessor.heartbeatArray[systemId] == null) + { + Debug.Log("mavlinkMessageProcessor or heartbeatArray[systemId] is null"); + return; + } + + // Check if nested properties are null + if (mavlinkMessageProcessor.heartbeatArray[systemId].message == null || mavlinkMessageProcessor.heartbeatArray[systemId].message.mavtype == null) + { + Debug.Log("Message or mavtype is null for systemId: " + systemId); + return; + } + // return if the aircraft type is already set + if (mavlinkMessageProcessor.heartbeatArray[systemId].message.mavtype.type == aircraftType) + { + return; + } + aircraftType = mavlinkMessageProcessor.heartbeatArray[systemId].message.mavtype.type; + Debug.Log("Aircraft type: " + aircraftType); + + // Assuming 'drone' is already assigned via the inspector or elsewhere in your code + // If 'drone' could be unassigned, add a check here + + // Show the right mesh + Transform planeTransform = drone.transform.Find("plane"); + Transform copterTransform = drone.transform.Find("copter"); + + if (planeTransform == null || copterTransform == null) + { + Debug.LogError("One of the required GameObjects (plane or copter) is missing in the children of " + drone.name); + return; + } + + if (mavlinkMessageProcessor.heartbeatArray[systemId].message.mavtype.type == "MAV_TYPE_FIXED_WING") + { + planeTransform.gameObject.SetActive(true); + copterTransform.gameObject.SetActive(false); + } + else if (mavlinkMessageProcessor.heartbeatArray[systemId].message.mavtype.type == "MAV_TYPE_QUADROTOR") + { + copterTransform.gameObject.SetActive(true); + planeTransform.gameObject.SetActive(false); + } + } + + + + private void FixedUpdate() + { + UpdateAircraftType(); + // check that globalPositionIntArray[systemId] is not null and attituteArray[systemId] is not null + if (mavlinkMessageProcessor.globalPositionIntArray[systemId].message == null || mavlinkMessageProcessor.attitudeArray[systemId] == null) + { + Debug.Log("globalPositionIntArray[systemId] or attituteArray[systemId] is null"); + return; + } + + updatellaPos(); + calculateNedPos(); + + // Integrate velocity to update position + // Vector3 velocityChange = new Vector3(localPosNed.message.vy, -localPosNed.message.vz, localPosNed.message.vx) * Time.fixedDeltaTime; + Vector3 velocityChange = new Vector3(mavlinkMessageProcessor.globalPositionIntArray[systemId].message.vy / 100f, -mavlinkMessageProcessor.globalPositionIntArray[systemId].message.vz / 100f, mavlinkMessageProcessor.globalPositionIntArray[systemId].message.vx / 100f) * Time.fixedDeltaTime; + Vector3 integratedPosition = drone.transform.position + velocityChange; + + // Retrieve the estimated position (assuming it's more accurate but updates less frequently) + Vector3 sensorEstimatedPosition = new Vector3(nedPos.x, nedPos.y, nedPos.z); + + // Apply the complementary filter for position + Vector3 filteredPosition = Vector3.Lerp(integratedPosition, sensorEstimatedPosition, 1 - positionAlpha); + + // Update the GameObject's position + drone.transform.position = filteredPosition; + + // Convert angular velocity from radians per second to degrees per second + float rollDegreesPerSecond = mavlinkMessageProcessor.attitudeArray[systemId].message.rollspeed * Mathf.Rad2Deg; + float pitchDegreesPerSecond = mavlinkMessageProcessor.attitudeArray[systemId].message.pitchspeed * Mathf.Rad2Deg; + float yawDegreesPerSecond = mavlinkMessageProcessor.attitudeArray[systemId].message.yawspeed * Mathf.Rad2Deg; + + // Calculate change in orientation based on angular velocity + Quaternion gyroDeltaRotation = Quaternion.Euler(-pitchDegreesPerSecond * Time.fixedDeltaTime, yawDegreesPerSecond * Time.fixedDeltaTime, -rollDegreesPerSecond * Time.fixedDeltaTime); + + // Apply the gyro-based orientation change to the last known orientation + Quaternion gyroBasedOrientation = lastOrientation * gyroDeltaRotation; + + // Convert the estimated orientation from Euler angles to a Quaternion + Quaternion sensorBasedOrientation = Quaternion.Euler(-mavlinkMessageProcessor.attitudeArray[systemId].message.pitch * Mathf.Rad2Deg, mavlinkMessageProcessor.attitudeArray[systemId].message.yaw * Mathf.Rad2Deg, -mavlinkMessageProcessor.attitudeArray[systemId].message.roll * Mathf.Rad2Deg); + + // Apply the complementary filter + drone.transform.rotation = Quaternion.Slerp(gyroBasedOrientation, sensorBasedOrientation, 1 - alpha); + + // Store the last known orientation + lastOrientation = drone.transform.rotation; + } + +} \ No newline at end of file diff --git a/Assets/Scripts/JSBSim/JSBSimDroneController.cs.meta b/Assets/Scripts/JSBSim/JSBSimDroneController.cs.meta new file mode 100644 index 0000000..63112a5 --- /dev/null +++ b/Assets/Scripts/JSBSim/JSBSimDroneController.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: d860125edf05d204db373663b8bb979e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/input_output.meta b/Assets/Scripts/input_output.meta new file mode 100644 index 0000000..24105f8 --- /dev/null +++ b/Assets/Scripts/input_output.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: 99212f099c4c3b540bba38b5b95a3b26 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/input_output/FGNetFDM.cs b/Assets/Scripts/input_output/FGNetFDM.cs new file mode 100644 index 0000000..de282f4 --- /dev/null +++ b/Assets/Scripts/input_output/FGNetFDM.cs @@ -0,0 +1,247 @@ +using System; +using System.Runtime.InteropServices; + +public static class FGNetConstants +{ + public const int FG_MAX_ENGINES = 4; + public const int FG_MAX_WHEELS = 3; + public const int FG_MAX_TANKS = 4; +} + +[StructLayout(LayoutKind.Sequential, Pack = 1)] +public struct FGNetFDM +{ + public uint version; + public uint padding; + + // Positions + public double longitude; + public double latitude; + public double altitude; + public float agl; + public float phi; + public float theta; + public float psi; + public float alpha; + public float beta; + + // Velocities + public float phidot; + public float thetadot; + public float psidot; + public float vcas; + public float climb_rate; + public float v_north; + public float v_east; + public float v_down; + public float v_body_u; + public float v_body_v; + public float v_body_w; + + // Accelerations + public float A_X_pilot; + public float A_Y_pilot; + public float A_Z_pilot; + + // Stall + public float stall_warning; + public float slip_deg; + + // Engine status + public uint num_engines; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FGNetConstants.FG_MAX_ENGINES)] + public uint[] eng_state; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FGNetConstants.FG_MAX_ENGINES)] + public float[] rpm; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FGNetConstants.FG_MAX_ENGINES)] + public float[] fuel_flow; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FGNetConstants.FG_MAX_ENGINES)] + public float[] fuel_px; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FGNetConstants.FG_MAX_ENGINES)] + public float[] egt; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FGNetConstants.FG_MAX_ENGINES)] + public float[] cht; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FGNetConstants.FG_MAX_ENGINES)] + public float[] mp_osi; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FGNetConstants.FG_MAX_ENGINES)] + public float[] tit; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FGNetConstants.FG_MAX_ENGINES)] + public float[] oil_temp; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FGNetConstants.FG_MAX_ENGINES)] + public float[] oil_px; + + // Consumables + public uint num_tanks; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FGNetConstants.FG_MAX_TANKS)] + public float[] fuel_quantity; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FGNetConstants.FG_MAX_TANKS)] + public uint[] tank_selected; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FGNetConstants.FG_MAX_TANKS)] + public double[] capacity_m3; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FGNetConstants.FG_MAX_TANKS)] + public double[] unusable_m3; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FGNetConstants.FG_MAX_TANKS)] + public double[] density_kgpm3; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FGNetConstants.FG_MAX_TANKS)] + public double[] level_m3; + + // Gear status + public uint num_wheels; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FGNetConstants.FG_MAX_WHEELS)] + public uint[] wow; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FGNetConstants.FG_MAX_WHEELS)] + public float[] gear_pos; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FGNetConstants.FG_MAX_WHEELS)] + public float[] gear_steer; + [MarshalAs(UnmanagedType.ByValArray, SizeConst = FGNetConstants.FG_MAX_WHEELS)] + public float[] gear_compression; + + // Environment + public uint cur_time; + public int warp; + public float visibility; + + // Control surface positions (normalized values) + public float elevator; + public float elevator_trim_tab; + public float left_flap; + public float right_flap; + public float left_aileron; + public float right_aileron; + public float rudder; + public float nose_wheel; + public float speedbrake; + public float spoilers; + + public void SwapEndian() + { + // Swap the endianness of all the elements + version = SwapBytes(version); + padding = SwapBytes(padding); + + longitude = SwapBytes(longitude); + latitude = SwapBytes(latitude); + altitude = SwapBytes(altitude); + agl = SwapBytes(agl); + phi = SwapBytes(phi); + theta = SwapBytes(theta); + psi = SwapBytes(psi); + alpha = SwapBytes(alpha); + beta = SwapBytes(beta); + + phidot = SwapBytes(phidot); + thetadot = SwapBytes(thetadot); + psidot = SwapBytes(psidot); + vcas = SwapBytes(vcas); + climb_rate = SwapBytes(climb_rate); + v_north = SwapBytes(v_north); + v_east = SwapBytes(v_east); + v_down = SwapBytes(v_down); + v_body_u = SwapBytes(v_body_u); + v_body_v = SwapBytes(v_body_v); + v_body_w = SwapBytes(v_body_w); + + A_X_pilot = SwapBytes(A_X_pilot); + A_Y_pilot = SwapBytes(A_Y_pilot); + A_Z_pilot = SwapBytes(A_Z_pilot); + + stall_warning = SwapBytes(stall_warning); + slip_deg = SwapBytes(slip_deg); + + num_engines = SwapBytes(num_engines); + SwapBytesArray(ref eng_state); + SwapBytesArray(ref rpm); + SwapBytesArray(ref fuel_flow); + SwapBytesArray(ref fuel_px); + SwapBytesArray(ref egt); + SwapBytesArray(ref cht); + SwapBytesArray(ref mp_osi); + SwapBytesArray(ref tit); + SwapBytesArray(ref oil_temp); + SwapBytesArray(ref oil_px); + + num_tanks = SwapBytes(num_tanks); + SwapBytesArray(ref fuel_quantity); + SwapBytesArray(ref tank_selected); + SwapBytesArray(ref capacity_m3); + SwapBytesArray(ref unusable_m3); + SwapBytesArray(ref density_kgpm3); + SwapBytesArray(ref level_m3); + + num_wheels = SwapBytes(num_wheels); + SwapBytesArray(ref wow); + SwapBytesArray(ref gear_pos); + SwapBytesArray(ref gear_steer); + SwapBytesArray(ref gear_compression); + + cur_time = SwapBytes(cur_time); + warp = SwapBytes(warp); + visibility = SwapBytes(visibility); + + elevator = SwapBytes(elevator); + elevator_trim_tab = SwapBytes(elevator_trim_tab); + left_flap = SwapBytes(left_flap); + right_flap = SwapBytes(right_flap); + left_aileron = SwapBytes(left_aileron); + right_aileron = SwapBytes(right_aileron); + rudder = SwapBytes(rudder); + nose_wheel = SwapBytes(nose_wheel); + speedbrake = SwapBytes(speedbrake); + spoilers = SwapBytes(spoilers); + } + + private uint SwapBytes(uint x) + { + return (x << 24) | + ((x << 8) & 0x00FF0000) | + ((x >> 8) & 0x0000FF00) | + (x >> 24); + } + + private int SwapBytes(int x) + { + return (int)SwapBytes((uint)x); + } + + private double SwapBytes(double x) + { + byte[] bytes = BitConverter.GetBytes(x); + Array.Reverse(bytes); + return BitConverter.ToDouble(bytes, 0); + } + + private float SwapBytes(float x) + { + byte[] bytes = BitConverter.GetBytes(x); + Array.Reverse(bytes); + return BitConverter.ToSingle(bytes, 0); + } + + private void SwapBytesArray(ref T[] array) where T : struct + { + if (array == null) + { + return; + } + + for (int i = 0; i < array.Length; i++) + { + array[i] = SwapBytes(array[i]); + } + } + + private T SwapBytes(T x) where T : struct + { + Type type = typeof(T); + if (type == typeof(uint)) + return (T)(object)SwapBytes((uint)(object)x); + if (type == typeof(int)) + return (T)(object)SwapBytes((int)(object)x); + if (type == typeof(double)) + return (T)(object)SwapBytes((double)(object)x); + if (type == typeof(float)) + return (T)(object)SwapBytes((float)(object)x); + + throw new ArgumentException("Unsupported type for endian swap"); + } +} diff --git a/Assets/Scripts/input_output/FGNetFDM.cs.meta b/Assets/Scripts/input_output/FGNetFDM.cs.meta new file mode 100644 index 0000000..5c447c8 --- /dev/null +++ b/Assets/Scripts/input_output/FGNetFDM.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 92c670efbb1184e468019b091dfd2d1e +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/input_output/TCPReceiver.cs b/Assets/Scripts/input_output/TCPReceiver.cs new file mode 100644 index 0000000..1eb1061 --- /dev/null +++ b/Assets/Scripts/input_output/TCPReceiver.cs @@ -0,0 +1,51 @@ +using System; +using System.Net; +using System.Net.Sockets; +using System.Runtime.InteropServices; +using UnityEngine; + +public class TCPReceiver : MonoBehaviour +{ + private TcpListener tcpListener; + private int port = 12345; // Adjust the port as needed + + void Start() + { + tcpListener = new TcpListener(IPAddress.Any, port); + tcpListener.Start(); + tcpListener.BeginAcceptTcpClient(new AsyncCallback(AcceptCallback), null); + } + + void AcceptCallback(IAsyncResult ar) + { + TcpClient client = tcpListener.EndAcceptTcpClient(ar); + NetworkStream stream = client.GetStream(); + byte[] buffer = new byte[Marshal.SizeOf(typeof(FGNetFDM))]; + stream.Read(buffer, 0, buffer.Length); + FGNetFDM fdm1 = ByteArrayToStructure(buffer); + fdm1.SwapEndian(); + Debug.Log($"Received: version = {fdm1.version}, latitude = {fdm1.latitude}"); + + stream.Close(); + client.Close(); + tcpListener.BeginAcceptTcpClient(new AsyncCallback(AcceptCallback), null); + } + + T ByteArrayToStructure(byte[] bytes) where T : struct + { + GCHandle handle = GCHandle.Alloc(bytes, GCHandleType.Pinned); + try + { + return (T)Marshal.PtrToStructure(handle.AddrOfPinnedObject(), typeof(T)); + } + finally + { + handle.Free(); + } + } + + private void OnDestroy() + { + tcpListener.Stop(); + } +} diff --git a/Assets/Scripts/input_output/TCPReceiver.cs.meta b/Assets/Scripts/input_output/TCPReceiver.cs.meta new file mode 100644 index 0000000..e7b9c9c --- /dev/null +++ b/Assets/Scripts/input_output/TCPReceiver.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 9565abd934a7a6c4a9ca1da76e5dd6b8 +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/Scripts/input_output/UDPReceiver.cs b/Assets/Scripts/input_output/UDPReceiver.cs new file mode 100644 index 0000000..3f1cb68 --- /dev/null +++ b/Assets/Scripts/input_output/UDPReceiver.cs @@ -0,0 +1,47 @@ +using System; +using System.Net; +using System.Net.Sockets; +using System.Runtime.InteropServices; +using UnityEngine; +// using Utilities; + +public class UDPReceiver : MonoBehaviour +{ + private UdpClient udpClient; + public int port = 12345; // Adjust the port as needed + + void Start() + { + udpClient = new UdpClient(port); + udpClient.BeginReceive(new AsyncCallback(ReceiveCallback), null); + } + + void ReceiveCallback(IAsyncResult ar) + { + IPEndPoint ip = new IPEndPoint(IPAddress.Any, port); + byte[] bytes = udpClient.EndReceive(ar, ref ip); + FGNetFDM fdm1 = ByteArrayToStructure(bytes); + fdm1.SwapEndian(); + Debug.Log($"Received: version = {fdm1.version}, latitude = {fdm1.latitude}"); + + udpClient.BeginReceive(new AsyncCallback(ReceiveCallback), null); + } + + T ByteArrayToStructure(byte[] bytes) where T : struct + { + GCHandle handle = GCHandle.Alloc(bytes, GCHandleType.Pinned); + try + { + return (T)Marshal.PtrToStructure(handle.AddrOfPinnedObject(), typeof(T)); + } + finally + { + handle.Free(); + } + } + + private void OnDestroy() + { + udpClient.Close(); + } +} diff --git a/Assets/Scripts/input_output/UDPReceiver.cs.meta b/Assets/Scripts/input_output/UDPReceiver.cs.meta new file mode 100644 index 0000000..41a14ca --- /dev/null +++ b/Assets/Scripts/input_output/UDPReceiver.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: 45d4fc40122054c4b9d035bf9999e32f +MonoImporter: + externalObjects: {} + serializedVersion: 2 + defaultReferences: [] + executionOrder: 0 + icon: {instanceID: 0} + userData: + assetBundleName: + assetBundleVariant: diff --git a/ProjectSettings/RenderStreamingProjectSettings.asset b/ProjectSettings/RenderStreamingProjectSettings.asset index 4705706..221e190 100644 --- a/ProjectSettings/RenderStreamingProjectSettings.asset +++ b/ProjectSettings/RenderStreamingProjectSettings.asset @@ -13,4 +13,4 @@ MonoBehaviour: m_Name: m_EditorClassIdentifier: m_WizardPopupAtStart: 1 - m_WizardPopupAlreadyShownOnce: 1 + m_WizardPopupAlreadyShownOnce: 0 From b73deb6c1b94e4a57735dc7c4113b801a21f6b1c Mon Sep 17 00:00:00 2001 From: eric Date: Sun, 9 Jun 2024 01:02:16 -0500 Subject: [PATCH 2/3] IG now able to follow JSB aircraft via FLIGHTGEAR message --- Assets/Scenes/ImageGenerator.unity | 57 ++++--- Assets/Scripts/ConfigLoader.cs | 43 ++++- .../Scripts/JSBSim/JSBSimDroneController.cs | 130 +++++++++------ Assets/Scripts/MavlinkMessageProcessor.cs | 1 + Assets/Scripts/WorldController.cs | 153 ++++++++++++------ .../{UDPReceiver.cs => JSBUDPReceiver.cs} | 13 +- ...eceiver.cs.meta => JSBUDPReceiver.cs.meta} | 0 Assets/Scripts/smoothfollow.cs | 12 +- .../RenderStreamingProjectSettings.asset | 2 +- 9 files changed, 280 insertions(+), 131 deletions(-) rename Assets/Scripts/input_output/{UDPReceiver.cs => JSBUDPReceiver.cs} (70%) rename Assets/Scripts/input_output/{UDPReceiver.cs.meta => JSBUDPReceiver.cs.meta} (100%) diff --git a/Assets/Scenes/ImageGenerator.unity b/Assets/Scenes/ImageGenerator.unity index df8eec9..6ee0b3b 100644 --- a/Assets/Scenes/ImageGenerator.unity +++ b/Assets/Scenes/ImageGenerator.unity @@ -170,7 +170,6 @@ GameObject: - component: {fileID: 110319848} - component: {fileID: 110319849} - component: {fileID: 110319850} - - component: {fileID: 110319851} m_Layer: 0 m_Name: IGcontroller m_TagString: Untagged @@ -10974,7 +10973,7 @@ MonoBehaviour: m_Name: m_EditorClassIdentifier: georeference: {fileID: 1628688806} - droneController: {fileID: 0} + isDynamicCameraSpawned: 0 droneTemplate: {fileID: 200453354} mavlinkMessageProcessor: {fileID: 110319845} currentOriginECEF: @@ -11016,6 +11015,7 @@ MonoBehaviour: tileURL: mavlink2RestURL: vehicles: [] + worldController: {fileID: 110319846} --- !u!114 &110319849 MonoBehaviour: m_ObjectHideFlags: 0 @@ -11042,19 +11042,6 @@ MonoBehaviour: m_Name: m_EditorClassIdentifier: cameras: [] ---- !u!114 &110319851 -MonoBehaviour: - m_ObjectHideFlags: 0 - m_CorrespondingSourceObject: {fileID: 0} - m_PrefabInstance: {fileID: 0} - m_PrefabAsset: {fileID: 0} - m_GameObject: {fileID: 110319842} - m_Enabled: 1 - m_EditorHideFlags: 0 - m_Script: {fileID: 11500000, guid: 45d4fc40122054c4b9d035bf9999e32f, type: 3} - m_Name: - m_EditorClassIdentifier: - port: 12345 --- !u!1 &134658867 GameObject: m_ObjectHideFlags: 0 @@ -11129,6 +11116,8 @@ GameObject: m_Component: - component: {fileID: 200453355} - component: {fileID: 200453357} + - component: {fileID: 200453358} + - component: {fileID: 200453359} m_Layer: 0 m_Name: drone m_TagString: Untagged @@ -11163,7 +11152,7 @@ MonoBehaviour: m_PrefabInstance: {fileID: 0} m_PrefabAsset: {fileID: 0} m_GameObject: {fileID: 200453354} - m_Enabled: 1 + m_Enabled: 0 m_EditorHideFlags: 0 m_Script: {fileID: 11500000, guid: 74d8ba08a3a6db2479ae04677a87515b, type: 3} m_Name: @@ -11175,6 +11164,38 @@ MonoBehaviour: latLonAlt: {x: 0, y: 0, z: 0} alpha: 0.98 positionAlpha: 0.99 +--- !u!114 &200453358 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 200453354} + m_Enabled: 0 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: d860125edf05d204db373663b8bb979e, type: 3} + m_Name: + m_EditorClassIdentifier: + georeference: {fileID: 0} + jsbUDPReceiver: {fileID: 0} + drone: {fileID: 0} + systemId: 1 + latLonAlt: {x: 0, y: 0, z: 0} + alpha: 0.98 + positionAlpha: 0.98 +--- !u!114 &200453359 +MonoBehaviour: + m_ObjectHideFlags: 0 + m_CorrespondingSourceObject: {fileID: 0} + m_PrefabInstance: {fileID: 0} + m_PrefabAsset: {fileID: 0} + m_GameObject: {fileID: 200453354} + m_Enabled: 1 + m_EditorHideFlags: 0 + m_Script: {fileID: 11500000, guid: 45d4fc40122054c4b9d035bf9999e32f, type: 3} + m_Name: + m_EditorClassIdentifier: + port: 12345 --- !u!1001 &254928480 PrefabInstance: m_ObjectHideFlags: 0 @@ -12239,7 +12260,7 @@ Light: m_Type: 1 m_Shape: 0 m_Color: {r: 1, g: 1, b: 1, a: 1} - m_Intensity: 0.8 + m_Intensity: 0.5 m_Range: 10 m_SpotAngle: 30 m_InnerSpotAngle: 21.80208 @@ -13224,7 +13245,7 @@ Camera: width: 1 height: 1 near clip plane: 0.5 - far clip plane: 1000000 + far clip plane: 30000 field of view: 60 orthographic: 0 orthographic size: 5 diff --git a/Assets/Scripts/ConfigLoader.cs b/Assets/Scripts/ConfigLoader.cs index 4fd589d..651b650 100644 --- a/Assets/Scripts/ConfigLoader.cs +++ b/Assets/Scripts/ConfigLoader.cs @@ -12,6 +12,9 @@ public class Config [System.Serializable] public class VehicleConfig { + public bool jsbsim = false; + public string type = "NONE"; + public int port = 12345; public int id; public CameraConfig[] cameras; } @@ -35,6 +38,8 @@ public class ConfigLoader : MonoBehaviour public Config config; + public WorldController worldController; + private void Start() { LoadConfig(); @@ -57,10 +62,46 @@ private void LoadConfig() } } + +// { +// "tileURL": "https://tile.googleapis.com/v1/3dtiles/root.json?key=AIzaSyCLQrJ7iJvSyCcs5MCQwINWxLrtu9tlnfA", +// "mavlink2RestURL" : "wss://sim.intelligentquads.com/60e8797ec8e541c2b50b191c4fda7d9c", +// "vehicles" : [ +// { +// "jsbsim" : false +// "id" : 1, +// "cameras" : [ +// { +// "id": 1, +// "position" : [3, 0, 0], +// "orientation" : [0, 0, 0], +// "vFOV" : 26, +// "streamingEnabled" : "false", +// "encoding" : "H264Nvidia", +// "destination" : "udp://127.0.0.1:5600" +// } +// ] +// } +// ] + +// } private void ApplySettings(Config config) { // Apply settings to tileset and mavlinkWS as before. tileset.url = config.tileURL; mavlinkWS.Connect(config.mavlink2RestURL); + + for (int i = 0; i < config.vehicles.Length; i++) + { + VehicleConfig vehicleConfig = config.vehicles[i]; + if (vehicleConfig.jsbsim) + { + Debug.Log("JSBSim vehicle detected. config: " + JsonUtility.ToJson(vehicleConfig, true)); + worldController.SpawnDrone(vehicleConfig.id, vehicleConfig.type, vehicleConfig.port); + } + + + + } } -} +} \ No newline at end of file diff --git a/Assets/Scripts/JSBSim/JSBSimDroneController.cs b/Assets/Scripts/JSBSim/JSBSimDroneController.cs index 34fd435..891ba7c 100644 --- a/Assets/Scripts/JSBSim/JSBSimDroneController.cs +++ b/Assets/Scripts/JSBSim/JSBSimDroneController.cs @@ -5,7 +5,9 @@ public class JSBSimDroneController : MonoBehaviour { public CesiumForUnity.CesiumGeoreference georeference; - public MavlinkMessageProcessor mavlinkMessageProcessor; + + private bool dynamicCameraController = false; + public JSBUDPReceiver jsbUDPReceiver; public GameObject drone; public int systemId = 1; @@ -18,12 +20,35 @@ public class JSBSimDroneController : MonoBehaviour private Quaternion lastOrientation = Quaternion.identity; - private void updatellaPos() + private double3 currentOriginECEF = new double3(0, 0, 0); + + + public void setUpJSBReceiver(JSBUDPReceiver rec, int connectionPort) { - latLonAlt.x = mavlinkMessageProcessor.globalPositionIntArray[systemId].message.lat / 1e7f; - latLonAlt.y = mavlinkMessageProcessor.globalPositionIntArray[systemId].message.lon / 1e7f; - latLonAlt.z = mavlinkMessageProcessor.globalPositionIntArray[systemId].message.alt / 1e3f; + jsbUDPReceiver = rec; + jsbUDPReceiver.port = connectionPort; + jsbUDPReceiver.SetupConnection(); + } + + public void setAsDynamicCameraController() + { + dynamicCameraController = true; + } + + private void updateWorldOriginIfNeeded(double newLongitude, double newLatitude, double newAltitude) + { + double3 newPositionECEF = CesiumForUnity.CesiumWgs84Ellipsoid.LongitudeLatitudeHeightToEarthCenteredEarthFixed(new double3(newLongitude, newLatitude, newAltitude)); + double distance = math.distance(currentOriginECEF, newPositionECEF); + + if (distance > 100000) + { + georeference.SetOriginLongitudeLatitudeHeight(newLongitude, newLatitude, newAltitude); + currentOriginECEF = newPositionECEF; + Debug.Log("World origin updated to drone's current location."); + } + } + public Vector3 ConvertGeoToUnityCoordinates(double longitude, double latitude, double altitude) { // Step 1: Convert to ECEF @@ -44,31 +69,8 @@ private void calculateNedPos() } // TODO: make the dynamic loading of the aircraft mesh better - private void UpdateAircraftType() + public void UpdateAircraftType(string type) { - // Check if mavlinkMessageProcessor or heartbeatArray[systemId] is null - if (mavlinkMessageProcessor == null || mavlinkMessageProcessor.heartbeatArray[systemId] == null) - { - Debug.Log("mavlinkMessageProcessor or heartbeatArray[systemId] is null"); - return; - } - - // Check if nested properties are null - if (mavlinkMessageProcessor.heartbeatArray[systemId].message == null || mavlinkMessageProcessor.heartbeatArray[systemId].message.mavtype == null) - { - Debug.Log("Message or mavtype is null for systemId: " + systemId); - return; - } - // return if the aircraft type is already set - if (mavlinkMessageProcessor.heartbeatArray[systemId].message.mavtype.type == aircraftType) - { - return; - } - aircraftType = mavlinkMessageProcessor.heartbeatArray[systemId].message.mavtype.type; - Debug.Log("Aircraft type: " + aircraftType); - - // Assuming 'drone' is already assigned via the inspector or elsewhere in your code - // If 'drone' could be unassigned, add a check here // Show the right mesh Transform planeTransform = drone.transform.Find("plane"); @@ -80,12 +82,12 @@ private void UpdateAircraftType() return; } - if (mavlinkMessageProcessor.heartbeatArray[systemId].message.mavtype.type == "MAV_TYPE_FIXED_WING") + if (type == "MAV_TYPE_FIXED_WING") { planeTransform.gameObject.SetActive(true); copterTransform.gameObject.SetActive(false); } - else if (mavlinkMessageProcessor.heartbeatArray[systemId].message.mavtype.type == "MAV_TYPE_QUADROTOR") + else if (type == "MAV_TYPE_QUADROTOR") { copterTransform.gameObject.SetActive(true); planeTransform.gameObject.SetActive(false); @@ -93,47 +95,71 @@ private void UpdateAircraftType() } - - private void FixedUpdate() + private void FixedUpdate() { - UpdateAircraftType(); - // check that globalPositionIntArray[systemId] is not null and attituteArray[systemId] is not null - if (mavlinkMessageProcessor.globalPositionIntArray[systemId].message == null || mavlinkMessageProcessor.attitudeArray[systemId] == null) + // Retrieve the FGNetFDM packet + FGNetFDM aircraftState = jsbUDPReceiver.AircraftState; + + UpdatePosition(aircraftState); + UpdateOrientation(aircraftState); + + if (dynamicCameraController) { - Debug.Log("globalPositionIntArray[systemId] or attituteArray[systemId] is null"); - return; + // Update the georeference's origin if the drone has moved significantly + updateWorldOriginIfNeeded(aircraftState.longitude * Mathf.Rad2Deg, aircraftState.latitude * Mathf.Rad2Deg, aircraftState.altitude); } + } - updatellaPos(); - calculateNedPos(); + private void UpdatePosition(FGNetFDM aircraftState) + { + // Convert velocities from m/s to Unity units per second and integrate to get position + Vector3 velocityChange = new Vector3( + aircraftState.v_east, + -aircraftState.v_down, + aircraftState.v_north + ) * Time.fixedDeltaTime; - // Integrate velocity to update position - // Vector3 velocityChange = new Vector3(localPosNed.message.vy, -localPosNed.message.vz, localPosNed.message.vx) * Time.fixedDeltaTime; - Vector3 velocityChange = new Vector3(mavlinkMessageProcessor.globalPositionIntArray[systemId].message.vy / 100f, -mavlinkMessageProcessor.globalPositionIntArray[systemId].message.vz / 100f, mavlinkMessageProcessor.globalPositionIntArray[systemId].message.vx / 100f) * Time.fixedDeltaTime; Vector3 integratedPosition = drone.transform.position + velocityChange; - // Retrieve the estimated position (assuming it's more accurate but updates less frequently) - Vector3 sensorEstimatedPosition = new Vector3(nedPos.x, nedPos.y, nedPos.z); + // Update NED position + Vector3 nedPoslla = new Vector3( + (float)aircraftState.longitude * Mathf.Rad2Deg, + (float)aircraftState.latitude * Mathf.Rad2Deg, + (float)aircraftState.altitude + ); + + nedPos = ConvertGeoToUnityCoordinates(nedPoslla.x, nedPoslla.y, nedPoslla.z); // Apply the complementary filter for position - Vector3 filteredPosition = Vector3.Lerp(integratedPosition, sensorEstimatedPosition, 1 - positionAlpha); + Vector3 filteredPosition = Vector3.Lerp(integratedPosition, nedPos, 1 - positionAlpha); // Update the GameObject's position drone.transform.position = filteredPosition; + } - // Convert angular velocity from radians per second to degrees per second - float rollDegreesPerSecond = mavlinkMessageProcessor.attitudeArray[systemId].message.rollspeed * Mathf.Rad2Deg; - float pitchDegreesPerSecond = mavlinkMessageProcessor.attitudeArray[systemId].message.pitchspeed * Mathf.Rad2Deg; - float yawDegreesPerSecond = mavlinkMessageProcessor.attitudeArray[systemId].message.yawspeed * Mathf.Rad2Deg; + private void UpdateOrientation(FGNetFDM aircraftState) + { + // Convert angular velocities from rad/s to degrees/s + float rollDegreesPerSecond = aircraftState.phidot * Mathf.Rad2Deg; + float pitchDegreesPerSecond = aircraftState.thetadot * Mathf.Rad2Deg; + float yawDegreesPerSecond = aircraftState.psidot * Mathf.Rad2Deg; // Calculate change in orientation based on angular velocity - Quaternion gyroDeltaRotation = Quaternion.Euler(-pitchDegreesPerSecond * Time.fixedDeltaTime, yawDegreesPerSecond * Time.fixedDeltaTime, -rollDegreesPerSecond * Time.fixedDeltaTime); + Quaternion gyroDeltaRotation = Quaternion.Euler( + -pitchDegreesPerSecond * Time.fixedDeltaTime, + yawDegreesPerSecond * Time.fixedDeltaTime, + -rollDegreesPerSecond * Time.fixedDeltaTime + ); // Apply the gyro-based orientation change to the last known orientation Quaternion gyroBasedOrientation = lastOrientation * gyroDeltaRotation; // Convert the estimated orientation from Euler angles to a Quaternion - Quaternion sensorBasedOrientation = Quaternion.Euler(-mavlinkMessageProcessor.attitudeArray[systemId].message.pitch * Mathf.Rad2Deg, mavlinkMessageProcessor.attitudeArray[systemId].message.yaw * Mathf.Rad2Deg, -mavlinkMessageProcessor.attitudeArray[systemId].message.roll * Mathf.Rad2Deg); + Quaternion sensorBasedOrientation = Quaternion.Euler( + -aircraftState.theta * Mathf.Rad2Deg, + aircraftState.psi * Mathf.Rad2Deg, + -aircraftState.phi * Mathf.Rad2Deg + ); // Apply the complementary filter drone.transform.rotation = Quaternion.Slerp(gyroBasedOrientation, sensorBasedOrientation, 1 - alpha); diff --git a/Assets/Scripts/MavlinkMessageProcessor.cs b/Assets/Scripts/MavlinkMessageProcessor.cs index 7cbae8b..fb21391 100644 --- a/Assets/Scripts/MavlinkMessageProcessor.cs +++ b/Assets/Scripts/MavlinkMessageProcessor.cs @@ -46,6 +46,7 @@ public void ProcessMessage(string message) systemId = newHeartbeat.header.system_id; if (heartbeatArray[systemId].header.system_id == -1) { + Debug.Log($"New drone detected with system ID {systemId}"); worldController?.SpawnDrone(newHeartbeat); } heartbeatArray[systemId] = newHeartbeat; diff --git a/Assets/Scripts/WorldController.cs b/Assets/Scripts/WorldController.cs index 4ffb67f..e7967b2 100644 --- a/Assets/Scripts/WorldController.cs +++ b/Assets/Scripts/WorldController.cs @@ -6,7 +6,7 @@ public class WorldController : MonoBehaviour { public CesiumForUnity.CesiumGeoreference georeference; - public DroneController droneController; + public bool isDynamicCameraSpawned = false; public GameObject droneTemplate; @@ -18,52 +18,118 @@ public class WorldController : MonoBehaviour public ConfigLoader configLoader; - private void updateWorldOriginIfNeeded(double newLongitude, double newLatitude, double newAltitude) + private void Start() { - double3 newPositionECEF = CesiumForUnity.CesiumWgs84Ellipsoid.LongitudeLatitudeHeightToEarthCenteredEarthFixed(new double3(newLongitude, newLatitude, newAltitude)); - double distance = math.distance(currentOriginECEF, newPositionECEF); - - if (distance > 100000) - { - georeference.SetOriginLongitudeLatitudeHeight(newLongitude, newLatitude, newAltitude); - currentOriginECEF = newPositionECEF; - Debug.Log("World origin updated to drone's current location."); - } + configLoader.worldController = this; } +// mavlink spawning of drone public void SpawnDrone(MavlinkMessages.Heartbeat heartbeat) { Debug.Log($"Spawned a new drone "); - // Optionally use systemId to customize the drone or its initialization + if (heartbeat.message.mavtype.type != "MAV_TYPE_FIXED_WING" && heartbeat.message.mavtype.type != "MAV_TYPE_QUADROTOR") + { + Debug.Log("Unsupported drone type. " + heartbeat.message.mavtype.type); + return; + } if (droneTemplate != null) { - if (heartbeat.message.mavtype.type != "MAV_TYPE_FIXED_WING" && heartbeat.message.mavtype.type != "MAV_TYPE_QUADROTOR") + // Instantiate the drone at the position and rotation of the georeference + GameObject newDrone = Instantiate(droneTemplate, georeference.transform.position, Quaternion.identity, georeference.transform); // Parent set here + + // Rename the drone object + newDrone.name = $"Drone_{heartbeat.header.system_id}"; + var droneController = newDrone.GetComponent(); + droneController.systemId = heartbeat.header.system_id; + droneController.georeference = georeference; + droneController.mavlinkMessageProcessor = mavlinkMessageProcessor; + droneController.drone = newDrone; + droneController.enabled = true; + newDrone.SetActive(true); + + // Hack to make sure there is only one Camera Named Dynamic Camera Active + if (isDynamicCameraSpawned == false) { - Debug.Log("Unsupported drone type. " + heartbeat.message.mavtype.type); - return; + isDynamicCameraSpawned = true; + GameObject dynamicCamera = GameObject.Find("DynamicCamera"); + + if (dynamicCamera == null) + { + Debug.LogError("DynamicCamera GameObject not found in the scene."); + } + else + { + dynamicCamera.transform.SetParent(newDrone.transform, false); + } } - + + CommonSpawnDrone(heartbeat.header.system_id, newDrone); + } + } + +// JSB spawning code + public void SpawnDrone(int id, string type, int port) + { + if (type != "MAV_TYPE_FIXED_WING" && type != "MAV_TYPE_QUADROTOR") + { + Debug.Log("Unsupported drone type. " + type); + return; + } + if (droneTemplate != null) + { // Instantiate the drone at the position and rotation of the georeference GameObject newDrone = Instantiate(droneTemplate, georeference.transform.position, Quaternion.identity, georeference.transform); // Parent set here // Rename the drone object - newDrone.name = $"Drone_{heartbeat.header.system_id}"; + newDrone.name = $"Drone_{id}"; + var droneController = newDrone.GetComponent(); + droneController.systemId = id; + droneController.georeference = georeference; + droneController.drone = newDrone; + droneController.enabled = true; + droneController.UpdateAircraftType(type); + Debug.Log("Setting up JSB receiver on port " + port); + droneController.setUpJSBReceiver( newDrone.GetComponent(), port); + if (id == 1) + { + droneController.setAsDynamicCameraController(); + } - newDrone.GetComponent().systemId = heartbeat.header.system_id; - newDrone.GetComponent().georeference = georeference; - newDrone.GetComponent().mavlinkMessageProcessor = mavlinkMessageProcessor; - newDrone.GetComponent().drone = newDrone; - newDrone.GetComponent().enabled = true; newDrone.SetActive(true); - VehicleConfig vehicleConfig = findVehicleConfig(heartbeat.header.system_id); + + // Hack to make sure there is only one Camera Named Dynamic Camera Active + if (isDynamicCameraSpawned == false) + { + isDynamicCameraSpawned = true; + GameObject dynamicCamera = GameObject.Find("DynamicCamera"); + + if (dynamicCamera == null) + { + Debug.LogError("DynamicCamera GameObject not found in the scene."); + } + else + { + dynamicCamera.transform.SetParent(newDrone.transform, false); + } + } + + + CommonSpawnDrone(id, newDrone); + } + } + + private void CommonSpawnDrone(int systemId, GameObject newDrone ) + { + + VehicleConfig vehicleConfig = findVehicleConfig(systemId); if (vehicleConfig != null) { setUpCameras(vehicleConfig, newDrone); } else { - Debug.LogError("Vehicle config not found for system ID: " + heartbeat.header.system_id); + Debug.LogError("Vehicle config not found for system ID: " + systemId); } Camera FollowCamera = newDrone.transform.Find("FollowCam").GetComponent(); @@ -79,30 +145,11 @@ public void SpawnDrone(MavlinkMessages.Heartbeat heartbeat) cameraListController.CreateEntry(newDrone); - if (droneController == null) - { - droneController = newDrone.GetComponent(); - // hack since cesium expects exactly one camera name Dynamic camera to be present - // find dynamic camera GameObject - GameObject dynamicCamera = GameObject.Find("DynamicCamera"); - - if (dynamicCamera == null) - { - Debug.LogError("DynamicCamera GameObject not found in the scene."); - }else{ - // Set the DynamicCamera GameObject as a child of the newDrone GameObject. - // This operation modifies the DynamicCamera's transform so that its position, rotation, and scale are now relative to the newDrone. - dynamicCamera.transform.SetParent(newDrone.transform, false); - - } - - - } - - Debug.Log($"Spawned a new drone for system ID: {heartbeat.header.system_id}."); - } + Debug.Log($"Spawned a new drone for system ID: {systemId}."); + } + public VehicleConfig findVehicleConfig(int systemId) { foreach (var vehicleConfig in configLoader.config.vehicles) @@ -147,11 +194,11 @@ private Quaternion convertEulerNEDToUnity(float roll, float pitch, float yaw) - private void FixedUpdate() - { - if (droneController != null) - { - updateWorldOriginIfNeeded(droneController.latLonAlt.y, droneController.latLonAlt.x, droneController.latLonAlt.z); - } - } + // private void FixedUpdate() + // { + // if (droneController != null) + // { + // updateWorldOriginIfNeeded(droneController.latLonAlt.y, droneController.latLonAlt.x, droneController.latLonAlt.z); + // } + // } } \ No newline at end of file diff --git a/Assets/Scripts/input_output/UDPReceiver.cs b/Assets/Scripts/input_output/JSBUDPReceiver.cs similarity index 70% rename from Assets/Scripts/input_output/UDPReceiver.cs rename to Assets/Scripts/input_output/JSBUDPReceiver.cs index 3f1cb68..9d609b5 100644 --- a/Assets/Scripts/input_output/UDPReceiver.cs +++ b/Assets/Scripts/input_output/JSBUDPReceiver.cs @@ -3,14 +3,17 @@ using System.Net.Sockets; using System.Runtime.InteropServices; using UnityEngine; +using Unity.Mathematics; // using Utilities; -public class UDPReceiver : MonoBehaviour +public class JSBUDPReceiver : MonoBehaviour { private UdpClient udpClient; public int port = 12345; // Adjust the port as needed - void Start() + public FGNetFDM AircraftState = new FGNetFDM(); + + public void SetupConnection() { udpClient = new UdpClient(port); udpClient.BeginReceive(new AsyncCallback(ReceiveCallback), null); @@ -20,9 +23,9 @@ void ReceiveCallback(IAsyncResult ar) { IPEndPoint ip = new IPEndPoint(IPAddress.Any, port); byte[] bytes = udpClient.EndReceive(ar, ref ip); - FGNetFDM fdm1 = ByteArrayToStructure(bytes); - fdm1.SwapEndian(); - Debug.Log($"Received: version = {fdm1.version}, latitude = {fdm1.latitude}"); + AircraftState = ByteArrayToStructure(bytes); + AircraftState.SwapEndian(); + Debug.Log($"Received: version = {AircraftState.version}, latitude = {AircraftState.latitude * Mathf.Rad2Deg} longitude = {AircraftState.longitude * Mathf.Rad2Deg}"); udpClient.BeginReceive(new AsyncCallback(ReceiveCallback), null); } diff --git a/Assets/Scripts/input_output/UDPReceiver.cs.meta b/Assets/Scripts/input_output/JSBUDPReceiver.cs.meta similarity index 100% rename from Assets/Scripts/input_output/UDPReceiver.cs.meta rename to Assets/Scripts/input_output/JSBUDPReceiver.cs.meta diff --git a/Assets/Scripts/smoothfollow.cs b/Assets/Scripts/smoothfollow.cs index 3260acd..288d83f 100644 --- a/Assets/Scripts/smoothfollow.cs +++ b/Assets/Scripts/smoothfollow.cs @@ -17,7 +17,7 @@ public class SmoothFollow : MonoBehaviour // Rotation and zoom speed public float rotationSpeed = 100.0f; // Adjust rotation speed as needed - public float zoomSpeed = 10.0f; // Adjust zoom speed as needed + public float zoomSpeed = 450.0f; // Adjust zoom speed as needed private void Start() { @@ -30,6 +30,16 @@ private void LateUpdate() return; float ScrollWheelChange = Input.GetAxis("Mouse ScrollWheel"); + if (Input.GetKeyDown(KeyCode.LeftShift)) + { + Debug.Log("Shift key is held down"); + zoomSpeed = 10000.0f; + } + if (Input.GetKeyUp(KeyCode.LeftShift)) + { + Debug.Log("Shift key is released"); + zoomSpeed = 450.0f; + } if (ScrollWheelChange != 0) { // Make zoom speed frame rate independent diff --git a/ProjectSettings/RenderStreamingProjectSettings.asset b/ProjectSettings/RenderStreamingProjectSettings.asset index 221e190..4705706 100644 --- a/ProjectSettings/RenderStreamingProjectSettings.asset +++ b/ProjectSettings/RenderStreamingProjectSettings.asset @@ -13,4 +13,4 @@ MonoBehaviour: m_Name: m_EditorClassIdentifier: m_WizardPopupAtStart: 1 - m_WizardPopupAlreadyShownOnce: 0 + m_WizardPopupAlreadyShownOnce: 1 From 1b84c69f63ab9ef9b22c918d77beb82596ac2a7c Mon Sep 17 00:00:00 2001 From: eric Date: Sun, 9 Jun 2024 01:54:56 -0500 Subject: [PATCH 3/3] fixing bug on mavlink side --- Assets/Scripts/DroneController.cs | 28 ++++++++++++++++++++++++++++ Assets/Scripts/WorldController.cs | 6 ++++++ 2 files changed, 34 insertions(+) diff --git a/Assets/Scripts/DroneController.cs b/Assets/Scripts/DroneController.cs index aef38ba..981e431 100644 --- a/Assets/Scripts/DroneController.cs +++ b/Assets/Scripts/DroneController.cs @@ -8,6 +8,8 @@ public class DroneController : MonoBehaviour public MavlinkMessageProcessor mavlinkMessageProcessor; public GameObject drone; + private bool dynamicCameraController = false; + public int systemId = 1; public Vector3 latLonAlt = new Vector3(0, 0, 0); @@ -18,6 +20,26 @@ public class DroneController : MonoBehaviour private Quaternion lastOrientation = Quaternion.identity; + private double3 currentOriginECEF = new double3(0, 0, 0); + + public void setAsDynamicCameraController() + { + dynamicCameraController = true; + } + + private void updateWorldOriginIfNeeded(double newLongitude, double newLatitude, double newAltitude) + { + double3 newPositionECEF = CesiumForUnity.CesiumWgs84Ellipsoid.LongitudeLatitudeHeightToEarthCenteredEarthFixed(new double3(newLongitude, newLatitude, newAltitude)); + double distance = math.distance(currentOriginECEF, newPositionECEF); + + if (distance > 100000) + { + georeference.SetOriginLongitudeLatitudeHeight(newLongitude, newLatitude, newAltitude); + currentOriginECEF = newPositionECEF; + Debug.Log("World origin updated to drone's current location."); + } + } + private void updatellaPos() { latLonAlt.x = mavlinkMessageProcessor.globalPositionIntArray[systemId].message.lat / 1e7f; @@ -140,6 +162,12 @@ private void FixedUpdate() // Store the last known orientation lastOrientation = drone.transform.rotation; + + if (dynamicCameraController) + { + // Update the georeference's origin if the drone has moved significantly + updateWorldOriginIfNeeded(latLonAlt.y, latLonAlt.x, latLonAlt.z); + } } } \ No newline at end of file diff --git a/Assets/Scripts/WorldController.cs b/Assets/Scripts/WorldController.cs index e7967b2..667e9bc 100644 --- a/Assets/Scripts/WorldController.cs +++ b/Assets/Scripts/WorldController.cs @@ -45,6 +45,12 @@ public void SpawnDrone(MavlinkMessages.Heartbeat heartbeat) droneController.mavlinkMessageProcessor = mavlinkMessageProcessor; droneController.drone = newDrone; droneController.enabled = true; + + if (heartbeat.header.system_id == 1) + { + droneController.setAsDynamicCameraController(); + } + newDrone.SetActive(true); // Hack to make sure there is only one Camera Named Dynamic Camera Active