diff --git a/common/params_keys.h b/common/params_keys.h index 607f7b79d..74e210cd3 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -367,6 +367,9 @@ inline static std::unordered_map keys = { {"LoudBlindspotAlert", {PERSISTENT, BOOL, "0", "0", 0}}, {"LowVoltageShutdown", {PERSISTENT, FLOAT, "11.8", "11.8", 3}}, {"ManualUpdateInitiated", {CLEAR_ON_MANAGER_START, BOOL, "0", "0"}}, + {"AMapKey1", {PERSISTENT | DONT_LOG, STRING, "", "", 0}}, + {"AMapKey2", {PERSISTENT | DONT_LOG, STRING, "", "", 0}}, + {"ApiCache_NavDestinations", {PERSISTENT, JSON, "[]", "[]"}}, {"MapAcceleration", {PERSISTENT, BOOL, "0", "0", 1}}, {"MapboxPublicKey", {PERSISTENT | DONT_LOG, STRING, "", "", 0}}, {"MapBoxRequests", {PERSISTENT, JSON, "{}", "{}"}}, @@ -376,6 +379,9 @@ inline static std::unordered_map keys = { {"MapGears", {PERSISTENT, BOOL, "0", "0", 2}}, {"MapsSelected", {PERSISTENT, STRING, "", "", 0}}, {"MapSpeedLimit", {CLEAR_ON_MANAGER_START, FLOAT, "0.0", "0.0"}}, + {"NavDesiresAllowed", {PERSISTENT, BOOL, "0", "0", 2}}, + {"NavDestination", {PERSISTENT, STRING, "", ""}}, + {"NavInstructionState", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, JSON, "{}", "{}"}}, {"NextMapSpeedLimit", {CLEAR_ON_MANAGER_START, JSON, "{}", "{}"}}, {"VisionSpeedLimit", {CLEAR_ON_MANAGER_START, FLOAT, "0.0", "0.0"}}, {"VisionSpeedLimitConfidence", {CLEAR_ON_MANAGER_START, FLOAT, "0.0", "0.0"}}, diff --git a/scripts/launch_onroad_desktop.sh b/scripts/launch_onroad_desktop.sh index 50d09ffeb..8cc879b8e 100755 --- a/scripts/launch_onroad_desktop.sh +++ b/scripts/launch_onroad_desktop.sh @@ -27,18 +27,20 @@ env_var_truthy() { usage() { cat <<'EOF' Usage: - ./onroad [jobs] (--c3 | --c4 | --raybig | --all | --replay-only) [--prefix name] + ./onroad [jobs] (--c3 | --c4 | --raybig | --all | --replay-only) [-nav] [--prefix name] Examples: - ./onroad --c3 f08912a233c1584f/2022-08-11--18-02-41/1 - ./onroad --c4 f08912a233c1584f/2022-08-11--18-02-41/1 --start 30 - ./onroad --all f08912a233c1584f/2022-08-11--18-02-41/1 + ./onroad --c3 + ./onroad --c4 --start 30 + ./onroad --c4 -nav + ./onroad --all ./onroad --replay-only --demo --no-vipc --no-loop Notes: - This is host/dev only. It uses the isolated host worktree and does not touch the device path. - A private comma connect route still requires tools/lib/auth.py before replay can download it. - Use multiple UI flags together if you want more than one desktop UI at once. + - -nav injects a fake navigation demo stream and blocks replay from publishing navInstruction/navRoute. EOF } @@ -53,7 +55,9 @@ REPLAY_ARGS=() UI_TARGETS=() LEGACY_UI_SELECTION="" REPLAY_ONLY=0 +NAV_DEMO=0 REPLAY_PID="" +NAV_PID="" UI_PIDS=() parse_args() { @@ -83,6 +87,10 @@ parse_args() { REPLAY_ONLY=1 shift ;; + -nav|--nav) + NAV_DEMO=1 + shift + ;; --ui) if [[ $# -lt 2 ]]; then echo "Missing value for --ui" >&2 @@ -194,6 +202,9 @@ cleanup() { if [[ -n "${REPLAY_PID}" ]]; then kill "${REPLAY_PID}" >/dev/null 2>&1 || true fi + if [[ -n "${NAV_PID}" ]]; then + kill "${NAV_PID}" >/dev/null 2>&1 || true + fi for pid in "${UI_PIDS[@]-}"; do if [[ -n "${pid}" ]]; then @@ -203,6 +214,9 @@ cleanup() { if [[ -n "${REPLAY_PID}" ]]; then wait "${REPLAY_PID}" >/dev/null 2>&1 || true fi + if [[ -n "${NAV_PID}" ]]; then + wait "${NAV_PID}" >/dev/null 2>&1 || true + fi if [[ -n "${OPENPILOT_PREFIX:-}" && "${OPENPILOT_PREFIX}" == desktop-onroad-* ]]; then echo "Cleaning up temporary prefix environment (${OPENPILOT_PREFIX})..." @@ -214,6 +228,68 @@ cleanup() { exit "${exit_code}" } +append_blocked_service_names() { + local existing="$1" + local required="$2" + local combined="${existing}" + + if [[ -n "${combined}" ]]; then + combined+=",${required}" + else + combined="${required}" + fi + + local deduped=() + local seen="," + local part="" + local parts=() + IFS=',' read -r -a parts <<< "${combined}" + for part in "${parts[@]-}"; do + part="${part// /}" + if [[ -z "${part}" ]]; then + continue + fi + if [[ "${seen}" != *",${part},"* ]]; then + deduped+=("${part}") + seen+="${part}," + fi + done + + local joined="" + for part in "${deduped[@]-}"; do + if [[ -n "${joined}" ]]; then + joined+="," + fi + joined+="${part}" + done + + printf '%s' "${joined}" +} + +ensure_nav_demo_replay_blocklist() { + local nav_services="navInstruction,navRoute" + local idx=0 + + for ((idx=0; idx<${#REPLAY_ARGS[@]}; idx++)); do + case "${REPLAY_ARGS[$idx]}" in + -b|--block) + if (( idx + 1 >= ${#REPLAY_ARGS[@]} )); then + echo "Missing value for ${REPLAY_ARGS[$idx]}" >&2 + exit 1 + fi + REPLAY_ARGS[$((idx + 1))]="$(append_blocked_service_names "${REPLAY_ARGS[$((idx + 1))]}" "${nav_services}")" + return + ;; + --block=*) + REPLAY_ARGS[$idx]="--block=$(append_blocked_service_names "${REPLAY_ARGS[$idx]#*=}" "${nav_services}")" + return + ;; + esac + done + + REPLAY_ARGS=(-b "${nav_services}" "${REPLAY_ARGS[@]}") +} + prepare_env() { source .venv/bin/activate @@ -238,6 +314,7 @@ prepare_env() { export SP_C4_FAKE_WIFI=0 export SP_RAYBIG_FAKE_WIFI=0 export SP_ALLOW_DESKTOP_FAKE_WIFI=0 + export SP_ONROAD_NAV_DEMO="${NAV_DEMO}" if [[ "$(uname -s)" == "Darwin" ]] || env_var_truthy "${ZMQ:-0}"; then export OPENPILOT_ZMQ_NAMESPACE="${PREFIX_ARG:-${OPENPILOT_ZMQ_NAMESPACE:-desktop-onroad-$$}}" @@ -250,6 +327,8 @@ prepare_env() { seed_params() { "${ROOT_DIR}/.venv/bin/python3" - <<'PY' +import os + from openpilot.common.params import Params from openpilot.system.version import terms_version, training_version @@ -260,6 +339,8 @@ params.put_bool("OpenpilotEnabledToggle", True) params.put_bool("IsDriverViewEnabled", False) params.put_bool("ForceOnroad", False) params.put_bool("ForceOffroad", False) +if os.getenv("SP_ONROAD_NAV_DEMO", "").lower() in {"1", "true", "yes", "on"}: + params.put_bool("NavigationUI", True) PY } @@ -302,6 +383,18 @@ launch_replay() { fi } +launch_nav_demo() { + echo "Starting fake nav demo publisher..." + "${ROOT_DIR}/.venv/bin/python3" "${ROOT_DIR}/tools/replay/fake_nav_demo.py" & + NAV_PID=$! + + sleep 0.5 + if ! kill -0 "${NAV_PID}" >/dev/null 2>&1; then + wait "${NAV_PID}" + return 1 + fi +} + launch_c3_ui() { local os_ext="linux" if [[ "$(uname -s)" == "Darwin" ]]; then @@ -339,6 +432,10 @@ else dedupe_ui_targets fi +if [[ "${NAV_DEMO}" == "1" ]]; then + ensure_nav_demo_replay_blocklist +fi + if [[ ${#REPLAY_ARGS[@]} -eq 0 ]]; then usage >&2 exit 1 @@ -378,6 +475,10 @@ seed_starpilot_theme echo "Starting replay: ${REPLAY_ARGS[*]}" launch_replay +if [[ "${NAV_DEMO}" == "1" ]]; then + launch_nav_demo +fi + if [[ ${#UI_TARGETS[@]} -eq 0 ]]; then echo "Replay is running without UI windows. Press Ctrl-C to stop." wait "${REPLAY_PID}" diff --git a/selfdrive/controls/lib/desire_helper.py b/selfdrive/controls/lib/desire_helper.py index 7afe9163f..da3e90464 100644 --- a/selfdrive/controls/lib/desire_helper.py +++ b/selfdrive/controls/lib/desire_helper.py @@ -1,5 +1,8 @@ +import json + from cereal import log from openpilot.common.constants import CV +from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL LaneChangeState = log.LaneChangeState @@ -40,6 +43,8 @@ TURN_DESIRES = { class DesireHelper: def __init__(self): + self.params = Params() + self.params_memory = Params(memory=True) self.lane_change_state = LaneChangeState.off self.lane_change_direction = LaneChangeDirection.none self.lane_change_timer = 0.0 @@ -51,6 +56,84 @@ class DesireHelper: self.lane_change_completed = False self.lane_change_wait_timer = 0.0 + self.nav_desires_allowed = False + self._nav_param_counter = -1 + self._nav_instruction_state_raw: object = None + self._nav_instruction_state: dict[str, object] = {} + + def _update_nav_params(self): + self._nav_param_counter += 1 + if self._nav_param_counter % 60 == 0: + self.nav_desires_allowed = self.params.get_bool("NavDesiresAllowed") + + raw = self.params_memory.get("NavInstructionState") or {} + if raw == self._nav_instruction_state_raw: + return + + self._nav_instruction_state_raw = raw + if not raw: + self._nav_instruction_state = {} + return + + if isinstance(raw, dict): + self._nav_instruction_state = raw + return + + if isinstance(raw, str): + try: + parsed = json.loads(raw) + self._nav_instruction_state = parsed if isinstance(parsed, dict) else {} + return + except Exception: + pass + + self._nav_instruction_state = {} + + @staticmethod + def _nav_keep_direction_is_clear(carstate, lane_change_direction): + return not ( + (lane_change_direction == LaneChangeDirection.left and carstate.leftBlindspot) or + (lane_change_direction == LaneChangeDirection.right and carstate.rightBlindspot) + ) + + @staticmethod + def _nav_torque_applied(carstate, lane_change_direction): + return carstate.steeringPressed and ( + (lane_change_direction == LaneChangeDirection.left and carstate.steeringTorque > 0) or + (lane_change_direction == LaneChangeDirection.right and carstate.steeringTorque < 0) + ) + + def _navigation_desire(self, carstate, lateral_active, starpilotPlan, starpilot_toggles): + self._update_nav_params() + if not self.nav_desires_allowed or not lateral_active or not bool(self._nav_instruction_state.get("valid", False)): + return log.Desire.none + + modifier = str(self._nav_instruction_state.get("maneuverModifier", "")) + if modifier == "": + return log.Desire.none + + if modifier == "slightLeft": + lane_change_direction = LaneChangeDirection.left + desired_lane_width = starpilotPlan.laneWidthLeft + nudgeless_allowed = starpilot_toggles.nudgeless and desired_lane_width >= starpilot_toggles.lane_detection_width + if not carstate.rightBlinker and self._nav_keep_direction_is_clear(carstate, lane_change_direction): + if self._nav_torque_applied(carstate, lane_change_direction) or nudgeless_allowed: + return log.Desire.keepLeft + elif modifier == "slightRight": + lane_change_direction = LaneChangeDirection.right + desired_lane_width = starpilotPlan.laneWidthRight + nudgeless_allowed = starpilot_toggles.nudgeless and desired_lane_width >= starpilot_toggles.lane_detection_width + if not carstate.leftBlinker and self._nav_keep_direction_is_clear(carstate, lane_change_direction): + if self._nav_torque_applied(carstate, lane_change_direction) or nudgeless_allowed: + return log.Desire.keepRight + elif modifier in ("left", "sharpLeft"): + if not carstate.rightBlinker and not carstate.leftBlindspot and carstate.vEgo < starpilot_toggles.minimum_lane_change_speed and not carstate.standstill: + return log.Desire.turnLeft + elif modifier in ("right", "sharpRight"): + if not carstate.leftBlinker and not carstate.rightBlindspot and carstate.vEgo < starpilot_toggles.minimum_lane_change_speed and not carstate.standstill: + return log.Desire.turnRight + + return log.Desire.none @staticmethod def get_lane_change_direction(CS): @@ -155,3 +238,7 @@ class DesireHelper: self.lane_change_completed = False self.lane_change_wait_timer = 0.0 + + nav_desire = self._navigation_desire(carstate, lateral_active, starpilotPlan, starpilot_toggles) + if nav_desire != log.Desire.none and self.lane_change_state == LaneChangeState.off: + self.desire = nav_desire diff --git a/selfdrive/controls/tests/test_navigation_desires.py b/selfdrive/controls/tests/test_navigation_desires.py new file mode 100644 index 000000000..6f47f0afc --- /dev/null +++ b/selfdrive/controls/tests/test_navigation_desires.py @@ -0,0 +1,113 @@ +from types import SimpleNamespace + +from cereal import log + +from openpilot.selfdrive.controls.lib.desire_helper import DesireHelper, LaneChangeDirection, LaneChangeState + + +def make_car_state(**overrides): + defaults = { + "vEgo": 20.0, + "leftBlinker": False, + "rightBlinker": False, + "leftBlindspot": False, + "rightBlindspot": False, + "steeringPressed": False, + "steeringTorque": 0.0, + "standstill": False, + } + defaults.update(overrides) + return SimpleNamespace(**defaults) + + +def make_toggles(**overrides): + defaults = { + "lane_changes": True, + "lane_change_delay": 0.0, + "lane_detection_width": 3.0, + "minimum_lane_change_speed": 10.0, + "nudgeless": True, + "one_lane_change": False, + "use_turn_desires": False, + } + defaults.update(overrides) + return SimpleNamespace(**defaults) + + +def make_plan(**overrides): + defaults = { + "laneWidthLeft": 4.0, + "laneWidthRight": 4.0, + } + defaults.update(overrides) + return SimpleNamespace(**defaults) + +def test_nav_desires_keep_left_when_route_requests_it(): + helper = DesireHelper() + helper.nav_desires_allowed = True + helper._update_nav_params = lambda: None + helper._nav_instruction_state = {"valid": True, "maneuverModifier": "slightLeft"} + + helper.update( + make_car_state(vEgo=20.0), + True, + 0.0, + make_plan(laneWidthLeft=4.2), + make_toggles(nudgeless=True), + ) + + assert helper.desire == log.Desire.keepLeft + + +def test_nav_desires_turn_right_below_lane_change_speed(): + helper = DesireHelper() + helper.nav_desires_allowed = True + helper._update_nav_params = lambda: None + helper._nav_instruction_state = {"valid": True, "maneuverModifier": "right"} + + helper.update( + make_car_state(vEgo=5.0), + True, + 0.0, + make_plan(), + make_toggles(minimum_lane_change_speed=10.0), + ) + + assert helper.desire == log.Desire.turnRight + + +def test_nav_desires_do_not_override_lane_change_state_machine(): + helper = DesireHelper() + helper.nav_desires_allowed = True + helper._update_nav_params = lambda: None + helper._nav_instruction_state = {"valid": True, "maneuverModifier": "slightRight"} + helper.lane_change_state = LaneChangeState.laneChangeStarting + helper.lane_change_direction = LaneChangeDirection.left + helper.lane_change_ll_prob = 0.5 + + helper.update( + make_car_state(vEgo=25.0, leftBlinker=True), + True, + 0.5, + make_plan(), + make_toggles(), + ) + + assert helper.desire == log.Desire.laneChangeLeft + + +def test_nav_desires_disabled_leave_desire_unchanged(): + helper = DesireHelper() + helper.nav_desires_allowed = False + helper._update_nav_params = lambda: None + helper._nav_instruction_state = {"valid": True, "maneuverModifier": "left"} + + helper.update( + make_car_state(vEgo=5.0), + True, + 0.0, + make_plan(), + make_toggles(minimum_lane_change_speed=10.0), + ) + + assert helper.desire == log.Desire.none diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index 9c54a64f4..a8e6e5336 100755 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -283,6 +283,7 @@ def main(): filter_initialized = False critcal_services = ["accelerometer", "gyroscope", "cameraOdometry"] observation_input_invalid = defaultdict(int) + last_inputs_diagnostics = None input_invalid_limit = {s: round(INPUT_INVALID_LIMIT * (SERVICE_LIST[s].frequency / 20.)) for s in critcal_services} input_invalid_threshold = {s: input_invalid_limit[s] - 0.5 for s in critcal_services} @@ -335,6 +336,31 @@ def main(): inputs_valid = sm.all_valid() and critical_service_inputs_valid sensors_valid = sensor_all_checks(acc_msgs, gyro_msgs, sensor_valid, sensor_recv_time, sensor_alive, SIMULATION) + invalid_services = [s for s, valid in sm.valid.items() if not valid] + over_threshold = { + s: round(float(observation_input_invalid[s]), 3) + for s in critcal_services + if observation_input_invalid[s] >= input_invalid_threshold[s] + } + inputs_diagnostics = { + "inputs_valid": inputs_valid, + "sensors_valid": sensors_valid, + "filter_initialized": filter_initialized, + "invalid_services": invalid_services, + "over_threshold": over_threshold, + } + if not inputs_valid and inputs_diagnostics != last_inputs_diagnostics: + cloudlog.event( + "locationdInputsInvalid", + error=True, + **inputs_diagnostics, + observation_input_invalid={s: round(float(observation_input_invalid[s]), 3) for s in critcal_services}, + observation_input_thresholds={s: float(input_invalid_threshold[s]) for s in critcal_services}, + ) + elif inputs_valid and last_inputs_diagnostics is not None: + cloudlog.event("locationdInputsRecovered", **inputs_diagnostics) + last_inputs_diagnostics = None if inputs_valid else inputs_diagnostics + msg = estimator.get_msg(sensors_valid, inputs_valid, filter_initialized) pm.send("livePose", msg) diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 1b1966bce..2c1ffcb59 100644 --- a/selfdrive/locationd/models/generated/car.cpp +++ b/selfdrive/locationd/models/generated/car.cpp @@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227; * * * This file is part of 'ekf' * ******************************************************************************/ -void err_fun(double *nom_x, double *delta_x, double *out_5406094264205448922) { - out_5406094264205448922[0] = delta_x[0] + nom_x[0]; - out_5406094264205448922[1] = delta_x[1] + nom_x[1]; - out_5406094264205448922[2] = delta_x[2] + nom_x[2]; - out_5406094264205448922[3] = delta_x[3] + nom_x[3]; - out_5406094264205448922[4] = delta_x[4] + nom_x[4]; - out_5406094264205448922[5] = delta_x[5] + nom_x[5]; - out_5406094264205448922[6] = delta_x[6] + nom_x[6]; - out_5406094264205448922[7] = delta_x[7] + nom_x[7]; - out_5406094264205448922[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_1976458697034392713) { + out_1976458697034392713[0] = delta_x[0] + nom_x[0]; + out_1976458697034392713[1] = delta_x[1] + nom_x[1]; + out_1976458697034392713[2] = delta_x[2] + nom_x[2]; + out_1976458697034392713[3] = delta_x[3] + nom_x[3]; + out_1976458697034392713[4] = delta_x[4] + nom_x[4]; + out_1976458697034392713[5] = delta_x[5] + nom_x[5]; + out_1976458697034392713[6] = delta_x[6] + nom_x[6]; + out_1976458697034392713[7] = delta_x[7] + nom_x[7]; + out_1976458697034392713[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_7302786751934147898) { - out_7302786751934147898[0] = -nom_x[0] + true_x[0]; - out_7302786751934147898[1] = -nom_x[1] + true_x[1]; - out_7302786751934147898[2] = -nom_x[2] + true_x[2]; - out_7302786751934147898[3] = -nom_x[3] + true_x[3]; - out_7302786751934147898[4] = -nom_x[4] + true_x[4]; - out_7302786751934147898[5] = -nom_x[5] + true_x[5]; - out_7302786751934147898[6] = -nom_x[6] + true_x[6]; - out_7302786751934147898[7] = -nom_x[7] + true_x[7]; - out_7302786751934147898[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_8774680380747325588) { + out_8774680380747325588[0] = -nom_x[0] + true_x[0]; + out_8774680380747325588[1] = -nom_x[1] + true_x[1]; + out_8774680380747325588[2] = -nom_x[2] + true_x[2]; + out_8774680380747325588[3] = -nom_x[3] + true_x[3]; + out_8774680380747325588[4] = -nom_x[4] + true_x[4]; + out_8774680380747325588[5] = -nom_x[5] + true_x[5]; + out_8774680380747325588[6] = -nom_x[6] + true_x[6]; + out_8774680380747325588[7] = -nom_x[7] + true_x[7]; + out_8774680380747325588[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_8564810489090289784) { - out_8564810489090289784[0] = 1.0; - out_8564810489090289784[1] = 0.0; - out_8564810489090289784[2] = 0.0; - out_8564810489090289784[3] = 0.0; - out_8564810489090289784[4] = 0.0; - out_8564810489090289784[5] = 0.0; - out_8564810489090289784[6] = 0.0; - out_8564810489090289784[7] = 0.0; - out_8564810489090289784[8] = 0.0; - out_8564810489090289784[9] = 0.0; - out_8564810489090289784[10] = 1.0; - out_8564810489090289784[11] = 0.0; - out_8564810489090289784[12] = 0.0; - out_8564810489090289784[13] = 0.0; - out_8564810489090289784[14] = 0.0; - out_8564810489090289784[15] = 0.0; - out_8564810489090289784[16] = 0.0; - out_8564810489090289784[17] = 0.0; - out_8564810489090289784[18] = 0.0; - out_8564810489090289784[19] = 0.0; - out_8564810489090289784[20] = 1.0; - out_8564810489090289784[21] = 0.0; - out_8564810489090289784[22] = 0.0; - out_8564810489090289784[23] = 0.0; - out_8564810489090289784[24] = 0.0; - out_8564810489090289784[25] = 0.0; - out_8564810489090289784[26] = 0.0; - out_8564810489090289784[27] = 0.0; - out_8564810489090289784[28] = 0.0; - out_8564810489090289784[29] = 0.0; - out_8564810489090289784[30] = 1.0; - out_8564810489090289784[31] = 0.0; - out_8564810489090289784[32] = 0.0; - out_8564810489090289784[33] = 0.0; - out_8564810489090289784[34] = 0.0; - out_8564810489090289784[35] = 0.0; - out_8564810489090289784[36] = 0.0; - out_8564810489090289784[37] = 0.0; - out_8564810489090289784[38] = 0.0; - out_8564810489090289784[39] = 0.0; - out_8564810489090289784[40] = 1.0; - out_8564810489090289784[41] = 0.0; - out_8564810489090289784[42] = 0.0; - out_8564810489090289784[43] = 0.0; - out_8564810489090289784[44] = 0.0; - out_8564810489090289784[45] = 0.0; - out_8564810489090289784[46] = 0.0; - out_8564810489090289784[47] = 0.0; - out_8564810489090289784[48] = 0.0; - out_8564810489090289784[49] = 0.0; - out_8564810489090289784[50] = 1.0; - out_8564810489090289784[51] = 0.0; - out_8564810489090289784[52] = 0.0; - out_8564810489090289784[53] = 0.0; - out_8564810489090289784[54] = 0.0; - out_8564810489090289784[55] = 0.0; - out_8564810489090289784[56] = 0.0; - out_8564810489090289784[57] = 0.0; - out_8564810489090289784[58] = 0.0; - out_8564810489090289784[59] = 0.0; - out_8564810489090289784[60] = 1.0; - out_8564810489090289784[61] = 0.0; - out_8564810489090289784[62] = 0.0; - out_8564810489090289784[63] = 0.0; - out_8564810489090289784[64] = 0.0; - out_8564810489090289784[65] = 0.0; - out_8564810489090289784[66] = 0.0; - out_8564810489090289784[67] = 0.0; - out_8564810489090289784[68] = 0.0; - out_8564810489090289784[69] = 0.0; - out_8564810489090289784[70] = 1.0; - out_8564810489090289784[71] = 0.0; - out_8564810489090289784[72] = 0.0; - out_8564810489090289784[73] = 0.0; - out_8564810489090289784[74] = 0.0; - out_8564810489090289784[75] = 0.0; - out_8564810489090289784[76] = 0.0; - out_8564810489090289784[77] = 0.0; - out_8564810489090289784[78] = 0.0; - out_8564810489090289784[79] = 0.0; - out_8564810489090289784[80] = 1.0; +void H_mod_fun(double *state, double *out_1571472922248018296) { + out_1571472922248018296[0] = 1.0; + out_1571472922248018296[1] = 0.0; + out_1571472922248018296[2] = 0.0; + out_1571472922248018296[3] = 0.0; + out_1571472922248018296[4] = 0.0; + out_1571472922248018296[5] = 0.0; + out_1571472922248018296[6] = 0.0; + out_1571472922248018296[7] = 0.0; + out_1571472922248018296[8] = 0.0; + out_1571472922248018296[9] = 0.0; + out_1571472922248018296[10] = 1.0; + out_1571472922248018296[11] = 0.0; + out_1571472922248018296[12] = 0.0; + out_1571472922248018296[13] = 0.0; + out_1571472922248018296[14] = 0.0; + out_1571472922248018296[15] = 0.0; + out_1571472922248018296[16] = 0.0; + out_1571472922248018296[17] = 0.0; + out_1571472922248018296[18] = 0.0; + out_1571472922248018296[19] = 0.0; + out_1571472922248018296[20] = 1.0; + out_1571472922248018296[21] = 0.0; + out_1571472922248018296[22] = 0.0; + out_1571472922248018296[23] = 0.0; + out_1571472922248018296[24] = 0.0; + out_1571472922248018296[25] = 0.0; + out_1571472922248018296[26] = 0.0; + out_1571472922248018296[27] = 0.0; + out_1571472922248018296[28] = 0.0; + out_1571472922248018296[29] = 0.0; + out_1571472922248018296[30] = 1.0; + out_1571472922248018296[31] = 0.0; + out_1571472922248018296[32] = 0.0; + out_1571472922248018296[33] = 0.0; + out_1571472922248018296[34] = 0.0; + out_1571472922248018296[35] = 0.0; + out_1571472922248018296[36] = 0.0; + out_1571472922248018296[37] = 0.0; + out_1571472922248018296[38] = 0.0; + out_1571472922248018296[39] = 0.0; + out_1571472922248018296[40] = 1.0; + out_1571472922248018296[41] = 0.0; + out_1571472922248018296[42] = 0.0; + out_1571472922248018296[43] = 0.0; + out_1571472922248018296[44] = 0.0; + out_1571472922248018296[45] = 0.0; + out_1571472922248018296[46] = 0.0; + out_1571472922248018296[47] = 0.0; + out_1571472922248018296[48] = 0.0; + out_1571472922248018296[49] = 0.0; + out_1571472922248018296[50] = 1.0; + out_1571472922248018296[51] = 0.0; + out_1571472922248018296[52] = 0.0; + out_1571472922248018296[53] = 0.0; + out_1571472922248018296[54] = 0.0; + out_1571472922248018296[55] = 0.0; + out_1571472922248018296[56] = 0.0; + out_1571472922248018296[57] = 0.0; + out_1571472922248018296[58] = 0.0; + out_1571472922248018296[59] = 0.0; + out_1571472922248018296[60] = 1.0; + out_1571472922248018296[61] = 0.0; + out_1571472922248018296[62] = 0.0; + out_1571472922248018296[63] = 0.0; + out_1571472922248018296[64] = 0.0; + out_1571472922248018296[65] = 0.0; + out_1571472922248018296[66] = 0.0; + out_1571472922248018296[67] = 0.0; + out_1571472922248018296[68] = 0.0; + out_1571472922248018296[69] = 0.0; + out_1571472922248018296[70] = 1.0; + out_1571472922248018296[71] = 0.0; + out_1571472922248018296[72] = 0.0; + out_1571472922248018296[73] = 0.0; + out_1571472922248018296[74] = 0.0; + out_1571472922248018296[75] = 0.0; + out_1571472922248018296[76] = 0.0; + out_1571472922248018296[77] = 0.0; + out_1571472922248018296[78] = 0.0; + out_1571472922248018296[79] = 0.0; + out_1571472922248018296[80] = 1.0; } -void f_fun(double *state, double dt, double *out_7344896958720977953) { - out_7344896958720977953[0] = state[0]; - out_7344896958720977953[1] = state[1]; - out_7344896958720977953[2] = state[2]; - out_7344896958720977953[3] = state[3]; - out_7344896958720977953[4] = state[4]; - out_7344896958720977953[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5]; - out_7344896958720977953[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6]; - out_7344896958720977953[7] = state[7]; - out_7344896958720977953[8] = state[8]; +void f_fun(double *state, double dt, double *out_3070664500164102760) { + out_3070664500164102760[0] = state[0]; + out_3070664500164102760[1] = state[1]; + out_3070664500164102760[2] = state[2]; + out_3070664500164102760[3] = state[3]; + out_3070664500164102760[4] = state[4]; + out_3070664500164102760[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5]; + out_3070664500164102760[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6]; + out_3070664500164102760[7] = state[7]; + out_3070664500164102760[8] = state[8]; } -void F_fun(double *state, double dt, double *out_4577316669100378715) { - out_4577316669100378715[0] = 1; - out_4577316669100378715[1] = 0; - out_4577316669100378715[2] = 0; - out_4577316669100378715[3] = 0; - out_4577316669100378715[4] = 0; - out_4577316669100378715[5] = 0; - out_4577316669100378715[6] = 0; - out_4577316669100378715[7] = 0; - out_4577316669100378715[8] = 0; - out_4577316669100378715[9] = 0; - out_4577316669100378715[10] = 1; - out_4577316669100378715[11] = 0; - out_4577316669100378715[12] = 0; - out_4577316669100378715[13] = 0; - out_4577316669100378715[14] = 0; - out_4577316669100378715[15] = 0; - out_4577316669100378715[16] = 0; - out_4577316669100378715[17] = 0; - out_4577316669100378715[18] = 0; - out_4577316669100378715[19] = 0; - out_4577316669100378715[20] = 1; - out_4577316669100378715[21] = 0; - out_4577316669100378715[22] = 0; - out_4577316669100378715[23] = 0; - out_4577316669100378715[24] = 0; - out_4577316669100378715[25] = 0; - out_4577316669100378715[26] = 0; - out_4577316669100378715[27] = 0; - out_4577316669100378715[28] = 0; - out_4577316669100378715[29] = 0; - out_4577316669100378715[30] = 1; - out_4577316669100378715[31] = 0; - out_4577316669100378715[32] = 0; - out_4577316669100378715[33] = 0; - out_4577316669100378715[34] = 0; - out_4577316669100378715[35] = 0; - out_4577316669100378715[36] = 0; - out_4577316669100378715[37] = 0; - out_4577316669100378715[38] = 0; - out_4577316669100378715[39] = 0; - out_4577316669100378715[40] = 1; - out_4577316669100378715[41] = 0; - out_4577316669100378715[42] = 0; - out_4577316669100378715[43] = 0; - out_4577316669100378715[44] = 0; - out_4577316669100378715[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4])); - out_4577316669100378715[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_4577316669100378715[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_4577316669100378715[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_4577316669100378715[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2))); - out_4577316669100378715[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_4577316669100378715[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_4577316669100378715[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_4577316669100378715[53] = -9.8100000000000005*dt; - out_4577316669100378715[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4])); - out_4577316669100378715[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_4577316669100378715[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_4577316669100378715[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_4577316669100378715[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2))); - out_4577316669100378715[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_4577316669100378715[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1; - out_4577316669100378715[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_4577316669100378715[62] = 0; - out_4577316669100378715[63] = 0; - out_4577316669100378715[64] = 0; - out_4577316669100378715[65] = 0; - out_4577316669100378715[66] = 0; - out_4577316669100378715[67] = 0; - out_4577316669100378715[68] = 0; - out_4577316669100378715[69] = 0; - out_4577316669100378715[70] = 1; - out_4577316669100378715[71] = 0; - out_4577316669100378715[72] = 0; - out_4577316669100378715[73] = 0; - out_4577316669100378715[74] = 0; - out_4577316669100378715[75] = 0; - out_4577316669100378715[76] = 0; - out_4577316669100378715[77] = 0; - out_4577316669100378715[78] = 0; - out_4577316669100378715[79] = 0; - out_4577316669100378715[80] = 1; +void F_fun(double *state, double dt, double *out_2365857006052744488) { + out_2365857006052744488[0] = 1; + out_2365857006052744488[1] = 0; + out_2365857006052744488[2] = 0; + out_2365857006052744488[3] = 0; + out_2365857006052744488[4] = 0; + out_2365857006052744488[5] = 0; + out_2365857006052744488[6] = 0; + out_2365857006052744488[7] = 0; + out_2365857006052744488[8] = 0; + out_2365857006052744488[9] = 0; + out_2365857006052744488[10] = 1; + out_2365857006052744488[11] = 0; + out_2365857006052744488[12] = 0; + out_2365857006052744488[13] = 0; + out_2365857006052744488[14] = 0; + out_2365857006052744488[15] = 0; + out_2365857006052744488[16] = 0; + out_2365857006052744488[17] = 0; + out_2365857006052744488[18] = 0; + out_2365857006052744488[19] = 0; + out_2365857006052744488[20] = 1; + out_2365857006052744488[21] = 0; + out_2365857006052744488[22] = 0; + out_2365857006052744488[23] = 0; + out_2365857006052744488[24] = 0; + out_2365857006052744488[25] = 0; + out_2365857006052744488[26] = 0; + out_2365857006052744488[27] = 0; + out_2365857006052744488[28] = 0; + out_2365857006052744488[29] = 0; + out_2365857006052744488[30] = 1; + out_2365857006052744488[31] = 0; + out_2365857006052744488[32] = 0; + out_2365857006052744488[33] = 0; + out_2365857006052744488[34] = 0; + out_2365857006052744488[35] = 0; + out_2365857006052744488[36] = 0; + out_2365857006052744488[37] = 0; + out_2365857006052744488[38] = 0; + out_2365857006052744488[39] = 0; + out_2365857006052744488[40] = 1; + out_2365857006052744488[41] = 0; + out_2365857006052744488[42] = 0; + out_2365857006052744488[43] = 0; + out_2365857006052744488[44] = 0; + out_2365857006052744488[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4])); + out_2365857006052744488[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_2365857006052744488[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2365857006052744488[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2365857006052744488[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2))); + out_2365857006052744488[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_2365857006052744488[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_2365857006052744488[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_2365857006052744488[53] = -9.8100000000000005*dt; + out_2365857006052744488[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4])); + out_2365857006052744488[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_2365857006052744488[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2365857006052744488[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2365857006052744488[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2))); + out_2365857006052744488[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_2365857006052744488[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1; + out_2365857006052744488[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2365857006052744488[62] = 0; + out_2365857006052744488[63] = 0; + out_2365857006052744488[64] = 0; + out_2365857006052744488[65] = 0; + out_2365857006052744488[66] = 0; + out_2365857006052744488[67] = 0; + out_2365857006052744488[68] = 0; + out_2365857006052744488[69] = 0; + out_2365857006052744488[70] = 1; + out_2365857006052744488[71] = 0; + out_2365857006052744488[72] = 0; + out_2365857006052744488[73] = 0; + out_2365857006052744488[74] = 0; + out_2365857006052744488[75] = 0; + out_2365857006052744488[76] = 0; + out_2365857006052744488[77] = 0; + out_2365857006052744488[78] = 0; + out_2365857006052744488[79] = 0; + out_2365857006052744488[80] = 1; } -void h_25(double *state, double *unused, double *out_6986192086353969465) { - out_6986192086353969465[0] = state[6]; +void h_25(double *state, double *unused, double *out_7208997417211176832) { + out_7208997417211176832[0] = state[6]; } -void H_25(double *state, double *unused, double *out_628377736821947453) { - out_628377736821947453[0] = 0; - out_628377736821947453[1] = 0; - out_628377736821947453[2] = 0; - out_628377736821947453[3] = 0; - out_628377736821947453[4] = 0; - out_628377736821947453[5] = 0; - out_628377736821947453[6] = 1; - out_628377736821947453[7] = 0; - out_628377736821947453[8] = 0; +void H_25(double *state, double *unused, double *out_3987544096248591949) { + out_3987544096248591949[0] = 0; + out_3987544096248591949[1] = 0; + out_3987544096248591949[2] = 0; + out_3987544096248591949[3] = 0; + out_3987544096248591949[4] = 0; + out_3987544096248591949[5] = 0; + out_3987544096248591949[6] = 1; + out_3987544096248591949[7] = 0; + out_3987544096248591949[8] = 0; } -void h_24(double *state, double *unused, double *out_434665502247836674) { - out_434665502247836674[0] = state[4]; - out_434665502247836674[1] = state[5]; +void h_24(double *state, double *unused, double *out_7481715524482624299) { + out_7481715524482624299[0] = state[4]; + out_7481715524482624299[1] = state[5]; } -void H_24(double *state, double *unused, double *out_2801027335827447019) { - out_2801027335827447019[0] = 0; - out_2801027335827447019[1] = 0; - out_2801027335827447019[2] = 0; - out_2801027335827447019[3] = 0; - out_2801027335827447019[4] = 1; - out_2801027335827447019[5] = 0; - out_2801027335827447019[6] = 0; - out_2801027335827447019[7] = 0; - out_2801027335827447019[8] = 0; - out_2801027335827447019[9] = 0; - out_2801027335827447019[10] = 0; - out_2801027335827447019[11] = 0; - out_2801027335827447019[12] = 0; - out_2801027335827447019[13] = 0; - out_2801027335827447019[14] = 1; - out_2801027335827447019[15] = 0; - out_2801027335827447019[16] = 0; - out_2801027335827447019[17] = 0; +void H_24(double *state, double *unused, double *out_1810329672641441976) { + out_1810329672641441976[0] = 0; + out_1810329672641441976[1] = 0; + out_1810329672641441976[2] = 0; + out_1810329672641441976[3] = 0; + out_1810329672641441976[4] = 1; + out_1810329672641441976[5] = 0; + out_1810329672641441976[6] = 0; + out_1810329672641441976[7] = 0; + out_1810329672641441976[8] = 0; + out_1810329672641441976[9] = 0; + out_1810329672641441976[10] = 0; + out_1810329672641441976[11] = 0; + out_1810329672641441976[12] = 0; + out_1810329672641441976[13] = 0; + out_1810329672641441976[14] = 1; + out_1810329672641441976[15] = 0; + out_1810329672641441976[16] = 0; + out_1810329672641441976[17] = 0; } -void h_30(double *state, double *unused, double *out_4915078315896353947) { - out_4915078315896353947[0] = state[4]; +void h_30(double *state, double *unused, double *out_438162190860825896) { + out_438162190860825896[0] = state[4]; } -void H_30(double *state, double *unused, double *out_6288312604669669302) { - out_6288312604669669302[0] = 0; - out_6288312604669669302[1] = 0; - out_6288312604669669302[2] = 0; - out_6288312604669669302[3] = 0; - out_6288312604669669302[4] = 1; - out_6288312604669669302[5] = 0; - out_6288312604669669302[6] = 0; - out_6288312604669669302[7] = 0; - out_6288312604669669302[8] = 0; +void H_30(double *state, double *unused, double *out_1469211137741343322) { + out_1469211137741343322[0] = 0; + out_1469211137741343322[1] = 0; + out_1469211137741343322[2] = 0; + out_1469211137741343322[3] = 0; + out_1469211137741343322[4] = 1; + out_1469211137741343322[5] = 0; + out_1469211137741343322[6] = 0; + out_1469211137741343322[7] = 0; + out_1469211137741343322[8] = 0; } -void h_26(double *state, double *unused, double *out_4943503076008356261) { - out_4943503076008356261[0] = state[7]; +void h_26(double *state, double *unused, double *out_4563511076800019749) { + out_4563511076800019749[0] = state[7]; } -void H_26(double *state, double *unused, double *out_4369881055696003677) { - out_4369881055696003677[0] = 0; - out_4369881055696003677[1] = 0; - out_4369881055696003677[2] = 0; - out_4369881055696003677[3] = 0; - out_4369881055696003677[4] = 0; - out_4369881055696003677[5] = 0; - out_4369881055696003677[6] = 0; - out_4369881055696003677[7] = 1; - out_4369881055696003677[8] = 0; +void H_26(double *state, double *unused, double *out_7729047415122648173) { + out_7729047415122648173[0] = 0; + out_7729047415122648173[1] = 0; + out_7729047415122648173[2] = 0; + out_7729047415122648173[3] = 0; + out_7729047415122648173[4] = 0; + out_7729047415122648173[5] = 0; + out_7729047415122648173[6] = 0; + out_7729047415122648173[7] = 1; + out_7729047415122648173[8] = 0; } -void h_27(double *state, double *unused, double *out_1984518818693123820) { - out_1984518818693123820[0] = state[3]; +void h_27(double *state, double *unused, double *out_6265487564309514397) { + out_6265487564309514397[0] = state[3]; } -void H_27(double *state, double *unused, double *out_2932479995765612434) { - out_2932479995765612434[0] = 0; - out_2932479995765612434[1] = 0; - out_2932479995765612434[2] = 0; - out_2932479995765612434[3] = 1; - out_2932479995765612434[4] = 0; - out_2932479995765612434[5] = 0; - out_2932479995765612434[6] = 0; - out_2932479995765612434[7] = 0; - out_2932479995765612434[8] = 0; +void H_27(double *state, double *unused, double *out_3643974449541768233) { + out_3643974449541768233[0] = 0; + out_3643974449541768233[1] = 0; + out_3643974449541768233[2] = 0; + out_3643974449541768233[3] = 1; + out_3643974449541768233[4] = 0; + out_3643974449541768233[5] = 0; + out_3643974449541768233[6] = 0; + out_3643974449541768233[7] = 0; + out_3643974449541768233[8] = 0; } -void h_29(double *state, double *unused, double *out_963207171662361141) { - out_963207171662361141[0] = state[1]; +void h_29(double *state, double *unused, double *out_6316447678419540984) { + out_6316447678419540984[0] = state[1]; } -void H_29(double *state, double *unused, double *out_4645842722635163467) { - out_4645842722635163467[0] = 0; - out_4645842722635163467[1] = 1; - out_4645842722635163467[2] = 0; - out_4645842722635163467[3] = 0; - out_4645842722635163467[4] = 0; - out_4645842722635163467[5] = 0; - out_4645842722635163467[6] = 0; - out_4645842722635163467[7] = 0; - out_4645842722635163467[8] = 0; +void H_29(double *state, double *unused, double *out_958979793426951138) { + out_958979793426951138[0] = 0; + out_958979793426951138[1] = 1; + out_958979793426951138[2] = 0; + out_958979793426951138[3] = 0; + out_958979793426951138[4] = 0; + out_958979793426951138[5] = 0; + out_958979793426951138[6] = 0; + out_958979793426951138[7] = 0; + out_958979793426951138[8] = 0; } -void h_28(double *state, double *unused, double *out_1002434725357825708) { - out_1002434725357825708[0] = state[0]; +void h_28(double *state, double *unused, double *out_3384947045134040079) { + out_3384947045134040079[0] = state[0]; } -void H_28(double *state, double *unused, double *out_2682212451069837216) { - out_2682212451069837216[0] = 1; - out_2682212451069837216[1] = 0; - out_2682212451069837216[2] = 0; - out_2682212451069837216[3] = 0; - out_2682212451069837216[4] = 0; - out_2682212451069837216[5] = 0; - out_2682212451069837216[6] = 0; - out_2682212451069837216[7] = 0; - out_2682212451069837216[8] = 0; +void H_28(double *state, double *unused, double *out_6041378810496481712) { + out_6041378810496481712[0] = 1; + out_6041378810496481712[1] = 0; + out_6041378810496481712[2] = 0; + out_6041378810496481712[3] = 0; + out_6041378810496481712[4] = 0; + out_6041378810496481712[5] = 0; + out_6041378810496481712[6] = 0; + out_6041378810496481712[7] = 0; + out_6041378810496481712[8] = 0; } -void h_31(double *state, double *unused, double *out_7261386148638475354) { - out_7261386148638475354[0] = state[8]; +void h_31(double *state, double *unused, double *out_2597869179779832900) { + out_2597869179779832900[0] = state[8]; } -void H_31(double *state, double *unused, double *out_597731774944987025) { - out_597731774944987025[0] = 0; - out_597731774944987025[1] = 0; - out_597731774944987025[2] = 0; - out_597731774944987025[3] = 0; - out_597731774944987025[4] = 0; - out_597731774944987025[5] = 0; - out_597731774944987025[6] = 0; - out_597731774944987025[7] = 0; - out_597731774944987025[8] = 1; +void H_31(double *state, double *unused, double *out_8355255517355999649) { + out_8355255517355999649[0] = 0; + out_8355255517355999649[1] = 0; + out_8355255517355999649[2] = 0; + out_8355255517355999649[3] = 0; + out_8355255517355999649[4] = 0; + out_8355255517355999649[5] = 0; + out_8355255517355999649[6] = 0; + out_8355255517355999649[7] = 0; + out_8355255517355999649[8] = 1; } #include #include @@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31); } -void car_err_fun(double *nom_x, double *delta_x, double *out_5406094264205448922) { - err_fun(nom_x, delta_x, out_5406094264205448922); +void car_err_fun(double *nom_x, double *delta_x, double *out_1976458697034392713) { + err_fun(nom_x, delta_x, out_1976458697034392713); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_7302786751934147898) { - inv_err_fun(nom_x, true_x, out_7302786751934147898); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_8774680380747325588) { + inv_err_fun(nom_x, true_x, out_8774680380747325588); } -void car_H_mod_fun(double *state, double *out_8564810489090289784) { - H_mod_fun(state, out_8564810489090289784); +void car_H_mod_fun(double *state, double *out_1571472922248018296) { + H_mod_fun(state, out_1571472922248018296); } -void car_f_fun(double *state, double dt, double *out_7344896958720977953) { - f_fun(state, dt, out_7344896958720977953); +void car_f_fun(double *state, double dt, double *out_3070664500164102760) { + f_fun(state, dt, out_3070664500164102760); } -void car_F_fun(double *state, double dt, double *out_4577316669100378715) { - F_fun(state, dt, out_4577316669100378715); +void car_F_fun(double *state, double dt, double *out_2365857006052744488) { + F_fun(state, dt, out_2365857006052744488); } -void car_h_25(double *state, double *unused, double *out_6986192086353969465) { - h_25(state, unused, out_6986192086353969465); +void car_h_25(double *state, double *unused, double *out_7208997417211176832) { + h_25(state, unused, out_7208997417211176832); } -void car_H_25(double *state, double *unused, double *out_628377736821947453) { - H_25(state, unused, out_628377736821947453); +void car_H_25(double *state, double *unused, double *out_3987544096248591949) { + H_25(state, unused, out_3987544096248591949); } -void car_h_24(double *state, double *unused, double *out_434665502247836674) { - h_24(state, unused, out_434665502247836674); +void car_h_24(double *state, double *unused, double *out_7481715524482624299) { + h_24(state, unused, out_7481715524482624299); } -void car_H_24(double *state, double *unused, double *out_2801027335827447019) { - H_24(state, unused, out_2801027335827447019); +void car_H_24(double *state, double *unused, double *out_1810329672641441976) { + H_24(state, unused, out_1810329672641441976); } -void car_h_30(double *state, double *unused, double *out_4915078315896353947) { - h_30(state, unused, out_4915078315896353947); +void car_h_30(double *state, double *unused, double *out_438162190860825896) { + h_30(state, unused, out_438162190860825896); } -void car_H_30(double *state, double *unused, double *out_6288312604669669302) { - H_30(state, unused, out_6288312604669669302); +void car_H_30(double *state, double *unused, double *out_1469211137741343322) { + H_30(state, unused, out_1469211137741343322); } -void car_h_26(double *state, double *unused, double *out_4943503076008356261) { - h_26(state, unused, out_4943503076008356261); +void car_h_26(double *state, double *unused, double *out_4563511076800019749) { + h_26(state, unused, out_4563511076800019749); } -void car_H_26(double *state, double *unused, double *out_4369881055696003677) { - H_26(state, unused, out_4369881055696003677); +void car_H_26(double *state, double *unused, double *out_7729047415122648173) { + H_26(state, unused, out_7729047415122648173); } -void car_h_27(double *state, double *unused, double *out_1984518818693123820) { - h_27(state, unused, out_1984518818693123820); +void car_h_27(double *state, double *unused, double *out_6265487564309514397) { + h_27(state, unused, out_6265487564309514397); } -void car_H_27(double *state, double *unused, double *out_2932479995765612434) { - H_27(state, unused, out_2932479995765612434); +void car_H_27(double *state, double *unused, double *out_3643974449541768233) { + H_27(state, unused, out_3643974449541768233); } -void car_h_29(double *state, double *unused, double *out_963207171662361141) { - h_29(state, unused, out_963207171662361141); +void car_h_29(double *state, double *unused, double *out_6316447678419540984) { + h_29(state, unused, out_6316447678419540984); } -void car_H_29(double *state, double *unused, double *out_4645842722635163467) { - H_29(state, unused, out_4645842722635163467); +void car_H_29(double *state, double *unused, double *out_958979793426951138) { + H_29(state, unused, out_958979793426951138); } -void car_h_28(double *state, double *unused, double *out_1002434725357825708) { - h_28(state, unused, out_1002434725357825708); +void car_h_28(double *state, double *unused, double *out_3384947045134040079) { + h_28(state, unused, out_3384947045134040079); } -void car_H_28(double *state, double *unused, double *out_2682212451069837216) { - H_28(state, unused, out_2682212451069837216); +void car_H_28(double *state, double *unused, double *out_6041378810496481712) { + H_28(state, unused, out_6041378810496481712); } -void car_h_31(double *state, double *unused, double *out_7261386148638475354) { - h_31(state, unused, out_7261386148638475354); +void car_h_31(double *state, double *unused, double *out_2597869179779832900) { + h_31(state, unused, out_2597869179779832900); } -void car_H_31(double *state, double *unused, double *out_597731774944987025) { - H_31(state, unused, out_597731774944987025); +void car_H_31(double *state, double *unused, double *out_8355255517355999649) { + H_31(state, unused, out_8355255517355999649); } void car_predict(double *in_x, double *in_P, double *in_Q, double dt) { predict(in_x, in_P, in_Q, dt); diff --git a/selfdrive/locationd/models/generated/car.h b/selfdrive/locationd/models/generated/car.h index f8813fec3..6b7db4049 100644 --- a/selfdrive/locationd/models/generated/car.h +++ b/selfdrive/locationd/models/generated/car.h @@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); -void car_err_fun(double *nom_x, double *delta_x, double *out_5406094264205448922); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_7302786751934147898); -void car_H_mod_fun(double *state, double *out_8564810489090289784); -void car_f_fun(double *state, double dt, double *out_7344896958720977953); -void car_F_fun(double *state, double dt, double *out_4577316669100378715); -void car_h_25(double *state, double *unused, double *out_6986192086353969465); -void car_H_25(double *state, double *unused, double *out_628377736821947453); -void car_h_24(double *state, double *unused, double *out_434665502247836674); -void car_H_24(double *state, double *unused, double *out_2801027335827447019); -void car_h_30(double *state, double *unused, double *out_4915078315896353947); -void car_H_30(double *state, double *unused, double *out_6288312604669669302); -void car_h_26(double *state, double *unused, double *out_4943503076008356261); -void car_H_26(double *state, double *unused, double *out_4369881055696003677); -void car_h_27(double *state, double *unused, double *out_1984518818693123820); -void car_H_27(double *state, double *unused, double *out_2932479995765612434); -void car_h_29(double *state, double *unused, double *out_963207171662361141); -void car_H_29(double *state, double *unused, double *out_4645842722635163467); -void car_h_28(double *state, double *unused, double *out_1002434725357825708); -void car_H_28(double *state, double *unused, double *out_2682212451069837216); -void car_h_31(double *state, double *unused, double *out_7261386148638475354); -void car_H_31(double *state, double *unused, double *out_597731774944987025); +void car_err_fun(double *nom_x, double *delta_x, double *out_1976458697034392713); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_8774680380747325588); +void car_H_mod_fun(double *state, double *out_1571472922248018296); +void car_f_fun(double *state, double dt, double *out_3070664500164102760); +void car_F_fun(double *state, double dt, double *out_2365857006052744488); +void car_h_25(double *state, double *unused, double *out_7208997417211176832); +void car_H_25(double *state, double *unused, double *out_3987544096248591949); +void car_h_24(double *state, double *unused, double *out_7481715524482624299); +void car_H_24(double *state, double *unused, double *out_1810329672641441976); +void car_h_30(double *state, double *unused, double *out_438162190860825896); +void car_H_30(double *state, double *unused, double *out_1469211137741343322); +void car_h_26(double *state, double *unused, double *out_4563511076800019749); +void car_H_26(double *state, double *unused, double *out_7729047415122648173); +void car_h_27(double *state, double *unused, double *out_6265487564309514397); +void car_H_27(double *state, double *unused, double *out_3643974449541768233); +void car_h_29(double *state, double *unused, double *out_6316447678419540984); +void car_H_29(double *state, double *unused, double *out_958979793426951138); +void car_h_28(double *state, double *unused, double *out_3384947045134040079); +void car_H_28(double *state, double *unused, double *out_6041378810496481712); +void car_h_31(double *state, double *unused, double *out_2597869179779832900); +void car_H_31(double *state, double *unused, double *out_8355255517355999649); void car_predict(double *in_x, double *in_P, double *in_Q, double dt); void car_set_mass(double x); void car_set_rotational_inertia(double x); diff --git a/selfdrive/locationd/models/generated/pose.cpp b/selfdrive/locationd/models/generated/pose.cpp index 2ef680f47..7134d9104 100644 --- a/selfdrive/locationd/models/generated/pose.cpp +++ b/selfdrive/locationd/models/generated/pose.cpp @@ -17,961 +17,961 @@ const static double MAHA_THRESH_14 = 7.814727903251177; * * * This file is part of 'ekf' * ******************************************************************************/ -void err_fun(double *nom_x, double *delta_x, double *out_6497612872484174174) { - out_6497612872484174174[0] = delta_x[0] + nom_x[0]; - out_6497612872484174174[1] = delta_x[1] + nom_x[1]; - out_6497612872484174174[2] = delta_x[2] + nom_x[2]; - out_6497612872484174174[3] = delta_x[3] + nom_x[3]; - out_6497612872484174174[4] = delta_x[4] + nom_x[4]; - out_6497612872484174174[5] = delta_x[5] + nom_x[5]; - out_6497612872484174174[6] = delta_x[6] + nom_x[6]; - out_6497612872484174174[7] = delta_x[7] + nom_x[7]; - out_6497612872484174174[8] = delta_x[8] + nom_x[8]; - out_6497612872484174174[9] = delta_x[9] + nom_x[9]; - out_6497612872484174174[10] = delta_x[10] + nom_x[10]; - out_6497612872484174174[11] = delta_x[11] + nom_x[11]; - out_6497612872484174174[12] = delta_x[12] + nom_x[12]; - out_6497612872484174174[13] = delta_x[13] + nom_x[13]; - out_6497612872484174174[14] = delta_x[14] + nom_x[14]; - out_6497612872484174174[15] = delta_x[15] + nom_x[15]; - out_6497612872484174174[16] = delta_x[16] + nom_x[16]; - out_6497612872484174174[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_812796242528121648) { + out_812796242528121648[0] = delta_x[0] + nom_x[0]; + out_812796242528121648[1] = delta_x[1] + nom_x[1]; + out_812796242528121648[2] = delta_x[2] + nom_x[2]; + out_812796242528121648[3] = delta_x[3] + nom_x[3]; + out_812796242528121648[4] = delta_x[4] + nom_x[4]; + out_812796242528121648[5] = delta_x[5] + nom_x[5]; + out_812796242528121648[6] = delta_x[6] + nom_x[6]; + out_812796242528121648[7] = delta_x[7] + nom_x[7]; + out_812796242528121648[8] = delta_x[8] + nom_x[8]; + out_812796242528121648[9] = delta_x[9] + nom_x[9]; + out_812796242528121648[10] = delta_x[10] + nom_x[10]; + out_812796242528121648[11] = delta_x[11] + nom_x[11]; + out_812796242528121648[12] = delta_x[12] + nom_x[12]; + out_812796242528121648[13] = delta_x[13] + nom_x[13]; + out_812796242528121648[14] = delta_x[14] + nom_x[14]; + out_812796242528121648[15] = delta_x[15] + nom_x[15]; + out_812796242528121648[16] = delta_x[16] + nom_x[16]; + out_812796242528121648[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_1975580934350294204) { - out_1975580934350294204[0] = -nom_x[0] + true_x[0]; - out_1975580934350294204[1] = -nom_x[1] + true_x[1]; - out_1975580934350294204[2] = -nom_x[2] + true_x[2]; - out_1975580934350294204[3] = -nom_x[3] + true_x[3]; - out_1975580934350294204[4] = -nom_x[4] + true_x[4]; - out_1975580934350294204[5] = -nom_x[5] + true_x[5]; - out_1975580934350294204[6] = -nom_x[6] + true_x[6]; - out_1975580934350294204[7] = -nom_x[7] + true_x[7]; - out_1975580934350294204[8] = -nom_x[8] + true_x[8]; - out_1975580934350294204[9] = -nom_x[9] + true_x[9]; - out_1975580934350294204[10] = -nom_x[10] + true_x[10]; - out_1975580934350294204[11] = -nom_x[11] + true_x[11]; - out_1975580934350294204[12] = -nom_x[12] + true_x[12]; - out_1975580934350294204[13] = -nom_x[13] + true_x[13]; - out_1975580934350294204[14] = -nom_x[14] + true_x[14]; - out_1975580934350294204[15] = -nom_x[15] + true_x[15]; - out_1975580934350294204[16] = -nom_x[16] + true_x[16]; - out_1975580934350294204[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_2630998712382937796) { + out_2630998712382937796[0] = -nom_x[0] + true_x[0]; + out_2630998712382937796[1] = -nom_x[1] + true_x[1]; + out_2630998712382937796[2] = -nom_x[2] + true_x[2]; + out_2630998712382937796[3] = -nom_x[3] + true_x[3]; + out_2630998712382937796[4] = -nom_x[4] + true_x[4]; + out_2630998712382937796[5] = -nom_x[5] + true_x[5]; + out_2630998712382937796[6] = -nom_x[6] + true_x[6]; + out_2630998712382937796[7] = -nom_x[7] + true_x[7]; + out_2630998712382937796[8] = -nom_x[8] + true_x[8]; + out_2630998712382937796[9] = -nom_x[9] + true_x[9]; + out_2630998712382937796[10] = -nom_x[10] + true_x[10]; + out_2630998712382937796[11] = -nom_x[11] + true_x[11]; + out_2630998712382937796[12] = -nom_x[12] + true_x[12]; + out_2630998712382937796[13] = -nom_x[13] + true_x[13]; + out_2630998712382937796[14] = -nom_x[14] + true_x[14]; + out_2630998712382937796[15] = -nom_x[15] + true_x[15]; + out_2630998712382937796[16] = -nom_x[16] + true_x[16]; + out_2630998712382937796[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_4617418789543565628) { - out_4617418789543565628[0] = 1.0; - out_4617418789543565628[1] = 0.0; - out_4617418789543565628[2] = 0.0; - out_4617418789543565628[3] = 0.0; - out_4617418789543565628[4] = 0.0; - out_4617418789543565628[5] = 0.0; - out_4617418789543565628[6] = 0.0; - out_4617418789543565628[7] = 0.0; - out_4617418789543565628[8] = 0.0; - out_4617418789543565628[9] = 0.0; - out_4617418789543565628[10] = 0.0; - out_4617418789543565628[11] = 0.0; - out_4617418789543565628[12] = 0.0; - out_4617418789543565628[13] = 0.0; - out_4617418789543565628[14] = 0.0; - out_4617418789543565628[15] = 0.0; - out_4617418789543565628[16] = 0.0; - out_4617418789543565628[17] = 0.0; - out_4617418789543565628[18] = 0.0; - out_4617418789543565628[19] = 1.0; - out_4617418789543565628[20] = 0.0; - out_4617418789543565628[21] = 0.0; - out_4617418789543565628[22] = 0.0; - out_4617418789543565628[23] = 0.0; - out_4617418789543565628[24] = 0.0; - out_4617418789543565628[25] = 0.0; - out_4617418789543565628[26] = 0.0; - out_4617418789543565628[27] = 0.0; - out_4617418789543565628[28] = 0.0; - out_4617418789543565628[29] = 0.0; - out_4617418789543565628[30] = 0.0; - out_4617418789543565628[31] = 0.0; - out_4617418789543565628[32] = 0.0; - out_4617418789543565628[33] = 0.0; - out_4617418789543565628[34] = 0.0; - out_4617418789543565628[35] = 0.0; - out_4617418789543565628[36] = 0.0; - out_4617418789543565628[37] = 0.0; - out_4617418789543565628[38] = 1.0; - out_4617418789543565628[39] = 0.0; - out_4617418789543565628[40] = 0.0; - out_4617418789543565628[41] = 0.0; - out_4617418789543565628[42] = 0.0; - out_4617418789543565628[43] = 0.0; - out_4617418789543565628[44] = 0.0; - out_4617418789543565628[45] = 0.0; - out_4617418789543565628[46] = 0.0; - out_4617418789543565628[47] = 0.0; - out_4617418789543565628[48] = 0.0; - out_4617418789543565628[49] = 0.0; - out_4617418789543565628[50] = 0.0; - out_4617418789543565628[51] = 0.0; - out_4617418789543565628[52] = 0.0; - out_4617418789543565628[53] = 0.0; - out_4617418789543565628[54] = 0.0; - out_4617418789543565628[55] = 0.0; - out_4617418789543565628[56] = 0.0; - out_4617418789543565628[57] = 1.0; - out_4617418789543565628[58] = 0.0; - out_4617418789543565628[59] = 0.0; - out_4617418789543565628[60] = 0.0; - out_4617418789543565628[61] = 0.0; - out_4617418789543565628[62] = 0.0; - out_4617418789543565628[63] = 0.0; - out_4617418789543565628[64] = 0.0; - out_4617418789543565628[65] = 0.0; - out_4617418789543565628[66] = 0.0; - out_4617418789543565628[67] = 0.0; - out_4617418789543565628[68] = 0.0; - out_4617418789543565628[69] = 0.0; - out_4617418789543565628[70] = 0.0; - out_4617418789543565628[71] = 0.0; - out_4617418789543565628[72] = 0.0; - out_4617418789543565628[73] = 0.0; - out_4617418789543565628[74] = 0.0; - out_4617418789543565628[75] = 0.0; - out_4617418789543565628[76] = 1.0; - out_4617418789543565628[77] = 0.0; - out_4617418789543565628[78] = 0.0; - out_4617418789543565628[79] = 0.0; - out_4617418789543565628[80] = 0.0; - out_4617418789543565628[81] = 0.0; - out_4617418789543565628[82] = 0.0; - out_4617418789543565628[83] = 0.0; - out_4617418789543565628[84] = 0.0; - out_4617418789543565628[85] = 0.0; - out_4617418789543565628[86] = 0.0; - out_4617418789543565628[87] = 0.0; - out_4617418789543565628[88] = 0.0; - out_4617418789543565628[89] = 0.0; - out_4617418789543565628[90] = 0.0; - out_4617418789543565628[91] = 0.0; - out_4617418789543565628[92] = 0.0; - out_4617418789543565628[93] = 0.0; - out_4617418789543565628[94] = 0.0; - out_4617418789543565628[95] = 1.0; - out_4617418789543565628[96] = 0.0; - out_4617418789543565628[97] = 0.0; - out_4617418789543565628[98] = 0.0; - out_4617418789543565628[99] = 0.0; - out_4617418789543565628[100] = 0.0; - out_4617418789543565628[101] = 0.0; - out_4617418789543565628[102] = 0.0; - out_4617418789543565628[103] = 0.0; - out_4617418789543565628[104] = 0.0; - out_4617418789543565628[105] = 0.0; - out_4617418789543565628[106] = 0.0; - out_4617418789543565628[107] = 0.0; - out_4617418789543565628[108] = 0.0; - out_4617418789543565628[109] = 0.0; - out_4617418789543565628[110] = 0.0; - out_4617418789543565628[111] = 0.0; - out_4617418789543565628[112] = 0.0; - out_4617418789543565628[113] = 0.0; - out_4617418789543565628[114] = 1.0; - out_4617418789543565628[115] = 0.0; - out_4617418789543565628[116] = 0.0; - out_4617418789543565628[117] = 0.0; - out_4617418789543565628[118] = 0.0; - out_4617418789543565628[119] = 0.0; - out_4617418789543565628[120] = 0.0; - out_4617418789543565628[121] = 0.0; - out_4617418789543565628[122] = 0.0; - out_4617418789543565628[123] = 0.0; - out_4617418789543565628[124] = 0.0; - out_4617418789543565628[125] = 0.0; - out_4617418789543565628[126] = 0.0; - out_4617418789543565628[127] = 0.0; - out_4617418789543565628[128] = 0.0; - out_4617418789543565628[129] = 0.0; - out_4617418789543565628[130] = 0.0; - out_4617418789543565628[131] = 0.0; - out_4617418789543565628[132] = 0.0; - out_4617418789543565628[133] = 1.0; - out_4617418789543565628[134] = 0.0; - out_4617418789543565628[135] = 0.0; - out_4617418789543565628[136] = 0.0; - out_4617418789543565628[137] = 0.0; - out_4617418789543565628[138] = 0.0; - out_4617418789543565628[139] = 0.0; - out_4617418789543565628[140] = 0.0; - out_4617418789543565628[141] = 0.0; - out_4617418789543565628[142] = 0.0; - out_4617418789543565628[143] = 0.0; - out_4617418789543565628[144] = 0.0; - out_4617418789543565628[145] = 0.0; - out_4617418789543565628[146] = 0.0; - out_4617418789543565628[147] = 0.0; - out_4617418789543565628[148] = 0.0; - out_4617418789543565628[149] = 0.0; - out_4617418789543565628[150] = 0.0; - out_4617418789543565628[151] = 0.0; - out_4617418789543565628[152] = 1.0; - out_4617418789543565628[153] = 0.0; - out_4617418789543565628[154] = 0.0; - out_4617418789543565628[155] = 0.0; - out_4617418789543565628[156] = 0.0; - out_4617418789543565628[157] = 0.0; - out_4617418789543565628[158] = 0.0; - out_4617418789543565628[159] = 0.0; - out_4617418789543565628[160] = 0.0; - out_4617418789543565628[161] = 0.0; - out_4617418789543565628[162] = 0.0; - out_4617418789543565628[163] = 0.0; - out_4617418789543565628[164] = 0.0; - out_4617418789543565628[165] = 0.0; - out_4617418789543565628[166] = 0.0; - out_4617418789543565628[167] = 0.0; - out_4617418789543565628[168] = 0.0; - out_4617418789543565628[169] = 0.0; - out_4617418789543565628[170] = 0.0; - out_4617418789543565628[171] = 1.0; - out_4617418789543565628[172] = 0.0; - out_4617418789543565628[173] = 0.0; - out_4617418789543565628[174] = 0.0; - out_4617418789543565628[175] = 0.0; - out_4617418789543565628[176] = 0.0; - out_4617418789543565628[177] = 0.0; - out_4617418789543565628[178] = 0.0; - out_4617418789543565628[179] = 0.0; - out_4617418789543565628[180] = 0.0; - out_4617418789543565628[181] = 0.0; - out_4617418789543565628[182] = 0.0; - out_4617418789543565628[183] = 0.0; - out_4617418789543565628[184] = 0.0; - out_4617418789543565628[185] = 0.0; - out_4617418789543565628[186] = 0.0; - out_4617418789543565628[187] = 0.0; - out_4617418789543565628[188] = 0.0; - out_4617418789543565628[189] = 0.0; - out_4617418789543565628[190] = 1.0; - out_4617418789543565628[191] = 0.0; - out_4617418789543565628[192] = 0.0; - out_4617418789543565628[193] = 0.0; - out_4617418789543565628[194] = 0.0; - out_4617418789543565628[195] = 0.0; - out_4617418789543565628[196] = 0.0; - out_4617418789543565628[197] = 0.0; - out_4617418789543565628[198] = 0.0; - out_4617418789543565628[199] = 0.0; - out_4617418789543565628[200] = 0.0; - out_4617418789543565628[201] = 0.0; - out_4617418789543565628[202] = 0.0; - out_4617418789543565628[203] = 0.0; - out_4617418789543565628[204] = 0.0; - out_4617418789543565628[205] = 0.0; - out_4617418789543565628[206] = 0.0; - out_4617418789543565628[207] = 0.0; - out_4617418789543565628[208] = 0.0; - out_4617418789543565628[209] = 1.0; - out_4617418789543565628[210] = 0.0; - out_4617418789543565628[211] = 0.0; - out_4617418789543565628[212] = 0.0; - out_4617418789543565628[213] = 0.0; - out_4617418789543565628[214] = 0.0; - out_4617418789543565628[215] = 0.0; - out_4617418789543565628[216] = 0.0; - out_4617418789543565628[217] = 0.0; - out_4617418789543565628[218] = 0.0; - out_4617418789543565628[219] = 0.0; - out_4617418789543565628[220] = 0.0; - out_4617418789543565628[221] = 0.0; - out_4617418789543565628[222] = 0.0; - out_4617418789543565628[223] = 0.0; - out_4617418789543565628[224] = 0.0; - out_4617418789543565628[225] = 0.0; - out_4617418789543565628[226] = 0.0; - out_4617418789543565628[227] = 0.0; - out_4617418789543565628[228] = 1.0; - out_4617418789543565628[229] = 0.0; - out_4617418789543565628[230] = 0.0; - out_4617418789543565628[231] = 0.0; - out_4617418789543565628[232] = 0.0; - out_4617418789543565628[233] = 0.0; - out_4617418789543565628[234] = 0.0; - out_4617418789543565628[235] = 0.0; - out_4617418789543565628[236] = 0.0; - out_4617418789543565628[237] = 0.0; - out_4617418789543565628[238] = 0.0; - out_4617418789543565628[239] = 0.0; - out_4617418789543565628[240] = 0.0; - out_4617418789543565628[241] = 0.0; - out_4617418789543565628[242] = 0.0; - out_4617418789543565628[243] = 0.0; - out_4617418789543565628[244] = 0.0; - out_4617418789543565628[245] = 0.0; - out_4617418789543565628[246] = 0.0; - out_4617418789543565628[247] = 1.0; - out_4617418789543565628[248] = 0.0; - out_4617418789543565628[249] = 0.0; - out_4617418789543565628[250] = 0.0; - out_4617418789543565628[251] = 0.0; - out_4617418789543565628[252] = 0.0; - out_4617418789543565628[253] = 0.0; - out_4617418789543565628[254] = 0.0; - out_4617418789543565628[255] = 0.0; - out_4617418789543565628[256] = 0.0; - out_4617418789543565628[257] = 0.0; - out_4617418789543565628[258] = 0.0; - out_4617418789543565628[259] = 0.0; - out_4617418789543565628[260] = 0.0; - out_4617418789543565628[261] = 0.0; - out_4617418789543565628[262] = 0.0; - out_4617418789543565628[263] = 0.0; - out_4617418789543565628[264] = 0.0; - out_4617418789543565628[265] = 0.0; - out_4617418789543565628[266] = 1.0; - out_4617418789543565628[267] = 0.0; - out_4617418789543565628[268] = 0.0; - out_4617418789543565628[269] = 0.0; - out_4617418789543565628[270] = 0.0; - out_4617418789543565628[271] = 0.0; - out_4617418789543565628[272] = 0.0; - out_4617418789543565628[273] = 0.0; - out_4617418789543565628[274] = 0.0; - out_4617418789543565628[275] = 0.0; - out_4617418789543565628[276] = 0.0; - out_4617418789543565628[277] = 0.0; - out_4617418789543565628[278] = 0.0; - out_4617418789543565628[279] = 0.0; - out_4617418789543565628[280] = 0.0; - out_4617418789543565628[281] = 0.0; - out_4617418789543565628[282] = 0.0; - out_4617418789543565628[283] = 0.0; - out_4617418789543565628[284] = 0.0; - out_4617418789543565628[285] = 1.0; - out_4617418789543565628[286] = 0.0; - out_4617418789543565628[287] = 0.0; - out_4617418789543565628[288] = 0.0; - out_4617418789543565628[289] = 0.0; - out_4617418789543565628[290] = 0.0; - out_4617418789543565628[291] = 0.0; - out_4617418789543565628[292] = 0.0; - out_4617418789543565628[293] = 0.0; - out_4617418789543565628[294] = 0.0; - out_4617418789543565628[295] = 0.0; - out_4617418789543565628[296] = 0.0; - out_4617418789543565628[297] = 0.0; - out_4617418789543565628[298] = 0.0; - out_4617418789543565628[299] = 0.0; - out_4617418789543565628[300] = 0.0; - out_4617418789543565628[301] = 0.0; - out_4617418789543565628[302] = 0.0; - out_4617418789543565628[303] = 0.0; - out_4617418789543565628[304] = 1.0; - out_4617418789543565628[305] = 0.0; - out_4617418789543565628[306] = 0.0; - out_4617418789543565628[307] = 0.0; - out_4617418789543565628[308] = 0.0; - out_4617418789543565628[309] = 0.0; - out_4617418789543565628[310] = 0.0; - out_4617418789543565628[311] = 0.0; - out_4617418789543565628[312] = 0.0; - out_4617418789543565628[313] = 0.0; - out_4617418789543565628[314] = 0.0; - out_4617418789543565628[315] = 0.0; - out_4617418789543565628[316] = 0.0; - out_4617418789543565628[317] = 0.0; - out_4617418789543565628[318] = 0.0; - out_4617418789543565628[319] = 0.0; - out_4617418789543565628[320] = 0.0; - out_4617418789543565628[321] = 0.0; - out_4617418789543565628[322] = 0.0; - out_4617418789543565628[323] = 1.0; +void H_mod_fun(double *state, double *out_8952125250503643232) { + out_8952125250503643232[0] = 1.0; + out_8952125250503643232[1] = 0.0; + out_8952125250503643232[2] = 0.0; + out_8952125250503643232[3] = 0.0; + out_8952125250503643232[4] = 0.0; + out_8952125250503643232[5] = 0.0; + out_8952125250503643232[6] = 0.0; + out_8952125250503643232[7] = 0.0; + out_8952125250503643232[8] = 0.0; + out_8952125250503643232[9] = 0.0; + out_8952125250503643232[10] = 0.0; + out_8952125250503643232[11] = 0.0; + out_8952125250503643232[12] = 0.0; + out_8952125250503643232[13] = 0.0; + out_8952125250503643232[14] = 0.0; + out_8952125250503643232[15] = 0.0; + out_8952125250503643232[16] = 0.0; + out_8952125250503643232[17] = 0.0; + out_8952125250503643232[18] = 0.0; + out_8952125250503643232[19] = 1.0; + out_8952125250503643232[20] = 0.0; + out_8952125250503643232[21] = 0.0; + out_8952125250503643232[22] = 0.0; + out_8952125250503643232[23] = 0.0; + out_8952125250503643232[24] = 0.0; + out_8952125250503643232[25] = 0.0; + out_8952125250503643232[26] = 0.0; + out_8952125250503643232[27] = 0.0; + out_8952125250503643232[28] = 0.0; + out_8952125250503643232[29] = 0.0; + out_8952125250503643232[30] = 0.0; + out_8952125250503643232[31] = 0.0; + out_8952125250503643232[32] = 0.0; + out_8952125250503643232[33] = 0.0; + out_8952125250503643232[34] = 0.0; + out_8952125250503643232[35] = 0.0; + out_8952125250503643232[36] = 0.0; + out_8952125250503643232[37] = 0.0; + out_8952125250503643232[38] = 1.0; + out_8952125250503643232[39] = 0.0; + out_8952125250503643232[40] = 0.0; + out_8952125250503643232[41] = 0.0; + out_8952125250503643232[42] = 0.0; + out_8952125250503643232[43] = 0.0; + out_8952125250503643232[44] = 0.0; + out_8952125250503643232[45] = 0.0; + out_8952125250503643232[46] = 0.0; + out_8952125250503643232[47] = 0.0; + out_8952125250503643232[48] = 0.0; + out_8952125250503643232[49] = 0.0; + out_8952125250503643232[50] = 0.0; + out_8952125250503643232[51] = 0.0; + out_8952125250503643232[52] = 0.0; + out_8952125250503643232[53] = 0.0; + out_8952125250503643232[54] = 0.0; + out_8952125250503643232[55] = 0.0; + out_8952125250503643232[56] = 0.0; + out_8952125250503643232[57] = 1.0; + out_8952125250503643232[58] = 0.0; + out_8952125250503643232[59] = 0.0; + out_8952125250503643232[60] = 0.0; + out_8952125250503643232[61] = 0.0; + out_8952125250503643232[62] = 0.0; + out_8952125250503643232[63] = 0.0; + out_8952125250503643232[64] = 0.0; + out_8952125250503643232[65] = 0.0; + out_8952125250503643232[66] = 0.0; + out_8952125250503643232[67] = 0.0; + out_8952125250503643232[68] = 0.0; + out_8952125250503643232[69] = 0.0; + out_8952125250503643232[70] = 0.0; + out_8952125250503643232[71] = 0.0; + out_8952125250503643232[72] = 0.0; + out_8952125250503643232[73] = 0.0; + out_8952125250503643232[74] = 0.0; + out_8952125250503643232[75] = 0.0; + out_8952125250503643232[76] = 1.0; + out_8952125250503643232[77] = 0.0; + out_8952125250503643232[78] = 0.0; + out_8952125250503643232[79] = 0.0; + out_8952125250503643232[80] = 0.0; + out_8952125250503643232[81] = 0.0; + out_8952125250503643232[82] = 0.0; + out_8952125250503643232[83] = 0.0; + out_8952125250503643232[84] = 0.0; + out_8952125250503643232[85] = 0.0; + out_8952125250503643232[86] = 0.0; + out_8952125250503643232[87] = 0.0; + out_8952125250503643232[88] = 0.0; + out_8952125250503643232[89] = 0.0; + out_8952125250503643232[90] = 0.0; + out_8952125250503643232[91] = 0.0; + out_8952125250503643232[92] = 0.0; + out_8952125250503643232[93] = 0.0; + out_8952125250503643232[94] = 0.0; + out_8952125250503643232[95] = 1.0; + out_8952125250503643232[96] = 0.0; + out_8952125250503643232[97] = 0.0; + out_8952125250503643232[98] = 0.0; + out_8952125250503643232[99] = 0.0; + out_8952125250503643232[100] = 0.0; + out_8952125250503643232[101] = 0.0; + out_8952125250503643232[102] = 0.0; + out_8952125250503643232[103] = 0.0; + out_8952125250503643232[104] = 0.0; + out_8952125250503643232[105] = 0.0; + out_8952125250503643232[106] = 0.0; + out_8952125250503643232[107] = 0.0; + out_8952125250503643232[108] = 0.0; + out_8952125250503643232[109] = 0.0; + out_8952125250503643232[110] = 0.0; + out_8952125250503643232[111] = 0.0; + out_8952125250503643232[112] = 0.0; + out_8952125250503643232[113] = 0.0; + out_8952125250503643232[114] = 1.0; + out_8952125250503643232[115] = 0.0; + out_8952125250503643232[116] = 0.0; + out_8952125250503643232[117] = 0.0; + out_8952125250503643232[118] = 0.0; + out_8952125250503643232[119] = 0.0; + out_8952125250503643232[120] = 0.0; + out_8952125250503643232[121] = 0.0; + out_8952125250503643232[122] = 0.0; + out_8952125250503643232[123] = 0.0; + out_8952125250503643232[124] = 0.0; + out_8952125250503643232[125] = 0.0; + out_8952125250503643232[126] = 0.0; + out_8952125250503643232[127] = 0.0; + out_8952125250503643232[128] = 0.0; + out_8952125250503643232[129] = 0.0; + out_8952125250503643232[130] = 0.0; + out_8952125250503643232[131] = 0.0; + out_8952125250503643232[132] = 0.0; + out_8952125250503643232[133] = 1.0; + out_8952125250503643232[134] = 0.0; + out_8952125250503643232[135] = 0.0; + out_8952125250503643232[136] = 0.0; + out_8952125250503643232[137] = 0.0; + out_8952125250503643232[138] = 0.0; + out_8952125250503643232[139] = 0.0; + out_8952125250503643232[140] = 0.0; + out_8952125250503643232[141] = 0.0; + out_8952125250503643232[142] = 0.0; + out_8952125250503643232[143] = 0.0; + out_8952125250503643232[144] = 0.0; + out_8952125250503643232[145] = 0.0; + out_8952125250503643232[146] = 0.0; + out_8952125250503643232[147] = 0.0; + out_8952125250503643232[148] = 0.0; + out_8952125250503643232[149] = 0.0; + out_8952125250503643232[150] = 0.0; + out_8952125250503643232[151] = 0.0; + out_8952125250503643232[152] = 1.0; + out_8952125250503643232[153] = 0.0; + out_8952125250503643232[154] = 0.0; + out_8952125250503643232[155] = 0.0; + out_8952125250503643232[156] = 0.0; + out_8952125250503643232[157] = 0.0; + out_8952125250503643232[158] = 0.0; + out_8952125250503643232[159] = 0.0; + out_8952125250503643232[160] = 0.0; + out_8952125250503643232[161] = 0.0; + out_8952125250503643232[162] = 0.0; + out_8952125250503643232[163] = 0.0; + out_8952125250503643232[164] = 0.0; + out_8952125250503643232[165] = 0.0; + out_8952125250503643232[166] = 0.0; + out_8952125250503643232[167] = 0.0; + out_8952125250503643232[168] = 0.0; + out_8952125250503643232[169] = 0.0; + out_8952125250503643232[170] = 0.0; + out_8952125250503643232[171] = 1.0; + out_8952125250503643232[172] = 0.0; + out_8952125250503643232[173] = 0.0; + out_8952125250503643232[174] = 0.0; + out_8952125250503643232[175] = 0.0; + out_8952125250503643232[176] = 0.0; + out_8952125250503643232[177] = 0.0; + out_8952125250503643232[178] = 0.0; + out_8952125250503643232[179] = 0.0; + out_8952125250503643232[180] = 0.0; + out_8952125250503643232[181] = 0.0; + out_8952125250503643232[182] = 0.0; + out_8952125250503643232[183] = 0.0; + out_8952125250503643232[184] = 0.0; + out_8952125250503643232[185] = 0.0; + out_8952125250503643232[186] = 0.0; + out_8952125250503643232[187] = 0.0; + out_8952125250503643232[188] = 0.0; + out_8952125250503643232[189] = 0.0; + out_8952125250503643232[190] = 1.0; + out_8952125250503643232[191] = 0.0; + out_8952125250503643232[192] = 0.0; + out_8952125250503643232[193] = 0.0; + out_8952125250503643232[194] = 0.0; + out_8952125250503643232[195] = 0.0; + out_8952125250503643232[196] = 0.0; + out_8952125250503643232[197] = 0.0; + out_8952125250503643232[198] = 0.0; + out_8952125250503643232[199] = 0.0; + out_8952125250503643232[200] = 0.0; + out_8952125250503643232[201] = 0.0; + out_8952125250503643232[202] = 0.0; + out_8952125250503643232[203] = 0.0; + out_8952125250503643232[204] = 0.0; + out_8952125250503643232[205] = 0.0; + out_8952125250503643232[206] = 0.0; + out_8952125250503643232[207] = 0.0; + out_8952125250503643232[208] = 0.0; + out_8952125250503643232[209] = 1.0; + out_8952125250503643232[210] = 0.0; + out_8952125250503643232[211] = 0.0; + out_8952125250503643232[212] = 0.0; + out_8952125250503643232[213] = 0.0; + out_8952125250503643232[214] = 0.0; + out_8952125250503643232[215] = 0.0; + out_8952125250503643232[216] = 0.0; + out_8952125250503643232[217] = 0.0; + out_8952125250503643232[218] = 0.0; + out_8952125250503643232[219] = 0.0; + out_8952125250503643232[220] = 0.0; + out_8952125250503643232[221] = 0.0; + out_8952125250503643232[222] = 0.0; + out_8952125250503643232[223] = 0.0; + out_8952125250503643232[224] = 0.0; + out_8952125250503643232[225] = 0.0; + out_8952125250503643232[226] = 0.0; + out_8952125250503643232[227] = 0.0; + out_8952125250503643232[228] = 1.0; + out_8952125250503643232[229] = 0.0; + out_8952125250503643232[230] = 0.0; + out_8952125250503643232[231] = 0.0; + out_8952125250503643232[232] = 0.0; + out_8952125250503643232[233] = 0.0; + out_8952125250503643232[234] = 0.0; + out_8952125250503643232[235] = 0.0; + out_8952125250503643232[236] = 0.0; + out_8952125250503643232[237] = 0.0; + out_8952125250503643232[238] = 0.0; + out_8952125250503643232[239] = 0.0; + out_8952125250503643232[240] = 0.0; + out_8952125250503643232[241] = 0.0; + out_8952125250503643232[242] = 0.0; + out_8952125250503643232[243] = 0.0; + out_8952125250503643232[244] = 0.0; + out_8952125250503643232[245] = 0.0; + out_8952125250503643232[246] = 0.0; + out_8952125250503643232[247] = 1.0; + out_8952125250503643232[248] = 0.0; + out_8952125250503643232[249] = 0.0; + out_8952125250503643232[250] = 0.0; + out_8952125250503643232[251] = 0.0; + out_8952125250503643232[252] = 0.0; + out_8952125250503643232[253] = 0.0; + out_8952125250503643232[254] = 0.0; + out_8952125250503643232[255] = 0.0; + out_8952125250503643232[256] = 0.0; + out_8952125250503643232[257] = 0.0; + out_8952125250503643232[258] = 0.0; + out_8952125250503643232[259] = 0.0; + out_8952125250503643232[260] = 0.0; + out_8952125250503643232[261] = 0.0; + out_8952125250503643232[262] = 0.0; + out_8952125250503643232[263] = 0.0; + out_8952125250503643232[264] = 0.0; + out_8952125250503643232[265] = 0.0; + out_8952125250503643232[266] = 1.0; + out_8952125250503643232[267] = 0.0; + out_8952125250503643232[268] = 0.0; + out_8952125250503643232[269] = 0.0; + out_8952125250503643232[270] = 0.0; + out_8952125250503643232[271] = 0.0; + out_8952125250503643232[272] = 0.0; + out_8952125250503643232[273] = 0.0; + out_8952125250503643232[274] = 0.0; + out_8952125250503643232[275] = 0.0; + out_8952125250503643232[276] = 0.0; + out_8952125250503643232[277] = 0.0; + out_8952125250503643232[278] = 0.0; + out_8952125250503643232[279] = 0.0; + out_8952125250503643232[280] = 0.0; + out_8952125250503643232[281] = 0.0; + out_8952125250503643232[282] = 0.0; + out_8952125250503643232[283] = 0.0; + out_8952125250503643232[284] = 0.0; + out_8952125250503643232[285] = 1.0; + out_8952125250503643232[286] = 0.0; + out_8952125250503643232[287] = 0.0; + out_8952125250503643232[288] = 0.0; + out_8952125250503643232[289] = 0.0; + out_8952125250503643232[290] = 0.0; + out_8952125250503643232[291] = 0.0; + out_8952125250503643232[292] = 0.0; + out_8952125250503643232[293] = 0.0; + out_8952125250503643232[294] = 0.0; + out_8952125250503643232[295] = 0.0; + out_8952125250503643232[296] = 0.0; + out_8952125250503643232[297] = 0.0; + out_8952125250503643232[298] = 0.0; + out_8952125250503643232[299] = 0.0; + out_8952125250503643232[300] = 0.0; + out_8952125250503643232[301] = 0.0; + out_8952125250503643232[302] = 0.0; + out_8952125250503643232[303] = 0.0; + out_8952125250503643232[304] = 1.0; + out_8952125250503643232[305] = 0.0; + out_8952125250503643232[306] = 0.0; + out_8952125250503643232[307] = 0.0; + out_8952125250503643232[308] = 0.0; + out_8952125250503643232[309] = 0.0; + out_8952125250503643232[310] = 0.0; + out_8952125250503643232[311] = 0.0; + out_8952125250503643232[312] = 0.0; + out_8952125250503643232[313] = 0.0; + out_8952125250503643232[314] = 0.0; + out_8952125250503643232[315] = 0.0; + out_8952125250503643232[316] = 0.0; + out_8952125250503643232[317] = 0.0; + out_8952125250503643232[318] = 0.0; + out_8952125250503643232[319] = 0.0; + out_8952125250503643232[320] = 0.0; + out_8952125250503643232[321] = 0.0; + out_8952125250503643232[322] = 0.0; + out_8952125250503643232[323] = 1.0; } -void f_fun(double *state, double dt, double *out_9094503671864348345) { - out_9094503671864348345[0] = atan2((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), -(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1])); - out_9094503671864348345[1] = asin(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8])); - out_9094503671864348345[2] = atan2(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), -(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2])); - out_9094503671864348345[3] = dt*state[12] + state[3]; - out_9094503671864348345[4] = dt*state[13] + state[4]; - out_9094503671864348345[5] = dt*state[14] + state[5]; - out_9094503671864348345[6] = state[6]; - out_9094503671864348345[7] = state[7]; - out_9094503671864348345[8] = state[8]; - out_9094503671864348345[9] = state[9]; - out_9094503671864348345[10] = state[10]; - out_9094503671864348345[11] = state[11]; - out_9094503671864348345[12] = state[12]; - out_9094503671864348345[13] = state[13]; - out_9094503671864348345[14] = state[14]; - out_9094503671864348345[15] = state[15]; - out_9094503671864348345[16] = state[16]; - out_9094503671864348345[17] = state[17]; +void f_fun(double *state, double dt, double *out_3519678704568830866) { + out_3519678704568830866[0] = atan2((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), -(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1])); + out_3519678704568830866[1] = asin(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8])); + out_3519678704568830866[2] = atan2(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), -(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2])); + out_3519678704568830866[3] = dt*state[12] + state[3]; + out_3519678704568830866[4] = dt*state[13] + state[4]; + out_3519678704568830866[5] = dt*state[14] + state[5]; + out_3519678704568830866[6] = state[6]; + out_3519678704568830866[7] = state[7]; + out_3519678704568830866[8] = state[8]; + out_3519678704568830866[9] = state[9]; + out_3519678704568830866[10] = state[10]; + out_3519678704568830866[11] = state[11]; + out_3519678704568830866[12] = state[12]; + out_3519678704568830866[13] = state[13]; + out_3519678704568830866[14] = state[14]; + out_3519678704568830866[15] = state[15]; + out_3519678704568830866[16] = state[16]; + out_3519678704568830866[17] = state[17]; } -void F_fun(double *state, double dt, double *out_1312959600226932432) { - out_1312959600226932432[0] = ((-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*cos(state[0])*cos(state[1]) - sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*cos(state[0])*cos(state[1]) - sin(dt*state[6])*sin(state[0])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_1312959600226932432[1] = ((-sin(dt*state[6])*sin(dt*state[8]) - sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*cos(state[1]) - (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*sin(state[1]) - sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(state[0]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*sin(state[1]) + (-sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) + sin(dt*state[8])*cos(dt*state[6]))*cos(state[1]) - sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(state[0]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_1312959600226932432[2] = 0; - out_1312959600226932432[3] = 0; - out_1312959600226932432[4] = 0; - out_1312959600226932432[5] = 0; - out_1312959600226932432[6] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(dt*cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) - dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_1312959600226932432[7] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*sin(dt*state[7])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[6])*sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) - dt*sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[7])*cos(dt*state[6])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[8])*sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]) - dt*sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_1312959600226932432[8] = ((dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((dt*sin(dt*state[6])*sin(dt*state[8]) + dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_1312959600226932432[9] = 0; - out_1312959600226932432[10] = 0; - out_1312959600226932432[11] = 0; - out_1312959600226932432[12] = 0; - out_1312959600226932432[13] = 0; - out_1312959600226932432[14] = 0; - out_1312959600226932432[15] = 0; - out_1312959600226932432[16] = 0; - out_1312959600226932432[17] = 0; - out_1312959600226932432[18] = (-sin(dt*state[7])*sin(state[0])*cos(state[1]) - sin(dt*state[8])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_1312959600226932432[19] = (-sin(dt*state[7])*sin(state[1])*cos(state[0]) + sin(dt*state[8])*sin(state[0])*sin(state[1])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_1312959600226932432[20] = 0; - out_1312959600226932432[21] = 0; - out_1312959600226932432[22] = 0; - out_1312959600226932432[23] = 0; - out_1312959600226932432[24] = 0; - out_1312959600226932432[25] = (dt*sin(dt*state[7])*sin(dt*state[8])*sin(state[0])*cos(state[1]) - dt*sin(dt*state[7])*sin(state[1])*cos(dt*state[8]) + dt*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_1312959600226932432[26] = (-dt*sin(dt*state[8])*sin(state[1])*cos(dt*state[7]) - dt*sin(state[0])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_1312959600226932432[27] = 0; - out_1312959600226932432[28] = 0; - out_1312959600226932432[29] = 0; - out_1312959600226932432[30] = 0; - out_1312959600226932432[31] = 0; - out_1312959600226932432[32] = 0; - out_1312959600226932432[33] = 0; - out_1312959600226932432[34] = 0; - out_1312959600226932432[35] = 0; - out_1312959600226932432[36] = ((sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_1312959600226932432[37] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-sin(dt*state[7])*sin(state[2])*cos(state[0])*cos(state[1]) + sin(dt*state[8])*sin(state[0])*sin(state[2])*cos(dt*state[7])*cos(state[1]) - sin(state[1])*sin(state[2])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(-sin(dt*state[7])*cos(state[0])*cos(state[1])*cos(state[2]) + sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1])*cos(state[2]) - sin(state[1])*cos(dt*state[7])*cos(dt*state[8])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_1312959600226932432[38] = ((-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (-sin(state[0])*sin(state[1])*sin(state[2]) - cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_1312959600226932432[39] = 0; - out_1312959600226932432[40] = 0; - out_1312959600226932432[41] = 0; - out_1312959600226932432[42] = 0; - out_1312959600226932432[43] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(dt*(sin(state[0])*cos(state[2]) - sin(state[1])*sin(state[2])*cos(state[0]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*sin(state[2])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(dt*(-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_1312959600226932432[44] = (dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*sin(state[2])*cos(dt*state[7])*cos(state[1]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + (dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[7])*cos(state[1])*cos(state[2]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_1312959600226932432[45] = 0; - out_1312959600226932432[46] = 0; - out_1312959600226932432[47] = 0; - out_1312959600226932432[48] = 0; - out_1312959600226932432[49] = 0; - out_1312959600226932432[50] = 0; - out_1312959600226932432[51] = 0; - out_1312959600226932432[52] = 0; - out_1312959600226932432[53] = 0; - out_1312959600226932432[54] = 0; - out_1312959600226932432[55] = 0; - out_1312959600226932432[56] = 0; - out_1312959600226932432[57] = 1; - out_1312959600226932432[58] = 0; - out_1312959600226932432[59] = 0; - out_1312959600226932432[60] = 0; - out_1312959600226932432[61] = 0; - out_1312959600226932432[62] = 0; - out_1312959600226932432[63] = 0; - out_1312959600226932432[64] = 0; - out_1312959600226932432[65] = 0; - out_1312959600226932432[66] = dt; - out_1312959600226932432[67] = 0; - out_1312959600226932432[68] = 0; - out_1312959600226932432[69] = 0; - out_1312959600226932432[70] = 0; - out_1312959600226932432[71] = 0; - out_1312959600226932432[72] = 0; - out_1312959600226932432[73] = 0; - out_1312959600226932432[74] = 0; - out_1312959600226932432[75] = 0; - out_1312959600226932432[76] = 1; - out_1312959600226932432[77] = 0; - out_1312959600226932432[78] = 0; - out_1312959600226932432[79] = 0; - out_1312959600226932432[80] = 0; - out_1312959600226932432[81] = 0; - out_1312959600226932432[82] = 0; - out_1312959600226932432[83] = 0; - out_1312959600226932432[84] = 0; - out_1312959600226932432[85] = dt; - out_1312959600226932432[86] = 0; - out_1312959600226932432[87] = 0; - out_1312959600226932432[88] = 0; - out_1312959600226932432[89] = 0; - out_1312959600226932432[90] = 0; - out_1312959600226932432[91] = 0; - out_1312959600226932432[92] = 0; - out_1312959600226932432[93] = 0; - out_1312959600226932432[94] = 0; - out_1312959600226932432[95] = 1; - out_1312959600226932432[96] = 0; - out_1312959600226932432[97] = 0; - out_1312959600226932432[98] = 0; - out_1312959600226932432[99] = 0; - out_1312959600226932432[100] = 0; - out_1312959600226932432[101] = 0; - out_1312959600226932432[102] = 0; - out_1312959600226932432[103] = 0; - out_1312959600226932432[104] = dt; - out_1312959600226932432[105] = 0; - out_1312959600226932432[106] = 0; - out_1312959600226932432[107] = 0; - out_1312959600226932432[108] = 0; - out_1312959600226932432[109] = 0; - out_1312959600226932432[110] = 0; - out_1312959600226932432[111] = 0; - out_1312959600226932432[112] = 0; - out_1312959600226932432[113] = 0; - out_1312959600226932432[114] = 1; - out_1312959600226932432[115] = 0; - out_1312959600226932432[116] = 0; - out_1312959600226932432[117] = 0; - out_1312959600226932432[118] = 0; - out_1312959600226932432[119] = 0; - out_1312959600226932432[120] = 0; - out_1312959600226932432[121] = 0; - out_1312959600226932432[122] = 0; - out_1312959600226932432[123] = 0; - out_1312959600226932432[124] = 0; - out_1312959600226932432[125] = 0; - out_1312959600226932432[126] = 0; - out_1312959600226932432[127] = 0; - out_1312959600226932432[128] = 0; - out_1312959600226932432[129] = 0; - out_1312959600226932432[130] = 0; - out_1312959600226932432[131] = 0; - out_1312959600226932432[132] = 0; - out_1312959600226932432[133] = 1; - out_1312959600226932432[134] = 0; - out_1312959600226932432[135] = 0; - out_1312959600226932432[136] = 0; - out_1312959600226932432[137] = 0; - out_1312959600226932432[138] = 0; - out_1312959600226932432[139] = 0; - out_1312959600226932432[140] = 0; - out_1312959600226932432[141] = 0; - out_1312959600226932432[142] = 0; - out_1312959600226932432[143] = 0; - out_1312959600226932432[144] = 0; - out_1312959600226932432[145] = 0; - out_1312959600226932432[146] = 0; - out_1312959600226932432[147] = 0; - out_1312959600226932432[148] = 0; - out_1312959600226932432[149] = 0; - out_1312959600226932432[150] = 0; - out_1312959600226932432[151] = 0; - out_1312959600226932432[152] = 1; - out_1312959600226932432[153] = 0; - out_1312959600226932432[154] = 0; - out_1312959600226932432[155] = 0; - out_1312959600226932432[156] = 0; - out_1312959600226932432[157] = 0; - out_1312959600226932432[158] = 0; - out_1312959600226932432[159] = 0; - out_1312959600226932432[160] = 0; - out_1312959600226932432[161] = 0; - out_1312959600226932432[162] = 0; - out_1312959600226932432[163] = 0; - out_1312959600226932432[164] = 0; - out_1312959600226932432[165] = 0; - out_1312959600226932432[166] = 0; - out_1312959600226932432[167] = 0; - out_1312959600226932432[168] = 0; - out_1312959600226932432[169] = 0; - out_1312959600226932432[170] = 0; - out_1312959600226932432[171] = 1; - out_1312959600226932432[172] = 0; - out_1312959600226932432[173] = 0; - out_1312959600226932432[174] = 0; - out_1312959600226932432[175] = 0; - out_1312959600226932432[176] = 0; - out_1312959600226932432[177] = 0; - out_1312959600226932432[178] = 0; - out_1312959600226932432[179] = 0; - out_1312959600226932432[180] = 0; - out_1312959600226932432[181] = 0; - out_1312959600226932432[182] = 0; - out_1312959600226932432[183] = 0; - out_1312959600226932432[184] = 0; - out_1312959600226932432[185] = 0; - out_1312959600226932432[186] = 0; - out_1312959600226932432[187] = 0; - out_1312959600226932432[188] = 0; - out_1312959600226932432[189] = 0; - out_1312959600226932432[190] = 1; - out_1312959600226932432[191] = 0; - out_1312959600226932432[192] = 0; - out_1312959600226932432[193] = 0; - out_1312959600226932432[194] = 0; - out_1312959600226932432[195] = 0; - out_1312959600226932432[196] = 0; - out_1312959600226932432[197] = 0; - out_1312959600226932432[198] = 0; - out_1312959600226932432[199] = 0; - out_1312959600226932432[200] = 0; - out_1312959600226932432[201] = 0; - out_1312959600226932432[202] = 0; - out_1312959600226932432[203] = 0; - out_1312959600226932432[204] = 0; - out_1312959600226932432[205] = 0; - out_1312959600226932432[206] = 0; - out_1312959600226932432[207] = 0; - out_1312959600226932432[208] = 0; - out_1312959600226932432[209] = 1; - out_1312959600226932432[210] = 0; - out_1312959600226932432[211] = 0; - out_1312959600226932432[212] = 0; - out_1312959600226932432[213] = 0; - out_1312959600226932432[214] = 0; - out_1312959600226932432[215] = 0; - out_1312959600226932432[216] = 0; - out_1312959600226932432[217] = 0; - out_1312959600226932432[218] = 0; - out_1312959600226932432[219] = 0; - out_1312959600226932432[220] = 0; - out_1312959600226932432[221] = 0; - out_1312959600226932432[222] = 0; - out_1312959600226932432[223] = 0; - out_1312959600226932432[224] = 0; - out_1312959600226932432[225] = 0; - out_1312959600226932432[226] = 0; - out_1312959600226932432[227] = 0; - out_1312959600226932432[228] = 1; - out_1312959600226932432[229] = 0; - out_1312959600226932432[230] = 0; - out_1312959600226932432[231] = 0; - out_1312959600226932432[232] = 0; - out_1312959600226932432[233] = 0; - out_1312959600226932432[234] = 0; - out_1312959600226932432[235] = 0; - out_1312959600226932432[236] = 0; - out_1312959600226932432[237] = 0; - out_1312959600226932432[238] = 0; - out_1312959600226932432[239] = 0; - out_1312959600226932432[240] = 0; - out_1312959600226932432[241] = 0; - out_1312959600226932432[242] = 0; - out_1312959600226932432[243] = 0; - out_1312959600226932432[244] = 0; - out_1312959600226932432[245] = 0; - out_1312959600226932432[246] = 0; - out_1312959600226932432[247] = 1; - out_1312959600226932432[248] = 0; - out_1312959600226932432[249] = 0; - out_1312959600226932432[250] = 0; - out_1312959600226932432[251] = 0; - out_1312959600226932432[252] = 0; - out_1312959600226932432[253] = 0; - out_1312959600226932432[254] = 0; - out_1312959600226932432[255] = 0; - out_1312959600226932432[256] = 0; - out_1312959600226932432[257] = 0; - out_1312959600226932432[258] = 0; - out_1312959600226932432[259] = 0; - out_1312959600226932432[260] = 0; - out_1312959600226932432[261] = 0; - out_1312959600226932432[262] = 0; - out_1312959600226932432[263] = 0; - out_1312959600226932432[264] = 0; - out_1312959600226932432[265] = 0; - out_1312959600226932432[266] = 1; - out_1312959600226932432[267] = 0; - out_1312959600226932432[268] = 0; - out_1312959600226932432[269] = 0; - out_1312959600226932432[270] = 0; - out_1312959600226932432[271] = 0; - out_1312959600226932432[272] = 0; - out_1312959600226932432[273] = 0; - out_1312959600226932432[274] = 0; - out_1312959600226932432[275] = 0; - out_1312959600226932432[276] = 0; - out_1312959600226932432[277] = 0; - out_1312959600226932432[278] = 0; - out_1312959600226932432[279] = 0; - out_1312959600226932432[280] = 0; - out_1312959600226932432[281] = 0; - out_1312959600226932432[282] = 0; - out_1312959600226932432[283] = 0; - out_1312959600226932432[284] = 0; - out_1312959600226932432[285] = 1; - out_1312959600226932432[286] = 0; - out_1312959600226932432[287] = 0; - out_1312959600226932432[288] = 0; - out_1312959600226932432[289] = 0; - out_1312959600226932432[290] = 0; - out_1312959600226932432[291] = 0; - out_1312959600226932432[292] = 0; - out_1312959600226932432[293] = 0; - out_1312959600226932432[294] = 0; - out_1312959600226932432[295] = 0; - out_1312959600226932432[296] = 0; - out_1312959600226932432[297] = 0; - out_1312959600226932432[298] = 0; - out_1312959600226932432[299] = 0; - out_1312959600226932432[300] = 0; - out_1312959600226932432[301] = 0; - out_1312959600226932432[302] = 0; - out_1312959600226932432[303] = 0; - out_1312959600226932432[304] = 1; - out_1312959600226932432[305] = 0; - out_1312959600226932432[306] = 0; - out_1312959600226932432[307] = 0; - out_1312959600226932432[308] = 0; - out_1312959600226932432[309] = 0; - out_1312959600226932432[310] = 0; - out_1312959600226932432[311] = 0; - out_1312959600226932432[312] = 0; - out_1312959600226932432[313] = 0; - out_1312959600226932432[314] = 0; - out_1312959600226932432[315] = 0; - out_1312959600226932432[316] = 0; - out_1312959600226932432[317] = 0; - out_1312959600226932432[318] = 0; - out_1312959600226932432[319] = 0; - out_1312959600226932432[320] = 0; - out_1312959600226932432[321] = 0; - out_1312959600226932432[322] = 0; - out_1312959600226932432[323] = 1; +void F_fun(double *state, double dt, double *out_6750093296288889328) { + out_6750093296288889328[0] = ((-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*cos(state[0])*cos(state[1]) - sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*cos(state[0])*cos(state[1]) - sin(dt*state[6])*sin(state[0])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_6750093296288889328[1] = ((-sin(dt*state[6])*sin(dt*state[8]) - sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*cos(state[1]) - (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*sin(state[1]) - sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(state[0]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*sin(state[1]) + (-sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) + sin(dt*state[8])*cos(dt*state[6]))*cos(state[1]) - sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(state[0]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_6750093296288889328[2] = 0; + out_6750093296288889328[3] = 0; + out_6750093296288889328[4] = 0; + out_6750093296288889328[5] = 0; + out_6750093296288889328[6] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(dt*cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) - dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_6750093296288889328[7] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*sin(dt*state[7])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[6])*sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) - dt*sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[7])*cos(dt*state[6])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[8])*sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]) - dt*sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_6750093296288889328[8] = ((dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((dt*sin(dt*state[6])*sin(dt*state[8]) + dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_6750093296288889328[9] = 0; + out_6750093296288889328[10] = 0; + out_6750093296288889328[11] = 0; + out_6750093296288889328[12] = 0; + out_6750093296288889328[13] = 0; + out_6750093296288889328[14] = 0; + out_6750093296288889328[15] = 0; + out_6750093296288889328[16] = 0; + out_6750093296288889328[17] = 0; + out_6750093296288889328[18] = (-sin(dt*state[7])*sin(state[0])*cos(state[1]) - sin(dt*state[8])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); + out_6750093296288889328[19] = (-sin(dt*state[7])*sin(state[1])*cos(state[0]) + sin(dt*state[8])*sin(state[0])*sin(state[1])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); + out_6750093296288889328[20] = 0; + out_6750093296288889328[21] = 0; + out_6750093296288889328[22] = 0; + out_6750093296288889328[23] = 0; + out_6750093296288889328[24] = 0; + out_6750093296288889328[25] = (dt*sin(dt*state[7])*sin(dt*state[8])*sin(state[0])*cos(state[1]) - dt*sin(dt*state[7])*sin(state[1])*cos(dt*state[8]) + dt*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); + out_6750093296288889328[26] = (-dt*sin(dt*state[8])*sin(state[1])*cos(dt*state[7]) - dt*sin(state[0])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); + out_6750093296288889328[27] = 0; + out_6750093296288889328[28] = 0; + out_6750093296288889328[29] = 0; + out_6750093296288889328[30] = 0; + out_6750093296288889328[31] = 0; + out_6750093296288889328[32] = 0; + out_6750093296288889328[33] = 0; + out_6750093296288889328[34] = 0; + out_6750093296288889328[35] = 0; + out_6750093296288889328[36] = ((sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_6750093296288889328[37] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-sin(dt*state[7])*sin(state[2])*cos(state[0])*cos(state[1]) + sin(dt*state[8])*sin(state[0])*sin(state[2])*cos(dt*state[7])*cos(state[1]) - sin(state[1])*sin(state[2])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(-sin(dt*state[7])*cos(state[0])*cos(state[1])*cos(state[2]) + sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1])*cos(state[2]) - sin(state[1])*cos(dt*state[7])*cos(dt*state[8])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_6750093296288889328[38] = ((-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (-sin(state[0])*sin(state[1])*sin(state[2]) - cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_6750093296288889328[39] = 0; + out_6750093296288889328[40] = 0; + out_6750093296288889328[41] = 0; + out_6750093296288889328[42] = 0; + out_6750093296288889328[43] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(dt*(sin(state[0])*cos(state[2]) - sin(state[1])*sin(state[2])*cos(state[0]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*sin(state[2])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(dt*(-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_6750093296288889328[44] = (dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*sin(state[2])*cos(dt*state[7])*cos(state[1]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + (dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[7])*cos(state[1])*cos(state[2]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_6750093296288889328[45] = 0; + out_6750093296288889328[46] = 0; + out_6750093296288889328[47] = 0; + out_6750093296288889328[48] = 0; + out_6750093296288889328[49] = 0; + out_6750093296288889328[50] = 0; + out_6750093296288889328[51] = 0; + out_6750093296288889328[52] = 0; + out_6750093296288889328[53] = 0; + out_6750093296288889328[54] = 0; + out_6750093296288889328[55] = 0; + out_6750093296288889328[56] = 0; + out_6750093296288889328[57] = 1; + out_6750093296288889328[58] = 0; + out_6750093296288889328[59] = 0; + out_6750093296288889328[60] = 0; + out_6750093296288889328[61] = 0; + out_6750093296288889328[62] = 0; + out_6750093296288889328[63] = 0; + out_6750093296288889328[64] = 0; + out_6750093296288889328[65] = 0; + out_6750093296288889328[66] = dt; + out_6750093296288889328[67] = 0; + out_6750093296288889328[68] = 0; + out_6750093296288889328[69] = 0; + out_6750093296288889328[70] = 0; + out_6750093296288889328[71] = 0; + out_6750093296288889328[72] = 0; + out_6750093296288889328[73] = 0; + out_6750093296288889328[74] = 0; + out_6750093296288889328[75] = 0; + out_6750093296288889328[76] = 1; + out_6750093296288889328[77] = 0; + out_6750093296288889328[78] = 0; + out_6750093296288889328[79] = 0; + out_6750093296288889328[80] = 0; + out_6750093296288889328[81] = 0; + out_6750093296288889328[82] = 0; + out_6750093296288889328[83] = 0; + out_6750093296288889328[84] = 0; + out_6750093296288889328[85] = dt; + out_6750093296288889328[86] = 0; + out_6750093296288889328[87] = 0; + out_6750093296288889328[88] = 0; + out_6750093296288889328[89] = 0; + out_6750093296288889328[90] = 0; + out_6750093296288889328[91] = 0; + out_6750093296288889328[92] = 0; + out_6750093296288889328[93] = 0; + out_6750093296288889328[94] = 0; + out_6750093296288889328[95] = 1; + out_6750093296288889328[96] = 0; + out_6750093296288889328[97] = 0; + out_6750093296288889328[98] = 0; + out_6750093296288889328[99] = 0; + out_6750093296288889328[100] = 0; + out_6750093296288889328[101] = 0; + out_6750093296288889328[102] = 0; + out_6750093296288889328[103] = 0; + out_6750093296288889328[104] = dt; + out_6750093296288889328[105] = 0; + out_6750093296288889328[106] = 0; + out_6750093296288889328[107] = 0; + out_6750093296288889328[108] = 0; + out_6750093296288889328[109] = 0; + out_6750093296288889328[110] = 0; + out_6750093296288889328[111] = 0; + out_6750093296288889328[112] = 0; + out_6750093296288889328[113] = 0; + out_6750093296288889328[114] = 1; + out_6750093296288889328[115] = 0; + out_6750093296288889328[116] = 0; + out_6750093296288889328[117] = 0; + out_6750093296288889328[118] = 0; + out_6750093296288889328[119] = 0; + out_6750093296288889328[120] = 0; + out_6750093296288889328[121] = 0; + out_6750093296288889328[122] = 0; + out_6750093296288889328[123] = 0; + out_6750093296288889328[124] = 0; + out_6750093296288889328[125] = 0; + out_6750093296288889328[126] = 0; + out_6750093296288889328[127] = 0; + out_6750093296288889328[128] = 0; + out_6750093296288889328[129] = 0; + out_6750093296288889328[130] = 0; + out_6750093296288889328[131] = 0; + out_6750093296288889328[132] = 0; + out_6750093296288889328[133] = 1; + out_6750093296288889328[134] = 0; + out_6750093296288889328[135] = 0; + out_6750093296288889328[136] = 0; + out_6750093296288889328[137] = 0; + out_6750093296288889328[138] = 0; + out_6750093296288889328[139] = 0; + out_6750093296288889328[140] = 0; + out_6750093296288889328[141] = 0; + out_6750093296288889328[142] = 0; + out_6750093296288889328[143] = 0; + out_6750093296288889328[144] = 0; + out_6750093296288889328[145] = 0; + out_6750093296288889328[146] = 0; + out_6750093296288889328[147] = 0; + out_6750093296288889328[148] = 0; + out_6750093296288889328[149] = 0; + out_6750093296288889328[150] = 0; + out_6750093296288889328[151] = 0; + out_6750093296288889328[152] = 1; + out_6750093296288889328[153] = 0; + out_6750093296288889328[154] = 0; + out_6750093296288889328[155] = 0; + out_6750093296288889328[156] = 0; + out_6750093296288889328[157] = 0; + out_6750093296288889328[158] = 0; + out_6750093296288889328[159] = 0; + out_6750093296288889328[160] = 0; + out_6750093296288889328[161] = 0; + out_6750093296288889328[162] = 0; + out_6750093296288889328[163] = 0; + out_6750093296288889328[164] = 0; + out_6750093296288889328[165] = 0; + out_6750093296288889328[166] = 0; + out_6750093296288889328[167] = 0; + out_6750093296288889328[168] = 0; + out_6750093296288889328[169] = 0; + out_6750093296288889328[170] = 0; + out_6750093296288889328[171] = 1; + out_6750093296288889328[172] = 0; + out_6750093296288889328[173] = 0; + out_6750093296288889328[174] = 0; + out_6750093296288889328[175] = 0; + out_6750093296288889328[176] = 0; + out_6750093296288889328[177] = 0; + out_6750093296288889328[178] = 0; + out_6750093296288889328[179] = 0; + out_6750093296288889328[180] = 0; + out_6750093296288889328[181] = 0; + out_6750093296288889328[182] = 0; + out_6750093296288889328[183] = 0; + out_6750093296288889328[184] = 0; + out_6750093296288889328[185] = 0; + out_6750093296288889328[186] = 0; + out_6750093296288889328[187] = 0; + out_6750093296288889328[188] = 0; + out_6750093296288889328[189] = 0; + out_6750093296288889328[190] = 1; + out_6750093296288889328[191] = 0; + out_6750093296288889328[192] = 0; + out_6750093296288889328[193] = 0; + out_6750093296288889328[194] = 0; + out_6750093296288889328[195] = 0; + out_6750093296288889328[196] = 0; + out_6750093296288889328[197] = 0; + out_6750093296288889328[198] = 0; + out_6750093296288889328[199] = 0; + out_6750093296288889328[200] = 0; + out_6750093296288889328[201] = 0; + out_6750093296288889328[202] = 0; + out_6750093296288889328[203] = 0; + out_6750093296288889328[204] = 0; + out_6750093296288889328[205] = 0; + out_6750093296288889328[206] = 0; + out_6750093296288889328[207] = 0; + out_6750093296288889328[208] = 0; + out_6750093296288889328[209] = 1; + out_6750093296288889328[210] = 0; + out_6750093296288889328[211] = 0; + out_6750093296288889328[212] = 0; + out_6750093296288889328[213] = 0; + out_6750093296288889328[214] = 0; + out_6750093296288889328[215] = 0; + out_6750093296288889328[216] = 0; + out_6750093296288889328[217] = 0; + out_6750093296288889328[218] = 0; + out_6750093296288889328[219] = 0; + out_6750093296288889328[220] = 0; + out_6750093296288889328[221] = 0; + out_6750093296288889328[222] = 0; + out_6750093296288889328[223] = 0; + out_6750093296288889328[224] = 0; + out_6750093296288889328[225] = 0; + out_6750093296288889328[226] = 0; + out_6750093296288889328[227] = 0; + out_6750093296288889328[228] = 1; + out_6750093296288889328[229] = 0; + out_6750093296288889328[230] = 0; + out_6750093296288889328[231] = 0; + out_6750093296288889328[232] = 0; + out_6750093296288889328[233] = 0; + out_6750093296288889328[234] = 0; + out_6750093296288889328[235] = 0; + out_6750093296288889328[236] = 0; + out_6750093296288889328[237] = 0; + out_6750093296288889328[238] = 0; + out_6750093296288889328[239] = 0; + out_6750093296288889328[240] = 0; + out_6750093296288889328[241] = 0; + out_6750093296288889328[242] = 0; + out_6750093296288889328[243] = 0; + out_6750093296288889328[244] = 0; + out_6750093296288889328[245] = 0; + out_6750093296288889328[246] = 0; + out_6750093296288889328[247] = 1; + out_6750093296288889328[248] = 0; + out_6750093296288889328[249] = 0; + out_6750093296288889328[250] = 0; + out_6750093296288889328[251] = 0; + out_6750093296288889328[252] = 0; + out_6750093296288889328[253] = 0; + out_6750093296288889328[254] = 0; + out_6750093296288889328[255] = 0; + out_6750093296288889328[256] = 0; + out_6750093296288889328[257] = 0; + out_6750093296288889328[258] = 0; + out_6750093296288889328[259] = 0; + out_6750093296288889328[260] = 0; + out_6750093296288889328[261] = 0; + out_6750093296288889328[262] = 0; + out_6750093296288889328[263] = 0; + out_6750093296288889328[264] = 0; + out_6750093296288889328[265] = 0; + out_6750093296288889328[266] = 1; + out_6750093296288889328[267] = 0; + out_6750093296288889328[268] = 0; + out_6750093296288889328[269] = 0; + out_6750093296288889328[270] = 0; + out_6750093296288889328[271] = 0; + out_6750093296288889328[272] = 0; + out_6750093296288889328[273] = 0; + out_6750093296288889328[274] = 0; + out_6750093296288889328[275] = 0; + out_6750093296288889328[276] = 0; + out_6750093296288889328[277] = 0; + out_6750093296288889328[278] = 0; + out_6750093296288889328[279] = 0; + out_6750093296288889328[280] = 0; + out_6750093296288889328[281] = 0; + out_6750093296288889328[282] = 0; + out_6750093296288889328[283] = 0; + out_6750093296288889328[284] = 0; + out_6750093296288889328[285] = 1; + out_6750093296288889328[286] = 0; + out_6750093296288889328[287] = 0; + out_6750093296288889328[288] = 0; + out_6750093296288889328[289] = 0; + out_6750093296288889328[290] = 0; + out_6750093296288889328[291] = 0; + out_6750093296288889328[292] = 0; + out_6750093296288889328[293] = 0; + out_6750093296288889328[294] = 0; + out_6750093296288889328[295] = 0; + out_6750093296288889328[296] = 0; + out_6750093296288889328[297] = 0; + out_6750093296288889328[298] = 0; + out_6750093296288889328[299] = 0; + out_6750093296288889328[300] = 0; + out_6750093296288889328[301] = 0; + out_6750093296288889328[302] = 0; + out_6750093296288889328[303] = 0; + out_6750093296288889328[304] = 1; + out_6750093296288889328[305] = 0; + out_6750093296288889328[306] = 0; + out_6750093296288889328[307] = 0; + out_6750093296288889328[308] = 0; + out_6750093296288889328[309] = 0; + out_6750093296288889328[310] = 0; + out_6750093296288889328[311] = 0; + out_6750093296288889328[312] = 0; + out_6750093296288889328[313] = 0; + out_6750093296288889328[314] = 0; + out_6750093296288889328[315] = 0; + out_6750093296288889328[316] = 0; + out_6750093296288889328[317] = 0; + out_6750093296288889328[318] = 0; + out_6750093296288889328[319] = 0; + out_6750093296288889328[320] = 0; + out_6750093296288889328[321] = 0; + out_6750093296288889328[322] = 0; + out_6750093296288889328[323] = 1; } -void h_4(double *state, double *unused, double *out_6400948089178504221) { - out_6400948089178504221[0] = state[6] + state[9]; - out_6400948089178504221[1] = state[7] + state[10]; - out_6400948089178504221[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_1078897140345555822) { + out_1078897140345555822[0] = state[6] + state[9]; + out_1078897140345555822[1] = state[7] + state[10]; + out_1078897140345555822[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_3419471307660667524) { - out_3419471307660667524[0] = 0; - out_3419471307660667524[1] = 0; - out_3419471307660667524[2] = 0; - out_3419471307660667524[3] = 0; - out_3419471307660667524[4] = 0; - out_3419471307660667524[5] = 0; - out_3419471307660667524[6] = 1; - out_3419471307660667524[7] = 0; - out_3419471307660667524[8] = 0; - out_3419471307660667524[9] = 1; - out_3419471307660667524[10] = 0; - out_3419471307660667524[11] = 0; - out_3419471307660667524[12] = 0; - out_3419471307660667524[13] = 0; - out_3419471307660667524[14] = 0; - out_3419471307660667524[15] = 0; - out_3419471307660667524[16] = 0; - out_3419471307660667524[17] = 0; - out_3419471307660667524[18] = 0; - out_3419471307660667524[19] = 0; - out_3419471307660667524[20] = 0; - out_3419471307660667524[21] = 0; - out_3419471307660667524[22] = 0; - out_3419471307660667524[23] = 0; - out_3419471307660667524[24] = 0; - out_3419471307660667524[25] = 1; - out_3419471307660667524[26] = 0; - out_3419471307660667524[27] = 0; - out_3419471307660667524[28] = 1; - out_3419471307660667524[29] = 0; - out_3419471307660667524[30] = 0; - out_3419471307660667524[31] = 0; - out_3419471307660667524[32] = 0; - out_3419471307660667524[33] = 0; - out_3419471307660667524[34] = 0; - out_3419471307660667524[35] = 0; - out_3419471307660667524[36] = 0; - out_3419471307660667524[37] = 0; - out_3419471307660667524[38] = 0; - out_3419471307660667524[39] = 0; - out_3419471307660667524[40] = 0; - out_3419471307660667524[41] = 0; - out_3419471307660667524[42] = 0; - out_3419471307660667524[43] = 0; - out_3419471307660667524[44] = 1; - out_3419471307660667524[45] = 0; - out_3419471307660667524[46] = 0; - out_3419471307660667524[47] = 1; - out_3419471307660667524[48] = 0; - out_3419471307660667524[49] = 0; - out_3419471307660667524[50] = 0; - out_3419471307660667524[51] = 0; - out_3419471307660667524[52] = 0; - out_3419471307660667524[53] = 0; +void H_4(double *state, double *unused, double *out_850583411531230682) { + out_850583411531230682[0] = 0; + out_850583411531230682[1] = 0; + out_850583411531230682[2] = 0; + out_850583411531230682[3] = 0; + out_850583411531230682[4] = 0; + out_850583411531230682[5] = 0; + out_850583411531230682[6] = 1; + out_850583411531230682[7] = 0; + out_850583411531230682[8] = 0; + out_850583411531230682[9] = 1; + out_850583411531230682[10] = 0; + out_850583411531230682[11] = 0; + out_850583411531230682[12] = 0; + out_850583411531230682[13] = 0; + out_850583411531230682[14] = 0; + out_850583411531230682[15] = 0; + out_850583411531230682[16] = 0; + out_850583411531230682[17] = 0; + out_850583411531230682[18] = 0; + out_850583411531230682[19] = 0; + out_850583411531230682[20] = 0; + out_850583411531230682[21] = 0; + out_850583411531230682[22] = 0; + out_850583411531230682[23] = 0; + out_850583411531230682[24] = 0; + out_850583411531230682[25] = 1; + out_850583411531230682[26] = 0; + out_850583411531230682[27] = 0; + out_850583411531230682[28] = 1; + out_850583411531230682[29] = 0; + out_850583411531230682[30] = 0; + out_850583411531230682[31] = 0; + out_850583411531230682[32] = 0; + out_850583411531230682[33] = 0; + out_850583411531230682[34] = 0; + out_850583411531230682[35] = 0; + out_850583411531230682[36] = 0; + out_850583411531230682[37] = 0; + out_850583411531230682[38] = 0; + out_850583411531230682[39] = 0; + out_850583411531230682[40] = 0; + out_850583411531230682[41] = 0; + out_850583411531230682[42] = 0; + out_850583411531230682[43] = 0; + out_850583411531230682[44] = 1; + out_850583411531230682[45] = 0; + out_850583411531230682[46] = 0; + out_850583411531230682[47] = 1; + out_850583411531230682[48] = 0; + out_850583411531230682[49] = 0; + out_850583411531230682[50] = 0; + out_850583411531230682[51] = 0; + out_850583411531230682[52] = 0; + out_850583411531230682[53] = 0; } -void h_10(double *state, double *unused, double *out_8540054287512262719) { - out_8540054287512262719[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_8540054287512262719[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_8540054287512262719[2] = -9.8100000000000005*cos(state[0])*cos(state[1]) - state[3]*state[7] + state[4]*state[6] + state[14] + state[17]; +void h_10(double *state, double *unused, double *out_3641708848879017741) { + out_3641708848879017741[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_3641708848879017741[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_3641708848879017741[2] = -9.8100000000000005*cos(state[0])*cos(state[1]) - state[3]*state[7] + state[4]*state[6] + state[14] + state[17]; } -void H_10(double *state, double *unused, double *out_5231659216258162963) { - out_5231659216258162963[0] = 0; - out_5231659216258162963[1] = 9.8100000000000005*cos(state[1]); - out_5231659216258162963[2] = 0; - out_5231659216258162963[3] = 0; - out_5231659216258162963[4] = -state[8]; - out_5231659216258162963[5] = state[7]; - out_5231659216258162963[6] = 0; - out_5231659216258162963[7] = state[5]; - out_5231659216258162963[8] = -state[4]; - out_5231659216258162963[9] = 0; - out_5231659216258162963[10] = 0; - out_5231659216258162963[11] = 0; - out_5231659216258162963[12] = 1; - out_5231659216258162963[13] = 0; - out_5231659216258162963[14] = 0; - out_5231659216258162963[15] = 1; - out_5231659216258162963[16] = 0; - out_5231659216258162963[17] = 0; - out_5231659216258162963[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_5231659216258162963[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_5231659216258162963[20] = 0; - out_5231659216258162963[21] = state[8]; - out_5231659216258162963[22] = 0; - out_5231659216258162963[23] = -state[6]; - out_5231659216258162963[24] = -state[5]; - out_5231659216258162963[25] = 0; - out_5231659216258162963[26] = state[3]; - out_5231659216258162963[27] = 0; - out_5231659216258162963[28] = 0; - out_5231659216258162963[29] = 0; - out_5231659216258162963[30] = 0; - out_5231659216258162963[31] = 1; - out_5231659216258162963[32] = 0; - out_5231659216258162963[33] = 0; - out_5231659216258162963[34] = 1; - out_5231659216258162963[35] = 0; - out_5231659216258162963[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_5231659216258162963[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_5231659216258162963[38] = 0; - out_5231659216258162963[39] = -state[7]; - out_5231659216258162963[40] = state[6]; - out_5231659216258162963[41] = 0; - out_5231659216258162963[42] = state[4]; - out_5231659216258162963[43] = -state[3]; - out_5231659216258162963[44] = 0; - out_5231659216258162963[45] = 0; - out_5231659216258162963[46] = 0; - out_5231659216258162963[47] = 0; - out_5231659216258162963[48] = 0; - out_5231659216258162963[49] = 0; - out_5231659216258162963[50] = 1; - out_5231659216258162963[51] = 0; - out_5231659216258162963[52] = 0; - out_5231659216258162963[53] = 1; +void H_10(double *state, double *unused, double *out_7494820718027871329) { + out_7494820718027871329[0] = 0; + out_7494820718027871329[1] = 9.8100000000000005*cos(state[1]); + out_7494820718027871329[2] = 0; + out_7494820718027871329[3] = 0; + out_7494820718027871329[4] = -state[8]; + out_7494820718027871329[5] = state[7]; + out_7494820718027871329[6] = 0; + out_7494820718027871329[7] = state[5]; + out_7494820718027871329[8] = -state[4]; + out_7494820718027871329[9] = 0; + out_7494820718027871329[10] = 0; + out_7494820718027871329[11] = 0; + out_7494820718027871329[12] = 1; + out_7494820718027871329[13] = 0; + out_7494820718027871329[14] = 0; + out_7494820718027871329[15] = 1; + out_7494820718027871329[16] = 0; + out_7494820718027871329[17] = 0; + out_7494820718027871329[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_7494820718027871329[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_7494820718027871329[20] = 0; + out_7494820718027871329[21] = state[8]; + out_7494820718027871329[22] = 0; + out_7494820718027871329[23] = -state[6]; + out_7494820718027871329[24] = -state[5]; + out_7494820718027871329[25] = 0; + out_7494820718027871329[26] = state[3]; + out_7494820718027871329[27] = 0; + out_7494820718027871329[28] = 0; + out_7494820718027871329[29] = 0; + out_7494820718027871329[30] = 0; + out_7494820718027871329[31] = 1; + out_7494820718027871329[32] = 0; + out_7494820718027871329[33] = 0; + out_7494820718027871329[34] = 1; + out_7494820718027871329[35] = 0; + out_7494820718027871329[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_7494820718027871329[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_7494820718027871329[38] = 0; + out_7494820718027871329[39] = -state[7]; + out_7494820718027871329[40] = state[6]; + out_7494820718027871329[41] = 0; + out_7494820718027871329[42] = state[4]; + out_7494820718027871329[43] = -state[3]; + out_7494820718027871329[44] = 0; + out_7494820718027871329[45] = 0; + out_7494820718027871329[46] = 0; + out_7494820718027871329[47] = 0; + out_7494820718027871329[48] = 0; + out_7494820718027871329[49] = 0; + out_7494820718027871329[50] = 1; + out_7494820718027871329[51] = 0; + out_7494820718027871329[52] = 0; + out_7494820718027871329[53] = 1; } -void h_13(double *state, double *unused, double *out_7953441679753433349) { - out_7953441679753433349[0] = state[3]; - out_7953441679753433349[1] = state[4]; - out_7953441679753433349[2] = state[5]; +void h_13(double *state, double *unused, double *out_515642654693887905) { + out_515642654693887905[0] = state[3]; + out_515642654693887905[1] = state[4]; + out_515642654693887905[2] = state[5]; } -void H_13(double *state, double *unused, double *out_207197482328334723) { - out_207197482328334723[0] = 0; - out_207197482328334723[1] = 0; - out_207197482328334723[2] = 0; - out_207197482328334723[3] = 1; - out_207197482328334723[4] = 0; - out_207197482328334723[5] = 0; - out_207197482328334723[6] = 0; - out_207197482328334723[7] = 0; - out_207197482328334723[8] = 0; - out_207197482328334723[9] = 0; - out_207197482328334723[10] = 0; - out_207197482328334723[11] = 0; - out_207197482328334723[12] = 0; - out_207197482328334723[13] = 0; - out_207197482328334723[14] = 0; - out_207197482328334723[15] = 0; - out_207197482328334723[16] = 0; - out_207197482328334723[17] = 0; - out_207197482328334723[18] = 0; - out_207197482328334723[19] = 0; - out_207197482328334723[20] = 0; - out_207197482328334723[21] = 0; - out_207197482328334723[22] = 1; - out_207197482328334723[23] = 0; - out_207197482328334723[24] = 0; - out_207197482328334723[25] = 0; - out_207197482328334723[26] = 0; - out_207197482328334723[27] = 0; - out_207197482328334723[28] = 0; - out_207197482328334723[29] = 0; - out_207197482328334723[30] = 0; - out_207197482328334723[31] = 0; - out_207197482328334723[32] = 0; - out_207197482328334723[33] = 0; - out_207197482328334723[34] = 0; - out_207197482328334723[35] = 0; - out_207197482328334723[36] = 0; - out_207197482328334723[37] = 0; - out_207197482328334723[38] = 0; - out_207197482328334723[39] = 0; - out_207197482328334723[40] = 0; - out_207197482328334723[41] = 1; - out_207197482328334723[42] = 0; - out_207197482328334723[43] = 0; - out_207197482328334723[44] = 0; - out_207197482328334723[45] = 0; - out_207197482328334723[46] = 0; - out_207197482328334723[47] = 0; - out_207197482328334723[48] = 0; - out_207197482328334723[49] = 0; - out_207197482328334723[50] = 0; - out_207197482328334723[51] = 0; - out_207197482328334723[52] = 0; - out_207197482328334723[53] = 0; +void H_13(double *state, double *unused, double *out_4062857236863563483) { + out_4062857236863563483[0] = 0; + out_4062857236863563483[1] = 0; + out_4062857236863563483[2] = 0; + out_4062857236863563483[3] = 1; + out_4062857236863563483[4] = 0; + out_4062857236863563483[5] = 0; + out_4062857236863563483[6] = 0; + out_4062857236863563483[7] = 0; + out_4062857236863563483[8] = 0; + out_4062857236863563483[9] = 0; + out_4062857236863563483[10] = 0; + out_4062857236863563483[11] = 0; + out_4062857236863563483[12] = 0; + out_4062857236863563483[13] = 0; + out_4062857236863563483[14] = 0; + out_4062857236863563483[15] = 0; + out_4062857236863563483[16] = 0; + out_4062857236863563483[17] = 0; + out_4062857236863563483[18] = 0; + out_4062857236863563483[19] = 0; + out_4062857236863563483[20] = 0; + out_4062857236863563483[21] = 0; + out_4062857236863563483[22] = 1; + out_4062857236863563483[23] = 0; + out_4062857236863563483[24] = 0; + out_4062857236863563483[25] = 0; + out_4062857236863563483[26] = 0; + out_4062857236863563483[27] = 0; + out_4062857236863563483[28] = 0; + out_4062857236863563483[29] = 0; + out_4062857236863563483[30] = 0; + out_4062857236863563483[31] = 0; + out_4062857236863563483[32] = 0; + out_4062857236863563483[33] = 0; + out_4062857236863563483[34] = 0; + out_4062857236863563483[35] = 0; + out_4062857236863563483[36] = 0; + out_4062857236863563483[37] = 0; + out_4062857236863563483[38] = 0; + out_4062857236863563483[39] = 0; + out_4062857236863563483[40] = 0; + out_4062857236863563483[41] = 1; + out_4062857236863563483[42] = 0; + out_4062857236863563483[43] = 0; + out_4062857236863563483[44] = 0; + out_4062857236863563483[45] = 0; + out_4062857236863563483[46] = 0; + out_4062857236863563483[47] = 0; + out_4062857236863563483[48] = 0; + out_4062857236863563483[49] = 0; + out_4062857236863563483[50] = 0; + out_4062857236863563483[51] = 0; + out_4062857236863563483[52] = 0; + out_4062857236863563483[53] = 0; } -void h_14(double *state, double *unused, double *out_8785755176393893631) { - out_8785755176393893631[0] = state[6]; - out_8785755176393893631[1] = state[7]; - out_8785755176393893631[2] = state[8]; +void h_14(double *state, double *unused, double *out_1617663395634793849) { + out_1617663395634793849[0] = state[6]; + out_1617663395634793849[1] = state[7]; + out_1617663395634793849[2] = state[8]; } -void H_14(double *state, double *unused, double *out_543769548678817005) { - out_543769548678817005[0] = 0; - out_543769548678817005[1] = 0; - out_543769548678817005[2] = 0; - out_543769548678817005[3] = 0; - out_543769548678817005[4] = 0; - out_543769548678817005[5] = 0; - out_543769548678817005[6] = 1; - out_543769548678817005[7] = 0; - out_543769548678817005[8] = 0; - out_543769548678817005[9] = 0; - out_543769548678817005[10] = 0; - out_543769548678817005[11] = 0; - out_543769548678817005[12] = 0; - out_543769548678817005[13] = 0; - out_543769548678817005[14] = 0; - out_543769548678817005[15] = 0; - out_543769548678817005[16] = 0; - out_543769548678817005[17] = 0; - out_543769548678817005[18] = 0; - out_543769548678817005[19] = 0; - out_543769548678817005[20] = 0; - out_543769548678817005[21] = 0; - out_543769548678817005[22] = 0; - out_543769548678817005[23] = 0; - out_543769548678817005[24] = 0; - out_543769548678817005[25] = 1; - out_543769548678817005[26] = 0; - out_543769548678817005[27] = 0; - out_543769548678817005[28] = 0; - out_543769548678817005[29] = 0; - out_543769548678817005[30] = 0; - out_543769548678817005[31] = 0; - out_543769548678817005[32] = 0; - out_543769548678817005[33] = 0; - out_543769548678817005[34] = 0; - out_543769548678817005[35] = 0; - out_543769548678817005[36] = 0; - out_543769548678817005[37] = 0; - out_543769548678817005[38] = 0; - out_543769548678817005[39] = 0; - out_543769548678817005[40] = 0; - out_543769548678817005[41] = 0; - out_543769548678817005[42] = 0; - out_543769548678817005[43] = 0; - out_543769548678817005[44] = 1; - out_543769548678817005[45] = 0; - out_543769548678817005[46] = 0; - out_543769548678817005[47] = 0; - out_543769548678817005[48] = 0; - out_543769548678817005[49] = 0; - out_543769548678817005[50] = 0; - out_543769548678817005[51] = 0; - out_543769548678817005[52] = 0; - out_543769548678817005[53] = 0; +void H_14(double *state, double *unused, double *out_4813824267870715211) { + out_4813824267870715211[0] = 0; + out_4813824267870715211[1] = 0; + out_4813824267870715211[2] = 0; + out_4813824267870715211[3] = 0; + out_4813824267870715211[4] = 0; + out_4813824267870715211[5] = 0; + out_4813824267870715211[6] = 1; + out_4813824267870715211[7] = 0; + out_4813824267870715211[8] = 0; + out_4813824267870715211[9] = 0; + out_4813824267870715211[10] = 0; + out_4813824267870715211[11] = 0; + out_4813824267870715211[12] = 0; + out_4813824267870715211[13] = 0; + out_4813824267870715211[14] = 0; + out_4813824267870715211[15] = 0; + out_4813824267870715211[16] = 0; + out_4813824267870715211[17] = 0; + out_4813824267870715211[18] = 0; + out_4813824267870715211[19] = 0; + out_4813824267870715211[20] = 0; + out_4813824267870715211[21] = 0; + out_4813824267870715211[22] = 0; + out_4813824267870715211[23] = 0; + out_4813824267870715211[24] = 0; + out_4813824267870715211[25] = 1; + out_4813824267870715211[26] = 0; + out_4813824267870715211[27] = 0; + out_4813824267870715211[28] = 0; + out_4813824267870715211[29] = 0; + out_4813824267870715211[30] = 0; + out_4813824267870715211[31] = 0; + out_4813824267870715211[32] = 0; + out_4813824267870715211[33] = 0; + out_4813824267870715211[34] = 0; + out_4813824267870715211[35] = 0; + out_4813824267870715211[36] = 0; + out_4813824267870715211[37] = 0; + out_4813824267870715211[38] = 0; + out_4813824267870715211[39] = 0; + out_4813824267870715211[40] = 0; + out_4813824267870715211[41] = 0; + out_4813824267870715211[42] = 0; + out_4813824267870715211[43] = 0; + out_4813824267870715211[44] = 1; + out_4813824267870715211[45] = 0; + out_4813824267870715211[46] = 0; + out_4813824267870715211[47] = 0; + out_4813824267870715211[48] = 0; + out_4813824267870715211[49] = 0; + out_4813824267870715211[50] = 0; + out_4813824267870715211[51] = 0; + out_4813824267870715211[52] = 0; + out_4813824267870715211[53] = 0; } #include #include @@ -1113,44 +1113,44 @@ void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, doub void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<3, 3, 0>(in_x, in_P, h_14, H_14, NULL, in_z, in_R, in_ea, MAHA_THRESH_14); } -void pose_err_fun(double *nom_x, double *delta_x, double *out_6497612872484174174) { - err_fun(nom_x, delta_x, out_6497612872484174174); +void pose_err_fun(double *nom_x, double *delta_x, double *out_812796242528121648) { + err_fun(nom_x, delta_x, out_812796242528121648); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1975580934350294204) { - inv_err_fun(nom_x, true_x, out_1975580934350294204); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2630998712382937796) { + inv_err_fun(nom_x, true_x, out_2630998712382937796); } -void pose_H_mod_fun(double *state, double *out_4617418789543565628) { - H_mod_fun(state, out_4617418789543565628); +void pose_H_mod_fun(double *state, double *out_8952125250503643232) { + H_mod_fun(state, out_8952125250503643232); } -void pose_f_fun(double *state, double dt, double *out_9094503671864348345) { - f_fun(state, dt, out_9094503671864348345); +void pose_f_fun(double *state, double dt, double *out_3519678704568830866) { + f_fun(state, dt, out_3519678704568830866); } -void pose_F_fun(double *state, double dt, double *out_1312959600226932432) { - F_fun(state, dt, out_1312959600226932432); +void pose_F_fun(double *state, double dt, double *out_6750093296288889328) { + F_fun(state, dt, out_6750093296288889328); } -void pose_h_4(double *state, double *unused, double *out_6400948089178504221) { - h_4(state, unused, out_6400948089178504221); +void pose_h_4(double *state, double *unused, double *out_1078897140345555822) { + h_4(state, unused, out_1078897140345555822); } -void pose_H_4(double *state, double *unused, double *out_3419471307660667524) { - H_4(state, unused, out_3419471307660667524); +void pose_H_4(double *state, double *unused, double *out_850583411531230682) { + H_4(state, unused, out_850583411531230682); } -void pose_h_10(double *state, double *unused, double *out_8540054287512262719) { - h_10(state, unused, out_8540054287512262719); +void pose_h_10(double *state, double *unused, double *out_3641708848879017741) { + h_10(state, unused, out_3641708848879017741); } -void pose_H_10(double *state, double *unused, double *out_5231659216258162963) { - H_10(state, unused, out_5231659216258162963); +void pose_H_10(double *state, double *unused, double *out_7494820718027871329) { + H_10(state, unused, out_7494820718027871329); } -void pose_h_13(double *state, double *unused, double *out_7953441679753433349) { - h_13(state, unused, out_7953441679753433349); +void pose_h_13(double *state, double *unused, double *out_515642654693887905) { + h_13(state, unused, out_515642654693887905); } -void pose_H_13(double *state, double *unused, double *out_207197482328334723) { - H_13(state, unused, out_207197482328334723); +void pose_H_13(double *state, double *unused, double *out_4062857236863563483) { + H_13(state, unused, out_4062857236863563483); } -void pose_h_14(double *state, double *unused, double *out_8785755176393893631) { - h_14(state, unused, out_8785755176393893631); +void pose_h_14(double *state, double *unused, double *out_1617663395634793849) { + h_14(state, unused, out_1617663395634793849); } -void pose_H_14(double *state, double *unused, double *out_543769548678817005) { - H_14(state, unused, out_543769548678817005); +void pose_H_14(double *state, double *unused, double *out_4813824267870715211) { + H_14(state, unused, out_4813824267870715211); } void pose_predict(double *in_x, double *in_P, double *in_Q, double dt) { predict(in_x, in_P, in_Q, dt); diff --git a/selfdrive/locationd/models/generated/pose.h b/selfdrive/locationd/models/generated/pose.h index e7632f322..556e33ecf 100644 --- a/selfdrive/locationd/models/generated/pose.h +++ b/selfdrive/locationd/models/generated/pose.h @@ -5,18 +5,18 @@ void pose_update_4(double *in_x, double *in_P, double *in_z, double *in_R, doubl void pose_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); -void pose_err_fun(double *nom_x, double *delta_x, double *out_6497612872484174174); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1975580934350294204); -void pose_H_mod_fun(double *state, double *out_4617418789543565628); -void pose_f_fun(double *state, double dt, double *out_9094503671864348345); -void pose_F_fun(double *state, double dt, double *out_1312959600226932432); -void pose_h_4(double *state, double *unused, double *out_6400948089178504221); -void pose_H_4(double *state, double *unused, double *out_3419471307660667524); -void pose_h_10(double *state, double *unused, double *out_8540054287512262719); -void pose_H_10(double *state, double *unused, double *out_5231659216258162963); -void pose_h_13(double *state, double *unused, double *out_7953441679753433349); -void pose_H_13(double *state, double *unused, double *out_207197482328334723); -void pose_h_14(double *state, double *unused, double *out_8785755176393893631); -void pose_H_14(double *state, double *unused, double *out_543769548678817005); +void pose_err_fun(double *nom_x, double *delta_x, double *out_812796242528121648); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2630998712382937796); +void pose_H_mod_fun(double *state, double *out_8952125250503643232); +void pose_f_fun(double *state, double dt, double *out_3519678704568830866); +void pose_F_fun(double *state, double dt, double *out_6750093296288889328); +void pose_h_4(double *state, double *unused, double *out_1078897140345555822); +void pose_H_4(double *state, double *unused, double *out_850583411531230682); +void pose_h_10(double *state, double *unused, double *out_3641708848879017741); +void pose_H_10(double *state, double *unused, double *out_7494820718027871329); +void pose_h_13(double *state, double *unused, double *out_515642654693887905); +void pose_H_13(double *state, double *unused, double *out_4062857236863563483); +void pose_h_14(double *state, double *unused, double *out_1617663395634793849); +void pose_H_14(double *state, double *unused, double *out_4813824267870715211); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/selfdrived/selfdrived.py b/selfdrive/selfdrived/selfdrived.py index 77365b91f..819c2bb23 100644 --- a/selfdrive/selfdrived/selfdrived.py +++ b/selfdrive/selfdrived/selfdrived.py @@ -125,6 +125,7 @@ class SelfdriveD: self.last_functional_fan_frame = 0 self.events_prev = [] self.logged_comm_issue = None + self.logged_locationd_issue = None self.not_running_prev = None self.experimental_mode = False self.safe_mode = self.params.get_bool("SafeMode") @@ -161,7 +162,6 @@ class SelfdriveD: self.cancel_pressed_previously = False self.distance_pressed_previously = False - self.display_timer = 0 self.last_below_steer_speed_alert_time = -float("inf") self.last_steer_saturated_alert_time = -float("inf") @@ -172,6 +172,22 @@ class SelfdriveD: self.FPCP = messaging.log_from_bytes(self.params.get("StarPilotCarParams", block=True), custom.StarPilotCarParams) + def _service_diagnostics(self, services: list[str]) -> dict[str, dict[str, object]]: + diagnostics: dict[str, dict[str, object]] = {} + for service in services: + if service not in self.sm.services: + continue + + recv_age_ms = round(max(0.0, (self.sm.frame - self.sm.recv_frame[service]) * DT_CTRL * 1000.0), 1) + diagnostics[service] = { + "valid": self.sm.valid[service], + "alive": self.sm.alive[service], + "freq_ok": self.sm.freq_ok[service], + "recv_age_ms": recv_age_ms, + } + + return diagnostics + def update_events(self, CS): """Compute onroadEvents from carState""" @@ -434,7 +450,22 @@ class SelfdriveD: 'not_freq_ok': [s for s, freq_ok in self.sm.freq_ok.items() if not freq_ok], } if logs != self.logged_comm_issue: - cloudlog.event("commIssue", error=True, **logs) + interesting_services = sorted(set( + logs['invalid'] + + logs['not_alive'] + + logs['not_freq_ok'] + + ['liveCalibration', 'livePose', 'liveParameters', 'radarState', 'driverAssistance', 'longitudinalPlan'] + )) + cloudlog.event( + "commIssue", + error=True, + **logs, + service_diagnostics=self._service_diagnostics(interesting_services), + nav_destination_active=self.params.get("NavDestination") is not None, + navigation_ui=self.params.get_bool("NavigationUI"), + can_timeout=CS.canTimeout, + can_valid=CS.canValid, + ) self.logged_comm_issue = logs else: self.logged_comm_issue = None @@ -444,6 +475,18 @@ class SelfdriveD: self.events.add(EventName.posenetInvalid) if not self.sm['livePose'].inputsOK: self.events.add(EventName.locationdTemporaryError) + locationd_issue = { + "inputs_ok": self.sm['livePose'].inputsOK, + "posenet_ok": self.sm['livePose'].posenetOK, + "live_pose_valid": self.sm.valid['livePose'], + "live_calibration_valid": self.sm.valid['liveCalibration'], + "live_parameters_valid": self.sm.valid['liveParameters'], + } + if locationd_issue != self.logged_locationd_issue: + cloudlog.event("locationdTemporaryError", error=True, **locationd_issue) + self.logged_locationd_issue = locationd_issue + else: + self.logged_locationd_issue = None if not self.sm['liveParameters'].valid and cal_status == log.LiveCalibrationData.Status.calibrated and not TESTING_CLOSET and (not SIMULATION or REPLAY): self.events.add(EventName.paramsdTemporaryError) diff --git a/selfdrive/ui/layouts/settings/starpilot/lateral.py b/selfdrive/ui/layouts/settings/starpilot/lateral.py index a86d5ad98..5c2b43f14 100644 --- a/selfdrive/ui/layouts/settings/starpilot/lateral.py +++ b/selfdrive/ui/layouts/settings/starpilot/lateral.py @@ -242,6 +242,11 @@ class StarPilotLateralLayout(_SettingsPage): get_state=lambda: self._params.get_bool("TurnDesires"), set_state=lambda s: self._params.put_bool("TurnDesires", s), visible=lambda: self._params.get_bool("LateralTune")), + SettingRow("NavDesiresAllowed", "toggle", tr_noop("Use Route Desires"), + subtitle=tr_noop("Allow an active navigation route to request keep-left, keep-right, and low-speed turn desires."), + get_state=lambda: self._params.get_bool("NavDesiresAllowed"), + set_state=lambda s: self._params.put_bool("NavDesiresAllowed", s), + visible=lambda: self._params.get_bool("LateralTune")), SettingRow("NNFF", "toggle", tr_noop("Neural Network Feedforward (NNFF)"), subtitle=tr_noop("Use the full neural-network feedforward steering controller when available."), get_state=lambda: self._params.get_bool("NNFF"), diff --git a/selfdrive/ui/mici/onroad/hud_renderer.py b/selfdrive/ui/mici/onroad/hud_renderer.py index 22d4c93f5..a2b37ab5f 100644 --- a/selfdrive/ui/mici/onroad/hud_renderer.py +++ b/selfdrive/ui/mici/onroad/hud_renderer.py @@ -3,6 +3,7 @@ from dataclasses import dataclass from openpilot.common.constants import CV from openpilot.selfdrive.ui.mici.onroad.torque_bar import TorqueBar from openpilot.selfdrive.ui.mici.onroad.speed_limit_utils import resolve_display_speed_limit_ms +from openpilot.selfdrive.ui.onroad.starpilot.navigation_card import NavigationCardRenderer from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus from openpilot.system.ui.lib.application import gui_app, FontWeight from openpilot.system.ui.lib.multilang import tr @@ -139,6 +140,7 @@ class HudRenderer(Widget): self._turn_intent = TurnIntent() self._torque_bar = TorqueBar() + self._navigation_card = NavigationCardRenderer("mici") self._txt_wheel: rl.Texture = gui_app.texture('icons_mici/wheel.png', 50, 50) self._txt_wheel_critical: rl.Texture = gui_app.texture('icons_mici/wheel_critical.png', 50, 50) @@ -254,6 +256,7 @@ class HudRenderer(Widget): if self.is_cruise_set: self._draw_set_speed(self._rect) + self._navigation_card.render(self._rect) self._draw_steering_wheel(self._rect) self._draw_speed_limit_prompt(self._rect) diff --git a/selfdrive/ui/onroad/hud_renderer.py b/selfdrive/ui/onroad/hud_renderer.py index 295053348..307bf9cec 100644 --- a/selfdrive/ui/onroad/hud_renderer.py +++ b/selfdrive/ui/onroad/hud_renderer.py @@ -2,6 +2,7 @@ import pyray as rl from dataclasses import dataclass from openpilot.common.constants import CV from openpilot.selfdrive.ui.onroad.exp_button import ExpButton +from openpilot.selfdrive.ui.onroad.starpilot.navigation_card import NavigationCardRenderer from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus from openpilot.system.ui.lib.application import gui_app, FontWeight from openpilot.system.ui.lib.multilang import tr @@ -71,6 +72,7 @@ class HudRenderer(Widget): self._font_medium: rl.Font = gui_app.font(FontWeight.MEDIUM) self._exp_button: ExpButton = ExpButton(UI_CONFIG.button_size, UI_CONFIG.wheel_icon_size) + self._navigation_card = NavigationCardRenderer() def _update_state(self) -> None: """Update HUD state based on car state and controls state.""" @@ -119,6 +121,8 @@ class HudRenderer(Widget): if not ui_state.starpilot_toggles.get("hide_speed", False): self._draw_current_speed(rect) + self._navigation_card.render(rect) + button_x = rect.x + rect.width - UI_CONFIG.border_size - UI_CONFIG.button_size button_y = rect.y + UI_CONFIG.border_size self._exp_button.render(rl.Rectangle(button_x, button_y, UI_CONFIG.button_size, UI_CONFIG.button_size)) diff --git a/selfdrive/ui/onroad/starpilot/navigation_card.py b/selfdrive/ui/onroad/starpilot/navigation_card.py new file mode 100644 index 000000000..e08814e2a --- /dev/null +++ b/selfdrive/ui/onroad/starpilot/navigation_card.py @@ -0,0 +1,384 @@ +from __future__ import annotations + +import math +from pathlib import Path + +import pyray as rl + +from openpilot.selfdrive.ui.ui_state import ui_state +from openpilot.system.ui.lib.application import FontWeight, gui_app +from openpilot.system.ui.lib.text_measure import measure_text_cached +from openpilot.system.ui.widgets import Widget + +ASSETS_PATH = Path(__file__).resolve().parents[4] / "starpilot" / "assets" / "navigation" +FALLBACK_ICON = "direction_turn_straight.png" + + +def _format_distance(distance_m: float, is_metric: bool) -> str: + if is_metric: + if distance_m < 1000: + step = 25 if distance_m < 500 else 50 + return f"{round(distance_m / step) * step} m" + distance_km = distance_m / 1000.0 + return f"{math.floor(distance_km)} km" if distance_km >= 10.0 else f"{distance_km:.1f} km" + + distance_ft = distance_m * 3.28084 + if distance_ft < 1000: + step = 10 if distance_ft <= 100 else 50 + return f"{round(distance_ft / step) * step} ft" + distance_mi = distance_ft / 5280.0 + return f"{math.floor(distance_mi)} mi" if distance_mi >= 10.0 else f"{distance_mi:.1f} mi" + + +def _modifier_suffix(modifier: str) -> str: + suffix_map = { + "left": "left", + "right": "right", + "straight": "straight", + "slightLeft": "slight_left", + "slightRight": "slight_right", + "sharpLeft": "sharp_left", + "sharpRight": "sharp_right", + } + return suffix_map.get(modifier, "") + + +def _normalize_maneuver_type(maneuver_type: str) -> str: + normalized = maneuver_type or "turn" + if normalized == "rotary": + normalized = "roundabout" + elif normalized in ("new name", "continue"): + normalized = "turn" + return normalized.replace(" ", "_") + + +class NavigationCardRenderer(Widget): + def __init__(self, layout_variant: str = "default"): + super().__init__() + self._font_bold = gui_app.font(FontWeight.BOLD) + self._font_medium = gui_app.font(FontWeight.MEDIUM) + self._icons: dict[str, rl.Texture2D] = {} + self._layout_variant = layout_variant + + self._enabled = False + self._valid = False + self._distance = "" + self._primary_text = "" + self._secondary_text = "" + self._maneuver_type = "turn" + self._modifier = "straight" + self._has_next = False + self._next_maneuver_type = "turn" + self._next_modifier = "straight" + + def _icon_filename(self, maneuver_type: str, modifier: str) -> str: + normalized_type = _normalize_maneuver_type(maneuver_type) + if modifier == "uturn": + return "direction_uturn.png" + + suffix = _modifier_suffix(modifier) + candidate = f"direction_{normalized_type}.png" if suffix == "" else f"direction_{normalized_type}_{suffix}.png" + return candidate if (ASSETS_PATH / candidate).exists() else FALLBACK_ICON + + def _get_icon(self, maneuver_type: str, modifier: str): + icon_name = self._icon_filename(maneuver_type, modifier) + if icon_name not in self._icons: + icon_path = ASSETS_PATH / icon_name + if not icon_path.exists(): + icon_path = ASSETS_PATH / FALLBACK_ICON + image = rl.load_image(str(icon_path)) + texture = rl.load_texture_from_image(image) + rl.set_texture_filter(texture, rl.TextureFilter.TEXTURE_FILTER_BILINEAR) + rl.set_texture_wrap(texture, rl.TextureWrap.TEXTURE_WRAP_CLAMP) + rl.unload_image(image) + self._icons[icon_name] = texture + return self._icons[icon_name] + + def _update_state(self) -> None: + sm = ui_state.sm + self._enabled = ui_state.params.get_bool("NavigationUI") + self._valid = False + + if not self._enabled: + return + if sm.recv_frame["navInstruction"] < ui_state.started_frame: + return + if not sm.valid.get("navInstruction", False): + return + + nav_instruction = sm["navInstruction"] + if nav_instruction.maneuverPrimaryText == "": + return + + self._distance = _format_distance(float(nav_instruction.maneuverDistance), ui_state.is_metric) + self._primary_text = nav_instruction.maneuverPrimaryText + self._secondary_text = nav_instruction.maneuverSecondaryText + self._maneuver_type = nav_instruction.maneuverType or "turn" + self._modifier = nav_instruction.maneuverModifier or "straight" + + maneuvers = nav_instruction.allManeuvers + if len(maneuvers) > 1: + self._has_next = True + self._next_maneuver_type = maneuvers[1].type or "turn" + self._next_modifier = maneuvers[1].modifier or "straight" + else: + self._has_next = False + + self._valid = True + + def _wrap_text_lines(self, text: str, max_width: float, font_size: int, max_lines: int = 2) -> tuple[list[str], bool]: + words = text.split() + if not words: + return [], False + + lines: list[str] = [] + current_words: list[str] = [] + word_index = 0 + + while word_index < len(words): + candidate_words = current_words + [words[word_index]] + candidate_line = " ".join(candidate_words) + text_width = measure_text_cached(self._font_bold, candidate_line, font_size).x + if text_width <= max_width or not current_words: + current_words = candidate_words + word_index += 1 + else: + lines.append(" ".join(current_words)) + current_words = [] + if len(lines) >= max_lines: + break + + if current_words and len(lines) < max_lines: + lines.append(" ".join(current_words)) + + truncated = word_index < len(words) + if truncated and lines: + shortened = lines[-1] + candidate = f"{shortened}…" + while shortened and measure_text_cached(self._font_bold, candidate, font_size).x > max_width: + if " " in shortened: + shortened = shortened.rsplit(" ", 1)[0] + else: + shortened = shortened[:-1] + candidate = f"{shortened}…" if shortened else "…" + lines[-1] = candidate + + return lines[:max_lines], truncated + + def _fit_title(self, text: str, max_width: float, preferred_size: int, minimum_size: int) -> tuple[list[str], int]: + font_size = preferred_size + while font_size > minimum_size: + lines, truncated = self._wrap_text_lines(text, max_width, font_size) + widest = max((measure_text_cached(self._font_bold, line, font_size).x for line in lines), default=0) + if not truncated and widest <= max_width: + return lines, font_size + font_size -= 2 + lines, _ = self._wrap_text_lines(text, max_width, minimum_size) + return lines, minimum_size + + def _truncate_text(self, text: str, max_width: float, font_size: int) -> str: + if measure_text_cached(self._font_medium, text, font_size).x <= max_width: + return text + + shortened = text.rstrip() + candidate = f"{shortened}…" + while shortened and measure_text_cached(self._font_medium, candidate, font_size).x > max_width: + shortened = shortened[:-1].rstrip() + candidate = f"{shortened}…" if shortened else "…" + return candidate + + def _draw_wrapped_lines(self, lines: list[str], x: float, y: float, font_size: int) -> None: + if not lines: + return + + text_y = y if len(lines) == 1 else y - (font_size * 0.35) + for index, line in enumerate(lines[:2]): + rl.draw_text_ex(self._font_bold, line, rl.Vector2(x, text_y + index * font_size), font_size, 0, rl.WHITE) + + def _draw_top_aligned_lines(self, lines: list[str], x: float, y: float, font_size: int, line_gap: int = 0) -> None: + for index, line in enumerate(lines[:2]): + line_y = y + index * (font_size + line_gap) + rl.draw_text_ex(self._font_bold, line, rl.Vector2(x, line_y), font_size, 0, rl.WHITE) + + def _render_default(self, rect: rl.Rectangle) -> None: + container_width = min(int(rect.width - 120), 1080) + container_width = max(container_width, 760) + container_height = 225 if rect.width >= 1200 else 195 + container_x = int(rect.x + (rect.width - container_width) / 2) + container_y = int(rect.y + 62) + border_radius = 42 if container_height == 225 else 34 + icon_size = 150 if container_height == 225 else 122 + icon_padding = 30 if container_height == 225 else 24 + then_section_width = 180 if container_height == 225 else 144 + then_icon_size = 105 if container_height == 225 else 82 + preferred_font_size = 75 if container_height == 225 else 58 + minimum_font_size = 60 if container_height == 225 else 46 + distance_font_size = 48 if container_height == 225 else 40 + + container = rl.Rectangle(container_x, container_y, container_width, container_height) + rl.draw_rectangle_rounded(container, border_radius / container_height, 10, rl.Color(0, 0, 0, 192)) + + icon_x = container_x + icon_padding + icon_y = container_y + (container_height - icon_size - 38) // 2 + icon = self._get_icon(self._maneuver_type, self._modifier) + dest_rect = rl.Rectangle(icon_x, icon_y, icon_size, icon_size) + rl.draw_texture_pro(icon, rl.Rectangle(0, 0, icon.width, icon.height), dest_rect, rl.Vector2(0, 0), 0, rl.WHITE) + + distance_size = measure_text_cached(self._font_bold, self._distance, distance_font_size) + distance_x = icon_x + (icon_size - distance_size.x) / 2 + distance_y = icon_y + icon_size + 5 + rl.draw_text_ex(self._font_bold, self._distance, rl.Vector2(distance_x, distance_y), distance_font_size, 0, rl.WHITE) + + show_next = self._has_next + text_x = icon_x + icon_size + 53 + right_gutter = icon_padding + (then_section_width if show_next else 0) + text_area_width = container_width - (text_x - container_x) - right_gutter + title_lines, title_font_size = self._fit_title(self._primary_text, text_area_width, preferred_font_size, minimum_font_size) + title_y = container_y + (container_height - title_font_size) / 2 + self._draw_wrapped_lines(title_lines, text_x, title_y, title_font_size) + + if self._secondary_text: + secondary_font_size = 38 if container_height == 225 else 30 + secondary_text = self._truncate_text(self._secondary_text, text_area_width, secondary_font_size) + secondary_y = container_y + container_height - (42 if container_height == 225 else 30) + rl.draw_text_ex( + self._font_medium, + secondary_text, + rl.Vector2(text_x, secondary_y), + secondary_font_size, + 0, + rl.Color(255, 255, 255, 180), + ) + + if not show_next: + return + + divider_x = container_x + container_width - then_section_width - 8 + rl.draw_line_ex( + rl.Vector2(divider_x, container_y + 23), + rl.Vector2(divider_x, container_y + container_height - 23), + 2, + rl.Color(255, 255, 255, 50), + ) + + then_x = divider_x + 15 + then_label = "Then" + then_font_size = 53 if container_height == 225 else 40 + then_size = measure_text_cached(self._font_medium, then_label, then_font_size) + then_label_x = then_x + (then_section_width - 23 - then_size.x) / 2 + rl.draw_text_ex(self._font_medium, then_label, rl.Vector2(then_label_x, container_y + 30), then_font_size, 0, rl.WHITE) + + next_icon = self._get_icon(self._next_maneuver_type, self._next_modifier) + then_icon_x = then_x + (then_section_width - 23 - then_icon_size) / 2 + then_icon_y = container_y + (84 if container_height == 225 else 72) + rl.draw_texture_pro( + next_icon, + rl.Rectangle(0, 0, next_icon.width, next_icon.height), + rl.Rectangle(then_icon_x, then_icon_y, then_icon_size, then_icon_size), + rl.Vector2(0, 0), + 0, + rl.WHITE, + ) + + def _render_mici(self, rect: rl.Rectangle) -> None: + left_safe = 96 + right_margin = 12 + available_width = max(260, int(rect.width - left_safe - right_margin)) + container_width = min(available_width, 420) + container_height = 142 + container_x = int(rect.x + left_safe) + container_y = int(rect.y + 16) + container = rl.Rectangle(container_x, container_y, container_width, container_height) + rl.draw_rectangle_rounded(container, 0.2, 10, rl.Color(7, 11, 18, 228)) + rl.draw_rectangle_rounded_lines_ex(container, 0.2, 10, 2, rl.Color(255, 255, 255, 34)) + + chip_size = 68 + chip_padding = 18 + chip_x = container_x + chip_padding + chip_y = container_y + 16 + chip_rect = rl.Rectangle(chip_x, chip_y, chip_size, chip_size) + rl.draw_rectangle_rounded(chip_rect, 0.34, 10, rl.Color(70, 80, 94, 228)) + + icon = self._get_icon(self._maneuver_type, self._modifier) + icon_size = 48 + icon_x = chip_x + (chip_size - icon_size) / 2 + icon_y = chip_y + (chip_size - icon_size) / 2 + rl.draw_texture_pro( + icon, + rl.Rectangle(0, 0, icon.width, icon.height), + rl.Rectangle(icon_x, icon_y, icon_size, icon_size), + rl.Vector2(0, 0), + 0, + rl.WHITE, + ) + + distance_font_size = 24 + distance_size = measure_text_cached(self._font_bold, self._distance, distance_font_size) + distance_x = chip_x + (chip_size - distance_size.x) / 2 + distance_y = container_y + container_height - 34 + rl.draw_text_ex(self._font_bold, self._distance, rl.Vector2(distance_x, distance_y), distance_font_size, 0, rl.WHITE) + + show_next = self._has_next and available_width >= 660 + next_section_width = 92 if show_next else 0 + text_x = chip_x + chip_size + 18 + text_width = container_width - (text_x - container_x) - 18 - next_section_width + title_lines, title_font_size = self._fit_title(self._primary_text, text_width, 32, 24) + title_top = container_y + (26 if len(title_lines) == 1 else 16) + self._draw_top_aligned_lines(title_lines, text_x, title_top, title_font_size, line_gap=2) + + show_secondary = bool(self._secondary_text) + if show_secondary: + secondary_font_size = 20 + secondary_text = self._truncate_text(self._secondary_text, text_width, secondary_font_size) + rl.draw_text_ex( + self._font_medium, + secondary_text, + rl.Vector2(text_x, container_y + container_height - 40), + secondary_font_size, + 0, + rl.Color(255, 255, 255, 172), + ) + + if not show_next: + return + + divider_x = container_x + container_width - next_section_width + rl.draw_line_ex( + rl.Vector2(divider_x, container_y + 16), + rl.Vector2(divider_x, container_y + container_height - 16), + 2, + rl.Color(255, 255, 255, 38), + ) + + then_label = "Then" + then_font_size = 20 + then_size = measure_text_cached(self._font_medium, then_label, then_font_size) + then_x = divider_x + (next_section_width - then_size.x) / 2 + rl.draw_text_ex(self._font_medium, then_label, rl.Vector2(then_x, container_y + 16), then_font_size, 0, rl.Color(255, 255, 255, 196)) + + next_icon = self._get_icon(self._next_maneuver_type, self._next_modifier) + next_chip_size = 54 + next_chip_x = divider_x + (next_section_width - next_chip_size) / 2 + next_chip_y = container_y + 46 + next_chip_rect = rl.Rectangle(next_chip_x, next_chip_y, next_chip_size, next_chip_size) + rl.draw_rectangle_rounded(next_chip_rect, 0.34, 10, rl.Color(70, 80, 94, 210)) + next_icon_size = 38 + next_icon_x = next_chip_x + (next_chip_size - next_icon_size) / 2 + next_icon_y = next_chip_y + (next_chip_size - next_icon_size) / 2 + rl.draw_texture_pro( + next_icon, + rl.Rectangle(0, 0, next_icon.width, next_icon.height), + rl.Rectangle(next_icon_x, next_icon_y, next_icon_size, next_icon_size), + rl.Vector2(0, 0), + 0, + rl.WHITE, + ) + + def _render(self, rect: rl.Rectangle) -> None: + self._update_state() + if not self._valid: + return + if self._layout_variant == "mici": + self._render_mici(rect) + else: + self._render_default(rect) diff --git a/selfdrive/ui/qt/onroad/hud.cc b/selfdrive/ui/qt/onroad/hud.cc index dbeabac55..76dcb6e6d 100644 --- a/selfdrive/ui/qt/onroad/hud.cc +++ b/selfdrive/ui/qt/onroad/hud.cc @@ -1,5 +1,7 @@ #include "selfdrive/ui/qt/onroad/hud.h" +#include +#include #include #include "selfdrive/ui/qt/util.h" @@ -40,6 +42,41 @@ void HudRenderer::updateState(const UIState &s) { v_ego_cluster_seen = v_ego_cluster_seen || car_state.getVEgoCluster() != 0.0; float v_ego = v_ego_cluster_seen && !starpilot_toggles.value("use_wheel_speed").toBool() ? car_state.getVEgoCluster() : car_state.getVEgo(); speed = std::max(0.0f, v_ego * (is_metric ? MS_TO_KPH : MS_TO_MPH)); + + navigation_enabled = params.getBool("NavigationUI"); + navigation_valid = false; + navigation_has_next = false; + nav_primary_text.clear(); + nav_secondary_text.clear(); + nav_distance.clear(); + nav_maneuver_type.clear(); + nav_modifier.clear(); + nav_next_maneuver_type.clear(); + nav_next_modifier.clear(); + + if (!navigation_enabled || sm.rcv_frame("navInstruction") < s.scene.started_frame || + !sm.alive("navInstruction") || !sm.valid("navInstruction")) { + return; + } + + const auto &nav = sm["navInstruction"].getNavInstruction(); + nav_primary_text = QString::fromUtf8(nav.getManeuverPrimaryText().cStr()).trimmed(); + if (nav_primary_text.isEmpty()) { + return; + } + + navigation_valid = true; + nav_secondary_text = QString::fromUtf8(nav.getManeuverSecondaryText().cStr()).trimmed(); + nav_distance = formatNavDistance(nav.getManeuverDistance()); + nav_maneuver_type = QString::fromUtf8(nav.getManeuverType().cStr()).trimmed(); + nav_modifier = QString::fromUtf8(nav.getManeuverModifier().cStr()).trimmed(); + + const auto maneuvers = nav.getAllManeuvers(); + if (maneuvers.size() > 1) { + navigation_has_next = true; + nav_next_maneuver_type = QString::fromUtf8(maneuvers[1].getType().cStr()).trimmed(); + nav_next_modifier = QString::fromUtf8(maneuvers[1].getModifier().cStr()).trimmed(); + } } void HudRenderer::draw(QPainter &p, const QRect &surface_rect) { @@ -61,6 +98,9 @@ void HudRenderer::draw(QPainter &p, const QRect &surface_rect) { if (starpilot_nvg->standstillDuration == 0 && !starpilot_toggles.value("hide_speed").toBool()) { drawCurrentSpeed(p, surface_rect); } + if (navigation_valid) { + drawNavigationCard(p, surface_rect); + } p.restore(); } @@ -134,3 +174,169 @@ void HudRenderer::drawText(QPainter &p, int x, int y, const QString &text, int a p.setPen(QColor(0xff, 0xff, 0xff, alpha)); p.drawText(real_rect.x(), real_rect.bottom(), text); } + +QString HudRenderer::formatNavDistance(float distance_m) const { + if (is_metric) { + if (distance_m < 1000.0f) { + int step = distance_m < 500.0f ? 25 : 50; + return QString("%1 m").arg(qRound(distance_m / step) * step); + } + float km = distance_m / 1000.0f; + return km >= 10.0f ? QString("%1 km").arg(static_cast(std::floor(km))) : QString("%1 km").arg(km, 0, 'f', 1); + } + + float distance_ft = distance_m * METER_TO_FOOT; + if (distance_ft < 1000.0f) { + int step = distance_ft <= 100.0f ? 10 : 50; + return QString("%1 ft").arg(qRound(distance_ft / step) * step); + } + + float miles = distance_ft / 5280.0f; + return miles >= 10.0f ? QString("%1 mi").arg(static_cast(std::floor(miles))) : QString("%1 mi").arg(miles, 0, 'f', 1); +} + +QString HudRenderer::navigationIconFilename(const QString &maneuver_type, const QString &modifier) const { + QString normalized_type = maneuver_type.isEmpty() ? "turn" : maneuver_type; + if (normalized_type == "rotary") { + normalized_type = "roundabout"; + } else if (normalized_type == "new name" || normalized_type == "continue") { + normalized_type = "turn"; + } + + if (modifier == "uturn") { + return "direction_uturn.png"; + } + + static const QHash suffixes = { + {"left", "left"}, + {"right", "right"}, + {"straight", "straight"}, + {"slightLeft", "slight_left"}, + {"slightRight", "slight_right"}, + {"sharpLeft", "sharp_left"}, + {"sharpRight", "sharp_right"}, + }; + + normalized_type.replace(" ", "_"); + const QString suffix = suffixes.value(modifier, ""); + const QString candidate = suffix.isEmpty() ? QString("direction_%1.png").arg(normalized_type) + : QString("direction_%1_%2.png").arg(normalized_type, suffix); + const QString candidate_path = QString("../../starpilot/assets/navigation/%1").arg(candidate); + return QFileInfo::exists(candidate_path) ? candidate : "direction_turn_straight.png"; +} + +QPixmap HudRenderer::getNavigationIcon(const QString &maneuver_type, const QString &modifier, int size) { + const QString icon_name = navigationIconFilename(maneuver_type, modifier); + const QString cache_key = QString("%1|%2").arg(icon_name).arg(size); + if (!nav_icon_cache.contains(cache_key)) { + nav_icon_cache.insert(cache_key, loadPixmap(QString("../../starpilot/assets/navigation/%1").arg(icon_name), QSize(size, size))); + } + return nav_icon_cache.value(cache_key); +} + +QStringList HudRenderer::wrapNavigationText(int max_width, int font_size, const QString &text) const { + QStringList words = text.split(' ', QString::SkipEmptyParts); + QStringList lines; + QString current_line; + const QFontMetrics metrics(InterFont(font_size, QFont::Bold)); + + for (const QString &word : words) { + QString test_line = current_line.isEmpty() ? word : current_line + " " + word; + if (metrics.horizontalAdvance(test_line) <= max_width) { + current_line = test_line; + } else { + if (!current_line.isEmpty()) { + lines.append(current_line); + } + current_line = word; + if (lines.size() >= 2) { + break; + } + } + } + + if (!current_line.isEmpty() && lines.size() < 2) { + lines.append(current_line); + } + return lines; +} + +void HudRenderer::drawNavigationCard(QPainter &p, const QRect &surface_rect) { + const int container_width = std::clamp(surface_rect.width() - 120, 760, 1080); + const int container_height = surface_rect.width() >= 1200 ? 238 : 206; + const int container_x = (surface_rect.width() - container_width) / 2; + const int container_y = surface_rect.width() >= 1200 ? 332 : 298; + const int border_radius = container_height == 238 ? 42 : 34; + const int icon_size = container_height == 238 ? 150 : 122; + const int icon_padding = container_height == 238 ? 30 : 24; + const int then_section_width = container_height == 238 ? 180 : 144; + const int then_icon_size = container_height == 238 ? 105 : 82; + const int title_font_size = container_height == 238 ? 75 : 58; + const int secondary_font_size = container_height == 238 ? 34 : 28; + const int distance_font_size = container_height == 238 ? 48 : 40; + const int title_top_padding = container_height == 238 ? 10 : 10; + const int title_line_spacing = container_height == 238 ? 68 : 56; + const int secondary_bottom_padding = container_height == 238 ? 24 : 20; + const int secondary_gap = container_height == 238 ? 10 : 8; + + QRect container(container_x, container_y, container_width, container_height); + p.setPen(Qt::NoPen); + p.setBrush(QColor(0, 0, 0, 180)); + p.drawRoundedRect(container, border_radius, border_radius); + + const int icon_x = container_x + icon_padding; + const int icon_y = container_y + title_top_padding + 4; + const QPixmap icon = getNavigationIcon(nav_maneuver_type, nav_modifier, icon_size); + p.drawPixmap(icon_x, icon_y, icon); + + p.setFont(InterFont(distance_font_size, QFont::Bold)); + p.setPen(Qt::white); + const int distance_width = p.fontMetrics().horizontalAdvance(nav_distance); + p.drawText(icon_x + (icon_size - distance_width) / 2, container_y + container_height - 18, nav_distance); + + const int text_x = icon_x + icon_size + 53; + const int right_gutter = icon_padding + (navigation_has_next ? then_section_width : 0); + const int text_width = container_width - (text_x - container_x) - right_gutter; + const QStringList title_lines = wrapNavigationText(text_width, title_font_size, nav_primary_text); + const QFont title_font = InterFont(title_font_size, QFont::Bold); + const QFontMetrics title_metrics(title_font); + const int title_baseline = container_y + title_top_padding + title_metrics.ascent(); + + p.setFont(title_font); + p.setPen(Qt::white); + for (int index = 0; index < title_lines.size() && index < 2; ++index) { + p.drawText(text_x, title_baseline + index * title_line_spacing, title_lines[index]); + } + + if (!nav_secondary_text.isEmpty()) { + const QFont secondary_font = InterFont(secondary_font_size, QFont::Medium); + const QFontMetrics secondary_metrics(secondary_font); + const int title_bottom = title_baseline + (std::max(title_lines.size(), 1) - 1) * title_line_spacing + title_metrics.descent(); + const int secondary_max_baseline = container_y + container_height - secondary_bottom_padding - secondary_metrics.descent(); + const int secondary_preferred_baseline = title_bottom + secondary_gap + secondary_metrics.ascent(); + const int secondary_baseline = std::min(secondary_preferred_baseline, secondary_max_baseline); + p.setFont(secondary_font); + p.setPen(QColor(255, 255, 255, 180)); + p.drawText(text_x, secondary_baseline, nav_secondary_text); + } + + if (!navigation_has_next) { + return; + } + + const int divider_x = container_x + container_width - then_section_width - 8; + p.setPen(QPen(QColor(255, 255, 255, 50), 2)); + p.drawLine(divider_x, container_y + 23, divider_x, container_y + container_height - 23); + + p.setFont(InterFont(container_height == 225 ? 53 : 40, QFont::Medium)); + p.setPen(Qt::white); + const QString then_label = tr("Then"); + const int then_x = divider_x + 15; + const int then_label_width = p.fontMetrics().horizontalAdvance(then_label); + p.drawText(then_x + (then_section_width - 23 - then_label_width) / 2, container_y + (container_height == 238 ? 66 : 60), then_label); + + const QPixmap next_icon = getNavigationIcon(nav_next_maneuver_type, nav_next_modifier, then_icon_size); + const int then_icon_x = then_x + (then_section_width - 23 - then_icon_size) / 2; + const int then_icon_y = container_y + (container_height == 238 ? 96 : 78); + p.drawPixmap(then_icon_x, then_icon_y, next_icon); +} diff --git a/selfdrive/ui/qt/onroad/hud.h b/selfdrive/ui/qt/onroad/hud.h index f7c6a465c..c5ea2539e 100644 --- a/selfdrive/ui/qt/onroad/hud.h +++ b/selfdrive/ui/qt/onroad/hud.h @@ -1,6 +1,8 @@ #pragma once +#include #include +#include #include "selfdrive/ui/ui.h" #include "starpilot/ui/qt/onroad/starpilot_annotated_camera.h" @@ -20,7 +22,12 @@ public: private: void drawSetSpeed(QPainter &p, const QRect &surface_rect); void drawCurrentSpeed(QPainter &p, const QRect &surface_rect); + void drawNavigationCard(QPainter &p, const QRect &surface_rect); void drawText(QPainter &p, int x, int y, const QString &text, int alpha = 255); + QStringList wrapNavigationText(int max_width, int font_size, const QString &text) const; + QString formatNavDistance(float distance_m) const; + QString navigationIconFilename(const QString &maneuver_type, const QString &modifier) const; + QPixmap getNavigationIcon(const QString &maneuver_type, const QString &modifier, int size); float speed = 0; float set_speed = 0; @@ -28,5 +35,17 @@ private: bool is_cruise_available = true; bool is_metric = false; bool v_ego_cluster_seen = false; + bool navigation_enabled = false; + bool navigation_valid = false; + bool navigation_has_next = false; int status = STATUS_DISENGAGED; + QString nav_distance; + QString nav_primary_text; + QString nav_secondary_text; + QString nav_maneuver_type; + QString nav_modifier; + QString nav_next_maneuver_type; + QString nav_next_modifier; + QHash nav_icon_cache; + Params params; }; diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 3279e77d5..d8d1b547e 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -367,6 +367,7 @@ UIState::UIState(QObject *parent) : QObject(parent) { "modelV2", "controlsState", "liveCalibration", "radarState", "deviceState", "pandaStates", "carParams", "driverMonitoringState", "carState", "driverStateV2", "wideRoadCameraState", "managerState", "selfdriveState", "longitudinalPlan", + "navInstruction", }); prime_state = new PrimeState(this); language = QString::fromStdString(Params().get("LanguageSetting")); diff --git a/selfdrive/ui/ui_state.py b/selfdrive/ui/ui_state.py index 220515bc0..e5512102f 100644 --- a/selfdrive/ui/ui_state.py +++ b/selfdrive/ui/ui_state.py @@ -54,6 +54,7 @@ class UIState: "longitudinalPlan", "gpsLocationExternal", "mapdOut", + "navInstruction", "carOutput", "carControl", "liveParameters", diff --git a/starpilot/assets/navigation/direction_arrive.png b/starpilot/assets/navigation/direction_arrive.png new file mode 100644 index 000000000..733c12909 Binary files /dev/null and b/starpilot/assets/navigation/direction_arrive.png differ diff --git a/starpilot/assets/navigation/direction_arrive_left.png b/starpilot/assets/navigation/direction_arrive_left.png new file mode 100644 index 000000000..92ff8e034 Binary files /dev/null and b/starpilot/assets/navigation/direction_arrive_left.png differ diff --git a/starpilot/assets/navigation/direction_arrive_right.png b/starpilot/assets/navigation/direction_arrive_right.png new file mode 100644 index 000000000..f5983bfe6 Binary files /dev/null and b/starpilot/assets/navigation/direction_arrive_right.png differ diff --git a/starpilot/assets/navigation/direction_arrive_straight.png b/starpilot/assets/navigation/direction_arrive_straight.png new file mode 100644 index 000000000..733c12909 Binary files /dev/null and b/starpilot/assets/navigation/direction_arrive_straight.png differ diff --git a/starpilot/assets/navigation/direction_close.png b/starpilot/assets/navigation/direction_close.png new file mode 100644 index 000000000..4fdb5d195 Binary files /dev/null and b/starpilot/assets/navigation/direction_close.png differ diff --git a/starpilot/assets/navigation/direction_continue.png b/starpilot/assets/navigation/direction_continue.png new file mode 100644 index 000000000..a01045ae6 Binary files /dev/null and b/starpilot/assets/navigation/direction_continue.png differ diff --git a/starpilot/assets/navigation/direction_continue_left.png b/starpilot/assets/navigation/direction_continue_left.png new file mode 100644 index 000000000..9a618026f Binary files /dev/null and b/starpilot/assets/navigation/direction_continue_left.png differ diff --git a/starpilot/assets/navigation/direction_continue_right.png b/starpilot/assets/navigation/direction_continue_right.png new file mode 100644 index 000000000..0fbaa3f25 Binary files /dev/null and b/starpilot/assets/navigation/direction_continue_right.png differ diff --git a/starpilot/assets/navigation/direction_continue_slight_left.png b/starpilot/assets/navigation/direction_continue_slight_left.png new file mode 100644 index 000000000..08e964dbd Binary files /dev/null and b/starpilot/assets/navigation/direction_continue_slight_left.png differ diff --git a/starpilot/assets/navigation/direction_continue_slight_right.png b/starpilot/assets/navigation/direction_continue_slight_right.png new file mode 100644 index 000000000..3e21cae11 Binary files /dev/null and b/starpilot/assets/navigation/direction_continue_slight_right.png differ diff --git a/starpilot/assets/navigation/direction_continue_straight.png b/starpilot/assets/navigation/direction_continue_straight.png new file mode 100644 index 000000000..a01045ae6 Binary files /dev/null and b/starpilot/assets/navigation/direction_continue_straight.png differ diff --git a/starpilot/assets/navigation/direction_continue_uturn.png b/starpilot/assets/navigation/direction_continue_uturn.png new file mode 100644 index 000000000..0bd1b9177 Binary files /dev/null and b/starpilot/assets/navigation/direction_continue_uturn.png differ diff --git a/starpilot/assets/navigation/direction_depart.png b/starpilot/assets/navigation/direction_depart.png new file mode 100644 index 000000000..4bf32c870 Binary files /dev/null and b/starpilot/assets/navigation/direction_depart.png differ diff --git a/starpilot/assets/navigation/direction_depart_left.png b/starpilot/assets/navigation/direction_depart_left.png new file mode 100644 index 000000000..1f8d72691 Binary files /dev/null and b/starpilot/assets/navigation/direction_depart_left.png differ diff --git a/starpilot/assets/navigation/direction_depart_right.png b/starpilot/assets/navigation/direction_depart_right.png new file mode 100644 index 000000000..f359a685f Binary files /dev/null and b/starpilot/assets/navigation/direction_depart_right.png differ diff --git a/starpilot/assets/navigation/direction_depart_straight.png b/starpilot/assets/navigation/direction_depart_straight.png new file mode 100644 index 000000000..4bf32c870 Binary files /dev/null and b/starpilot/assets/navigation/direction_depart_straight.png differ diff --git a/starpilot/assets/navigation/direction_end_of_road_left.png b/starpilot/assets/navigation/direction_end_of_road_left.png new file mode 100644 index 000000000..5c0a24e7c Binary files /dev/null and b/starpilot/assets/navigation/direction_end_of_road_left.png differ diff --git a/starpilot/assets/navigation/direction_end_of_road_right.png b/starpilot/assets/navigation/direction_end_of_road_right.png new file mode 100644 index 000000000..8d9b89d36 Binary files /dev/null and b/starpilot/assets/navigation/direction_end_of_road_right.png differ diff --git a/starpilot/assets/navigation/direction_flag.png b/starpilot/assets/navigation/direction_flag.png new file mode 100644 index 000000000..bad12ec66 Binary files /dev/null and b/starpilot/assets/navigation/direction_flag.png differ diff --git a/starpilot/assets/navigation/direction_fork.png b/starpilot/assets/navigation/direction_fork.png new file mode 100644 index 000000000..3e0c262e2 Binary files /dev/null and b/starpilot/assets/navigation/direction_fork.png differ diff --git a/starpilot/assets/navigation/direction_fork_left.png b/starpilot/assets/navigation/direction_fork_left.png new file mode 100644 index 000000000..b244b42b5 Binary files /dev/null and b/starpilot/assets/navigation/direction_fork_left.png differ diff --git a/starpilot/assets/navigation/direction_fork_right.png b/starpilot/assets/navigation/direction_fork_right.png new file mode 100644 index 000000000..aa3efaabc Binary files /dev/null and b/starpilot/assets/navigation/direction_fork_right.png differ diff --git a/starpilot/assets/navigation/direction_fork_slight_left.png b/starpilot/assets/navigation/direction_fork_slight_left.png new file mode 100644 index 000000000..82fa59859 Binary files /dev/null and b/starpilot/assets/navigation/direction_fork_slight_left.png differ diff --git a/starpilot/assets/navigation/direction_fork_slight_right.png b/starpilot/assets/navigation/direction_fork_slight_right.png new file mode 100644 index 000000000..3596a2fbf Binary files /dev/null and b/starpilot/assets/navigation/direction_fork_slight_right.png differ diff --git a/starpilot/assets/navigation/direction_fork_straight.png b/starpilot/assets/navigation/direction_fork_straight.png new file mode 100644 index 000000000..86f30ab9b Binary files /dev/null and b/starpilot/assets/navigation/direction_fork_straight.png differ diff --git a/starpilot/assets/navigation/direction_invalid.png b/starpilot/assets/navigation/direction_invalid.png new file mode 100644 index 000000000..a01045ae6 Binary files /dev/null and b/starpilot/assets/navigation/direction_invalid.png differ diff --git a/starpilot/assets/navigation/direction_invalid_left.png b/starpilot/assets/navigation/direction_invalid_left.png new file mode 100644 index 000000000..9a618026f Binary files /dev/null and b/starpilot/assets/navigation/direction_invalid_left.png differ diff --git a/starpilot/assets/navigation/direction_invalid_right.png b/starpilot/assets/navigation/direction_invalid_right.png new file mode 100644 index 000000000..0fbaa3f25 Binary files /dev/null and b/starpilot/assets/navigation/direction_invalid_right.png differ diff --git a/starpilot/assets/navigation/direction_invalid_slight_left.png b/starpilot/assets/navigation/direction_invalid_slight_left.png new file mode 100644 index 000000000..08e964dbd Binary files /dev/null and b/starpilot/assets/navigation/direction_invalid_slight_left.png differ diff --git a/starpilot/assets/navigation/direction_invalid_slight_right.png b/starpilot/assets/navigation/direction_invalid_slight_right.png new file mode 100644 index 000000000..3e21cae11 Binary files /dev/null and b/starpilot/assets/navigation/direction_invalid_slight_right.png differ diff --git a/starpilot/assets/navigation/direction_invalid_straight.png b/starpilot/assets/navigation/direction_invalid_straight.png new file mode 100644 index 000000000..a01045ae6 Binary files /dev/null and b/starpilot/assets/navigation/direction_invalid_straight.png differ diff --git a/starpilot/assets/navigation/direction_invalid_uturn.png b/starpilot/assets/navigation/direction_invalid_uturn.png new file mode 100644 index 000000000..0bd1b9177 Binary files /dev/null and b/starpilot/assets/navigation/direction_invalid_uturn.png differ diff --git a/starpilot/assets/navigation/direction_merge_left.png b/starpilot/assets/navigation/direction_merge_left.png new file mode 100644 index 000000000..a713f52c5 Binary files /dev/null and b/starpilot/assets/navigation/direction_merge_left.png differ diff --git a/starpilot/assets/navigation/direction_merge_right.png b/starpilot/assets/navigation/direction_merge_right.png new file mode 100644 index 000000000..3390b31a0 Binary files /dev/null and b/starpilot/assets/navigation/direction_merge_right.png differ diff --git a/starpilot/assets/navigation/direction_merge_slight_left.png b/starpilot/assets/navigation/direction_merge_slight_left.png new file mode 100644 index 000000000..308f97b5a Binary files /dev/null and b/starpilot/assets/navigation/direction_merge_slight_left.png differ diff --git a/starpilot/assets/navigation/direction_merge_slight_right.png b/starpilot/assets/navigation/direction_merge_slight_right.png new file mode 100644 index 000000000..8f5289011 Binary files /dev/null and b/starpilot/assets/navigation/direction_merge_slight_right.png differ diff --git a/starpilot/assets/navigation/direction_merge_straight.png b/starpilot/assets/navigation/direction_merge_straight.png new file mode 100644 index 000000000..49c464389 Binary files /dev/null and b/starpilot/assets/navigation/direction_merge_straight.png differ diff --git a/starpilot/assets/navigation/direction_new_name_left.png b/starpilot/assets/navigation/direction_new_name_left.png new file mode 100644 index 000000000..9a618026f Binary files /dev/null and b/starpilot/assets/navigation/direction_new_name_left.png differ diff --git a/starpilot/assets/navigation/direction_new_name_right.png b/starpilot/assets/navigation/direction_new_name_right.png new file mode 100644 index 000000000..0fbaa3f25 Binary files /dev/null and b/starpilot/assets/navigation/direction_new_name_right.png differ diff --git a/starpilot/assets/navigation/direction_new_name_sharp_left.png b/starpilot/assets/navigation/direction_new_name_sharp_left.png new file mode 100644 index 000000000..77106b493 Binary files /dev/null and b/starpilot/assets/navigation/direction_new_name_sharp_left.png differ diff --git a/starpilot/assets/navigation/direction_new_name_sharp_right.png b/starpilot/assets/navigation/direction_new_name_sharp_right.png new file mode 100644 index 000000000..eb3a02f8b Binary files /dev/null and b/starpilot/assets/navigation/direction_new_name_sharp_right.png differ diff --git a/starpilot/assets/navigation/direction_new_name_slight_left.png b/starpilot/assets/navigation/direction_new_name_slight_left.png new file mode 100644 index 000000000..08e964dbd Binary files /dev/null and b/starpilot/assets/navigation/direction_new_name_slight_left.png differ diff --git a/starpilot/assets/navigation/direction_new_name_slight_right.png b/starpilot/assets/navigation/direction_new_name_slight_right.png new file mode 100644 index 000000000..3e21cae11 Binary files /dev/null and b/starpilot/assets/navigation/direction_new_name_slight_right.png differ diff --git a/starpilot/assets/navigation/direction_new_name_straight.png b/starpilot/assets/navigation/direction_new_name_straight.png new file mode 100644 index 000000000..a01045ae6 Binary files /dev/null and b/starpilot/assets/navigation/direction_new_name_straight.png differ diff --git a/starpilot/assets/navigation/direction_notificaiton_right.png b/starpilot/assets/navigation/direction_notificaiton_right.png new file mode 100644 index 000000000..0fbaa3f25 Binary files /dev/null and b/starpilot/assets/navigation/direction_notificaiton_right.png differ diff --git a/starpilot/assets/navigation/direction_notificaiton_sharp_right.png b/starpilot/assets/navigation/direction_notificaiton_sharp_right.png new file mode 100644 index 000000000..a7e3c4cee Binary files /dev/null and b/starpilot/assets/navigation/direction_notificaiton_sharp_right.png differ diff --git a/starpilot/assets/navigation/direction_notification_left.png b/starpilot/assets/navigation/direction_notification_left.png new file mode 100644 index 000000000..9a618026f Binary files /dev/null and b/starpilot/assets/navigation/direction_notification_left.png differ diff --git a/starpilot/assets/navigation/direction_notification_right.png b/starpilot/assets/navigation/direction_notification_right.png new file mode 100644 index 000000000..0fbaa3f25 Binary files /dev/null and b/starpilot/assets/navigation/direction_notification_right.png differ diff --git a/starpilot/assets/navigation/direction_notification_sharp_left.png b/starpilot/assets/navigation/direction_notification_sharp_left.png new file mode 100644 index 000000000..dd8a4301d Binary files /dev/null and b/starpilot/assets/navigation/direction_notification_sharp_left.png differ diff --git a/starpilot/assets/navigation/direction_notification_slight_left.png b/starpilot/assets/navigation/direction_notification_slight_left.png new file mode 100644 index 000000000..08e964dbd Binary files /dev/null and b/starpilot/assets/navigation/direction_notification_slight_left.png differ diff --git a/starpilot/assets/navigation/direction_notification_slight_right.png b/starpilot/assets/navigation/direction_notification_slight_right.png new file mode 100644 index 000000000..3e21cae11 Binary files /dev/null and b/starpilot/assets/navigation/direction_notification_slight_right.png differ diff --git a/starpilot/assets/navigation/direction_notification_straight.png b/starpilot/assets/navigation/direction_notification_straight.png new file mode 100644 index 000000000..a01045ae6 Binary files /dev/null and b/starpilot/assets/navigation/direction_notification_straight.png differ diff --git a/starpilot/assets/navigation/direction_off_ramp_left.png b/starpilot/assets/navigation/direction_off_ramp_left.png new file mode 100644 index 000000000..d3fd18289 Binary files /dev/null and b/starpilot/assets/navigation/direction_off_ramp_left.png differ diff --git a/starpilot/assets/navigation/direction_off_ramp_right.png b/starpilot/assets/navigation/direction_off_ramp_right.png new file mode 100644 index 000000000..722e3f808 Binary files /dev/null and b/starpilot/assets/navigation/direction_off_ramp_right.png differ diff --git a/starpilot/assets/navigation/direction_off_ramp_slight_left.png b/starpilot/assets/navigation/direction_off_ramp_slight_left.png new file mode 100644 index 000000000..ddac4aad6 Binary files /dev/null and b/starpilot/assets/navigation/direction_off_ramp_slight_left.png differ diff --git a/starpilot/assets/navigation/direction_off_ramp_slight_right.png b/starpilot/assets/navigation/direction_off_ramp_slight_right.png new file mode 100644 index 000000000..ed5760886 Binary files /dev/null and b/starpilot/assets/navigation/direction_off_ramp_slight_right.png differ diff --git a/starpilot/assets/navigation/direction_on_ramp_left.png b/starpilot/assets/navigation/direction_on_ramp_left.png new file mode 100644 index 000000000..9a618026f Binary files /dev/null and b/starpilot/assets/navigation/direction_on_ramp_left.png differ diff --git a/starpilot/assets/navigation/direction_on_ramp_right.png b/starpilot/assets/navigation/direction_on_ramp_right.png new file mode 100644 index 000000000..0fbaa3f25 Binary files /dev/null and b/starpilot/assets/navigation/direction_on_ramp_right.png differ diff --git a/starpilot/assets/navigation/direction_on_ramp_sharp_left.png b/starpilot/assets/navigation/direction_on_ramp_sharp_left.png new file mode 100644 index 000000000..77106b493 Binary files /dev/null and b/starpilot/assets/navigation/direction_on_ramp_sharp_left.png differ diff --git a/starpilot/assets/navigation/direction_on_ramp_sharp_right.png b/starpilot/assets/navigation/direction_on_ramp_sharp_right.png new file mode 100644 index 000000000..a7e3c4cee Binary files /dev/null and b/starpilot/assets/navigation/direction_on_ramp_sharp_right.png differ diff --git a/starpilot/assets/navigation/direction_on_ramp_slight_left.png b/starpilot/assets/navigation/direction_on_ramp_slight_left.png new file mode 100644 index 000000000..a5ea8a881 Binary files /dev/null and b/starpilot/assets/navigation/direction_on_ramp_slight_left.png differ diff --git a/starpilot/assets/navigation/direction_on_ramp_slight_right.png b/starpilot/assets/navigation/direction_on_ramp_slight_right.png new file mode 100644 index 000000000..f8ea3800e Binary files /dev/null and b/starpilot/assets/navigation/direction_on_ramp_slight_right.png differ diff --git a/starpilot/assets/navigation/direction_on_ramp_straight.png b/starpilot/assets/navigation/direction_on_ramp_straight.png new file mode 100644 index 000000000..a01045ae6 Binary files /dev/null and b/starpilot/assets/navigation/direction_on_ramp_straight.png differ diff --git a/starpilot/assets/navigation/direction_rotary.png b/starpilot/assets/navigation/direction_rotary.png new file mode 100644 index 000000000..2a5d264bd Binary files /dev/null and b/starpilot/assets/navigation/direction_rotary.png differ diff --git a/starpilot/assets/navigation/direction_rotary_left.png b/starpilot/assets/navigation/direction_rotary_left.png new file mode 100644 index 000000000..0c4e4ab5e Binary files /dev/null and b/starpilot/assets/navigation/direction_rotary_left.png differ diff --git a/starpilot/assets/navigation/direction_rotary_right.png b/starpilot/assets/navigation/direction_rotary_right.png new file mode 100644 index 000000000..32a6b2504 Binary files /dev/null and b/starpilot/assets/navigation/direction_rotary_right.png differ diff --git a/starpilot/assets/navigation/direction_rotary_sharp_left.png b/starpilot/assets/navigation/direction_rotary_sharp_left.png new file mode 100644 index 000000000..c84a6d96c Binary files /dev/null and b/starpilot/assets/navigation/direction_rotary_sharp_left.png differ diff --git a/starpilot/assets/navigation/direction_rotary_sharp_right.png b/starpilot/assets/navigation/direction_rotary_sharp_right.png new file mode 100644 index 000000000..d15cbee00 Binary files /dev/null and b/starpilot/assets/navigation/direction_rotary_sharp_right.png differ diff --git a/starpilot/assets/navigation/direction_rotary_slight_left.png b/starpilot/assets/navigation/direction_rotary_slight_left.png new file mode 100644 index 000000000..3838e720a Binary files /dev/null and b/starpilot/assets/navigation/direction_rotary_slight_left.png differ diff --git a/starpilot/assets/navigation/direction_rotary_slight_right.png b/starpilot/assets/navigation/direction_rotary_slight_right.png new file mode 100644 index 000000000..8cd45fe61 Binary files /dev/null and b/starpilot/assets/navigation/direction_rotary_slight_right.png differ diff --git a/starpilot/assets/navigation/direction_rotary_straight.png b/starpilot/assets/navigation/direction_rotary_straight.png new file mode 100644 index 000000000..b6b0a7311 Binary files /dev/null and b/starpilot/assets/navigation/direction_rotary_straight.png differ diff --git a/starpilot/assets/navigation/direction_roundabout.png b/starpilot/assets/navigation/direction_roundabout.png new file mode 100644 index 000000000..2a5d264bd Binary files /dev/null and b/starpilot/assets/navigation/direction_roundabout.png differ diff --git a/starpilot/assets/navigation/direction_roundabout_left.png b/starpilot/assets/navigation/direction_roundabout_left.png new file mode 100644 index 000000000..0c4e4ab5e Binary files /dev/null and b/starpilot/assets/navigation/direction_roundabout_left.png differ diff --git a/starpilot/assets/navigation/direction_roundabout_right.png b/starpilot/assets/navigation/direction_roundabout_right.png new file mode 100644 index 000000000..32a6b2504 Binary files /dev/null and b/starpilot/assets/navigation/direction_roundabout_right.png differ diff --git a/starpilot/assets/navigation/direction_roundabout_sharp_left.png b/starpilot/assets/navigation/direction_roundabout_sharp_left.png new file mode 100644 index 000000000..1e8cce8c8 Binary files /dev/null and b/starpilot/assets/navigation/direction_roundabout_sharp_left.png differ diff --git a/starpilot/assets/navigation/direction_roundabout_sharp_right.png b/starpilot/assets/navigation/direction_roundabout_sharp_right.png new file mode 100644 index 000000000..d15cbee00 Binary files /dev/null and b/starpilot/assets/navigation/direction_roundabout_sharp_right.png differ diff --git a/starpilot/assets/navigation/direction_roundabout_slight_left.png b/starpilot/assets/navigation/direction_roundabout_slight_left.png new file mode 100644 index 000000000..da1b11270 Binary files /dev/null and b/starpilot/assets/navigation/direction_roundabout_slight_left.png differ diff --git a/starpilot/assets/navigation/direction_roundabout_slight_right.png b/starpilot/assets/navigation/direction_roundabout_slight_right.png new file mode 100644 index 000000000..8cd45fe61 Binary files /dev/null and b/starpilot/assets/navigation/direction_roundabout_slight_right.png differ diff --git a/starpilot/assets/navigation/direction_roundabout_straight.png b/starpilot/assets/navigation/direction_roundabout_straight.png new file mode 100644 index 000000000..b6b0a7311 Binary files /dev/null and b/starpilot/assets/navigation/direction_roundabout_straight.png differ diff --git a/starpilot/assets/navigation/direction_turn_left.png b/starpilot/assets/navigation/direction_turn_left.png new file mode 100644 index 000000000..9a618026f Binary files /dev/null and b/starpilot/assets/navigation/direction_turn_left.png differ diff --git a/starpilot/assets/navigation/direction_turn_right.png b/starpilot/assets/navigation/direction_turn_right.png new file mode 100644 index 000000000..0fbaa3f25 Binary files /dev/null and b/starpilot/assets/navigation/direction_turn_right.png differ diff --git a/starpilot/assets/navigation/direction_turn_sharp_left.png b/starpilot/assets/navigation/direction_turn_sharp_left.png new file mode 100644 index 000000000..dd8a4301d Binary files /dev/null and b/starpilot/assets/navigation/direction_turn_sharp_left.png differ diff --git a/starpilot/assets/navigation/direction_turn_sharp_right.png b/starpilot/assets/navigation/direction_turn_sharp_right.png new file mode 100644 index 000000000..a7e3c4cee Binary files /dev/null and b/starpilot/assets/navigation/direction_turn_sharp_right.png differ diff --git a/starpilot/assets/navigation/direction_turn_slight_left.png b/starpilot/assets/navigation/direction_turn_slight_left.png new file mode 100644 index 000000000..08e964dbd Binary files /dev/null and b/starpilot/assets/navigation/direction_turn_slight_left.png differ diff --git a/starpilot/assets/navigation/direction_turn_slight_right.png b/starpilot/assets/navigation/direction_turn_slight_right.png new file mode 100644 index 000000000..3e21cae11 Binary files /dev/null and b/starpilot/assets/navigation/direction_turn_slight_right.png differ diff --git a/starpilot/assets/navigation/direction_turn_straight.png b/starpilot/assets/navigation/direction_turn_straight.png new file mode 100644 index 000000000..a01045ae6 Binary files /dev/null and b/starpilot/assets/navigation/direction_turn_straight.png differ diff --git a/starpilot/assets/navigation/direction_updown.png b/starpilot/assets/navigation/direction_updown.png new file mode 100644 index 000000000..16d0979f3 Binary files /dev/null and b/starpilot/assets/navigation/direction_updown.png differ diff --git a/starpilot/assets/navigation/direction_uturn.png b/starpilot/assets/navigation/direction_uturn.png new file mode 100644 index 000000000..0bd1b9177 Binary files /dev/null and b/starpilot/assets/navigation/direction_uturn.png differ diff --git a/starpilot/common/safe_mode.py b/starpilot/common/safe_mode.py index 86355cdbd..6b9a502cc 100644 --- a/starpilot/common/safe_mode.py +++ b/starpilot/common/safe_mode.py @@ -41,6 +41,7 @@ SAFE_MODE_MANAGED_KEYS = ( "NNFF", "NNFFLite", "TurnDesires", + "NavDesiresAllowed", "QOLLateral", "PauseLateralSpeed", "PauseLateralOnSignal", diff --git a/starpilot/navigation/destination_store.py b/starpilot/navigation/destination_store.py new file mode 100644 index 000000000..8393d28a1 --- /dev/null +++ b/starpilot/navigation/destination_store.py @@ -0,0 +1,104 @@ +from __future__ import annotations + +import json +from typing import Any + +RECENT_DESTINATIONS_LIMIT = 10 + + +def _coerce_float(value: Any) -> float | None: + try: + return float(value) + except (TypeError, ValueError): + return None + + +def normalize_destination_payload(payload: Any) -> dict[str, Any] | None: + if not isinstance(payload, dict): + return None + + name = str(payload.get("place_name") or payload.get("name") or "").strip() + latitude = _coerce_float(payload.get("latitude")) + longitude = _coerce_float(payload.get("longitude")) + + if not name or latitude is None or longitude is None: + return None + + return { + "name": name, + "place_name": name, + "latitude": latitude, + "longitude": longitude, + } + + +def parse_destination_json(raw_value: str | bytes | None) -> dict[str, Any] | None: + if not raw_value: + return None + + try: + payload = json.loads(raw_value) + except (TypeError, ValueError, json.JSONDecodeError): + return None + + return normalize_destination_payload(payload) + + +def normalize_recent_destination_entry(entry: Any) -> dict[str, Any] | None: + if not isinstance(entry, dict): + return None + + place_name = str(entry.get("place_name") or entry.get("name") or "").strip() + if not place_name: + return None + + normalized: dict[str, Any] = {"place_name": place_name} + latitude = _coerce_float(entry.get("latitude")) + longitude = _coerce_float(entry.get("longitude")) + if latitude is not None and longitude is not None: + normalized["latitude"] = latitude + normalized["longitude"] = longitude + normalized["name"] = place_name + + return normalized + + +def load_recent_destinations(raw_value: str | bytes | None) -> list[dict[str, Any]]: + if not raw_value: + return [] + + try: + payload = json.loads(raw_value) + except (TypeError, ValueError, json.JSONDecodeError): + return [] + + if not isinstance(payload, list): + return [] + + normalized: list[dict[str, Any]] = [] + for entry in payload: + recent = normalize_recent_destination_entry(entry) + if recent is not None: + normalized.append(recent) + return normalized + + +def update_recent_destinations(raw_value: str | bytes | None, destination: dict[str, Any], limit: int = RECENT_DESTINATIONS_LIMIT) -> list[dict[str, Any]]: + normalized_destination = normalize_destination_payload(destination) + if normalized_destination is None: + return load_recent_destinations(raw_value)[:limit] + + current = load_recent_destinations(raw_value) + updated = [normalize_recent_destination_entry(normalized_destination)] + seen = {normalized_destination["place_name"].casefold()} + + for entry in current: + place_name = entry["place_name"].casefold() + if place_name in seen: + continue + seen.add(place_name) + updated.append(entry) + if len(updated) >= limit: + break + + return [entry for entry in updated if entry is not None] diff --git a/starpilot/navigation/navigationd.py b/starpilot/navigation/navigationd.py new file mode 100644 index 000000000..955960de1 --- /dev/null +++ b/starpilot/navigation/navigationd.py @@ -0,0 +1,306 @@ +#!/usr/bin/env python3 +from __future__ import annotations +import threading +from math import isfinite +from time import monotonic + +import cereal.messaging as messaging +from openpilot.common.gps import get_gps_location_service +from openpilot.common.params import Params +from openpilot.common.realtime import Ratekeeper +from openpilot.common.swaglog import cloudlog + +from openpilot.starpilot.navigation.destination_store import parse_destination_json +from openpilot.starpilot.navigation.route_engine import Coordinate, MapboxRouteEngine, NavigationRoute, RouteProgress + +NAVIGATIOND_HZ = 1 +REROUTE_TRIGGER_SECONDS = 3.0 +ARRIVAL_CLEAR_SECONDS = 5.0 + + +class Navigationd: + def __init__(self, route_engine: MapboxRouteEngine | None = None): + self.params = Params() + self.params_memory = Params(memory=True) + self.route_engine = route_engine or MapboxRouteEngine() + self.gps_location_service = get_gps_location_service(self.params) + + self.sm = messaging.SubMaster([self.gps_location_service, "carState"]) + self.pm = messaging.PubMaster(["navInstruction", "navRoute"]) + self.rk = Ratekeeper(NAVIGATIOND_HZ) + + self._route_lock = threading.Lock() + self._route: NavigationRoute | None = None + self._active_destination: dict[str, object] | None = None + self._requested_destination_key: tuple[str, float, float] | None = None + self._route_fetch_inflight = False + self._route_generation = 0 + self._published_route_generation = -1 + + self._last_position: Coordinate | None = None + self._last_bearing: float | None = None + self._off_route_started_at: float | None = None + self._bearing_misaligned_started_at: float | None = None + self._arrival_started_at: float | None = None + self._last_nav_state: dict[str, object] | None = None + + @staticmethod + def _destination_key(destination: dict[str, object] | None) -> tuple[str, float, float] | None: + if destination is None: + return None + return ( + str(destination["place_name"]).casefold(), + round(float(destination["latitude"]), 6), + round(float(destination["longitude"]), 6), + ) + + def _snapshot_route(self) -> tuple[NavigationRoute | None, dict[str, object] | None, int]: + with self._route_lock: + return self._route, self._active_destination, self._route_generation + + def _route_token(self) -> str: + return self.params.get("MapboxSecretKey", encoding="utf-8") or "" + + def _clear_route(self, *, remove_destination: bool = False) -> None: + with self._route_lock: + self._route = None + self._active_destination = None + self._requested_destination_key = None + self._route_generation += 1 + + self._off_route_started_at = None + self._bearing_misaligned_started_at = None + self._arrival_started_at = None + self._publish_nav_state(None, None, False) + + if remove_destination: + self.params.remove("NavDestination") + + def _start_route_fetch(self, destination: dict[str, object]) -> None: + if self._last_position is None or self._route_fetch_inflight: + return + + token = self._route_token() + if not token: + return + + position = self._last_position + bearing = self._last_bearing + destination_key = self._destination_key(destination) + if destination_key is None: + return + + with self._route_lock: + self._route_fetch_inflight = True + self._requested_destination_key = destination_key + + def worker(): + route = self.route_engine.fetch_route(token, position, destination, bearing) + with self._route_lock: + still_current = self._requested_destination_key == destination_key + self._route_fetch_inflight = False + if not still_current: + return + if route is None: + return + + self._route = route + self._active_destination = destination + self._route_generation += 1 + + self._off_route_started_at = None + self._bearing_misaligned_started_at = None + self._arrival_started_at = None + + threading.Thread(target=worker, daemon=True).start() + + @staticmethod + def _timer_expired(started_at: float | None, threshold: float, now: float) -> bool: + return started_at is not None and (now - started_at) >= threshold + + @staticmethod + def _bump_timer(started_at: float | None, condition: bool, now: float) -> float | None: + if not condition: + return None + return started_at if started_at is not None else now + + def _update_location(self) -> bool: + if self.sm.recv_frame[self.gps_location_service] <= 0: + return False + + gps = self.sm[self.gps_location_service] + if gps is None or not gps.hasFix: + return False + + latitude = float(gps.latitude) + longitude = float(gps.longitude) + if not isfinite(latitude) or not isfinite(longitude) or (abs(latitude) < 1e-6 and abs(longitude) < 1e-6): + return False + + self._last_position = Coordinate(latitude, longitude) + + bearing = float(gps.bearingDeg) + self._last_bearing = bearing if isfinite(bearing) else None + return True + + def _maybe_update_route(self, current_destination: dict[str, object] | None) -> tuple[NavigationRoute | None, dict[str, object] | None, int]: + route, active_destination, route_generation = self._snapshot_route() + + if current_destination is None: + if route is not None or active_destination is not None: + self._clear_route() + route, active_destination, route_generation = self._snapshot_route() + return route, active_destination, route_generation + + if route is None or self._destination_key(current_destination) != self._destination_key(active_destination): + self._start_route_fetch(current_destination) + + return self._snapshot_route() + + def _build_progress(self, route: NavigationRoute | None, location_valid: bool, v_ego: float) -> tuple[RouteProgress | None, dict[str, object] | None]: + if route is None or not location_valid or self._last_position is None: + self._off_route_started_at = None + self._bearing_misaligned_started_at = None + self._arrival_started_at = None + return None, None + + progress = route.get_progress(self._last_position) + if progress is None: + return None, None + + now = monotonic() + off_route = route.off_route_distance_exceeded(progress, v_ego) + misaligned = route.route_bearing_misaligned(progress.closest_index, self._last_bearing, v_ego) + arrived = route.arrived(progress, v_ego) + + self._off_route_started_at = self._bump_timer(self._off_route_started_at, off_route and not arrived, now) + self._bearing_misaligned_started_at = self._bump_timer(self._bearing_misaligned_started_at, misaligned and not arrived, now) + self._arrival_started_at = self._bump_timer(self._arrival_started_at, arrived, now) + + if not off_route: + self._off_route_started_at = None + if not misaligned: + self._bearing_misaligned_started_at = None + if not arrived: + self._arrival_started_at = None + + return progress, { + "offRoute": off_route, + "misaligned": misaligned, + "arrived": arrived, + "now": now, + } + + def _maybe_recompute(self, route: NavigationRoute | None, destination: dict[str, object] | None, progress: RouteProgress | None, route_state: dict[str, object] | None) -> None: + if route is None or destination is None or progress is None or route_state is None: + return + + if progress.current_step_index >= len(route.steps) - 1: + return + + now = float(route_state["now"]) + should_reroute = self._timer_expired(self._off_route_started_at, REROUTE_TRIGGER_SECONDS, now) + should_reroute |= self._timer_expired(self._bearing_misaligned_started_at, REROUTE_TRIGGER_SECONDS, now) + if should_reroute: + self._start_route_fetch(destination) + + if self._timer_expired(self._arrival_started_at, ARRIVAL_CLEAR_SECONDS, now): + self._clear_route(remove_destination=True) + + def _publish_nav_instruction(self, route: NavigationRoute | None, progress: RouteProgress | None, location_valid: bool) -> None: + msg = messaging.new_message("navInstruction") + msg.valid = bool(route is not None and progress is not None and location_valid) + + if msg.valid and route is not None and progress is not None and self._last_position is not None: + payload = route.build_instruction_payload( + progress, + use_vienna_sign=self.params.get_bool("UseVienna"), + ) + nav_instruction = msg.navInstruction + nav_instruction.maneuverPrimaryText = payload["maneuverPrimaryText"] + nav_instruction.maneuverSecondaryText = payload["maneuverSecondaryText"] + nav_instruction.maneuverDistance = float(payload["maneuverDistance"]) + nav_instruction.maneuverType = str(payload["maneuverType"]) + nav_instruction.maneuverModifier = str(payload["maneuverModifier"]) + nav_instruction.distanceRemaining = float(payload["distanceRemaining"]) + nav_instruction.timeRemaining = float(payload["timeRemaining"]) + nav_instruction.timeRemainingTypical = float(payload["timeRemainingTypical"]) + nav_instruction.lanes = payload["lanes"] + nav_instruction.showFull = bool(payload["showFull"]) + nav_instruction.speedLimit = float(payload["speedLimit"]) + nav_instruction.speedLimitSign = payload["speedLimitSign"] + nav_instruction.allManeuvers = payload["allManeuvers"] + + self.pm.send("navInstruction", msg) + + def _publish_nav_state(self, route: NavigationRoute | None, progress: RouteProgress | None, location_valid: bool) -> None: + if route is None or progress is None or not location_valid: + if self._last_nav_state is not None: + self.params_memory.remove("NavInstructionState") + self._last_nav_state = None + return + + modifier = progress.current_step.modifier or "" + if progress.current_step.banner_instructions: + try: + payload = route.build_instruction_payload(progress, use_vienna_sign=self.params.get_bool("UseVienna")) + modifier = str(payload.get("maneuverModifier") or modifier) + except Exception: + pass + + state = { + "valid": True, + "maneuverModifier": modifier, + } + if state != self._last_nav_state: + self.params_memory.put_nonblocking("NavInstructionState", state) + self._last_nav_state = state + + def _publish_nav_route_if_needed(self) -> None: + route, _, route_generation = self._snapshot_route() + if route_generation == self._published_route_generation: + return + + msg = messaging.new_message("navRoute") + msg.valid = route is not None + if route is not None: + msg.navRoute.coordinates = [ + {"latitude": coordinate.latitude, "longitude": coordinate.longitude} + for coordinate in route.geometry + ] + + self.pm.send("navRoute", msg) + self._published_route_generation = route_generation + + def run(self) -> None: + cloudlog.warning("navigationd init") + + while True: + self.sm.update(0) + + location_valid = self._update_location() + current_destination = parse_destination_json(self.params.get("NavDestination", encoding="utf-8")) + route, active_destination, _ = self._maybe_update_route(current_destination) + + v_ego = float(max(self.sm["carState"].vEgo, 0.0)) + progress, route_state = self._build_progress(route, location_valid, v_ego) + self._maybe_recompute(route, current_destination or active_destination, progress, route_state) + updated_route, active_destination, _ = self._snapshot_route() + if updated_route is not route and updated_route is not None and location_valid and self._last_position is not None: + progress = updated_route.get_progress(self._last_position) + route = updated_route + if route is None: + progress = None + + self._publish_nav_instruction(route, progress, location_valid) + self._publish_nav_state(route, progress, location_valid) + self._publish_nav_route_if_needed() + self.rk.keep_time() + + +def main() -> None: + Navigationd().run() + + +if __name__ == "__main__": + main() diff --git a/starpilot/navigation/route_engine.py b/starpilot/navigation/route_engine.py new file mode 100644 index 000000000..71e905414 --- /dev/null +++ b/starpilot/navigation/route_engine.py @@ -0,0 +1,434 @@ +from __future__ import annotations + +import math +from dataclasses import dataclass +from typing import Any + +import numpy as np +import requests + +from cereal import log +from openpilot.common.constants import CV + +DIRECTIONS = ("left", "right", "straight") +MODIFIABLE_DIRECTIONS = ("left", "right") + +EARTH_MEAN_RADIUS = 6371007.2 +SPEED_CONVERSIONS = { + "km/h": CV.KPH_TO_MS, + "mph": CV.MPH_TO_MS, +} + +OFF_ROUTE_SPEED_BREAKPOINTS = [0.0, 5.0, 10.0, 20.0, 40.0] +OFF_ROUTE_DISTANCE_BREAKPOINTS = [100.0, 125.0, 150.0, 200.0, 250.0] +UPCOMING_TURN_SPEED_BREAKPOINTS = [0.0, 5.0, 10.0, 15.0, 20.0, 25.0, 30.0, 35.0, 40.0] +UPCOMING_TURN_DISTANCE_BREAKPOINTS = [20.0, 25.0, 30.0, 45.0, 60.0, 75.0, 90.0, 105.0, 120.0] +UTURN_MODIFIER = "uturn" + +LANE_DIRECTIONS = frozenset({ + "none", + "left", + "right", + "straight", + "slightLeft", + "slightRight", +}) + + +@dataclass(frozen=True) +class Coordinate: + latitude: float + longitude: float + + def __sub__(self, other: "Coordinate") -> "Coordinate": + return Coordinate(self.latitude - other.latitude, self.longitude - other.longitude) + + def __add__(self, other: "Coordinate") -> "Coordinate": + return Coordinate(self.latitude + other.latitude, self.longitude + other.longitude) + + def __mul__(self, scale: float) -> "Coordinate": + return Coordinate(self.latitude * scale, self.longitude * scale) + + def dot(self, other: "Coordinate") -> float: + return self.latitude * other.latitude + self.longitude * other.longitude + + def distance_to(self, other: "Coordinate") -> float: + dlat = math.radians(other.latitude - self.latitude) + dlon = math.radians(other.longitude - self.longitude) + + haversine_dlat = math.sin(dlat / 2.0) ** 2 + haversine_dlon = math.sin(dlon / 2.0) ** 2 + + a = haversine_dlat + math.cos(math.radians(self.latitude)) * math.cos(math.radians(other.latitude)) * haversine_dlon + return 2.0 * math.asin(math.sqrt(a)) * EARTH_MEAN_RADIUS + + +@dataclass(frozen=True) +class RouteStep: + banner_instructions: list[dict[str, Any]] + distance: float + duration: float + maneuver: str + location: Coordinate + cumulative_distance: float + maxspeed_ms: float + modifier: str + instruction: str + + +@dataclass(frozen=True) +class RouteProgress: + closest_index: int + distance_from_route: float + current_step: RouteStep + next_step: RouteStep | None + current_step_index: int + distance_to_end_of_step: float + distance_remaining: float + time_remaining: float + current_speed_limit_ms: float + all_maneuvers: list[dict[str, Any]] + + +def bearing_between_two_points(point_one: Coordinate, point_two: Coordinate) -> float: + lat_one = math.radians(point_one.latitude) + lat_two = math.radians(point_two.latitude) + dlon = math.radians(point_two.longitude - point_one.longitude) + + bearing_radians = math.atan2( + math.sin(dlon) * math.cos(lat_two), + math.cos(lat_one) * math.sin(lat_two) - math.sin(lat_one) * math.cos(lat_two) * math.cos(dlon), + ) + return (math.degrees(bearing_radians) + 360.0) % 360.0 + + +def minimum_distance(a: Coordinate, b: Coordinate, p: Coordinate) -> float: + if a.distance_to(b) < 0.01: + return a.distance_to(p) + + ap = p - a + ab = b - a + t = np.clip(ap.dot(ab) / ab.dot(ab), 0.0, 1.0) + projection = a + ab * t + return projection.distance_to(p) + + +def distance_along_geometry(geometry: list[Coordinate], position: Coordinate) -> float: + if len(geometry) <= 2: + return geometry[0].distance_to(position) + + total_distance = 0.0 + closest_total_distance = 0.0 + closest_distance = 1e9 + + for index in range(len(geometry) - 1): + segment_distance = minimum_distance(geometry[index], geometry[index + 1], position) + if segment_distance < closest_distance: + closest_distance = segment_distance + closest_total_distance = total_distance + geometry[index].distance_to(position) + total_distance += geometry[index].distance_to(geometry[index + 1]) + + return closest_total_distance + + +def string_to_direction(direction: str) -> str: + normalized = direction or "" + for direction_name in DIRECTIONS: + if direction_name not in normalized: + continue + if "slight" in normalized and direction_name in MODIFIABLE_DIRECTIONS: + return f"slight{direction_name.capitalize()}" + if "sharp" in normalized and direction_name in MODIFIABLE_DIRECTIONS: + return f"sharp{direction_name.capitalize()}" + return direction_name + + if "uturn" in normalized or "u-turn" in normalized: + return UTURN_MODIFIER + + return "none" + + +def normalize_lane_direction(direction: str) -> str: + normalized = string_to_direction(direction) + return normalized if normalized in LANE_DIRECTIONS else "none" + + +def maxspeed_to_ms(maxspeed: dict[str, str | float]) -> float: + unit = str(maxspeed["unit"]) + speed = float(maxspeed["speed"]) + return float(SPEED_CONVERSIONS[unit] * speed) + + +def field_valid(dat: dict[str, Any], field: str) -> bool: + return field in dat and dat[field] is not None + + +def parse_banner_instructions(banners: Any, distance_to_maneuver: float = 0.0) -> dict[str, Any] | None: + if not banners: + return None + + instruction: dict[str, Any] = {} + current_banner = banners[0] + for banner in banners: + if distance_to_maneuver < banner["distanceAlongGeometry"]: + current_banner = banner + + instruction["showFull"] = distance_to_maneuver < current_banner["distanceAlongGeometry"] + + primary = current_banner["primary"] + if field_valid(primary, "text"): + instruction["maneuverPrimaryText"] = primary["text"] + if field_valid(primary, "type"): + instruction["maneuverType"] = primary["type"] + if field_valid(primary, "modifier"): + instruction["maneuverModifier"] = string_to_direction(primary["modifier"]) + + if field_valid(current_banner, "secondary"): + instruction["maneuverSecondaryText"] = current_banner["secondary"]["text"] + + if field_valid(current_banner, "sub"): + lanes = [] + for component in current_banner["sub"]["components"]: + if component["type"] != "lane": + continue + + lane = { + "active": component["active"], + "directions": [normalize_lane_direction(direction) for direction in component["directions"]], + } + if field_valid(component, "active_direction"): + lane["activeDirection"] = normalize_lane_direction(component["active_direction"]) + lanes.append(lane) + instruction["lanes"] = lanes + + return instruction + + +@dataclass +class NavigationRoute: + geometry: list[Coordinate] + bearings: list[float] + steps: list[RouteStep] + total_distance: float + total_duration: float + + @classmethod + def from_mapbox_route(cls, route_data: dict[str, Any]) -> "NavigationRoute" | None: + geometry_data = route_data.get("geometry") or [] + steps_data = route_data.get("steps") or [] + if not geometry_data or not steps_data: + return None + + geometry = [Coordinate(float(coord["latitude"]), float(coord["longitude"])) for coord in geometry_data] + cumulative_distances = [0.0] + for index in range(1, len(geometry)): + cumulative_distances.append(cumulative_distances[-1] + geometry[index - 1].distance_to(geometry[index])) + + maxspeeds = [maxspeed_to_ms(item) for item in route_data.get("maxspeed", []) if field_valid(item, "speed") and field_valid(item, "unit")] + + steps: list[RouteStep] = [] + for step in steps_data: + location = Coordinate(float(step["location"]["latitude"]), float(step["location"]["longitude"])) + closest_index = min(range(len(geometry)), key=lambda idx: location.distance_to(geometry[idx])) + maxspeed_ms = maxspeeds[min(closest_index, len(maxspeeds) - 1)] if maxspeeds else 0.0 + steps.append(RouteStep( + banner_instructions=step.get("bannerInstructions", []), + distance=float(step["distance"]), + duration=float(step["duration"]), + maneuver=str(step["maneuver"]), + location=location, + cumulative_distance=cumulative_distances[closest_index], + maxspeed_ms=maxspeed_ms, + modifier=string_to_direction(str(step.get("modifier", "none"))), + instruction=str(step.get("instruction", "")), + )) + + bearings = [bearing_between_two_points(geometry[index], geometry[index + 2]) for index in range(len(geometry) - 2)] + return cls( + geometry=geometry, + bearings=bearings, + steps=steps, + total_distance=float(route_data.get("totalDistance", 0.0)), + total_duration=float(route_data.get("totalDuration", 0.0)), + ) + + def route_bearing_misaligned(self, closest_index: int, current_bearing: float | None, v_ego: float) -> bool: + if current_bearing is None or v_ego < 5.0 or closest_index <= 0 or closest_index >= len(self.geometry) - 1: + return False + if closest_index - 1 >= len(self.bearings): + return False + + route_bearing = self.bearings[closest_index - 1] + normalized_bearing = (current_bearing + 360.0) % 360.0 + bearing_difference = abs(normalized_bearing - route_bearing) + return min(bearing_difference, 360.0 - bearing_difference) > 110.0 + + def get_progress(self, position: Coordinate) -> RouteProgress | None: + if not self.geometry or not self.steps: + return None + + closest_index, min_distance = min( + ((idx, position.distance_to(coord)) for idx, coord in enumerate(self.geometry)), + key=lambda item: item[1], + ) + closest_cumulative = distance_along_geometry(self.geometry, position) + + current_step_index = max( + (idx for idx, step in enumerate(self.steps) if step.cumulative_distance <= closest_cumulative), + default=-1, + ) + current_step = self.steps[current_step_index if current_step_index >= 0 else 0] + next_step_index = current_step_index + 1 + next_step = self.steps[next_step_index] if 0 <= next_step_index < len(self.steps) else None + + distance_to_end_of_step = max(0.0, current_step.distance - (closest_cumulative - current_step.cumulative_distance)) + distance_remaining = max(0.0, self.total_distance - closest_cumulative) + + current_step_distance = max(current_step.distance, 1.0) + remaining_current_duration = current_step.duration * min(distance_to_end_of_step / current_step_distance, 1.0) + later_duration = sum(step.duration for step in self.steps[next_step_index:]) + time_remaining = max(0.0, remaining_current_duration + later_duration) + + all_maneuvers: list[dict[str, Any]] = [] + start_index = max(current_step_index, 0) + end_index = min(start_index + 3, len(self.steps)) + for index in range(start_index, end_index): + step = self.steps[index] + maneuver_distance = distance_to_end_of_step if index == start_index else max(0.0, step.cumulative_distance - closest_cumulative) + all_maneuvers.append({ + "distance": maneuver_distance, + "type": step.maneuver, + "modifier": step.modifier, + }) + + return RouteProgress( + closest_index=closest_index, + distance_from_route=min_distance, + current_step=current_step, + next_step=next_step, + current_step_index=max(current_step_index, 0), + distance_to_end_of_step=distance_to_end_of_step, + distance_remaining=distance_remaining, + time_remaining=time_remaining, + current_speed_limit_ms=current_step.maxspeed_ms, + all_maneuvers=all_maneuvers, + ) + + def upcoming_turn_modifier(self, progress: RouteProgress, position: Coordinate, v_ego: float) -> str: + if progress.next_step is None: + return "none" + + distance_threshold = float(np.interp(v_ego, UPCOMING_TURN_SPEED_BREAKPOINTS, UPCOMING_TURN_DISTANCE_BREAKPOINTS)) + if position.distance_to(progress.next_step.location) <= distance_threshold: + return progress.next_step.modifier + return "none" + + def off_route_distance_exceeded(self, progress: RouteProgress, v_ego: float) -> bool: + distance_threshold = float(np.interp(v_ego, OFF_ROUTE_SPEED_BREAKPOINTS, OFF_ROUTE_DISTANCE_BREAKPOINTS)) + return progress.distance_from_route > distance_threshold + + def arrived(self, progress: RouteProgress, v_ego: float) -> bool: + if v_ego >= 1.0 or not progress.all_maneuvers: + return False + current = progress.all_maneuvers[0] + return current["type"] == "arrive" or progress.current_step.instruction.startswith("Your destination") + + def build_instruction_payload(self, progress: RouteProgress, *, use_vienna_sign: bool = False) -> dict[str, Any]: + parsed = parse_banner_instructions(progress.current_step.banner_instructions, progress.distance_to_end_of_step) or {} + primary_text = parsed.get("maneuverPrimaryText") or progress.current_step.instruction + secondary_text = parsed.get("maneuverSecondaryText") or "" + maneuver_type = parsed.get("maneuverType") or progress.current_step.maneuver + maneuver_modifier = parsed.get("maneuverModifier") or progress.current_step.modifier + lanes = [] + for lane in parsed.get("lanes", []): + directions = [direction for direction in lane.get("directions", []) if direction in LANE_DIRECTIONS] + active_direction = lane.get("activeDirection", "none") + lanes.append({ + "directions": directions or ["none"], + "active": bool(lane.get("active", False)), + "activeDirection": active_direction if active_direction in LANE_DIRECTIONS else "none", + }) + + return { + "maneuverPrimaryText": primary_text, + "maneuverSecondaryText": secondary_text, + "maneuverDistance": progress.distance_to_end_of_step, + "maneuverType": maneuver_type, + "maneuverModifier": maneuver_modifier, + "distanceRemaining": progress.distance_remaining, + "timeRemaining": progress.time_remaining, + "timeRemainingTypical": progress.time_remaining, + "lanes": lanes, + "showFull": bool(parsed.get("showFull", True)), + "speedLimit": progress.current_speed_limit_ms, + "speedLimitSign": log.NavInstruction.SpeedLimitSign.vienna if use_vienna_sign else log.NavInstruction.SpeedLimitSign.mutcd, + "allManeuvers": progress.all_maneuvers, + } + + +class MapboxRouteEngine: + DIRECTIONS_URL = "https://api.mapbox.com/directions/v5/mapbox/driving" + + def __init__(self, session: Any = requests): + self._session = session + + def fetch_route(self, token: str, start: Coordinate, destination: dict[str, Any], bearing: float | None = None) -> NavigationRoute | None: + if not token: + return None + + end = Coordinate(float(destination["latitude"]), float(destination["longitude"])) + params: dict[str, str] = { + "access_token": token, + "geometries": "geojson", + "steps": "true", + "overview": "full", + "annotations": "maxspeed", + "alternatives": "false", + "banner_instructions": "true", + } + if bearing is not None: + params["bearings"] = f"{int((bearing + 360.0) % 360.0)},90;" + + url = f"{self.DIRECTIONS_URL}/{start.longitude},{start.latitude};{end.longitude},{end.latitude}" + try: + response = self._session.get(url, params=params, timeout=5) + data = response.json() if response.status_code == 200 else {} + except requests.RequestException: + return None + + routes = data.get("routes") or [] + route = routes[0] if routes else None + legs = route.get("legs") if route else None + leg = legs[0] if legs else None + if data.get("code") != "Ok" or route is None or leg is None: + return None + + route_data = { + "steps": [ + { + "maneuver": step["maneuver"]["type"], + "instruction": step["maneuver"]["instruction"], + "distance": step["distance"], + "duration": step["duration"], + "location": { + "longitude": step["maneuver"]["location"][0], + "latitude": step["maneuver"]["location"][1], + }, + "modifier": step["maneuver"].get("modifier", "none"), + "bannerInstructions": step.get("bannerInstructions", []), + } + for step in leg.get("steps", []) + ], + "totalDistance": route["distance"], + "totalDuration": route["duration"], + "geometry": [ + {"longitude": coord[0], "latitude": coord[1]} + for coord in route["geometry"]["coordinates"] + ], + "maxspeed": [ + {"speed": item["speed"], "unit": item["unit"]} + for item in leg.get("annotation", {}).get("maxspeed", []) + if field_valid(item, "speed") and field_valid(item, "unit") + ], + } + return NavigationRoute.from_mapbox_route(route_data) diff --git a/starpilot/navigation/test_destination_store.py b/starpilot/navigation/test_destination_store.py new file mode 100644 index 000000000..cab470df7 --- /dev/null +++ b/starpilot/navigation/test_destination_store.py @@ -0,0 +1,48 @@ +import json + +from openpilot.starpilot.navigation.destination_store import ( + normalize_destination_payload, + update_recent_destinations, +) + + +def test_normalize_destination_payload_requires_name_and_coordinates(): + payload = { + "name": "123 Main St", + "latitude": "41.881832", + "longitude": "-87.623177", + } + + assert normalize_destination_payload(payload) == { + "name": "123 Main St", + "place_name": "123 Main St", + "latitude": 41.881832, + "longitude": -87.623177, + } + assert normalize_destination_payload({"name": "Missing coords"}) is None + + +def test_recent_destinations_dedupe_and_cap(): + existing = json.dumps([ + {"place_name": "Old 1"}, + {"place_name": "Old 2"}, + {"place_name": "Home"}, + {"place_name": "Old 3"}, + {"place_name": "Old 4"}, + {"place_name": "Old 5"}, + {"place_name": "Old 6"}, + {"place_name": "Old 7"}, + {"place_name": "Old 8"}, + {"place_name": "Old 9"}, + ]) + + updated = update_recent_destinations(existing, { + "name": "Home", + "latitude": 1.0, + "longitude": 2.0, + }) + + assert updated[0]["place_name"] == "Home" + assert len(updated) == 10 + assert [entry["place_name"] for entry in updated].count("Home") == 1 + assert updated[-1]["place_name"] == "Old 9" diff --git a/starpilot/navigation/test_route_engine.py b/starpilot/navigation/test_route_engine.py new file mode 100644 index 000000000..bb6b7133f --- /dev/null +++ b/starpilot/navigation/test_route_engine.py @@ -0,0 +1,153 @@ +from cereal import log + +from openpilot.starpilot.navigation.route_engine import Coordinate, NavigationRoute + + +def make_route() -> NavigationRoute: + route_data = { + "geometry": [ + {"latitude": 0.0, "longitude": 0.0}, + {"latitude": 0.0, "longitude": 0.001}, + {"latitude": 0.0, "longitude": 0.002}, + {"latitude": 0.001, "longitude": 0.002}, + ], + "steps": [ + { + "maneuver": "depart", + "instruction": "Head east", + "distance": 220.0, + "duration": 20.0, + "location": {"latitude": 0.0, "longitude": 0.0}, + "modifier": "straight", + "bannerInstructions": [ + { + "distanceAlongGeometry": 80.0, + "primary": { + "text": "Turn right", + "type": "turn", + "modifier": "right", + }, + "secondary": {"text": "onto Market St"}, + }, + ], + }, + { + "maneuver": "turn", + "instruction": "Turn right onto Market St", + "distance": 111.0, + "duration": 10.0, + "location": {"latitude": 0.0, "longitude": 0.002}, + "modifier": "right", + "bannerInstructions": [], + }, + { + "maneuver": "arrive", + "instruction": "Your destination is on the right", + "distance": 0.0, + "duration": 0.0, + "location": {"latitude": 0.001, "longitude": 0.002}, + "modifier": "right", + "bannerInstructions": [], + }, + ], + "totalDistance": 331.0, + "totalDuration": 30.0, + "maxspeed": [ + {"speed": 35.0, "unit": "mph"}, + {"speed": 35.0, "unit": "mph"}, + {"speed": 25.0, "unit": "mph"}, + {"speed": 25.0, "unit": "mph"}, + ], + } + route = NavigationRoute.from_mapbox_route(route_data) + assert route is not None + return route + + +def test_route_progress_and_instruction_payload(): + route = make_route() + progress = route.get_progress(Coordinate(0.0, 0.0015)) + + assert progress is not None + assert progress.current_step_index == 0 + assert progress.distance_remaining > 0 + assert route.upcoming_turn_modifier(progress, Coordinate(0.0, 0.0015), 20.0) == "right" + + payload = route.build_instruction_payload(progress) + assert payload["maneuverPrimaryText"] == "Turn right" + assert payload["maneuverSecondaryText"] == "onto Market St" + assert payload["maneuverType"] == "turn" + assert payload["maneuverModifier"] == "right" + + +def test_route_off_route_and_arrival_detection(): + route = make_route() + off_route_progress = route.get_progress(Coordinate(0.0020, 0.0010)) + misaligned_progress = route.get_progress(Coordinate(0.0, 0.0015)) + arrive_progress = route.get_progress(Coordinate(0.0010, 0.0020)) + + assert off_route_progress is not None + assert misaligned_progress is not None + assert arrive_progress is not None + assert route.off_route_distance_exceeded(off_route_progress, 10.0) + assert route.route_bearing_misaligned(misaligned_progress.closest_index, 270.0, 10.0) + assert route.arrived(arrive_progress, 0.5) + + +def test_lane_payload_uses_capnp_enum_names(): + route_data = { + "geometry": [ + {"latitude": 0.0, "longitude": 0.0}, + {"latitude": 0.0, "longitude": 0.001}, + {"latitude": 0.0, "longitude": 0.002}, + ], + "steps": [ + { + "maneuver": "turn", + "instruction": "Keep left", + "distance": 120.0, + "duration": 12.0, + "location": {"latitude": 0.0, "longitude": 0.001}, + "modifier": "slight left", + "bannerInstructions": [ + { + "distanceAlongGeometry": 80.0, + "primary": { + "text": "Keep left", + "type": "turn", + "modifier": "slight left", + }, + "sub": { + "components": [ + { + "type": "lane", + "active": True, + "directions": ["straight", "slight left"], + "active_direction": "slight left", + }, + ], + }, + }, + ], + }, + ], + "totalDistance": 120.0, + "totalDuration": 12.0, + } + + route = NavigationRoute.from_mapbox_route(route_data) + assert route is not None + progress = route.get_progress(Coordinate(0.0, 0.0005)) + assert progress is not None + + payload = route.build_instruction_payload(progress) + msg = log.Event.new_message() + msg.init("navInstruction") + msg.navInstruction.lanes = payload["lanes"] + + assert len(msg.navInstruction.lanes) == 1 + assert list(msg.navInstruction.lanes[0].directions) == [ + log.NavInstruction.Direction.straight, + log.NavInstruction.Direction.slightLeft, + ] + assert msg.navInstruction.lanes[0].activeDirection == log.NavInstruction.Direction.slightLeft diff --git a/starpilot/system/the_pond/assets/components/sidebar.js b/starpilot/system/the_pond/assets/components/sidebar.js index ed5a71b03..968a79356 100644 --- a/starpilot/system/the_pond/assets/components/sidebar.js +++ b/starpilot/system/the_pond/assets/components/sidebar.js @@ -17,6 +17,7 @@ const MENU_ITEMS = { { name: "Lateral Maneuvers", link: "/lateral_maneuvers", icon: "bi-sign-turn-right" }, { name: "Long Maneuvers", link: "/longitudinal_maneuvers", icon: "bi-signpost-split" }, { name: "Maps", link: "/manage_maps", icon: "bi-map" }, + { name: "Navigation", link: "/set_navigation_destination", icon: "bi-geo-alt-fill" }, { name: "Navigation Keys", link: "/manage_navigation_keys", icon: "bi-key-fill" }, { name: "Model Manager", link: "/manage_models", icon: "bi-cpu" }, { name: "Plots", link: "/plots", icon: "bi-graph-up-arrow" }, diff --git a/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json b/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json index c9c126426..37935567f 100644 --- a/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json +++ b/starpilot/system/the_pond/assets/components/tools/device_settings_layout.json @@ -200,6 +200,14 @@ "ui_type": "toggle", "parent_key": "LateralTune" }, + { + "key": "NavDesiresAllowed", + "label": "Use Route Desires", + "description": "Allow an active navigation route to request keep-left, keep-right, and low-speed turn desires.", + "data_type": "bool", + "ui_type": "toggle", + "parent_key": "LateralTune" + }, { "key": "NNFF", "label": "Neural Network Feedforward (NNFF)", diff --git a/starpilot/system/the_pond/the_pond.py b/starpilot/system/the_pond/the_pond.py index cd2cbd2ff..bd0ad356e 100644 --- a/starpilot/system/the_pond/the_pond.py +++ b/starpilot/system/the_pond/the_pond.py @@ -77,6 +77,7 @@ from openpilot.starpilot.common.testing_grounds import ( TESTING_GROUNDS_SLOT_DEFINITIONS as SHARED_TESTING_GROUNDS_SLOT_DEFINITIONS, TESTING_GROUNDS_STATE_PATH as SHARED_TESTING_GROUNDS_STATE_PATH, ) +from openpilot.starpilot.navigation.destination_store import normalize_destination_payload, update_recent_destinations from openpilot.starpilot.system.the_pond import utilities DISCORD_WEBHOOK_URL = os.getenv("DISCORD_WEBHOOK_URL") @@ -3464,11 +3465,16 @@ def setup(app): @app.route("/api/navigation", methods=["POST"]) def set_navigation(): - params.remove("NavDestination") + destination = normalize_destination_payload(request.json) + if destination is None: + return {"message": "Invalid destination payload"}, 400 - time.sleep(1) - - params.put("NavDestination", json.dumps(request.json)) + recent_destinations = update_recent_destinations( + params.get("ApiCache_NavDestinations", encoding="utf8") or "", + destination, + ) + params.put("NavDestination", json.dumps(destination)) + params.put("ApiCache_NavDestinations", json.dumps(recent_destinations)) return {"message": "Destination set"} @app.route("/api/navigation/favorite", methods=["DELETE"]) diff --git a/starpilot/ui/qt/offroad/lateral_settings.cc b/starpilot/ui/qt/offroad/lateral_settings.cc index 10dbd392a..a5d5d9791 100644 --- a/starpilot/ui/qt/offroad/lateral_settings.cc +++ b/starpilot/ui/qt/offroad/lateral_settings.cc @@ -55,6 +55,7 @@ StarPilotLateralPanel::StarPilotLateralPanel(StarPilotSettingsWindow *parent, bo {"LateralTune", tr("Lateral Tuning"), tr("Miscellaneous steering control changes to fine-tune how openpilot drives."), "../../starpilot/assets/toggle_icons/icon_lateral_tune.png"}, {"TurnDesires", tr("Force Turn Desires Below Lane Change Speed"), tr("While driving below the minimum lane change speed with an active turn signal, instruct openpilot to turn left/right."), ""}, + {"NavDesiresAllowed", tr("Use Route Desires"), tr("Allow an active navigation route to request keep-left, keep-right, and low-speed turn desires."), ""}, {"NNFF", tr("Neural Network Feedforward (NNFF)"), tr("Twilsonco's \"Neural Network FeedForward\" controller. Uses a trained neural network model to predict steering torque based on vehicle speed, roll, and past/future planned path data for smoother, model-based steering."), ""}, {"NNFFLite", tr("Neural Network Feedforward (NNFF) Lite"), tr("A lightweight version of Twilsonco's \"Neural Network FeedForward\" controller. Uses the \"look-ahead\" planned lateral jerk logic from the full model to help smoothen steering adjustments in curves, but does not use the full neural network for torque calculation."), ""}, diff --git a/starpilot/ui/qt/offroad/lateral_settings.h b/starpilot/ui/qt/offroad/lateral_settings.h index a8eac5635..5bf83e39c 100644 --- a/starpilot/ui/qt/offroad/lateral_settings.h +++ b/starpilot/ui/qt/offroad/lateral_settings.h @@ -27,7 +27,7 @@ private: QSet advancedLateralTuneKeys = {"ForceAutoTune", "ForceAutoTuneOff", "ForceTorqueController", "SteerDelay", "SteerFriction", "SteerLatAccel", "SteerKP", "SteerRatio"}; QSet aolKeys = {"AlwaysOnLateralLKAS", "PauseAOLOnBrake"}; QSet laneChangeKeys = {"LaneChangeSmoothing", "LaneChangeTime", "LaneDetectionWidth", "MinimumLaneChangeSpeed", "NudgelessLaneChange", "OneLaneChange"}; - QSet lateralTuneKeys = {"NNFF", "NNFFLite", "TurnDesires"}; + QSet lateralTuneKeys = {"NNFF", "NNFFLite", "TurnDesires", "NavDesiresAllowed"}; QSet qolKeys = {"PauseLateralSpeed", "LateralResumeDelay"}; QSet parentKeys; diff --git a/system/manager/process_config.py b/system/manager/process_config.py index abfd71546..420a90823 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -80,6 +80,9 @@ def run_speed_limit_filler(started: bool, params: Params, CP: car.CarParams, sta def run_speed_limit_vision(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool: return starpilot_toggles.vision_speed_limit_detection +def run_navigationd(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool: + return started and params.get("NavDestination") is not None + procs = [ DaemonProcess("manage_athenad", "system.athena.manage_athenad", "AthenadPid"), @@ -150,7 +153,8 @@ else: procs += [ PythonProcess("device_syncd", "starpilot.system.device_syncd", always_run), PythonProcess("starpilot_process", "starpilot.starpilot_process", always_run), - PythonProcess("mapd", "starpilot.navigation.mapd_wrapper", always_run), + PythonProcess("mapd", "starpilot.navigation.mapd_wrapper", always_run, nice=19), + PythonProcess("navigationd", "starpilot.navigation.navigationd", run_navigationd, nice=19), PythonProcess("speed_limit_filler", "starpilot.system.speed_limit_filler", run_speed_limit_filler, nice=19), PythonProcess("speed_limit_vision", "starpilot.system.speed_limit_vision", run_speed_limit_vision, nice=19), ] diff --git a/tools/replay/fake_nav_demo.py b/tools/replay/fake_nav_demo.py new file mode 100644 index 000000000..9424577f3 --- /dev/null +++ b/tools/replay/fake_nav_demo.py @@ -0,0 +1,168 @@ +#!/usr/bin/env python3 + +import argparse +import time +from itertools import cycle + +from cereal import log, messaging + + +ROUTE_COORDS = [ + {"latitude": 0.0, "longitude": 0.0}, + {"latitude": 0.0, "longitude": 0.01}, + {"latitude": 0.01, "longitude": 0.02}, +] + + +SCENARIOS = [ + { + "primary": "Turn right onto Main St", + "secondary": "Then keep left", + "distance": 120.0, + "type": "turn", + "modifier": "right", + "next_type": "fork", + "next_modifier": "slightLeft", + }, + { + "primary": "Turn left onto Pine Ave", + "secondary": "Then merge right", + "distance": 180.0, + "type": "turn", + "modifier": "left", + "next_type": "merge", + "next_modifier": "right", + }, + { + "primary": "Keep right to stay on I-90", + "secondary": "Then continue straight", + "distance": 260.0, + "type": "fork", + "modifier": "slightRight", + "next_type": "continue", + "next_modifier": "straight", + }, + { + "primary": "Keep left to stay on I-90", + "secondary": "Then take exit 14A", + "distance": 210.0, + "type": "fork", + "modifier": "slightLeft", + "next_type": "off ramp", + "next_modifier": "right", + }, + { + "primary": "Take exit 14A", + "secondary": "Then turn sharp right", + "distance": 340.0, + "type": "off ramp", + "modifier": "right", + "next_type": "turn", + "next_modifier": "sharpRight", + }, + { + "primary": "Continue straight for 1 mile", + "secondary": "Then enter the roundabout", + "distance": 1600.0, + "type": "continue", + "modifier": "straight", + "next_type": "roundabout", + "next_modifier": "right", + }, + { + "primary": "Enter the roundabout", + "secondary": "Take the second exit", + "distance": 90.0, + "type": "roundabout", + "modifier": "right", + "next_type": "turn", + "next_modifier": "left", + }, + { + "primary": "Turn sharp left onto Oak St", + "secondary": "Then make a U-turn", + "distance": 75.0, + "type": "turn", + "modifier": "sharpLeft", + "next_type": "continue", + "next_modifier": "uturn", + }, + { + "primary": "Make a U-turn", + "secondary": "Then arrive at destination", + "distance": 60.0, + "type": "continue", + "modifier": "uturn", + "next_type": "arrive", + "next_modifier": "right", + }, + { + "primary": "Arrive at destination", + "secondary": "Destination is on the right", + "distance": 25.0, + "type": "arrive", + "modifier": "right", + "next_type": "turn", + "next_modifier": "straight", + }, +] + + +def build_instruction(pm: messaging.PubMaster, scenario: dict) -> None: + msg = messaging.new_message("navInstruction") + msg.valid = True + + nav = msg.navInstruction + nav.maneuverPrimaryText = scenario["primary"] + nav.maneuverSecondaryText = scenario["secondary"] + nav.maneuverDistance = scenario["distance"] + nav.maneuverType = scenario["type"] + nav.maneuverModifier = scenario["modifier"] + nav.distanceRemaining = max(scenario["distance"] * 8, 400.0) + nav.timeRemaining = max(scenario["distance"] / 10.0, 20.0) + nav.timeRemainingTypical = nav.timeRemaining + nav.showFull = True + nav.speedLimit = 31.2928 + nav.speedLimitSign = log.NavInstruction.SpeedLimitSign.mutcd + nav.allManeuvers = [ + {"distance": scenario["distance"], "type": scenario["type"], "modifier": scenario["modifier"]}, + {"distance": scenario["distance"] + 220.0, "type": scenario["next_type"], "modifier": scenario["next_modifier"]}, + ] + + pm.send("navInstruction", msg) + + +def build_route(pm: messaging.PubMaster) -> None: + msg = messaging.new_message("navRoute") + msg.valid = True + msg.navRoute.coordinates = ROUTE_COORDS + pm.send("navRoute", msg) + + +def main() -> None: + parser = argparse.ArgumentParser(description="Publish fake navigation instructions for desktop onroad replay.") + parser.add_argument("--hold-seconds", type=float, default=3.0, help="How long each maneuver stays active.") + parser.add_argument("--publish-interval", type=float, default=0.25, help="Seconds between navInstruction publishes.") + args = parser.parse_args() + + pm = messaging.PubMaster(["navInstruction", "navRoute"]) + + scenario_cycle = cycle(SCENARIOS) + current = next(scenario_cycle) + next_switch = time.monotonic() + args.hold_seconds + print(f"showing: {current['type']} / {current['modifier']} -> {current['next_type']} / {current['next_modifier']}", flush=True) + + while True: + now = time.monotonic() + if now >= next_switch: + current = next(scenario_cycle) + next_switch = now + args.hold_seconds + print(f"showing: {current['type']} / {current['modifier']} -> {current['next_type']} / {current['next_modifier']}", flush=True) + + build_instruction(pm, current) + build_route(pm) + time.sleep(args.publish_interval) + + +if __name__ == "__main__": + main()