diff --git a/src/marshconnector.cpp b/src/marshconnector.cpp index 28fd1d9..fd11205 100644 --- a/src/marshconnector.cpp +++ b/src/marshconnector.cpp @@ -221,7 +221,10 @@ float MarshConnector::get_parameter(const String &id) { } Transform3D MarshConnector::get_aircraft() { - return Transform3D{Basis{last_rotation}, last_location}; + float heading_rad = parameters[NAV_OFS_HDG] * Math_PI / 180.0; + Quaternion heading_offset = Quaternion(Vector3(0, 1, 0), heading_rad); + Quaternion rotation = heading_offset * last_rotation; + return Transform3D{Basis{rotation}, last_location + Vector3{-parameters[NAV_OFS_Y], 0.0, parameters[NAV_OFS_X]}}; } Vector2 MarshConnector::get_cyclic() { diff --git a/src/marshconnector.h b/src/marshconnector.h index 5f3279d..029f831 100644 --- a/src/marshconnector.h +++ b/src/marshconnector.h @@ -96,7 +96,7 @@ private: LOCAL_FRAME_LON, PARAM_COUNT, }; - float parameters[PARAM_COUNT] = {0.0, 0.0, 0.0, 0.0, 0.0}; + float parameters[PARAM_COUNT] = {90.0, -100.0, 300.0, 0.0, 0.0}; const String parameter_names[PARAM_COUNT] = { // "FOG_DENSITY", "NAV_OFS_HDG", "NAV_OFS_X", "NAV_OFS_Y",