IQ.Pilot Release Commit @ 0798119

iqpilot/common/atlas_alerts.py

@@ -0,0 +1,281 @@
+from __future__ import annotations
+
+from abc import ABC, abstractmethod
+from bisect import insort
+from collections.abc import Callable, Iterable
+from dataclasses import dataclass, field
+from enum import IntEnum
+
+import cereal.messaging as messaging
+from cereal import car, log
+from openpilot.common.realtime import DT_CTRL
+from openpilot.system.hardware import HARDWARE
+
+AlertSize = log.SelfdriveState.AlertSize
+AlertStatus = log.SelfdriveState.AlertStatus
+VisualAlert = car.CarControl.HUDControl.VisualAlert
+AudibleAlert = car.CarControl.HUDControl.AudibleAlert
+
+
+def _frames_for(seconds: float) -> int:
+  return int(seconds / DT_CTRL)
+
+
+class Tier(IntEnum):
+  LOWEST = 0
+  LOWER = 1
+  LOW = 2
+  MID = 3
+  HIGH = 4
+  HIGHEST = 5
+
+
+class Tags:
+  ENABLE = "enable"
+  PRE_ENABLE = "preEnable"
+  OVERRIDE_LATERAL = "overrideLateral"
+  OVERRIDE_LONGITUDINAL = "overrideLongitudinal"
+  NO_ENTRY = "noEntry"
+  WARNING = "warning"
+  USER_DISABLE = "userDisable"
+  SOFT_DISABLE = "softDisable"
+  IMMEDIATE_DISABLE = "immediateDisable"
+  PERMANENT = "permanent"
+
+
+@dataclass(slots=True)
+class AlertCard:
+  alert_text_1: str
+  alert_text_2: str
+  alert_status: log.SelfdriveState.AlertStatus
+  alert_size: log.SelfdriveState.AlertSize
+  priority: Tier
+  visual_alert: car.CarControl.HUDControl.VisualAlert
+  audible_alert: car.CarControl.HUDControl.AudibleAlert
+  duration: int
+  creation_delay: float = 0.0
+  alert_type: str = field(default="", init=False)
+  event_type: str | None = field(default=None, init=False)
+
+  def __init__(self,
+               alert_text_1: str,
+               alert_text_2: str,
+               alert_status: log.SelfdriveState.AlertStatus,
+               alert_size: log.SelfdriveState.AlertSize,
+               priority: Tier,
+               visual_alert: car.CarControl.HUDControl.VisualAlert,
+               audible_alert: car.CarControl.HUDControl.AudibleAlert,
+               duration: float,
+               creation_delay: float = 0.0):
+    self.alert_text_1 = alert_text_1
+    self.alert_text_2 = alert_text_2
+    self.alert_status = alert_status
+    self.alert_size = alert_size
+    self.priority = priority
+    self.visual_alert = visual_alert
+    self.audible_alert = audible_alert
+    self.duration = _frames_for(duration)
+    self.creation_delay = creation_delay
+    self.alert_type = ""
+    self.event_type = None
+
+  def __str__(self) -> str:
+    return f"{self.alert_text_1}/{self.alert_text_2} {self.priority} {self.visual_alert} {self.audible_alert}"
+
+
+AlertFactory = Callable[[car.CarParams, car.CarState, messaging.SubMaster, bool, int, log.ControlsState], AlertCard]
+
+
+def car_mode_entry_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int, personality) -> AlertCard:
+  del CS, sm, metric, soft_disable_time, personality
+  headline = "Enable Adaptive Cruise to Engage"
+  if CP.brand == "honda":
+    headline = "Enable Main Switch to Engage"
+  return NoEntryCard(headline)
+
+
+class EventBook(ABC):
+  def __init__(self):
+    self._live_names: list[int] = []
+    self._latched_names: list[int] = []
+    self.event_counters: dict[int, int] = {}
+
+  @property
+  def events(self) -> list[int]:
+    return self._live_names
+
+  @events.setter
+  def events(self, values: list[int]) -> None:
+    self._live_names = values
+
+  @property
+  def static_events(self) -> list[int]:
+    return self._latched_names
+
+  @static_events.setter
+  def static_events(self, values: list[int]) -> None:
+    self._latched_names = values
+
+  @property
+  def names(self) -> list[int]:
+    return list(self._live_names)
+
+  def __len__(self) -> int:
+    return len(self._live_names)
+
+  def add(self, event_name: int, static: bool = False) -> None:
+    if static:
+      insort(self._latched_names, event_name)
+    insort(self._live_names, event_name)
+
+  def clear(self) -> None:
+    refreshed: dict[int, int] = {}
+    for event_name, frames_seen in self.event_counters.items():
+      refreshed[event_name] = frames_seen + 1 if event_name in self._live_names else 0
+    self.event_counters = refreshed
+    self._live_names = list(self._latched_names)
+
+  def contains(self, event_type: str) -> bool:
+    board = self.get_events_mapping()
+    return any(event_type in board.get(event_name, {}) for event_name in self._live_names)
+
+  def has(self, event_name: int) -> bool:
+    return event_name in self._live_names
+
+  def contains_in_list(self, events_list: list[int]) -> bool:
+    return any(event_name in self._live_names for event_name in events_list)
+
+  def remove(self, event_name: int, static: bool = False) -> None:
+    if static and event_name in self._latched_names:
+      self._latched_names.remove(event_name)
+
+    if event_name in self._live_names:
+      self.event_counters[event_name] = self.event_counters.get(event_name, 0) + 1
+      self._live_names.remove(event_name)
+
+  def add_from_msg(self, events: Iterable) -> None:
+    for event in events:
+      insort(self._live_names, event.name.raw)
+
+  def to_msg(self):
+    board = self.get_events_mapping()
+    outbound = []
+    for event_name in self._live_names:
+      msg = self.get_event_msg_type().new_message()
+      msg.name = event_name
+      for event_kind in board.get(event_name, {}):
+        setattr(msg, event_kind, True)
+      outbound.append(msg)
+    return outbound
+
+  def create_alerts(self, event_types: list[str], callback_args=None):
+    callback_args = [] if callback_args is None else callback_args
+    board = self.get_events_mapping()
+    spawned: list[AlertCard] = []
+    for event_name in self._live_names:
+      variants = board.get(event_name, {})
+      for event_type in event_types:
+        chosen = variants.get(event_type)
+        if chosen is None:
+          continue
+        alert = self._realize(chosen, callback_args)
+        age_frames = self.event_counters.get(event_name, 0) + 1
+        if age_frames * DT_CTRL < alert.creation_delay:
+          continue
+        alert.alert_type = f"{self.get_event_name(event_name)}/{event_type}"
+        alert.event_type = event_type
+        spawned.append(alert)
+    return spawned
+
+  @staticmethod
+  def _realize(candidate: AlertCard | AlertFactory, callback_args: list) -> AlertCard:
+    return candidate if isinstance(candidate, AlertCard) else candidate(*callback_args)
+
+  @abstractmethod
+  def get_events_mapping(self) -> dict[int, dict[str, AlertCard | AlertFactory]]:
+    raise NotImplementedError
+
+  @abstractmethod
+  def get_event_name(self, event: int) -> str:
+    raise NotImplementedError
+
+  @abstractmethod
+  def get_event_msg_type(self):
+    raise NotImplementedError
+
+
+def _mici_reframe(primary: str, secondary: str) -> tuple[str, str, log.SelfdriveState.AlertSize]:
+  if HARDWARE.get_device_type() == "mici":
+    return secondary, primary, AlertSize.small
+  return primary, secondary, AlertSize.mid
+
+
+class NoEntryCard(AlertCard):
+  def __init__(self,
+               alert_text_2: str,
+               alert_text_1: str = "openpilot Unavailable",
+               visual_alert: car.CarControl.HUDControl.VisualAlert = VisualAlert.none,
+               priority: Tier = Tier.LOW):
+    primary, secondary, size = _mici_reframe(alert_text_1, alert_text_2)
+    super().__init__(primary, secondary, AlertStatus.normal, size, priority, visual_alert, AudibleAlert.refuse, 3.0)
+
+
+class GentleDisableCard(AlertCard):
+  def __init__(self, alert_text_2: str):
+    super().__init__(
+      "TAKE CONTROL IMMEDIATELY",
+      alert_text_2,
+      AlertStatus.userPrompt,
+      AlertSize.full,
+      Tier.MID,
+      VisualAlert.steerRequired,
+      AudibleAlert.warningSoft,
+      2.0,
+    )
+
+
+class PendingDisableCard(GentleDisableCard):
+  def __init__(self, alert_text_2: str):
+    super().__init__(alert_text_2)
+    self.alert_text_1 = "openpilot will disengage"
+
+
+class HardDisableCard(AlertCard):
+  def __init__(self, alert_text_2: str):
+    super().__init__(
+      "TAKE CONTROL IMMEDIATELY",
+      alert_text_2,
+      AlertStatus.critical,
+      AlertSize.full,
+      Tier.HIGHEST,
+      VisualAlert.steerRequired,
+      AudibleAlert.warningImmediate,
+      4.0,
+    )
+
+
+class ChimeCard(AlertCard):
+  def __init__(self, audible_alert: car.CarControl.HUDControl.AudibleAlert):
+    super().__init__("", "", AlertStatus.normal, AlertSize.none, Tier.MID, VisualAlert.none, audible_alert, 0.2)
+
+
+class BannerCard(AlertCard):
+  def __init__(self, alert_text_1: str, alert_text_2: str = "", duration: float = 0.2, priority: Tier = Tier.LOWER, creation_delay: float = 0.0):
+    size = AlertSize.mid if alert_text_2 else AlertSize.small
+    super().__init__(alert_text_1, alert_text_2, AlertStatus.normal, size, priority, VisualAlert.none, AudibleAlert.none, duration, creation_delay)
+
+
+class BootCard(AlertCard):
+  def __init__(self, alert_text_1: str, alert_text_2: str = "Always keep hands on wheel and eyes on road", alert_status=AlertStatus.normal):
+    if HARDWARE.get_device_type() == "mici":
+      compact_secondary = "" if alert_text_2 == "Always keep hands on wheel and eyes on road" else alert_text_2
+      super().__init__(alert_text_1, compact_secondary, alert_status, AlertSize.small, Tier.LOWER, VisualAlert.none, AudibleAlert.none, 5.0)
+    else:
+      super().__init__(alert_text_1, alert_text_2, alert_status, AlertSize.mid, Tier.LOWER, VisualAlert.none, AudibleAlert.none, 5.0)
+
+
+class AlertBase(AlertCard):
+  pass
+
+
+NULL_ALERT = AlertCard("", "", AlertStatus.normal, AlertSize.none, Tier.LOWEST, VisualAlert.none, AudibleAlert.none, 0.0)

iqpilot/common/k3_slc_log.py

@@ -0,0 +1,15 @@
+from datetime import datetime
+
+from openpilot.common.swaglog import cloudlog
+
+K3_SLC_LOG_FILE = "/data/openpilot/k3_slc.txt"
+
+
+def k3_slc_log(message: str) -> None:
+  try:
+    with open(K3_SLC_LOG_FILE, "a") as f:
+      timestamp = datetime.now().strftime("%Y-%m-%d %H:%M:%S.%f")[:-3]
+      f.write(f"[{timestamp}] {message}\n")
+      f.flush()
+  except Exception as e:
+    cloudlog.error(f"[K3_SLC] Failed to write debug log: {e}")

iqpilot/common/slc_utilities.py

@@ -0,0 +1,126 @@
+import math
+import numpy as np
+
+try:
+  import requests
+except ImportError:
+  requests = None
+
+from openpilot.iqpilot.common.slc_variables import EARTH_RADIUS
+
+
+def calculate_bearing_offset(latitude, longitude, current_bearing, distance):
+  """
+  Calculate new GPS coordinates given a starting point, bearing, and distance.
+  Used for Mapbox API lookahead calculations.
+
+  Args:
+    latitude: Starting latitude in degrees
+    longitude: Starting longitude in degrees
+    current_bearing: Bearing in degrees (0-360)
+    distance: Distance to project in meters
+
+  Returns:
+    Tuple of (new_latitude, new_longitude) in degrees
+  """
+  bearing = math.radians(current_bearing)
+  lat_rad = math.radians(latitude)
+  lon_rad = math.radians(longitude)
+
+  delta = distance / EARTH_RADIUS
+
+  new_lat = math.asin(math.sin(lat_rad) * math.cos(delta) + math.cos(lat_rad) * math.sin(delta) * math.cos(bearing))
+  new_lon = lon_rad + math.atan2(math.sin(bearing) * math.sin(delta) * math.cos(lat_rad), math.cos(delta) - math.sin(lat_rad) * math.sin(new_lat))
+  return math.degrees(new_lat), math.degrees(new_lon)
+
+
+def calculate_distance_to_point(lat1, lon1, lat2, lon2):
+  """
+  Calculate the great circle distance between two GPS points using the Haversine formula.
+
+  Args:
+    lat1, lon1: First point coordinates in degrees
+    lat2, lon2: Second point coordinates in degrees
+
+  Returns:
+    Distance in meters
+  """
+  lat1_rad = math.radians(lat1)
+  lon1_rad = math.radians(lon1)
+  lat2_rad = math.radians(lat2)
+  lon2_rad = math.radians(lon2)
+
+  delta_lat = lat2_rad - lat1_rad
+  delta_lon = lon2_rad - lon1_rad
+
+  a = (math.sin(delta_lat / 2) ** 2) + math.cos(lat1_rad) * math.cos(lat2_rad) * (math.sin(delta_lon / 2) ** 2)
+  c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a))
+
+  return EARTH_RADIUS * c
+
+
+def calculate_lane_width(lane_line1, lane_line2, road_edge=None):
+  """
+  Calculate the width of a lane based on lane line positions.
+  Used for speed limit filler to determine road width.
+
+  Args:
+    lane_line1: First lane line object with x, y coordinates
+    lane_line2: Second lane line object with x, y coordinates
+    road_edge: Optional road edge object with x, y coordinates
+
+  Returns:
+    Lane width in meters
+  """
+  lane_line1_x = np.asarray(lane_line1.x)
+  lane_line1_y = np.asarray(lane_line1.y)
+
+  lane_line2_x = np.asarray(lane_line2.x)
+  lane_line2_y = np.asarray(lane_line2.y)
+
+  lane_y_interp = np.interp(lane_line2_x, lane_line1_x, lane_line1_y)
+  distance_to_lane = np.median(np.abs(lane_line2_y - lane_y_interp))
+
+  if road_edge is None:
+    return distance_to_lane
+
+  road_edge_x = np.asarray(road_edge.x)
+  road_edge_y = np.asarray(road_edge.y)
+
+  edge_y_interp = np.interp(lane_line2_x, road_edge_x, road_edge_y)
+  distance_to_edge = np.median(np.abs(lane_line2_y - edge_y_interp))
+
+  return max(distance_to_lane, distance_to_edge)
+
+
+def is_url_pingable(url):
+  """
+  Check if a URL is accessible and responding.
+  Used to verify Mapbox/Overpass API availability before making requests.
+
+  Args:
+    url: URL to ping
+
+  Returns:
+    Boolean indicating if URL is accessible
+  """
+  if not url:
+    return False
+
+  if requests is None:
+    return False
+
+  if not hasattr(is_url_pingable, "session"):
+    is_url_pingable.session = requests.Session()
+    is_url_pingable.session.headers.update({"User-Agent": "iqpilot-ping-test/1.0"})
+
+  try:
+    response = is_url_pingable.session.head(url, timeout=10, allow_redirects=True)
+    if response.status_code in (405, 501):
+      response = is_url_pingable.session.get(url, timeout=10, allow_redirects=True, stream=True)
+
+    is_accessible = response.ok
+    response.close()
+    return is_accessible
+  except Exception:
+    return False

iqpilot/common/slc_variables.py

@@ -0,0 +1,35 @@
+# Earth radius in meters (for GPS calculations)
+EARTH_RADIUS = 6378137
+
+# Mapbox API limits
+FREE_MAPBOX_REQUESTS = 100_000
+
+# Speed limit offset zones for different unit systems
+# Each entry is (min_speed_ms, max_speed_ms, param_name); the param value is a
+# percent offset applied to the resolved limit (e.g. 10 -> +10%), lower bound inclusive
+
+OFFSET_PERCENT_MAX = 50.0
+
+OFFSET_MAP_IMPERIAL = [
+  (0, 8.94, "speed_limit_offset1"),               # 0-20 mph
+  (8.94, 17.88, "speed_limit_offset2"),           # 20-40 mph
+  (17.88, float("inf"), "speed_limit_offset3"),   # 40+ mph
+]
+
+OFFSET_MAP_METRIC = [
+  (0, 8.33, "speed_limit_offset1"),               # 0-30 km/h
+  (8.33, 16.67, "speed_limit_offset2"),           # 30-60 km/h
+  (16.67, float("inf"), "speed_limit_offset3"),   # 60+ km/h
+]
+
+# Speed limit filler constants
+BOUNDING_BOX_RADIUS_DEGREE = 0.1
+MAX_ENTRIES = 1_000_000
+MAX_OVERPASS_DATA_BYTES = 1_073_741_824
+MAX_OVERPASS_REQUESTS = 10_000
+METERS_PER_DEG_LAT = 111_320
+VETTING_INTERVAL_DAYS = 7
+
+# Overpass API URLs
+OVERPASS_API_URL = "https://overpass-api.de/api/interpreter"
+OVERPASS_STATUS_URL = "https://overpass-api.de/api/status"

iqpilot/common/speed_assist_tiers.py

@@ -0,0 +1,20 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+
+Engagement tiers for the speed-assist feature. A tier is persisted as an integer
+under the "IQSpeedAssistMode" param; the ordinal IS the stored value and must remain
+stable (0..3), ordered by how much the tier is allowed to intervene.
+"""
+from enum import IntEnum
+
+STORE_KEY = "IQSpeedAssistMode"
+
+# none -> just display the limit -> highlight overspeed -> move the set speed
+SpeedAssistTier = IntEnum("SpeedAssistTier", "DISABLED ADVISORY ALERTING ACTUATING", start=0)
+
+DEFAULT_TIER = SpeedAssistTier.ADVISORY
+
+
+def actuates_speed(tier) -> bool:
+  """Only the top tier is permitted to drive the cruise set speed."""
+  return int(tier) == SpeedAssistTier.ACTUATING

iqpilot/common/steer_delay.py

@@ -0,0 +1,43 @@
+"""
+Copyright © IQ.Lvbs, apart of Project Teal Lvbs, All Rights Reserved, licensed under https://konn3kt.com/tos
+
+Chooses which steer-actuator delay the lateral controllers run with: the value the
+live estimator learned, or the driver's fixed software delay — gated by the
+"IQLiveSteerDelay" param. The pick is mirrored into "IQSteerDelayCache" so consumers that do
+not subscribe to liveDelay can still read the current value.
+"""
+from openpilot.common.params import Params
+
+_ENABLE_KEY = "IQLiveSteerDelay"
+_FIXED_KEY = "IQSoftwareSteerDelay"
+_CACHE_KEY = "IQSteerDelayCache"
+
+
+def resolve_steer_delay(params, stock_delay):
+  """Learned lateral delay while live-learning is enabled, otherwise the stock delay."""
+  if not params.get_bool(_ENABLE_KEY):
+    return stock_delay
+  return float(params.get(_CACHE_KEY, return_default=True))
+
+
+def cached_steer_delay():
+  """Last value SteerDelayPublisher mirrored into the param — usable without a
+  liveDelay subscription (e.g. at process startup)."""
+  return Params().get(_CACHE_KEY, return_default=True)
+
+
+class SteerDelayPublisher:
+  """Refreshes IQSteerDelayCache every lag message: the learned live delay when the
+  toggle is on, else the actuator delay plus the driver's fixed software offset."""
+
+  def __init__(self, car_params):
+    self._params = Params()
+    self._actuator_delay = car_params.steerActuatorDelay
+
+  def _fixed_delay(self):
+    return self._actuator_delay + self._params.get(_FIXED_KEY, return_default=True)
+
+  def update(self, lag_msg):
+    live = self._params.get_bool(_ENABLE_KEY)
+    value = lag_msg.liveDelay.lateralDelay if live else self._fixed_delay()
+    self._params.put_nonblocking(_CACHE_KEY, value)

iqpilot/common/transformations/SConscript

@@ -0,0 +1,4 @@
+Import('env')
+
+transformations = env.Library('transformations', ['orientation.cc', 'coordinates.cc'])
+Export('transformations')

iqpilot/common/transformations/coordinates.cc

@@ -0,0 +1,100 @@
+#define _USE_MATH_DEFINES
+
+#include "iqpilot/common/transformations/coordinates.hpp"
+
+#include <iostream>
+#include <cmath>
+#include <eigen3/Eigen/Dense>
+
+double a = 6378137; // lgtm [cpp/short-global-name]
+double b = 6356752.3142; // lgtm [cpp/short-global-name]
+double esq = 6.69437999014 * 0.001; // lgtm [cpp/short-global-name]
+double e1sq = 6.73949674228 * 0.001;
+
+
+static Geodetic to_degrees(Geodetic geodetic){
+  geodetic.lat = RAD2DEG(geodetic.lat);
+  geodetic.lon = RAD2DEG(geodetic.lon);
+  return geodetic;
+}
+
+static Geodetic to_radians(Geodetic geodetic){
+  geodetic.lat = DEG2RAD(geodetic.lat);
+  geodetic.lon = DEG2RAD(geodetic.lon);
+  return geodetic;
+}
+
+
+ECEF geodetic2ecef(const Geodetic &geodetic) {
+  auto g = to_radians(geodetic);
+  double xi = sqrt(1.0 - esq * pow(sin(g.lat), 2));
+  double x = (a / xi + g.alt) * cos(g.lat) * cos(g.lon);
+  double y = (a / xi + g.alt) * cos(g.lat) * sin(g.lon);
+  double z = (a / xi * (1.0 - esq) + g.alt) * sin(g.lat);
+  return {x, y, z};
+}
+
+Geodetic ecef2geodetic(const ECEF &e) {
+  // Convert from ECEF to geodetic using Ferrari's methods
+  // https://en.wikipedia.org/wiki/Geographic_coordinate_conversion#Ferrari.27s_solution
+  double x = e.x;
+  double y = e.y;
+  double z = e.z;
+
+  double r = sqrt(x * x + y * y);
+  double Esq = a * a - b * b;
+  double F = 54 * b * b * z * z;
+  double G = r * r + (1 - esq) * z * z - esq * Esq;
+  double C = (esq * esq * F * r * r) / (pow(G, 3));
+  double S = cbrt(1 + C + sqrt(C * C + 2 * C));
+  double P = F / (3 * pow((S + 1 / S + 1), 2) * G * G);
+  double Q = sqrt(1 + 2 * esq * esq * P);
+  double r_0 = -(P * esq * r) / (1 + Q) + sqrt(0.5 * a * a*(1 + 1.0 / Q) - P * (1 - esq) * z * z / (Q * (1 + Q)) - 0.5 * P * r * r);
+  double U = sqrt(pow((r - esq * r_0), 2) + z * z);
+  double V = sqrt(pow((r - esq * r_0), 2) + (1 - esq) * z * z);
+  double Z_0 = b * b * z / (a * V);
+  double h = U * (1 - b * b / (a * V));
+
+  double lat = atan((z + e1sq * Z_0) / r);
+  double lon = atan2(y, x);
+
+  return to_degrees({lat, lon, h});
+}
+
+LocalCoord::LocalCoord(const Geodetic &geodetic, const ECEF &e) {
+  init_ecef <<  e.x, e.y, e.z;
+
+  auto g = to_radians(geodetic);
+
+  ned2ecef_matrix <<
+    -sin(g.lat)*cos(g.lon), -sin(g.lon), -cos(g.lat)*cos(g.lon),
+    -sin(g.lat)*sin(g.lon), cos(g.lon), -cos(g.lat)*sin(g.lon),
+    cos(g.lat), 0, -sin(g.lat);
+  ecef2ned_matrix = ned2ecef_matrix.transpose();
+}
+
+NED LocalCoord::ecef2ned(const ECEF &e) {
+  Eigen::Vector3d ecef;
+  ecef << e.x, e.y, e.z;
+
+  Eigen::Vector3d ned = (ecef2ned_matrix * (ecef - init_ecef));
+  return {ned[0], ned[1], ned[2]};
+}
+
+ECEF LocalCoord::ned2ecef(const NED &n) {
+  Eigen::Vector3d ned;
+  ned << n.n, n.e, n.d;
+
+  Eigen::Vector3d ecef = (ned2ecef_matrix * ned) + init_ecef;
+  return {ecef[0], ecef[1], ecef[2]};
+}
+
+NED LocalCoord::geodetic2ned(const Geodetic &g) {
+  ECEF e = ::geodetic2ecef(g);
+  return ecef2ned(e);
+}
+
+Geodetic LocalCoord::ned2geodetic(const NED &n) {
+  ECEF e = ned2ecef(n);
+  return ::ecef2geodetic(e);
+}

iqpilot/common/transformations/coordinates.hpp

@@ -0,0 +1,43 @@
+#pragma once
+
+#include <eigen3/Eigen/Dense>
+
+#define DEG2RAD(x) ((x) * M_PI / 180.0)
+#define RAD2DEG(x) ((x) * 180.0 / M_PI)
+
+struct ECEF {
+  double x, y, z;
+  Eigen::Vector3d to_vector() const {
+    return Eigen::Vector3d(x, y, z);
+  }
+};
+
+struct NED {
+  double n, e, d;
+  Eigen::Vector3d to_vector() const {
+    return Eigen::Vector3d(n, e, d);
+  }
+};
+
+struct Geodetic {
+  double lat, lon, alt;
+  bool radians=false;
+};
+
+ECEF geodetic2ecef(const Geodetic &g);
+Geodetic ecef2geodetic(const ECEF &e);
+
+class LocalCoord {
+public:
+  Eigen::Matrix3d ned2ecef_matrix;
+  Eigen::Matrix3d ecef2ned_matrix;
+  Eigen::Vector3d init_ecef;
+  LocalCoord(const Geodetic &g, const ECEF &e);
+  LocalCoord(const Geodetic &g) : LocalCoord(g, ::geodetic2ecef(g)) {}
+  LocalCoord(const ECEF &e) : LocalCoord(::ecef2geodetic(e), e) {}
+
+  NED ecef2ned(const ECEF &e);
+  ECEF ned2ecef(const NED &n);
+  NED geodetic2ned(const Geodetic &g);
+  Geodetic ned2geodetic(const NED &n);
+};

iqpilot/common/transformations/orientation.cc

@@ -0,0 +1,143 @@
+#define _USE_MATH_DEFINES
+
+#include <iostream>
+#include <cmath>
+#include <eigen3/Eigen/Dense>
+
+#include "iqpilot/common/transformations/orientation.hpp"
+#include "iqpilot/common/transformations/coordinates.hpp"
+
+Eigen::Quaterniond ensure_unique(const Eigen::Quaterniond &quat) {
+  if (quat.w() > 0){
+    return quat;
+  } else {
+    return Eigen::Quaterniond(-quat.w(), -quat.x(), -quat.y(), -quat.z());
+  }
+}
+
+Eigen::Quaterniond euler2quat(const Eigen::Vector3d &euler) {
+  Eigen::Quaterniond q;
+
+  q = Eigen::AngleAxisd(euler(2), Eigen::Vector3d::UnitZ())
+    * Eigen::AngleAxisd(euler(1), Eigen::Vector3d::UnitY())
+    * Eigen::AngleAxisd(euler(0), Eigen::Vector3d::UnitX());
+  return ensure_unique(q);
+}
+
+
+Eigen::Vector3d quat2euler(const Eigen::Quaterniond &quat) {
+  // TODO: switch to eigen implementation if the range of the Euler angles doesn't matter anymore
+  // Eigen::Vector3d euler = quat.toRotationMatrix().eulerAngles(2, 1, 0);
+  // return {euler(2), euler(1), euler(0)};
+  double gamma = atan2(2 * (quat.w() * quat.x() + quat.y() * quat.z()), 1 - 2 * (quat.x()*quat.x() + quat.y()*quat.y()));
+  double asin_arg_clipped = std::clamp(2 * (quat.w() * quat.y() - quat.z() * quat.x()), -1.0, 1.0);
+  double theta = asin(asin_arg_clipped);
+  double psi = atan2(2 * (quat.w() * quat.z() + quat.x() * quat.y()), 1 - 2 * (quat.y()*quat.y() + quat.z()*quat.z()));
+  return {gamma, theta, psi};
+}
+
+Eigen::Matrix3d quat2rot(const Eigen::Quaterniond &quat) {
+  return quat.toRotationMatrix();
+}
+
+Eigen::Quaterniond rot2quat(const Eigen::Matrix3d &rot) {
+  return ensure_unique(Eigen::Quaterniond(rot));
+}
+
+Eigen::Matrix3d euler2rot(const Eigen::Vector3d &euler) {
+  return quat2rot(euler2quat(euler));
+}
+
+Eigen::Vector3d rot2euler(const Eigen::Matrix3d &rot) {
+  return quat2euler(rot2quat(rot));
+}
+
+Eigen::Matrix3d rot_matrix(double roll, double pitch, double yaw) {
+  return euler2rot({roll, pitch, yaw});
+}
+
+Eigen::Matrix3d rot(const Eigen::Vector3d &axis, double angle) {
+  Eigen::Quaterniond q;
+  q = Eigen::AngleAxisd(angle, axis);
+  return q.toRotationMatrix();
+}
+
+
+Eigen::Vector3d ecef_euler_from_ned(const ECEF &ecef_init, const Eigen::Vector3d &ned_pose) {
+  /*
+    Using Rotations to Build Aerospace Coordinate Systems
+    Don Koks
+    https://apps.dtic.mil/dtic/tr/fulltext/u2/a484864.pdf
+  */
+  LocalCoord converter = LocalCoord(ecef_init);
+  Eigen::Vector3d zero = ecef_init.to_vector();
+
+  Eigen::Vector3d x0 = converter.ned2ecef({1, 0, 0}).to_vector() - zero;
+  Eigen::Vector3d y0 = converter.ned2ecef({0, 1, 0}).to_vector() - zero;
+  Eigen::Vector3d z0 = converter.ned2ecef({0, 0, 1}).to_vector() - zero;
+
+  Eigen::Vector3d x1 = rot(z0, ned_pose(2)) * x0;
+  Eigen::Vector3d y1 = rot(z0, ned_pose(2)) * y0;
+  Eigen::Vector3d z1 = rot(z0, ned_pose(2)) * z0;
+
+  Eigen::Vector3d x2 = rot(y1, ned_pose(1)) * x1;
+  Eigen::Vector3d y2 = rot(y1, ned_pose(1)) * y1;
+  Eigen::Vector3d z2 = rot(y1, ned_pose(1)) * z1;
+
+  Eigen::Vector3d x3 = rot(x2, ned_pose(0)) * x2;
+  Eigen::Vector3d y3 = rot(x2, ned_pose(0)) * y2;
+
+
+  x0 = Eigen::Vector3d(1, 0, 0);
+  y0 = Eigen::Vector3d(0, 1, 0);
+  z0 = Eigen::Vector3d(0, 0, 1);
+
+  double psi = atan2(x3.dot(y0), x3.dot(x0));
+  double theta = atan2(-x3.dot(z0), sqrt(pow(x3.dot(x0), 2) + pow(x3.dot(y0), 2)));
+
+  y2 = rot(z0, psi) * y0;
+  z2 = rot(y2, theta) * z0;
+
+  double phi = atan2(y3.dot(z2), y3.dot(y2));
+
+  return {phi, theta, psi};
+}
+
+Eigen::Vector3d ned_euler_from_ecef(const ECEF &ecef_init, const Eigen::Vector3d &ecef_pose) {
+  /*
+    Using Rotations to Build Aerospace Coordinate Systems
+    Don Koks
+    https://apps.dtic.mil/dtic/tr/fulltext/u2/a484864.pdf
+  */
+  LocalCoord converter = LocalCoord(ecef_init);
+
+  Eigen::Vector3d x0 = Eigen::Vector3d(1, 0, 0);
+  Eigen::Vector3d y0 = Eigen::Vector3d(0, 1, 0);
+  Eigen::Vector3d z0 = Eigen::Vector3d(0, 0, 1);
+
+  Eigen::Vector3d x1 = rot(z0, ecef_pose(2)) * x0;
+  Eigen::Vector3d y1 = rot(z0, ecef_pose(2)) * y0;
+  Eigen::Vector3d z1 = rot(z0, ecef_pose(2)) * z0;
+
+  Eigen::Vector3d x2 = rot(y1, ecef_pose(1)) * x1;
+  Eigen::Vector3d y2 = rot(y1, ecef_pose(1)) * y1;
+  Eigen::Vector3d z2 = rot(y1, ecef_pose(1)) * z1;
+
+  Eigen::Vector3d x3 = rot(x2, ecef_pose(0)) * x2;
+  Eigen::Vector3d y3 = rot(x2, ecef_pose(0)) * y2;
+
+  Eigen::Vector3d zero = ecef_init.to_vector();
+  x0 = converter.ned2ecef({1, 0, 0}).to_vector() - zero;
+  y0 = converter.ned2ecef({0, 1, 0}).to_vector() - zero;
+  z0 = converter.ned2ecef({0, 0, 1}).to_vector() - zero;
+
+  double psi = atan2(x3.dot(y0), x3.dot(x0));
+  double theta = atan2(-x3.dot(z0), sqrt(pow(x3.dot(x0), 2) + pow(x3.dot(y0), 2)));
+
+  y2 = rot(z0, psi) * y0;
+  z2 = rot(y2, theta) * z0;
+
+  double phi = atan2(y3.dot(z2), y3.dot(y2));
+
+  return {phi, theta, psi};
+}

iqpilot/common/transformations/orientation.hpp

@@ -0,0 +1,17 @@
+#pragma once
+#include <eigen3/Eigen/Dense>
+#include "iqpilot/common/transformations/coordinates.hpp"
+
+
+Eigen::Quaterniond ensure_unique(const Eigen::Quaterniond &quat);
+
+Eigen::Quaterniond euler2quat(const Eigen::Vector3d &euler);
+Eigen::Vector3d quat2euler(const Eigen::Quaterniond &quat);
+Eigen::Matrix3d quat2rot(const Eigen::Quaterniond &quat);
+Eigen::Quaterniond rot2quat(const Eigen::Matrix3d &rot);
+Eigen::Matrix3d euler2rot(const Eigen::Vector3d &euler);
+Eigen::Vector3d rot2euler(const Eigen::Matrix3d &rot);
+Eigen::Matrix3d rot_matrix(double roll, double pitch, double yaw);
+Eigen::Matrix3d rot(const Eigen::Vector3d &axis, double angle);
+Eigen::Vector3d ecef_euler_from_ned(const ECEF &ecef_init, const Eigen::Vector3d &ned_pose);
+Eigen::Vector3d ned_euler_from_ecef(const ECEF &ecef_init, const Eigen::Vector3d &ecef_pose);

iqpilot/common/version.h

@@ -0,0 +1 @@
+#define IQPILOT_VERSION "IQ.Pilot 1.0c"