From cd9a08f677cb55ccd57e813c4f66f647c5198717 Mon Sep 17 00:00:00 2001 From: Richard Bunt Date: Tue, 23 Jun 2015 23:04:11 +0100 Subject: [PATCH 1/2] added new API calls which calculate pitch, roll, yaw etc using the root parts position --- Telemachus/src/DataLinkHandlers.cs | 76 ++++++++++++++----- .../src/websockets-telemachus.html | 10 ++- 2 files changed, 67 insertions(+), 19 deletions(-) diff --git a/Telemachus/src/DataLinkHandlers.cs b/Telemachus/src/DataLinkHandlers.cs index f4c07a1..1090be7 100644 --- a/Telemachus/src/DataLinkHandlers.cs +++ b/Telemachus/src/DataLinkHandlers.cs @@ -1130,7 +1130,7 @@ public NavBallDataLinkHandler(FormatterProvider formatters) registerAPI(new PlotableAPIEntry( dataSources => { - Quaternion result = updateHeadingPitchRollField(dataSources.vessel); + Quaternion result = updateHeadingPitchRollField(dataSources.vessel, dataSources.vessel.findWorldCenterOfMass()); return result.eulerAngles.y; }, "n.heading", "Heading", formatters.Default, APIEntry.UnitType.DEG)); @@ -1138,7 +1138,7 @@ public NavBallDataLinkHandler(FormatterProvider formatters) registerAPI(new PlotableAPIEntry( dataSources => { - Quaternion result = updateHeadingPitchRollField(dataSources.vessel); + Quaternion result = updateHeadingPitchRollField(dataSources.vessel, dataSources.vessel.findWorldCenterOfMass()); return (result.eulerAngles.x > 180) ? (360.0 - result.eulerAngles.x) : -result.eulerAngles.x; }, "n.pitch", "Pitch", formatters.Default, APIEntry.UnitType.DEG)); @@ -1146,7 +1146,7 @@ public NavBallDataLinkHandler(FormatterProvider formatters) registerAPI(new PlotableAPIEntry( dataSources => { - Quaternion result = updateHeadingPitchRollField(dataSources.vessel); + Quaternion result = updateHeadingPitchRollField(dataSources.vessel, dataSources.vessel.findWorldCenterOfMass()); return (result.eulerAngles.z > 180) ? (result.eulerAngles.z - 360.0) : result.eulerAngles.z; }, @@ -1155,7 +1155,7 @@ public NavBallDataLinkHandler(FormatterProvider formatters) registerAPI(new PlotableAPIEntry( dataSources => { - Quaternion result = updateHeadingPitchRollField(dataSources.vessel); + Quaternion result = updateHeadingPitchRollField(dataSources.vessel, dataSources.vessel.findWorldCenterOfMass()); return result.eulerAngles.y; }, "n.rawheading", "Raw Heading", formatters.Default, APIEntry.UnitType.DEG)); @@ -1163,7 +1163,7 @@ public NavBallDataLinkHandler(FormatterProvider formatters) registerAPI(new PlotableAPIEntry( dataSources => { - Quaternion result = updateHeadingPitchRollField(dataSources.vessel); + Quaternion result = updateHeadingPitchRollField(dataSources.vessel, dataSources.vessel.findWorldCenterOfMass()); return result.eulerAngles.x; }, "n.rawpitch", "Raw Pitch", formatters.Default, APIEntry.UnitType.DEG)); @@ -1171,10 +1171,59 @@ public NavBallDataLinkHandler(FormatterProvider formatters) registerAPI(new PlotableAPIEntry( dataSources => { - Quaternion result = updateHeadingPitchRollField(dataSources.vessel); + Quaternion result = updateHeadingPitchRollField(dataSources.vessel, dataSources.vessel.findWorldCenterOfMass()); return result.eulerAngles.z; }, "n.rawroll", "Raw Roll", formatters.Default, APIEntry.UnitType.DEG)); + + registerAPI(new PlotableAPIEntry( + dataSources => + { + Quaternion result = updateHeadingPitchRollField(dataSources.vessel, dataSources.vessel.rootPart.transform.position); + return result.eulerAngles.y; + }, + "n.headingRoot", "Heading calculated using the position of the vessels root part", formatters.Default, APIEntry.UnitType.DEG)); + + registerAPI(new PlotableAPIEntry( + dataSources => + { + Quaternion result = updateHeadingPitchRollField(dataSources.vessel, dataSources.vessel.rootPart.transform.position); + return (result.eulerAngles.x > 180) ? (360.0 - result.eulerAngles.x) : -result.eulerAngles.x; + }, + "n.pitchRoot", "Pitch calculated using the position of the vessels root part", formatters.Default, APIEntry.UnitType.DEG)); + + registerAPI(new PlotableAPIEntry( + dataSources => + { + Quaternion result = updateHeadingPitchRollField(dataSources.vessel, dataSources.vessel.rootPart.transform.position); + return (result.eulerAngles.z > 180) ? + (result.eulerAngles.z - 360.0) : result.eulerAngles.z; + }, + "n.rollRoot", "Roll calculated using the position of the vessels root part", formatters.Default, APIEntry.UnitType.DEG)); + + registerAPI(new PlotableAPIEntry( + dataSources => + { + Quaternion result = updateHeadingPitchRollField(dataSources.vessel, dataSources.vessel.rootPart.transform.position); + return result.eulerAngles.y; + }, + "n.rawheadingRoot", "Raw Heading calculated using the position of the vessels root part", formatters.Default, APIEntry.UnitType.DEG)); + + registerAPI(new PlotableAPIEntry( + dataSources => + { + Quaternion result = updateHeadingPitchRollField(dataSources.vessel, dataSources.vessel.rootPart.transform.position); + return result.eulerAngles.x; + }, + "n.rawpitchRoot", "Raw Pitch calculated using the position of the vessels root part", formatters.Default, APIEntry.UnitType.DEG)); + + registerAPI(new PlotableAPIEntry( + dataSources => + { + Quaternion result = updateHeadingPitchRollField(dataSources.vessel, dataSources.vessel.rootPart.transform.position); + return result.eulerAngles.z; + }, + "n.rawrollRoot", "Raw Roll calculated using the position of the vessels root part", formatters.Default, APIEntry.UnitType.DEG)); } #endregion @@ -1182,12 +1231,11 @@ public NavBallDataLinkHandler(FormatterProvider formatters) #region Methods //Borrowed from MechJeb2 - private Quaternion updateHeadingPitchRollField(Vessel v) + private Quaternion updateHeadingPitchRollField(Vessel v, Vector3d CoM) { - Vector3d CoM, north, up; + Vector3d north, up; Quaternion rotationSurface; - - CoM = v.findWorldCenterOfMass(); + up = (CoM - v.mainBody.position).normalized; north = Vector3d.Exclude(up, (v.mainBody.position + v.mainBody.transform.up * @@ -1198,14 +1246,6 @@ private Quaternion updateHeadingPitchRollField(Vessel v) Quaternion.Inverse(v.GetTransform().rotation) * rotationSurface); } - /*private double calculatePitch(Vessel v) - { - Vector3d worldUp = (v.CoM - v.mainBody.position).normalized; - double angle = Vector3d.Angle(worldUp, v.transform.up); - - return worldUp.x - v.transform.up.x < 0 ? angle : -angle; - }*/ - #endregion } diff --git a/WebPages/WebPagesTest/src/websockets-telemachus.html b/WebPages/WebPagesTest/src/websockets-telemachus.html index e42769e..fa23411 100644 --- a/WebPages/WebPagesTest/src/websockets-telemachus.html +++ b/WebPages/WebPagesTest/src/websockets-telemachus.html @@ -42,7 +42,15 @@ function doTest() { - doBasicTest(); + doStressRollRootTest(); + } + + function doStressRollTest() { + doSend(JSON.stringify({ "+": ["n.roll", "n.pitch", "n.heading"], "rate": 10 })); + } + + function doStressRollRootTest() { + doSend(JSON.stringify({ "+": ["n.rollRoot", "n.pitchRoot", "n.headingRoot"], "rate": 10 })); } function doStressTest() { From 3aca130c023209dcbc5ab29ee66d45eeeaca359d Mon Sep 17 00:00:00 2001 From: Richard Bunt Date: Fri, 3 Jul 2015 23:07:32 +0100 Subject: [PATCH 2/2] swaped root nav ball calls to primary --- Telemachus/src/DataLinkHandlers.cs | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/Telemachus/src/DataLinkHandlers.cs b/Telemachus/src/DataLinkHandlers.cs index 1090be7..d101ea2 100644 --- a/Telemachus/src/DataLinkHandlers.cs +++ b/Telemachus/src/DataLinkHandlers.cs @@ -1133,7 +1133,7 @@ public NavBallDataLinkHandler(FormatterProvider formatters) Quaternion result = updateHeadingPitchRollField(dataSources.vessel, dataSources.vessel.findWorldCenterOfMass()); return result.eulerAngles.y; }, - "n.heading", "Heading", formatters.Default, APIEntry.UnitType.DEG)); + "n.heading2", "Heading", formatters.Default, APIEntry.UnitType.DEG)); registerAPI(new PlotableAPIEntry( dataSources => @@ -1141,7 +1141,7 @@ public NavBallDataLinkHandler(FormatterProvider formatters) Quaternion result = updateHeadingPitchRollField(dataSources.vessel, dataSources.vessel.findWorldCenterOfMass()); return (result.eulerAngles.x > 180) ? (360.0 - result.eulerAngles.x) : -result.eulerAngles.x; }, - "n.pitch", "Pitch", formatters.Default, APIEntry.UnitType.DEG)); + "n.pitch2", "Pitch", formatters.Default, APIEntry.UnitType.DEG)); registerAPI(new PlotableAPIEntry( dataSources => @@ -1150,7 +1150,7 @@ public NavBallDataLinkHandler(FormatterProvider formatters) return (result.eulerAngles.z > 180) ? (result.eulerAngles.z - 360.0) : result.eulerAngles.z; }, - "n.roll", "Roll", formatters.Default, APIEntry.UnitType.DEG)); + "n.roll2", "Roll", formatters.Default, APIEntry.UnitType.DEG)); registerAPI(new PlotableAPIEntry( dataSources => @@ -1158,7 +1158,7 @@ public NavBallDataLinkHandler(FormatterProvider formatters) Quaternion result = updateHeadingPitchRollField(dataSources.vessel, dataSources.vessel.findWorldCenterOfMass()); return result.eulerAngles.y; }, - "n.rawheading", "Raw Heading", formatters.Default, APIEntry.UnitType.DEG)); + "n.rawheading2", "Raw Heading", formatters.Default, APIEntry.UnitType.DEG)); registerAPI(new PlotableAPIEntry( dataSources => @@ -1166,7 +1166,7 @@ public NavBallDataLinkHandler(FormatterProvider formatters) Quaternion result = updateHeadingPitchRollField(dataSources.vessel, dataSources.vessel.findWorldCenterOfMass()); return result.eulerAngles.x; }, - "n.rawpitch", "Raw Pitch", formatters.Default, APIEntry.UnitType.DEG)); + "n.rawpitch2", "Raw Pitch", formatters.Default, APIEntry.UnitType.DEG)); registerAPI(new PlotableAPIEntry( dataSources => @@ -1174,7 +1174,7 @@ public NavBallDataLinkHandler(FormatterProvider formatters) Quaternion result = updateHeadingPitchRollField(dataSources.vessel, dataSources.vessel.findWorldCenterOfMass()); return result.eulerAngles.z; }, - "n.rawroll", "Raw Roll", formatters.Default, APIEntry.UnitType.DEG)); + "n.rawroll2", "Raw Roll", formatters.Default, APIEntry.UnitType.DEG)); registerAPI(new PlotableAPIEntry( dataSources => @@ -1182,7 +1182,7 @@ public NavBallDataLinkHandler(FormatterProvider formatters) Quaternion result = updateHeadingPitchRollField(dataSources.vessel, dataSources.vessel.rootPart.transform.position); return result.eulerAngles.y; }, - "n.headingRoot", "Heading calculated using the position of the vessels root part", formatters.Default, APIEntry.UnitType.DEG)); + "n.heading", "Heading calculated using the position of the vessels root part", formatters.Default, APIEntry.UnitType.DEG)); registerAPI(new PlotableAPIEntry( dataSources => @@ -1190,7 +1190,7 @@ public NavBallDataLinkHandler(FormatterProvider formatters) Quaternion result = updateHeadingPitchRollField(dataSources.vessel, dataSources.vessel.rootPart.transform.position); return (result.eulerAngles.x > 180) ? (360.0 - result.eulerAngles.x) : -result.eulerAngles.x; }, - "n.pitchRoot", "Pitch calculated using the position of the vessels root part", formatters.Default, APIEntry.UnitType.DEG)); + "n.pitch", "Pitch calculated using the position of the vessels root part", formatters.Default, APIEntry.UnitType.DEG)); registerAPI(new PlotableAPIEntry( dataSources => @@ -1199,7 +1199,7 @@ public NavBallDataLinkHandler(FormatterProvider formatters) return (result.eulerAngles.z > 180) ? (result.eulerAngles.z - 360.0) : result.eulerAngles.z; }, - "n.rollRoot", "Roll calculated using the position of the vessels root part", formatters.Default, APIEntry.UnitType.DEG)); + "n.roll", "Roll calculated using the position of the vessels root part", formatters.Default, APIEntry.UnitType.DEG)); registerAPI(new PlotableAPIEntry( dataSources => @@ -1207,7 +1207,7 @@ public NavBallDataLinkHandler(FormatterProvider formatters) Quaternion result = updateHeadingPitchRollField(dataSources.vessel, dataSources.vessel.rootPart.transform.position); return result.eulerAngles.y; }, - "n.rawheadingRoot", "Raw Heading calculated using the position of the vessels root part", formatters.Default, APIEntry.UnitType.DEG)); + "n.rawheading", "Raw Heading calculated using the position of the vessels root part", formatters.Default, APIEntry.UnitType.DEG)); registerAPI(new PlotableAPIEntry( dataSources => @@ -1215,7 +1215,7 @@ public NavBallDataLinkHandler(FormatterProvider formatters) Quaternion result = updateHeadingPitchRollField(dataSources.vessel, dataSources.vessel.rootPart.transform.position); return result.eulerAngles.x; }, - "n.rawpitchRoot", "Raw Pitch calculated using the position of the vessels root part", formatters.Default, APIEntry.UnitType.DEG)); + "n.rawpitch", "Raw Pitch calculated using the position of the vessels root part", formatters.Default, APIEntry.UnitType.DEG)); registerAPI(new PlotableAPIEntry( dataSources => @@ -1223,7 +1223,7 @@ public NavBallDataLinkHandler(FormatterProvider formatters) Quaternion result = updateHeadingPitchRollField(dataSources.vessel, dataSources.vessel.rootPart.transform.position); return result.eulerAngles.z; }, - "n.rawrollRoot", "Raw Roll calculated using the position of the vessels root part", formatters.Default, APIEntry.UnitType.DEG)); + "n.rawroll", "Raw Roll calculated using the position of the vessels root part", formatters.Default, APIEntry.UnitType.DEG)); } #endregion