Add option to change base frame for orientation and base station (ENU, NWU, NED)

docs/src/functions.md

@@ -51,6 +51,10 @@ The function ```set_data_path(data_path)``` can be used to set the directory for
 ## Rotation matrices and conversions
 ```@docs
 calc_orient_rot
+euler_convert
+euler_enu2ned
+euler_ned2enu
+frame_transform
 enu2ned
 ned2enu
 is_right_handed_orthonormal

docs/src/reference_frames.md

@@ -38,11 +38,26 @@ The position of the kite can be described with two angles, the azimuth angle φ
 Three azimuth angles are used, the azimuth angle in the wind reference frame and $\mathrm{azimuth\_east}$ and $\mathrm{azimuth\_north}$. The azimuth angles in wind reference frame and $\mathrm{azimuth\_north}$ are defined positive anti-clockwise when seen from above, $\mathrm{azimuth\_east}$ is defined positive clockwise when seen from above. In the log file and the system state $\mathrm{azimuth}$ in wind reference frame is used (for KiteUtils 0.8.2 and higher).
 
 ## Orientation of the kite
-For the orientation, either a quaternion or roll, pitch and yaw angles are used. The
-orientation is defined with respect to the NED reference frame (see above), because
-that is the convention used by the Xsens IMU. Quaternions and Euler angles stored in
-`SysState.orient` follow this NED convention. The function `quat2euler()` expects a
-NED-based quaternion as input.
+For the orientation, either a quaternion or roll, pitch and yaw angles are used.
+
+The orientation convention is controlled by the `orientation_frame` setting in
+`settings.yaml`, which accepts `"NED"`, `"ENU"`, or `"NWU"`. The default is
+`NED` (North, East, Down), the convention used by the Xsens IMU.
+
+The ground station frame is controlled by `ground_station_frame` (default
+`"NWU"`, North-West-Up, historically called **EG** in this codebase). This
+frame is used as the intermediate step when converting to the wind reference
+frame.
+
+The `InertialFrame` enum (`NED`, `ENU`, `NWU`) is used throughout the API.
+Most orientation functions accept an `orientation_frame` keyword argument that
+defaults to `se().orientation_frame`.
+
+The general conversion function `euler_convert(r, p, y, from, to)` converts
+Euler angles between any two `InertialFrame` conventions. Convenience aliases
+`euler_enu2ned()` and `euler_ned2enu()` are provided. The function
+`frame_transform(from, to)` returns the 3×3 transformation matrix between
+any two frames.
 
 The origin of the kite reference frame around which it rotates is the centre point
 defined as $0.5 * (C + D)$, where C and D are positions of the point masses of the

docs/src/types.md

@@ -11,6 +11,11 @@ Settings(project)
 ```
 Also look at the default file that is parsed to fill the Settings struct with data: [settings.yaml](https://github.com/ufechner7/KiteUtils.jl/blob/main/data/settings.yaml) .
 
+## Reference frame convention
+```@docs
+InertialFrame
+```
+
 ## Abstract interface
 ```@docs
 AbstractKiteModel

src/KiteUtils.jl

@@ -59,6 +59,9 @@ export acos2, quat2euler, quat2viewer, wrap2pi                           # geome
 export fromEG2W, fromENU2EG, fromEX2EG, fromKS2EX, fromW2SE              # reference frame transformations
 export azn2azw,  calc_course , calc_heading, calc_heading_w              # geometric functions
 export calc_orient_rot, enu2ned, is_right_handed_orthonormal, ned2enu
+export InertialFrame, NED, ENU, NWU
+export euler_enu2ned, euler_ned2enu, euler_convert
+export frame_transform
 export wind_vec_from_angles, angles_from_wind_vec
 export copy_settings, get_data_path, load_settings, set_data_path        # functions for reading and copying parameters
 export fpc_settings, fpp_settings, se, se_dict, update_settings, wc_settings
@@ -90,9 +93,9 @@ with all kite models, or a specific method has to be defined for the specific ki
 """
 abstract type AbstractKiteModel end
 
+include("transformations.jl")
 include("settings.jl")
 include("yaml_utils.jl")
-include("transformations.jl")
 include("trafo.jl")
 
 include("_sysstate.jl")

src/settings.jl

@@ -21,6 +21,10 @@ $(TYPEDFIELDS)
 @with_kw mutable struct Settings @deftype Float64
     "name of the yaml file with the settings"
     sim_settings::String      = ""
+    "orientation convention: NED, ENU, or NWU"
+    orientation_frame::InertialFrame = NED
+    "ground station frame: NED, ENU, or NWU"
+    ground_station_frame::InertialFrame = NWU
 
     "filename without extension  [replay only]"
     log_file::String      = ""
@@ -377,6 +381,15 @@ end
 function StructTypes.constructfrom(::Type{SVec3}, vec::AbstractVector)
     SVec3(vec[1], vec[2], vec[3])
 end
+function StructTypes.constructfrom(
+        ::Type{InertialFrame}, val::AbstractString)
+    upper = uppercase(val)
+    upper == "NED" && return NED
+    upper == "ENU" && return ENU
+    upper == "NWU" && return NWU
+    error("Unknown InertialFrame: $val. " *
+          "Expected \"NED\", \"ENU\", or \"NWU\".")
+end
 PROJECT::String = "system.yaml"
 
 """

src/trafo.jl

@@ -37,14 +37,20 @@ function fromW2SE(vector, elevation, azimuth)
     rotate_elevation * rotate_azimuth * rotate_first_step * vector
 end
 
-""" 
-    fromKS2EX(vector, orientation)
+"""
+    fromKS2EX(vector, orientation;
+              orientation_frame=se().orientation_frame)
 
-Transform a vector (x,y,z) from KiteSensor to Earth Xsens reference frame.
+Transform a vector (x,y,z) from KiteSensor to the inertial
+frame specified by `orientation_frame`.
 
 - orientation in Euler angles (roll, pitch, yaw)
+- When `orientation_frame == NED`: output is in NED (EX)
+- When `orientation_frame == ENU`: output is in ENU
 """
-function fromKS2EX(vector, orientation)
+function fromKS2EX(vector, orientation;
+        orientation_frame::InertialFrame =
+            se().orientation_frame)
     roll, pitch, yaw  = orientation[1], orientation[2], orientation[3]
     rotateYAW = @SMatrix[cos(yaw) -sin(yaw) 0;
                          sin(yaw)  cos(yaw) 0;
@@ -83,44 +89,59 @@ function fromEG2W(vector, down_wind_direction = pi/2.0)
 end
 
 """
-    calc_heading_w(orientation, down_wind_direction = pi/2.0)
+    calc_heading_w(orientation,
+                   down_wind_direction = pi/2.0;
+                   orientation_frame=se().orientation_frame)
 
 Calculate the heading vector in wind reference frame.
 """
-function calc_heading_w(orientation, down_wind_direction = pi/2.0)
-    # create a unit heading vector in the Xsens reference frame
-    heading_sensor =  SVector(1, 0, 0)
-    # rotate headingSensor to the Earth Xsens reference frame
-    headingEX = fromKS2EX(heading_sensor, orientation)
-    # rotate headingEX to earth groundstation reference frame
-    headingEG = fromEX2EG(headingEX)
-    # rotate headingEG to headingW and convert to 2d HeadingW vector
-    fromEG2W(headingEG, down_wind_direction)
+function calc_heading_w(orientation,
+        down_wind_direction = pi/2.0;
+        orientation_frame::InertialFrame =
+            se().orientation_frame)
+    gs_frame = se().ground_station_frame
+    heading_sensor = SVector(1, 0, 0)
+    heading = fromKS2EX(heading_sensor, orientation;
+                        orientation_frame)
+    T = frame_transform(orientation_frame, gs_frame)
+    heading_gs = T * heading
+    fromEG2W(heading_gs, down_wind_direction)
 end
 
 """
-    calc_heading(orientation, elevation, azimuth; upwind_dir=-pi/2, respos=true)
+    calc_heading(orientation, elevation, azimuth;
+                 upwind_dir=-pi/2, respos=true,
+                 orientation_frame=se().orientation_frame)
 
-Calculate the heading angle of the kite in radians. The heading is the direction the nose 
-of the kite is pointing to, expressed in the Small Earth (SE) reference frame.
+Calculate the heading angle of the kite in radians. The
+heading is the direction the nose of the kite is pointing to,
+expressed in the Small Earth (SE) reference frame.
 
 # Arguments
-- `orientation`: Euler angles (roll, pitch, yaw) in radians, calculated with respect to 
-                 the North, East, Down (NED) reference frame
+- `orientation`: Euler angles (roll, pitch, yaw) in radians
 - `elevation`:   Elevation angle of the kite in radians
 - `azimuth`:     Azimuth angle of the kite in radians
-- `upwind_dir`:  Direction the wind is coming from in radians; zero at north; clockwise 
-                 positive from above (default: -π/2, wind from west)
-- `respos`:      If true, return angle in range [0, 2π]; if false, return in range [-π, π] 
-                 (default: true)
+- `upwind_dir`:  Direction the wind is coming from in radians;
+  zero at north; clockwise positive from above
+  (default: -π/2, wind from west)
+- `respos`:      If true, return angle in range [0, 2π];
+  if false, return in range [-π, π] (default: true)
+- `orientation_frame`: `NED` or `ENU` convention for the
+  orientation angles
 
 # Returns
-The heading angle in radians, measured from the positive x-axis of the SE reference frame.
+The heading angle in radians, measured from the positive
+x-axis of the SE reference frame.
 """
-function calc_heading(orientation, elevation, azimuth; upwind_dir=-pi/2, respos=true)
+function calc_heading(orientation, elevation, azimuth;
+        upwind_dir=-pi/2, respos=true,
+        orientation_frame::InertialFrame =
+            se().orientation_frame)
     down_wind_direction = wrap2pi(upwind_dir + π)
-    headingSE = fromW2SE(calc_heading_w(orientation, down_wind_direction), elevation, azimuth)
-    angle = atan(headingSE.y, headingSE.x)
+    heading_w = calc_heading_w(orientation,
+        down_wind_direction; orientation_frame)
+    heading_se = fromW2SE(heading_w, elevation, azimuth)
+    angle = atan(heading_se.y, heading_se.x)
     if angle < 0 && respos
         angle += 2π
     end
@@ -139,8 +160,10 @@ Calculate the course angle in radian.
 """
 function calc_course(velocityENU, elevation, azimuth, upwind_dir=-pi/2, respos=true)
     down_wind_direction = wrap2pi(upwind_dir + π)
-    velocityEG = fromENU2EG(velocityENU)
-    velocityW = fromEG2W(velocityEG, down_wind_direction)
+    gs_frame = se().ground_station_frame
+    T = frame_transform(ENU, gs_frame)
+    velocity_gs = T * velocityENU
+    velocityW = fromEG2W(velocity_gs, down_wind_direction)
     velocitySE = fromW2SE(velocityW, elevation, azimuth)
     angle = atan(velocitySE.y, velocitySE.x)
     if angle < 0  && respos