From 59c9c54e358488ee949b15a0f7709e0b4e9da708 Mon Sep 17 00:00:00 2001 From: "Marek S. Lukasiewicz" Date: Wed, 17 Dec 2025 15:36:17 +0100 Subject: [PATCH] Fix velocity conversion when using NAV_OFS_HDG --- src/marshconnector.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/marshconnector.cpp b/src/marshconnector.cpp index 5d4266e..d43e511 100644 --- a/src/marshconnector.cpp +++ b/src/marshconnector.cpp @@ -243,7 +243,12 @@ Transform3D MarshConnector::get_aircraft() { return offset * marsh; } -Vector3 MarshConnector::get_velocity() { return last_velocity; } +Vector3 MarshConnector::get_velocity() { + float heading_rad = parameters[NAV_OFS_HDG] * Math_PI / 180.0; + // To rotate clockwise looking from above, that's negative Y in Godot + Quaternion heading_offset = Quaternion(Vector3(0, 1, 0), -heading_rad); + return heading_offset.xform(last_velocity); +} Vector2 MarshConnector::get_cyclic() { return Vector2{last_controls.x, last_controls.y};