diff --git a/common/libcommon.a b/common/libcommon.a index d9abaa650..06e26b99b 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_keys.h b/common/params_keys.h index a157672ab..0bf615d9e 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -150,7 +150,7 @@ inline static std::unordered_map keys = { {"AdvancedLateralTune", {PERSISTENT, BOOL, "1", "0", 2}}, {"AdvancedLongitudinalTune", {PERSISTENT, BOOL, "1", "0", 3}}, {"AggressiveFollow", {PERSISTENT, FLOAT, "1.25", "1.25", 2}}, - {"AggressiveFollowHigh", {PERSISTENT, FLOAT, "1.25", "1.25", 2}}, + {"AggressiveFollowHigh", {PERSISTENT, FLOAT, "1.0", "1.0", 2}}, {"AggressiveJerkAcceleration", {PERSISTENT, FLOAT, "50.0", "50.0", 3}}, {"AggressiveJerkDanger", {PERSISTENT, FLOAT, "100.0", "100.0", 3}}, {"AggressiveJerkDeceleration", {PERSISTENT, FLOAT, "50.0", "50.0", 3}}, @@ -193,7 +193,7 @@ inline static std::unordered_map keys = { {"CESpeedLead", {PERSISTENT, FLOAT, "0.0", "0.0", 1}}, {"CEStatus", {CLEAR_ON_OFFROAD_TRANSITION, INT, "0", "0"}}, {"CEStopLights", {PERSISTENT, BOOL, "1", "0", 1}}, - {"CEStoppedLead", {PERSISTENT, BOOL, "1", "0", 1}}, + {"CEStoppedLead", {PERSISTENT, BOOL, "0", "0", 1}}, {"ClusterOffset", {PERSISTENT, FLOAT, "1.0", "1.0", 2}}, {"ColorScheme", {PERSISTENT, STRING, "frog", "stock", 0}}, {"ColorToDownload", {CLEAR_ON_MANAGER_START, STRING, "", ""}}, @@ -275,7 +275,7 @@ inline static std::unordered_map keys = { {"ForceFingerprint", {PERSISTENT, BOOL, "0", "0", 2}}, {"ForceOffroad", {CLEAR_ON_MANAGER_START, BOOL, "0", "0"}}, {"ForceOnroad", {CLEAR_ON_MANAGER_START, BOOL, "0", "0"}}, - {"ForceStops", {PERSISTENT, BOOL, "0", "0", 2}}, + {"ForceStops", {PERSISTENT, BOOL, "1", "0", 2}}, {"ForceStopDistanceOffset", {PERSISTENT, INT, "0", "0", 2}}, {"ForceStandstill", {PERSISTENT, BOOL, "0", "0", 2}}, {"ForceTorqueController", {PERSISTENT, BOOL, "0", "0", 3}}, @@ -298,7 +298,6 @@ inline static std::unordered_map keys = { {"HigherBitrate", {PERSISTENT, BOOL, "0", "0", 2}}, {"HolidayThemes", {PERSISTENT, BOOL, "1", "0", 0}}, {"HumanAcceleration", {PERSISTENT, BOOL, "0", "0", 2}}, - {"HumanFollowing", {PERSISTENT, BOOL, "0", "0", 2}}, {"CoastUpToLeads", {PERSISTENT, BOOL, "1", "1", 2}}, {"HumanLaneChanges", {PERSISTENT, BOOL, "0", "0", 2}}, {"IconPack", {PERSISTENT, STRING, "frog-animated", "stock", 0}}, @@ -425,13 +424,13 @@ inline static std::unordered_map keys = { {"ReduceLateralAccelerationRainStorm", {PERSISTENT, INT, "0", "0", 2}}, {"ReduceLateralAccelerationSnow", {PERSISTENT, INT, "0", "0", 2}}, {"RefuseVolume", {PERSISTENT, INT, "101", "101", 2}}, - {"RelaxedFollow", {PERSISTENT, FLOAT, "1.75", "1.75", 2}}, - {"RelaxedFollowHigh", {PERSISTENT, FLOAT, "1.75", "1.75", 2}}, - {"RelaxedJerkAcceleration", {PERSISTENT, FLOAT, "50.0", "50.0", 3}}, + {"RelaxedFollow", {PERSISTENT, FLOAT, "1.6", "1.6", 2}}, + {"RelaxedFollowHigh", {PERSISTENT, FLOAT, "1.4", "1.4", 2}}, + {"RelaxedJerkAcceleration", {PERSISTENT, FLOAT, "100.0", "100.0", 3}}, {"RelaxedJerkDanger", {PERSISTENT, FLOAT, "100.0", "100.0", 3}}, - {"RelaxedJerkDeceleration", {PERSISTENT, FLOAT, "50.0", "50.0", 3}}, - {"RelaxedJerkSpeed", {PERSISTENT, FLOAT, "50.0", "50.0", 3}}, - {"RelaxedJerkSpeedDecrease", {PERSISTENT, FLOAT, "50.0", "50.0", 3}}, + {"RelaxedJerkDeceleration", {PERSISTENT, FLOAT, "100.0", "100.0", 3}}, + {"RelaxedJerkSpeed", {PERSISTENT, FLOAT, "100.0", "100.0", 3}}, + {"RelaxedJerkSpeedDecrease", {PERSISTENT, FLOAT, "100.0", "100.0", 3}}, {"ReverseCruise", {PERSISTENT, BOOL, "0", "0", 1}}, {"RecoveryPower", {PERSISTENT, FLOAT, "1.0", "1.0", 2}}, {"RoadEdgesWidth", {PERSISTENT, FLOAT, "2.0", "2.0", 2}}, @@ -496,12 +495,12 @@ inline static std::unordered_map keys = { {"VisionSpeedLimitDetection", {PERSISTENT, BOOL, "0", "0", 0}}, {"VisionSpeedLimitTrainingCollector", {PERSISTENT, BOOL, "1", "1", 0}}, {"StandardFollow", {PERSISTENT, FLOAT, "1.45", "1.45", 2}}, - {"StandardFollowHigh", {PERSISTENT, FLOAT, "1.45", "1.45", 2}}, - {"StandardJerkAcceleration", {PERSISTENT, FLOAT, "50.0", "50.0", 3}}, + {"StandardFollowHigh", {PERSISTENT, FLOAT, "1.2", "1.2", 2}}, + {"StandardJerkAcceleration", {PERSISTENT, FLOAT, "100.0", "100.0", 3}}, {"StandardJerkDanger", {PERSISTENT, FLOAT, "100.0", "100.0", 3}}, - {"StandardJerkDeceleration", {PERSISTENT, FLOAT, "50.0", "50.0", 3}}, - {"StandardJerkSpeed", {PERSISTENT, FLOAT, "50.0", "50.0", 3}}, - {"StandardJerkSpeedDecrease", {PERSISTENT, FLOAT, "50.0", "50.0", 3}}, + {"StandardJerkDeceleration", {PERSISTENT, FLOAT, "100.0", "100.0", 3}}, + {"StandardJerkSpeed", {PERSISTENT, FLOAT, "100.0", "100.0", 3}}, + {"StandardJerkSpeedDecrease", {PERSISTENT, FLOAT, "100.0", "100.0", 3}}, {"StandbyMode", {PERSISTENT, BOOL, "0", "0", 1}}, {"StartAccel", {PERSISTENT, FLOAT, "0.0", "0.0", 3}}, {"StartAccelStock", {PERSISTENT, FLOAT, "0.0", "0.0", 3}}, diff --git a/common/params_pyx.so b/common/params_pyx.so index bec91ba81..23fbd225d 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/opendbc_repo/opendbc/car/car_helpers.py b/opendbc_repo/opendbc/car/car_helpers.py index 9b8ff70da..5732e3209 100644 --- a/opendbc_repo/opendbc/car/car_helpers.py +++ b/opendbc_repo/opendbc/car/car_helpers.py @@ -11,6 +11,7 @@ from opendbc.car.structs import CarParams, CarParamsT from opendbc.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars from opendbc.car.fw_versions import ObdCallback, get_fw_versions_ordered, get_present_ecus, match_fw_to_car from opendbc.car.mock.values import CAR as MOCK +from opendbc.car.toyota.values import ToyotaSafetyFlags from opendbc.car.values import BRANDS from opendbc.car.vin import get_vin, is_valid_vin, VIN_UNKNOWN from openpilot.common.params import Params @@ -108,6 +109,20 @@ def _normalize_gm_bolt_candidate(candidate: str | None, fingerprints: dict[int, return candidate +def _apply_disable_openpilot_long(CP: CarParams, FPCP: StarPilotCarParams) -> None: + CP.openpilotLongitudinalControl = False + CP.pcmCruise = True + FPCP.openpilotLongitudinalControlDisabled = True + + if CP.brand == "toyota": + # Toyota stock longitudinal safety changes the forwarding rules so the + # camera's ACC_CONTROL message can reach the PT bus again. + for cfg in CP.safetyConfigs: + cfg.safetyParam |= ToyotaSafetyFlags.STOCK_LONGITUDINAL.value + for cfg in FPCP.safetyConfigs: + cfg.safetyParam |= ToyotaSafetyFlags.STOCK_LONGITUDINAL.value + + def _normalize_gm_volt_candidate(candidate: str | None, fingerprints: dict[int, dict]) -> str | None: cam = fingerprints.get(GM_CAMERA_BUS, {}) has_live_camera_msg = GM_VOLT_CAMERA_MSG in cam @@ -318,8 +333,7 @@ def get_car(can_recv: CanRecvCallable, can_send: CanSendCallable, set_obd_multip FPCP: StarPilotCarParams = CarInterface.get_starpilot_params(candidate, fingerprints, car_fw, CP, starpilot_toggles) if not CP.alphaLongitudinalAvailable and starpilot_toggles.disable_openpilot_long: - CP.openpilotLongitudinalControl = False - FPCP.openpilotLongitudinalControlDisabled = True + _apply_disable_openpilot_long(CP, FPCP) return interfaces[CP.carFingerprint](CP, FPCP) diff --git a/opendbc_repo/opendbc/car/gm/interface.py b/opendbc_repo/opendbc/car/gm/interface.py index 09e4d7f6a..b875836da 100755 --- a/opendbc_repo/opendbc/car/gm/interface.py +++ b/opendbc_repo/opendbc/car/gm/interface.py @@ -571,9 +571,9 @@ class CarInterface(CarInterfaceBase): ret.openpilotLongitudinalControl = not disable_openpilot_long volt_test_tune_active = ( - testing_ground.use_2 and ret.openpilotLongitudinalControl and - candidate in VOLT_LONG_TEST_TUNE_CARS + candidate in VOLT_LONG_TEST_TUNE_CARS and + (testing_ground.use_2 or not ret.enableGasInterceptorDEPRECATED) ) if volt_test_tune_active: # Volt long can still fall back to an all-I tune on both the interceptor and diff --git a/opendbc_repo/opendbc/car/gm/tests/test_gm.py b/opendbc_repo/opendbc/car/gm/tests/test_gm.py index f90109ca1..35303d60e 100644 --- a/opendbc_repo/opendbc/car/gm/tests/test_gm.py +++ b/opendbc_repo/opendbc/car/gm/tests/test_gm.py @@ -60,24 +60,39 @@ class TestGMInterface: assert car_params.minSteerSpeed == pytest.approx(7 * CV.MPH_TO_MS) - @parameterized.expand([ - ("interceptor", True), - ("ascm_int", False), - ]) - def test_volt_testing_ground_tune_sets_nonzero_p_and_starting_state(self, _name, pedal_present, monkeypatch): + def test_volt_testing_ground_tune_sets_nonzero_p_and_starting_state(self, monkeypatch): CarInterface = interfaces[CAR.CHEVROLET_VOLT_ASCM] fingerprint = _empty_fingerprint() - if pedal_present: - fingerprint[0][0x201] = 8 # pedal detected + fingerprint[0][0x201] = 8 # pedal detected fingerprint[0][0x2FF] = 8 # SASCM detected - monkeypatch.setattr(gm_interface.testing_ground, "use_2", True, raising=False) + monkeypatch.setattr(gm_interface, "testing_ground", SimpleNamespace(use_2=True), raising=False) car_params = CarInterface.get_params(CAR.CHEVROLET_VOLT_ASCM, fingerprint, [], alpha_long=False, is_release=False, docs=False, starpilot_toggles=_test_starpilot_toggles()) - assert list(car_params.longitudinalTuning.kpV) == [0.10, 0.072, 0.05, 0.04] - assert list(car_params.longitudinalTuning.kiV) == [0.025, 0.03, 0.04, 0.055] + assert list(car_params.longitudinalTuning.kpV) == pytest.approx([0.10, 0.072, 0.05, 0.04]) + assert list(car_params.longitudinalTuning.kiV) == pytest.approx([0.025, 0.03, 0.04, 0.055]) + assert car_params.startingState + assert car_params.startAccel == pytest.approx(1.15) + + @parameterized.expand([ + ("volt_2019", CAR.CHEVROLET_VOLT_2019, True), + ("volt_ascm", CAR.CHEVROLET_VOLT_ASCM, True), + ]) + def test_non_pedal_volt_alpha_long_uses_volt_tune_by_default(self, _name, car_model, needs_sascm): + CarInterface = interfaces[car_model] + fingerprint = _empty_fingerprint() + if needs_sascm: + fingerprint[0][0x2FF] = 8 # SASCM detected + + car_params = CarInterface.get_params(car_model, fingerprint, [], alpha_long=True, is_release=False, docs=False, + starpilot_toggles=_test_starpilot_toggles()) + + assert car_params.openpilotLongitudinalControl + assert not car_params.enableGasInterceptorDEPRECATED + assert list(car_params.longitudinalTuning.kpV) == pytest.approx([0.10, 0.072, 0.05, 0.04]) + assert list(car_params.longitudinalTuning.kiV) == pytest.approx([0.025, 0.03, 0.04, 0.055]) assert car_params.startingState assert car_params.startAccel == pytest.approx(1.15) diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 60abed1ea..3081d0634 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -27,10 +27,10 @@ HYUNDAI_CANFD_SCC_DECEL_STEP = 12.5 / 50.0 IONIQ_6_RESPONSE_MULTIPLIER = 1.2 IONIQ_6_CANFD_SCC_ACCEL_STEP = (6.0 / 50.0) * IONIQ_6_RESPONSE_MULTIPLIER IONIQ_6_CANFD_SCC_DECEL_STEP = (15.0 / 50.0) * IONIQ_6_RESPONSE_MULTIPLIER -GENESIS_G90_STOP_HOLD_SPEED_BP = [0.0, 0.03, 0.08, 0.16, 0.3, 0.5] -GENESIS_G90_STOP_HOLD_ACCEL_V = [-0.12, -0.12, -0.15, -0.22, -0.50, -0.95] -GENESIS_G90_STOP_HOLD_RELAX_SPEED_BP = [0.0, 0.08, 0.16, 0.3, 0.5] -GENESIS_G90_STOP_HOLD_RELAX_STEP_V = [0.08, 0.08, 0.06, 0.04, 0.025] +GENESIS_G90_STOP_HOLD_SPEED_BP = [0.0, 0.03, 0.08, 0.16, 0.3, 0.5, 0.8, 1.2, 2.0, 3.0] +GENESIS_G90_STOP_HOLD_ACCEL_V = [-0.10, -0.10, -0.12, -0.18, -0.30, -0.50, -0.75, -1.00, -1.40, -1.80] +GENESIS_G90_STOP_HOLD_RELAX_SPEED_BP = [0.0, 0.08, 0.16, 0.3, 0.5, 0.8, 1.2, 2.0, 3.0] +GENESIS_G90_STOP_HOLD_RELAX_STEP_V = [0.10, 0.10, 0.08, 0.06, 0.04, 0.035, 0.03, 0.022, 0.018] GENESIS_G90_RELEASE_SPEED_BP = [0.0, 0.3, 0.6] GENESIS_G90_RELEASE_ACCEL_STEP_V = [0.05, 0.07, 0.11] GENESIS_G90_RELEASE_DECEL_STEP_V = [0.16, 0.18, 0.18] @@ -163,7 +163,8 @@ def update_genesis_g90_longitudinal_tuning(state: GenesisG90LongitudinalTuningSt stopping = long_control_state == LongCtrlState.stopping if stopping and v_ego <= GENESIS_G90_STOP_HOLD_SPEED_BP[-1]: state.release_active = False - target_hold = float(np.interp(v_ego, GENESIS_G90_STOP_HOLD_SPEED_BP, GENESIS_G90_STOP_HOLD_ACCEL_V)) + stop_brake_cap = float(np.interp(v_ego, GENESIS_G90_STOP_HOLD_SPEED_BP, GENESIS_G90_STOP_HOLD_ACCEL_V)) + target_hold = min(0.0, max(accel_cmd, stop_brake_cap)) if state.actual_accel < target_hold: relax_step = float(np.interp(v_ego, GENESIS_G90_STOP_HOLD_RELAX_SPEED_BP, GENESIS_G90_STOP_HOLD_RELAX_STEP_V)) state.actual_accel = min(state.actual_accel + relax_step, target_hold) diff --git a/opendbc_repo/opendbc/car/hyundai/carstate.py b/opendbc_repo/opendbc/car/hyundai/carstate.py index 45e5db884..f7f096775 100644 --- a/opendbc_repo/opendbc/car/hyundai/carstate.py +++ b/opendbc_repo/opendbc/car/hyundai/carstate.py @@ -7,7 +7,8 @@ from opendbc.can import CANDefine, CANParser from opendbc.car import Bus, create_button_events, structs from opendbc.car.common.conversions import Conversions as CV from opendbc.car.hyundai.hyundaicanfd import CanBus -from opendbc.car.hyundai.values import HyundaiFlags, HyundaiStarPilotFlags, CAR, DBC, Buttons, CarControllerParams +from opendbc.car.hyundai.values import HyundaiFlags, HyundaiStarPilotFlags, CAR, DBC, Buttons, CarControllerParams, \ + hyundai_cancel_button_enables_cruise from opendbc.car.interfaces import CarStateBase ButtonType = structs.CarState.ButtonEvent.Type @@ -59,6 +60,7 @@ class CarState(CarStateBase): self.left_paddle = 0 self.mode_button = 0 self.custom_button = 0 + self.cancel_button_enable_in_progress = False self.gear_msg_canfd = "ACCELERATOR" if CP.flags & HyundaiFlags.EV else \ "GEAR_ALT" if CP.flags & HyundaiFlags.CANFD_ALT_GEARS else \ @@ -109,6 +111,25 @@ class CarState(CarStateBase): # Main button also can trigger an engagement on these cars return any(btn in ENABLE_BUTTONS for btn in self.cruise_buttons) or any(self.main_buttons) + def create_cruise_button_events(self, cur_button: int, prev_button: int) -> list[structs.CarState.ButtonEvent]: + if cur_button != prev_button and prev_button != Buttons.CANCEL and cur_button == Buttons.CANCEL: + self.cancel_button_enable_in_progress = ( + self.CP.openpilotLongitudinalControl and + hyundai_cancel_button_enables_cruise(self.CP.carFingerprint) and + not self.out.cruiseState.enabled + ) + + buttons_dict = BUTTONS_DICT + if self.cancel_button_enable_in_progress: + buttons_dict = BUTTONS_DICT | {Buttons.CANCEL: ButtonType.accelCruise} + + events = create_button_events(cur_button, prev_button, buttons_dict) + + if cur_button != Buttons.CANCEL: + self.cancel_button_enable_in_progress = False + + return events + def update(self, can_parsers, starpilot_toggles) -> structs.CarState: cp = can_parsers[Bus.pt] cp_cam = can_parsers[Bus.cam] @@ -249,7 +270,7 @@ class CarState(CarStateBase): if self.CP.flags & HyundaiFlags.HAS_LDA_BUTTON: self.lda_button = cp.vl["BCM_PO_11"]["LDA_BTN"] - ret.buttonEvents = [*create_button_events(self.cruise_buttons[-1], prev_cruise_buttons, BUTTONS_DICT), + ret.buttonEvents = [*self.create_cruise_button_events(self.cruise_buttons[-1], prev_cruise_buttons), *create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise}), *create_button_events(self.lda_button, prev_lda_button, {1: ButtonType.lkas})] @@ -375,7 +396,7 @@ class CarState(CarStateBase): if cp.ts_nanos["BLINKER_STALKS"]["CHECKSUM_MAYBE"] > 0: self.stock_blinker_stalks_ts = cp.ts_nanos["BLINKER_STALKS"]["CHECKSUM_MAYBE"] - ret.buttonEvents = [*create_button_events(self.cruise_buttons[-1], prev_cruise_buttons, BUTTONS_DICT), + ret.buttonEvents = [*self.create_cruise_button_events(self.cruise_buttons[-1], prev_cruise_buttons), *create_button_events(self.main_buttons[-1], prev_main_buttons, {1: ButtonType.mainCruise}), *create_button_events(self.lda_button, prev_lda_button, {1: ButtonType.lkas}), *create_button_events(self.left_paddle, prev_left_paddle, {1: ButtonType.altButton2})] diff --git a/opendbc_repo/opendbc/car/hyundai/interface.py b/opendbc_repo/opendbc/car/hyundai/interface.py index a11343b8f..13342f2a9 100644 --- a/opendbc_repo/opendbc/car/hyundai/interface.py +++ b/opendbc_repo/opendbc/car/hyundai/interface.py @@ -4,7 +4,8 @@ from opendbc.car.hyundai.hyundaicanfd import CanBus from opendbc.car.hyundai.values import HyundaiFlags, CAR, DBC, \ CANFD_UNSUPPORTED_LONGITUDINAL_CAR, \ CANFD_SECURITYACCESS_CAR, \ - UNSUPPORTED_LONGITUDINAL_CAR, HyundaiSafetyFlags + UNSUPPORTED_LONGITUDINAL_CAR, HyundaiSafetyFlags, \ + hyundai_cancel_button_enables_cruise from opendbc.car.hyundai.radar_interface import RADAR_START_ADDR from opendbc.car.interfaces import CarInterfaceBase from opendbc.car.disable_ecu import disable_ecu, ecu_log @@ -138,6 +139,8 @@ class CarInterface(CarInterfaceBase): ret.flags |= HyundaiFlags.HAS_LDA_BUTTON.value if ret.flags & HyundaiFlags.CAN_CANFD_BLENDED: ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CAN_CANFD_BLENDED.value + if hyundai_cancel_button_enables_cruise(candidate): + ret.safetyConfigs[-1].safetyParam |= HyundaiSafetyFlags.CANCEL_BTN_ENABLE.value if candidate == CAR.KIA_FORTE: has_scc_fw = any(fw.ecu == Ecu.fwdRadar for fw in car_fw) @@ -189,6 +192,10 @@ class CarInterface(CarInterfaceBase): # Car specific configuration overrides + if candidate == CAR.GENESIS_G90: + ret.stoppingDecelRate = 0.55 + ret.vEgoStopping = 0.8 + if candidate == CAR.KIA_OPTIMA_G4_FL: ret.steerActuatorDelay = 0.2 diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 60b73c0d5..517731d54 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -17,7 +17,7 @@ from opendbc.car.hyundai.radar_interface import RADAR_START_ADDR from opendbc.car.hyundai.values import CAMERA_SCC_CAR, CANFD_CAR, CAN_GEARS, CAR, CHECKSUM, DATE_FW_ECUS, \ HYBRID_CAR, EV_CAR, FW_QUERY_CONFIG, LEGACY_SAFETY_MODE_CAR, CANFD_FUZZY_WHITELIST, \ UNSUPPORTED_LONGITUDINAL_CAR, PLATFORM_CODE_ECUS, HYUNDAI_VERSION_REQUEST_LONG, \ - CarControllerParams, DBC, HyundaiFlags, get_platform_codes, HyundaiSafetyFlags + CarControllerParams, DBC, HyundaiFlags, get_platform_codes, HyundaiSafetyFlags, Buttons LongCtrlState = CarControl.Actuators.LongControlState from opendbc.car.hyundai.fingerprints import FW_VERSIONS @@ -101,6 +101,24 @@ class TestHyundaiFingerprint: assert palisade_2023.flags & HyundaiFlags.CAN_CANFD_BLENDED assert DBC[palisade_2023.carFingerprint][Bus.pt] == "hyundai_palisade_2023_generated" assert palisade_2023.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CAN_CANFD_BLENDED + assert palisade_2023.safetyConfigs[-1].safetyParam & HyundaiSafetyFlags.CANCEL_BTN_ENABLE + + def test_palisade_2023_pause_resume_button_maps_to_enable(self): + toggles = get_test_toggles() + CP = CarInterface.get_params(CAR.HYUNDAI_PALISADE_2023, gen_empty_fingerprint(), [], True, False, False, toggles) + FPCP = CarInterface.get_starpilot_params(CAR.HYUNDAI_PALISADE_2023, gen_empty_fingerprint(), [], CP, toggles) + car_state = CarState(CP, FPCP) + + car_state.out.cruiseState.enabled = False + events = car_state.create_cruise_button_events(Buttons.CANCEL, Buttons.NONE) + assert [(be.type, be.pressed) for be in events] == [(ButtonType.accelCruise, True)] + + events = car_state.create_cruise_button_events(Buttons.NONE, Buttons.CANCEL) + assert [(be.type, be.pressed) for be in events] == [(ButtonType.accelCruise, False)] + + car_state.out.cruiseState.enabled = True + events = car_state.create_cruise_button_events(Buttons.CANCEL, Buttons.NONE) + assert [(be.type, be.pressed) for be in events] == [(ButtonType.cancel, True)] def test_palisade_2023_disable_failure_falls_back_to_stock_acc(self, monkeypatch): toggles = get_test_toggles() @@ -130,6 +148,13 @@ class TestHyundaiFingerprint: assert CP.longitudinalActuatorDelay == pytest.approx(0.5) assert CP.startingState + def test_genesis_g90_longitudinal_params_bias_toward_earlier_stop_handoff(self): + toggles = get_test_toggles() + CP = CarInterface.get_params(CAR.GENESIS_G90, gen_empty_fingerprint(), [], True, False, False, toggles) + + assert CP.vEgoStopping == pytest.approx(0.8) + assert CP.stoppingDecelRate == pytest.approx(0.55) + def test_kia_forte_no_scc_fw_match(self): car_fw = [ CarParams.CarFw( @@ -333,7 +358,7 @@ class TestHyundaiFingerprint: state = update_genesis_g90_longitudinal_tuning(state, accel_cmd=-2.0, v_ego=0.02, long_control_state=LongCtrlState.stopping, long_active=True) - assert state.actual_accel == pytest.approx(-0.12) + assert state.actual_accel == pytest.approx(-0.10) assert not state.release_active def test_genesis_g90_longitudinal_tuning_gradually_unwinds_into_stop_hold(self): @@ -341,11 +366,22 @@ class TestHyundaiFingerprint: state = update_genesis_g90_longitudinal_tuning(state, accel_cmd=-2.0, v_ego=0.5, long_control_state=LongCtrlState.stopping, long_active=True) - assert state.actual_accel == pytest.approx(-1.975) + assert state.actual_accel == pytest.approx(-1.96) state = update_genesis_g90_longitudinal_tuning(state, accel_cmd=-2.0, v_ego=0.3, long_control_state=LongCtrlState.stopping, long_active=True) - assert state.actual_accel == pytest.approx(-1.935) + assert state.actual_accel == pytest.approx(-1.9) + + def test_genesis_g90_longitudinal_tuning_caps_low_speed_stop_brake(self): + state = GenesisG90LongitudinalTuningState(actual_accel=-1.0) + + state = update_genesis_g90_longitudinal_tuning(state, accel_cmd=-0.4, v_ego=0.3, + long_control_state=LongCtrlState.stopping, long_active=True) + assert state.actual_accel == pytest.approx(-0.94) + + state = update_genesis_g90_longitudinal_tuning(state, accel_cmd=-0.4, v_ego=0.3, + long_control_state=LongCtrlState.stopping, long_active=True) + assert state.actual_accel == pytest.approx(-0.88) def test_genesis_g90_longitudinal_tuning_ramps_out_of_stop_hold(self): state = GenesisG90LongitudinalTuningState(actual_accel=-0.12, long_control_state_last=LongCtrlState.stopping) diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index b4155975b..f8bbadcc6 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -107,6 +107,7 @@ class HyundaiSafetyFlags(IntFlag): CANFD_ANGLE_STEERING = 1024 NON_SCC = 4096 CAN_CANFD_BLENDED = 8192 + CANCEL_BTN_ENABLE = 16384 class HyundaiStarPilotSafetyFlags(IntFlag): @@ -672,6 +673,15 @@ class Buttons: CANCEL = 4 # on newer models, this is a pause/resume button +CANCEL_BUTTON_ENABLE_CARS = frozenset({ + CAR.HYUNDAI_PALISADE_2023, +}) + + +def hyundai_cancel_button_enables_cruise(car_fingerprint) -> bool: + return car_fingerprint in CANCEL_BUTTON_ENABLE_CARS + + def get_platform_codes(fw_versions: list[bytes]) -> set[tuple[bytes, bytes | None]]: # Returns unique, platform-specific identification codes for a set of versions codes = set() # (code-Optional[part], date) diff --git a/opendbc_repo/opendbc/car/tests/test_car_interfaces.py b/opendbc_repo/opendbc/car/tests/test_car_interfaces.py index 67101f05e..3b50894a1 100644 --- a/opendbc_repo/opendbc/car/tests/test_car_interfaces.py +++ b/opendbc_repo/opendbc/car/tests/test_car_interfaces.py @@ -9,7 +9,7 @@ from typing import Any from cereal import custom from opendbc.car import DT_CTRL, CanData, structs -from opendbc.car.car_helpers import interfaces +from opendbc.car.car_helpers import _apply_disable_openpilot_long, interfaces from opendbc.car.chrysler.carstate import CarState as ChryslerCarState from opendbc.car.fingerprints import FW_VERSIONS from opendbc.car.fw_versions import FW_QUERY_CONFIGS @@ -18,6 +18,7 @@ from opendbc.car.hyundai.values import CAR as HYUNDAI_CAR, HyundaiStarPilotSafet from opendbc.car.gm.values import CAR as GM_CAR, CanBus as GMCanBus, GMSafetyFlags from opendbc.car.interfaces import CarInterfaceBase, CarStateBase, get_interface_attr from opendbc.car.mock.values import CAR as MOCK +from opendbc.car.toyota.values import CAR as TOYOTA_CAR, ToyotaSafetyFlags from opendbc.car.values import PLATFORMS DrawType = Callable[[st.SearchStrategy], Any] @@ -213,6 +214,40 @@ class TestCarInterfaces: ) assert fp_car_params.safetyConfigs[-1].safetyParam & HyundaiStarPilotSafetyFlags.AOL_LKAS_ON_ENGAGE.value + def test_toyota_disable_openpilot_long_sets_stock_long_safety_flag(self): + CarInterface = interfaces[TOYOTA_CAR.TOYOTA_PRIUS_TSS2] + fingerprint = {bus: {} for bus in range(8)} + toggles = get_test_starpilot_toggles() + + car_params = CarInterface.get_params( + TOYOTA_CAR.TOYOTA_PRIUS_TSS2, + fingerprint, + [], + alpha_long=False, + is_release=False, + docs=False, + starpilot_toggles=toggles, + ) + assert car_params.openpilotLongitudinalControl + assert not car_params.alphaLongitudinalAvailable + assert not (car_params.safetyConfigs[0].safetyParam & ToyotaSafetyFlags.STOCK_LONGITUDINAL.value) + + fp_car_params = CarInterface.get_starpilot_params( + TOYOTA_CAR.TOYOTA_PRIUS_TSS2, + fingerprint, + [], + car_params, + toggles, + ) + + _apply_disable_openpilot_long(car_params, fp_car_params) + + assert not car_params.openpilotLongitudinalControl + assert car_params.pcmCruise + assert fp_car_params.openpilotLongitudinalControlDisabled + assert car_params.safetyConfigs[0].safetyParam & ToyotaSafetyFlags.STOCK_LONGITUDINAL.value + assert fp_car_params.safetyConfigs[0].safetyParam & ToyotaSafetyFlags.STOCK_LONGITUDINAL.value + # FIXME: Due to the lists used in carParams, Phase.target is very slow and will cause # many generated examples to overrun when max_examples > ~20, don't use it @pytest.mark.parametrize("car_name", sorted(PLATFORMS)) diff --git a/opendbc_repo/opendbc/safety/modes/hyundai_common.h b/opendbc_repo/opendbc/safety/modes/hyundai_common.h index f9ab4e02d..0b2d91e3d 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai_common.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai_common.h @@ -54,6 +54,9 @@ bool hyundai_aol_lkas_on_engage = false; extern bool hyundai_non_scc; bool hyundai_non_scc = false; +extern bool hyundai_cancel_button_enable; +bool hyundai_cancel_button_enable = false; + static uint8_t hyundai_last_button_interaction; // button messages since the user pressed an enable button void hyundai_common_init(uint16_t param) { @@ -69,6 +72,7 @@ void hyundai_common_init(uint16_t param) { const uint16_t HYUNDAI_PARAM_AOL_LKAS_ON_ENGAGE = 2048; const uint16_t HYUNDAI_PARAM_NON_SCC = 4096; const uint16_t HYUNDAI_PARAM_CAN_CANFD_BLENDED = 8192; + const uint16_t HYUNDAI_PARAM_CANCEL_BTN_ENABLE = 16384; hyundai_ev_gas_signal = GET_FLAG(param, HYUNDAI_PARAM_EV_GAS); hyundai_hybrid_gas_signal = !hyundai_ev_gas_signal && GET_FLAG(param, HYUNDAI_PARAM_HYBRID_GAS); @@ -82,6 +86,7 @@ void hyundai_common_init(uint16_t param) { hyundai_has_lda_button = GET_FLAG(param, HYUNDAI_PARAM_HAS_LDA_BUTTON); hyundai_aol_lkas_on_engage = GET_FLAG(param, HYUNDAI_PARAM_AOL_LKAS_ON_ENGAGE); hyundai_non_scc = GET_FLAG(param, HYUNDAI_PARAM_NON_SCC); + hyundai_cancel_button_enable = GET_FLAG(param, HYUNDAI_PARAM_CANCEL_BTN_ENABLE); hyundai_last_button_interaction = HYUNDAI_PREV_BUTTON_SAMPLES; @@ -125,7 +130,9 @@ void hyundai_common_cruise_buttons_check(const int cruise_button, const bool mai // enter controls on falling edge of resume or set bool set = (cruise_button != HYUNDAI_BTN_SET) && (cruise_button_prev == HYUNDAI_BTN_SET); bool res = (cruise_button != HYUNDAI_BTN_RESUME) && (cruise_button_prev == HYUNDAI_BTN_RESUME); - if (set || res) { + bool cancel_enable = hyundai_cancel_button_enable && !cruise_engaged_prev && + (cruise_button != HYUNDAI_BTN_CANCEL) && (cruise_button_prev == HYUNDAI_BTN_CANCEL); + if (set || res || cancel_enable) { controls_allowed = true; if (hyundai_aol_lkas_on_engage && ((alternative_experience & ALT_EXP_ALWAYS_ON_LATERAL) != 0)) { @@ -134,7 +141,7 @@ void hyundai_common_cruise_buttons_check(const int cruise_button, const bool mai } // exit controls on cancel press - if (cruise_button == HYUNDAI_BTN_CANCEL) { + if ((cruise_button == HYUNDAI_BTN_CANCEL) && !(hyundai_cancel_button_enable && !cruise_engaged_prev)) { controls_allowed = false; } diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai.py b/opendbc_repo/opendbc/safety/tests/test_hyundai.py index f41bf3128..db09e39c3 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai.py @@ -291,7 +291,8 @@ class TestHyundaiCanCanfdBlendedLongitudinalSafety(HyundaiLongitudinalBase, Test def setUp(self): self.packer = CANPackerSafety("hyundai_palisade_2023_generated") self.safety = libsafety_py.libsafety - self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, HyundaiSafetyFlags.CAN_CANFD_BLENDED | HyundaiSafetyFlags.LONG) + self.safety.set_safety_hooks(CarParams.SafetyModel.hyundai, + HyundaiSafetyFlags.CAN_CANFD_BLENDED | HyundaiSafetyFlags.LONG | HyundaiSafetyFlags.CANCEL_BTN_ENABLE) self.safety.init_tests() def _accel_msg(self, accel, aeb_req=False, aeb_decel=0): @@ -304,6 +305,15 @@ class TestHyundaiCanCanfdBlendedLongitudinalSafety(HyundaiLongitudinalBase, Test def test_no_aeb_fca11(self): pass + def test_cancel_button_enables_controls_from_standby(self): + self.assertFalse(self.safety.get_controls_allowed()) + + self._rx(self._button_msg(Buttons.CANCEL)) + self.assertFalse(self.safety.get_controls_allowed()) + + self._rx(self._button_msg(Buttons.NONE)) + self.assertTrue(self.safety.get_controls_allowed()) + class TestHyundaiLongitudinalSafetyCameraSCC(HyundaiLongitudinalBase, TestHyundaiSafety): TX_MSGS = [[0x340, 0], [0x4F1, 2], [0x485, 0], [0x420, 0], [0x421, 0], [0x50A, 0], [0x389, 0], [0x4A2, 0]] diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 0282bf95a..dacc62d6a 100644 Binary files a/panda/board/obj/body_h7.bin.signed and b/panda/board/obj/body_h7.bin.signed differ diff --git a/panda/board/obj/body_h7/bootstub.elf b/panda/board/obj/body_h7/bootstub.elf index 09fc1ac57..a73402cf8 100755 Binary files a/panda/board/obj/body_h7/bootstub.elf and b/panda/board/obj/body_h7/bootstub.elf differ diff --git a/panda/board/obj/body_h7/main.bin b/panda/board/obj/body_h7/main.bin index a999ddda6..7c0782403 100755 Binary files a/panda/board/obj/body_h7/main.bin and b/panda/board/obj/body_h7/main.bin differ diff --git a/panda/board/obj/body_h7/main.elf b/panda/board/obj/body_h7/main.elf index e2d2a425e..76cb8aa0d 100755 Binary files a/panda/board/obj/body_h7/main.elf and b/panda/board/obj/body_h7/main.elf differ diff --git a/panda/board/obj/bootstub.body_h7.bin b/panda/board/obj/bootstub.body_h7.bin index 5a5f4a434..bbf36b7fa 100755 Binary files a/panda/board/obj/bootstub.body_h7.bin and b/panda/board/obj/bootstub.body_h7.bin differ diff --git a/panda/board/obj/bootstub.panda.bin b/panda/board/obj/bootstub.panda.bin index af672ae66..f099ead16 100755 Binary files a/panda/board/obj/bootstub.panda.bin and b/panda/board/obj/bootstub.panda.bin differ diff --git a/panda/board/obj/bootstub.panda_h7.bin b/panda/board/obj/bootstub.panda_h7.bin index 8a606d3e3..bd0992e8e 100755 Binary files a/panda/board/obj/bootstub.panda_h7.bin and b/panda/board/obj/bootstub.panda_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_remote.bin b/panda/board/obj/bootstub.panda_h7_remote.bin index 8a606d3e3..bd0992e8e 100755 Binary files a/panda/board/obj/bootstub.panda_h7_remote.bin and b/panda/board/obj/bootstub.panda_h7_remote.bin differ diff --git a/panda/board/obj/bootstub.panda_jungle_h7.bin b/panda/board/obj/bootstub.panda_jungle_h7.bin index 8ffd5945b..2d4aa7d49 100755 Binary files a/panda/board/obj/bootstub.panda_jungle_h7.bin and b/panda/board/obj/bootstub.panda_jungle_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_remote.bin b/panda/board/obj/bootstub.panda_remote.bin index af672ae66..f099ead16 100755 Binary files a/panda/board/obj/bootstub.panda_remote.bin and b/panda/board/obj/bootstub.panda_remote.bin differ diff --git a/panda/board/obj/gitversion.h b/panda/board/obj/gitversion.h index e1536bfee..84ade634b 100644 --- a/panda/board/obj/gitversion.h +++ b/panda/board/obj/gitversion.h @@ -1,2 +1,2 @@ extern const uint8_t gitversion[19]; -const uint8_t gitversion[19] = "DEV-87327537-DEBUG"; +const uint8_t gitversion[19] = "DEV-5312ce30-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 098935441..7a9dba447 100644 Binary files a/panda/board/obj/panda.bin.signed and b/panda/board/obj/panda.bin.signed differ diff --git a/panda/board/obj/panda/bootstub.elf b/panda/board/obj/panda/bootstub.elf index 495404e16..fa68f6bf7 100755 Binary files a/panda/board/obj/panda/bootstub.elf and b/panda/board/obj/panda/bootstub.elf differ diff --git a/panda/board/obj/panda/main.bin b/panda/board/obj/panda/main.bin index 1c44a379c..9f7b0f0e9 100755 Binary files a/panda/board/obj/panda/main.bin and b/panda/board/obj/panda/main.bin differ diff --git a/panda/board/obj/panda/main.elf b/panda/board/obj/panda/main.elf index 3482bf11d..4bf137d38 100755 Binary files a/panda/board/obj/panda/main.elf and b/panda/board/obj/panda/main.elf differ diff --git a/panda/board/obj/panda_h7.bin.signed b/panda/board/obj/panda_h7.bin.signed index 7f732d313..94f79a12a 100644 Binary files a/panda/board/obj/panda_h7.bin.signed and b/panda/board/obj/panda_h7.bin.signed differ diff --git a/panda/board/obj/panda_h7/bootstub.elf b/panda/board/obj/panda_h7/bootstub.elf index ab40185ad..f53e3ccda 100755 Binary files a/panda/board/obj/panda_h7/bootstub.elf and b/panda/board/obj/panda_h7/bootstub.elf differ diff --git a/panda/board/obj/panda_h7/main.bin b/panda/board/obj/panda_h7/main.bin index 63f3e17ed..debeb0b54 100755 Binary files a/panda/board/obj/panda_h7/main.bin and b/panda/board/obj/panda_h7/main.bin differ diff --git a/panda/board/obj/panda_h7/main.elf b/panda/board/obj/panda_h7/main.elf index c08ae6700..237ab3a54 100755 Binary files a/panda/board/obj/panda_h7/main.elf and b/panda/board/obj/panda_h7/main.elf differ diff --git a/panda/board/obj/panda_h7_remote.bin.signed b/panda/board/obj/panda_h7_remote.bin.signed index 677c26614..e539d72a6 100644 Binary files a/panda/board/obj/panda_h7_remote.bin.signed and b/panda/board/obj/panda_h7_remote.bin.signed differ diff --git a/panda/board/obj/panda_h7_remote/bootstub.elf b/panda/board/obj/panda_h7_remote/bootstub.elf index 437276a85..d8d582430 100755 Binary files a/panda/board/obj/panda_h7_remote/bootstub.elf and b/panda/board/obj/panda_h7_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_remote/main.bin b/panda/board/obj/panda_h7_remote/main.bin index c70aa891f..4f492ac35 100755 Binary files a/panda/board/obj/panda_h7_remote/main.bin and b/panda/board/obj/panda_h7_remote/main.bin differ diff --git a/panda/board/obj/panda_h7_remote/main.elf b/panda/board/obj/panda_h7_remote/main.elf index 79b613601..fc0bd0b8b 100755 Binary files a/panda/board/obj/panda_h7_remote/main.elf and b/panda/board/obj/panda_h7_remote/main.elf differ diff --git a/panda/board/obj/panda_jungle_h7.bin.signed b/panda/board/obj/panda_jungle_h7.bin.signed index 523b2413c..31de76a33 100644 Binary files a/panda/board/obj/panda_jungle_h7.bin.signed and b/panda/board/obj/panda_jungle_h7.bin.signed differ diff --git a/panda/board/obj/panda_jungle_h7/bootstub.elf b/panda/board/obj/panda_jungle_h7/bootstub.elf index 3e5b1342d..6aad6d5bb 100755 Binary files a/panda/board/obj/panda_jungle_h7/bootstub.elf and b/panda/board/obj/panda_jungle_h7/bootstub.elf differ diff --git a/panda/board/obj/panda_jungle_h7/main.bin b/panda/board/obj/panda_jungle_h7/main.bin index f9f2ae4b2..c85453a0c 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.bin and b/panda/board/obj/panda_jungle_h7/main.bin differ diff --git a/panda/board/obj/panda_jungle_h7/main.elf b/panda/board/obj/panda_jungle_h7/main.elf index 958ea465f..31a56dc1d 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.elf and b/panda/board/obj/panda_jungle_h7/main.elf differ diff --git a/panda/board/obj/panda_remote.bin.signed b/panda/board/obj/panda_remote.bin.signed index 88df71442..85bb2491e 100644 Binary files a/panda/board/obj/panda_remote.bin.signed and b/panda/board/obj/panda_remote.bin.signed differ diff --git a/panda/board/obj/panda_remote/bootstub.elf b/panda/board/obj/panda_remote/bootstub.elf index 459b28cb9..4f67db0fe 100755 Binary files a/panda/board/obj/panda_remote/bootstub.elf and b/panda/board/obj/panda_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_remote/main.bin b/panda/board/obj/panda_remote/main.bin index 7bb7b49d9..d3a24caed 100755 Binary files a/panda/board/obj/panda_remote/main.bin and b/panda/board/obj/panda_remote/main.bin differ diff --git a/panda/board/obj/panda_remote/main.elf b/panda/board/obj/panda_remote/main.elf index 3e5fcedba..fce025783 100755 Binary files a/panda/board/obj/panda_remote/main.elf and b/panda/board/obj/panda_remote/main.elf differ diff --git a/panda/board/obj/version b/panda/board/obj/version index 6cbaaa652..4a55a7f95 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-87327537-DEBUG \ No newline at end of file +DEV-5312ce30-DEBUG \ No newline at end of file diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 1c8449e9b..4cdfaf703 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -15,13 +15,13 @@ def civic_bosch_modified_lateral_testing_ground_active() -> bool: def get_civic_bosch_modified_pid_output_scale(desired_angle_deg: float, desired_angle_delta_deg: float, v_ego: float) -> float: abs_angle = abs(desired_angle_deg) speed_weight = min(max((v_ego - 4.0) / 10.0, 0.0), 1.0) - center_speed_weight = 0.55 + (0.45 * speed_weight) - center_weight = min(max((12.0 - abs_angle) / 12.0, 0.0), 1.0) - angle_weight = min(max((abs_angle - 12.0) / 16.0, 0.0), 1.0) + center_speed_weight = 0.60 + (0.40 * speed_weight) + center_weight = min(max((14.0 - abs_angle) / 14.0, 0.0), 1.0) + angle_weight = min(max((abs_angle - 14.0) / 14.0, 0.0), 1.0) phase = desired_angle_deg * desired_angle_delta_deg is_left = desired_angle_deg > 0.0 - center_taper = 0.22 + center_taper = 0.26 base_scale = 0.10 if is_left else 0.12 turn_in_scale = 0.10 if is_left else 0.14 unwind_scale = 0.14 if is_left else 0.20 diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 61a89041e..a0eb8a976 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -211,21 +211,21 @@ IONIQ_6_FF_CUTOFF = 0.48 IONIQ_6_FF_CUTOFF_WIDTH = 0.12 IONIQ_6_TRANSITION_SPEED = 10.0 IONIQ_6_PHASE_SCALE = 0.10 -IONIQ_6_TURN_IN_BOOST_LEFT = 1.02 -IONIQ_6_TURN_IN_BOOST_RIGHT = 1.12 -IONIQ_6_UNWIND_TAPER_LEFT = 1.66 -IONIQ_6_UNWIND_TAPER_RIGHT = 3.50 +IONIQ_6_TURN_IN_BOOST_LEFT = 1.08 +IONIQ_6_TURN_IN_BOOST_RIGHT = 1.24 +IONIQ_6_UNWIND_TAPER_LEFT = 1.84 +IONIQ_6_UNWIND_TAPER_RIGHT = 4.10 IONIQ_6_FRICTION_MULT = 0.995 IONIQ_6_FRICTION_LAT_RISE = 0.20 IONIQ_6_FRICTION_JERK_RISE = 0.24 -IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.31 -IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.46 -IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 1.92 -IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 4.40 -IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.16 -IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.28 -IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 1.66 -IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 3.95 +IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.34 +IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.54 +IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 2.15 +IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 5.00 +IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.18 +IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.34 +IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 1.86 +IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 4.55 IONIQ_6_CENTER_TAPER_MAX = 0.046 IONIQ_6_CENTER_TAPER_LAT = 0.18 IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.02 @@ -242,8 +242,8 @@ IONIQ_6_DIRECTIONAL_TAPER_LAT_END = 0.90 IONIQ_6_DIRECTIONAL_TAPER_LAT_WIDTH = 0.08 IONIQ_6_DIRECTIONAL_TAPER_BASE_LEFT = 0.05 IONIQ_6_DIRECTIONAL_TAPER_BASE_RIGHT = 0.47 -IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 0.90 -IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 2.30 +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_LEFT = 1.00 +IONIQ_6_DIRECTIONAL_TAPER_UNWIND_RIGHT = 2.65 IONIQ_6_OUTPUT_TAPER_SPEED = 8.5 IONIQ_6_OUTPUT_TAPER_SPEED_WIDTH = 2.5 IONIQ_6_OUTPUT_CENTER_TAPER_BLEND = 0.90 diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 44c9c5721..811a2bf3a 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -383,8 +383,9 @@ class TestLatControl: assert get_civic_bosch_modified_pid_output_scale(0.0, 0.0, 12.0) < 1.0 assert get_civic_bosch_modified_pid_output_scale(8.0, 0.0, 12.0) < 1.0 assert get_civic_bosch_modified_pid_output_scale(10.0, 0.0, 12.0) < 1.0 + assert get_civic_bosch_modified_pid_output_scale(12.0, 0.0, 12.0) < 1.0 assert get_civic_bosch_modified_pid_output_scale(0.0, 0.0, 6.0) < 0.9 - assert get_civic_bosch_modified_pid_output_scale(12.0, 0.0, 12.0) > get_civic_bosch_modified_pid_output_scale(8.0, 0.0, 12.0) + assert get_civic_bosch_modified_pid_output_scale(14.0, 0.0, 12.0) > get_civic_bosch_modified_pid_output_scale(8.0, 0.0, 12.0) assert get_civic_bosch_modified_pid_output_scale(20.0, 0.5, 12.0) > get_civic_bosch_modified_pid_output_scale(20.0, 0.0, 12.0) assert get_civic_bosch_modified_pid_output_scale(-20.0, -0.5, 12.0) > get_civic_bosch_modified_pid_output_scale(-20.0, 0.0, 12.0) assert get_civic_bosch_modified_pid_output_scale(20.0, -0.5, 12.0) < get_civic_bosch_modified_pid_output_scale(20.0, 0.0, 12.0) diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 019e9d8cd..95603dbf6 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_7404715378150782082) { - out_7404715378150782082[0] = delta_x[0] + nom_x[0]; - out_7404715378150782082[1] = delta_x[1] + nom_x[1]; - out_7404715378150782082[2] = delta_x[2] + nom_x[2]; - out_7404715378150782082[3] = delta_x[3] + nom_x[3]; - out_7404715378150782082[4] = delta_x[4] + nom_x[4]; - out_7404715378150782082[5] = delta_x[5] + nom_x[5]; - out_7404715378150782082[6] = delta_x[6] + nom_x[6]; - out_7404715378150782082[7] = delta_x[7] + nom_x[7]; - out_7404715378150782082[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_8994217345304474551) { + out_8994217345304474551[0] = delta_x[0] + nom_x[0]; + out_8994217345304474551[1] = delta_x[1] + nom_x[1]; + out_8994217345304474551[2] = delta_x[2] + nom_x[2]; + out_8994217345304474551[3] = delta_x[3] + nom_x[3]; + out_8994217345304474551[4] = delta_x[4] + nom_x[4]; + out_8994217345304474551[5] = delta_x[5] + nom_x[5]; + out_8994217345304474551[6] = delta_x[6] + nom_x[6]; + out_8994217345304474551[7] = delta_x[7] + nom_x[7]; + out_8994217345304474551[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_1067581222048175770) { - out_1067581222048175770[0] = -nom_x[0] + true_x[0]; - out_1067581222048175770[1] = -nom_x[1] + true_x[1]; - out_1067581222048175770[2] = -nom_x[2] + true_x[2]; - out_1067581222048175770[3] = -nom_x[3] + true_x[3]; - out_1067581222048175770[4] = -nom_x[4] + true_x[4]; - out_1067581222048175770[5] = -nom_x[5] + true_x[5]; - out_1067581222048175770[6] = -nom_x[6] + true_x[6]; - out_1067581222048175770[7] = -nom_x[7] + true_x[7]; - out_1067581222048175770[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_8635883422281677209) { + out_8635883422281677209[0] = -nom_x[0] + true_x[0]; + out_8635883422281677209[1] = -nom_x[1] + true_x[1]; + out_8635883422281677209[2] = -nom_x[2] + true_x[2]; + out_8635883422281677209[3] = -nom_x[3] + true_x[3]; + out_8635883422281677209[4] = -nom_x[4] + true_x[4]; + out_8635883422281677209[5] = -nom_x[5] + true_x[5]; + out_8635883422281677209[6] = -nom_x[6] + true_x[6]; + out_8635883422281677209[7] = -nom_x[7] + true_x[7]; + out_8635883422281677209[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_7894099950222537712) { - out_7894099950222537712[0] = 1.0; - out_7894099950222537712[1] = 0.0; - out_7894099950222537712[2] = 0.0; - out_7894099950222537712[3] = 0.0; - out_7894099950222537712[4] = 0.0; - out_7894099950222537712[5] = 0.0; - out_7894099950222537712[6] = 0.0; - out_7894099950222537712[7] = 0.0; - out_7894099950222537712[8] = 0.0; - out_7894099950222537712[9] = 0.0; - out_7894099950222537712[10] = 1.0; - out_7894099950222537712[11] = 0.0; - out_7894099950222537712[12] = 0.0; - out_7894099950222537712[13] = 0.0; - out_7894099950222537712[14] = 0.0; - out_7894099950222537712[15] = 0.0; - out_7894099950222537712[16] = 0.0; - out_7894099950222537712[17] = 0.0; - out_7894099950222537712[18] = 0.0; - out_7894099950222537712[19] = 0.0; - out_7894099950222537712[20] = 1.0; - out_7894099950222537712[21] = 0.0; - out_7894099950222537712[22] = 0.0; - out_7894099950222537712[23] = 0.0; - out_7894099950222537712[24] = 0.0; - out_7894099950222537712[25] = 0.0; - out_7894099950222537712[26] = 0.0; - out_7894099950222537712[27] = 0.0; - out_7894099950222537712[28] = 0.0; - out_7894099950222537712[29] = 0.0; - out_7894099950222537712[30] = 1.0; - out_7894099950222537712[31] = 0.0; - out_7894099950222537712[32] = 0.0; - out_7894099950222537712[33] = 0.0; - out_7894099950222537712[34] = 0.0; - out_7894099950222537712[35] = 0.0; - out_7894099950222537712[36] = 0.0; - out_7894099950222537712[37] = 0.0; - out_7894099950222537712[38] = 0.0; - out_7894099950222537712[39] = 0.0; - out_7894099950222537712[40] = 1.0; - out_7894099950222537712[41] = 0.0; - out_7894099950222537712[42] = 0.0; - out_7894099950222537712[43] = 0.0; - out_7894099950222537712[44] = 0.0; - out_7894099950222537712[45] = 0.0; - out_7894099950222537712[46] = 0.0; - out_7894099950222537712[47] = 0.0; - out_7894099950222537712[48] = 0.0; - out_7894099950222537712[49] = 0.0; - out_7894099950222537712[50] = 1.0; - out_7894099950222537712[51] = 0.0; - out_7894099950222537712[52] = 0.0; - out_7894099950222537712[53] = 0.0; - out_7894099950222537712[54] = 0.0; - out_7894099950222537712[55] = 0.0; - out_7894099950222537712[56] = 0.0; - out_7894099950222537712[57] = 0.0; - out_7894099950222537712[58] = 0.0; - out_7894099950222537712[59] = 0.0; - out_7894099950222537712[60] = 1.0; - out_7894099950222537712[61] = 0.0; - out_7894099950222537712[62] = 0.0; - out_7894099950222537712[63] = 0.0; - out_7894099950222537712[64] = 0.0; - out_7894099950222537712[65] = 0.0; - out_7894099950222537712[66] = 0.0; - out_7894099950222537712[67] = 0.0; - out_7894099950222537712[68] = 0.0; - out_7894099950222537712[69] = 0.0; - out_7894099950222537712[70] = 1.0; - out_7894099950222537712[71] = 0.0; - out_7894099950222537712[72] = 0.0; - out_7894099950222537712[73] = 0.0; - out_7894099950222537712[74] = 0.0; - out_7894099950222537712[75] = 0.0; - out_7894099950222537712[76] = 0.0; - out_7894099950222537712[77] = 0.0; - out_7894099950222537712[78] = 0.0; - out_7894099950222537712[79] = 0.0; - out_7894099950222537712[80] = 1.0; +void H_mod_fun(double *state, double *out_5835492163818982048) { + out_5835492163818982048[0] = 1.0; + out_5835492163818982048[1] = 0.0; + out_5835492163818982048[2] = 0.0; + out_5835492163818982048[3] = 0.0; + out_5835492163818982048[4] = 0.0; + out_5835492163818982048[5] = 0.0; + out_5835492163818982048[6] = 0.0; + out_5835492163818982048[7] = 0.0; + out_5835492163818982048[8] = 0.0; + out_5835492163818982048[9] = 0.0; + out_5835492163818982048[10] = 1.0; + out_5835492163818982048[11] = 0.0; + out_5835492163818982048[12] = 0.0; + out_5835492163818982048[13] = 0.0; + out_5835492163818982048[14] = 0.0; + out_5835492163818982048[15] = 0.0; + out_5835492163818982048[16] = 0.0; + out_5835492163818982048[17] = 0.0; + out_5835492163818982048[18] = 0.0; + out_5835492163818982048[19] = 0.0; + out_5835492163818982048[20] = 1.0; + out_5835492163818982048[21] = 0.0; + out_5835492163818982048[22] = 0.0; + out_5835492163818982048[23] = 0.0; + out_5835492163818982048[24] = 0.0; + out_5835492163818982048[25] = 0.0; + out_5835492163818982048[26] = 0.0; + out_5835492163818982048[27] = 0.0; + out_5835492163818982048[28] = 0.0; + out_5835492163818982048[29] = 0.0; + out_5835492163818982048[30] = 1.0; + out_5835492163818982048[31] = 0.0; + out_5835492163818982048[32] = 0.0; + out_5835492163818982048[33] = 0.0; + out_5835492163818982048[34] = 0.0; + out_5835492163818982048[35] = 0.0; + out_5835492163818982048[36] = 0.0; + out_5835492163818982048[37] = 0.0; + out_5835492163818982048[38] = 0.0; + out_5835492163818982048[39] = 0.0; + out_5835492163818982048[40] = 1.0; + out_5835492163818982048[41] = 0.0; + out_5835492163818982048[42] = 0.0; + out_5835492163818982048[43] = 0.0; + out_5835492163818982048[44] = 0.0; + out_5835492163818982048[45] = 0.0; + out_5835492163818982048[46] = 0.0; + out_5835492163818982048[47] = 0.0; + out_5835492163818982048[48] = 0.0; + out_5835492163818982048[49] = 0.0; + out_5835492163818982048[50] = 1.0; + out_5835492163818982048[51] = 0.0; + out_5835492163818982048[52] = 0.0; + out_5835492163818982048[53] = 0.0; + out_5835492163818982048[54] = 0.0; + out_5835492163818982048[55] = 0.0; + out_5835492163818982048[56] = 0.0; + out_5835492163818982048[57] = 0.0; + out_5835492163818982048[58] = 0.0; + out_5835492163818982048[59] = 0.0; + out_5835492163818982048[60] = 1.0; + out_5835492163818982048[61] = 0.0; + out_5835492163818982048[62] = 0.0; + out_5835492163818982048[63] = 0.0; + out_5835492163818982048[64] = 0.0; + out_5835492163818982048[65] = 0.0; + out_5835492163818982048[66] = 0.0; + out_5835492163818982048[67] = 0.0; + out_5835492163818982048[68] = 0.0; + out_5835492163818982048[69] = 0.0; + out_5835492163818982048[70] = 1.0; + out_5835492163818982048[71] = 0.0; + out_5835492163818982048[72] = 0.0; + out_5835492163818982048[73] = 0.0; + out_5835492163818982048[74] = 0.0; + out_5835492163818982048[75] = 0.0; + out_5835492163818982048[76] = 0.0; + out_5835492163818982048[77] = 0.0; + out_5835492163818982048[78] = 0.0; + out_5835492163818982048[79] = 0.0; + out_5835492163818982048[80] = 1.0; } -void f_fun(double *state, double dt, double *out_18918990587173632) { - out_18918990587173632[0] = state[0]; - out_18918990587173632[1] = state[1]; - out_18918990587173632[2] = state[2]; - out_18918990587173632[3] = state[3]; - out_18918990587173632[4] = state[4]; - out_18918990587173632[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_18918990587173632[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_18918990587173632[7] = state[7]; - out_18918990587173632[8] = state[8]; +void f_fun(double *state, double dt, double *out_9046400166044584491) { + out_9046400166044584491[0] = state[0]; + out_9046400166044584491[1] = state[1]; + out_9046400166044584491[2] = state[2]; + out_9046400166044584491[3] = state[3]; + out_9046400166044584491[4] = state[4]; + out_9046400166044584491[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_9046400166044584491[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_9046400166044584491[7] = state[7]; + out_9046400166044584491[8] = state[8]; } -void F_fun(double *state, double dt, double *out_164532203047016632) { - out_164532203047016632[0] = 1; - out_164532203047016632[1] = 0; - out_164532203047016632[2] = 0; - out_164532203047016632[3] = 0; - out_164532203047016632[4] = 0; - out_164532203047016632[5] = 0; - out_164532203047016632[6] = 0; - out_164532203047016632[7] = 0; - out_164532203047016632[8] = 0; - out_164532203047016632[9] = 0; - out_164532203047016632[10] = 1; - out_164532203047016632[11] = 0; - out_164532203047016632[12] = 0; - out_164532203047016632[13] = 0; - out_164532203047016632[14] = 0; - out_164532203047016632[15] = 0; - out_164532203047016632[16] = 0; - out_164532203047016632[17] = 0; - out_164532203047016632[18] = 0; - out_164532203047016632[19] = 0; - out_164532203047016632[20] = 1; - out_164532203047016632[21] = 0; - out_164532203047016632[22] = 0; - out_164532203047016632[23] = 0; - out_164532203047016632[24] = 0; - out_164532203047016632[25] = 0; - out_164532203047016632[26] = 0; - out_164532203047016632[27] = 0; - out_164532203047016632[28] = 0; - out_164532203047016632[29] = 0; - out_164532203047016632[30] = 1; - out_164532203047016632[31] = 0; - out_164532203047016632[32] = 0; - out_164532203047016632[33] = 0; - out_164532203047016632[34] = 0; - out_164532203047016632[35] = 0; - out_164532203047016632[36] = 0; - out_164532203047016632[37] = 0; - out_164532203047016632[38] = 0; - out_164532203047016632[39] = 0; - out_164532203047016632[40] = 1; - out_164532203047016632[41] = 0; - out_164532203047016632[42] = 0; - out_164532203047016632[43] = 0; - out_164532203047016632[44] = 0; - out_164532203047016632[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_164532203047016632[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_164532203047016632[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_164532203047016632[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_164532203047016632[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_164532203047016632[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_164532203047016632[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_164532203047016632[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_164532203047016632[53] = -9.8100000000000005*dt; - out_164532203047016632[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_164532203047016632[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_164532203047016632[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_164532203047016632[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_164532203047016632[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_164532203047016632[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_164532203047016632[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_164532203047016632[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_164532203047016632[62] = 0; - out_164532203047016632[63] = 0; - out_164532203047016632[64] = 0; - out_164532203047016632[65] = 0; - out_164532203047016632[66] = 0; - out_164532203047016632[67] = 0; - out_164532203047016632[68] = 0; - out_164532203047016632[69] = 0; - out_164532203047016632[70] = 1; - out_164532203047016632[71] = 0; - out_164532203047016632[72] = 0; - out_164532203047016632[73] = 0; - out_164532203047016632[74] = 0; - out_164532203047016632[75] = 0; - out_164532203047016632[76] = 0; - out_164532203047016632[77] = 0; - out_164532203047016632[78] = 0; - out_164532203047016632[79] = 0; - out_164532203047016632[80] = 1; +void F_fun(double *state, double dt, double *out_8585883916966185499) { + out_8585883916966185499[0] = 1; + out_8585883916966185499[1] = 0; + out_8585883916966185499[2] = 0; + out_8585883916966185499[3] = 0; + out_8585883916966185499[4] = 0; + out_8585883916966185499[5] = 0; + out_8585883916966185499[6] = 0; + out_8585883916966185499[7] = 0; + out_8585883916966185499[8] = 0; + out_8585883916966185499[9] = 0; + out_8585883916966185499[10] = 1; + out_8585883916966185499[11] = 0; + out_8585883916966185499[12] = 0; + out_8585883916966185499[13] = 0; + out_8585883916966185499[14] = 0; + out_8585883916966185499[15] = 0; + out_8585883916966185499[16] = 0; + out_8585883916966185499[17] = 0; + out_8585883916966185499[18] = 0; + out_8585883916966185499[19] = 0; + out_8585883916966185499[20] = 1; + out_8585883916966185499[21] = 0; + out_8585883916966185499[22] = 0; + out_8585883916966185499[23] = 0; + out_8585883916966185499[24] = 0; + out_8585883916966185499[25] = 0; + out_8585883916966185499[26] = 0; + out_8585883916966185499[27] = 0; + out_8585883916966185499[28] = 0; + out_8585883916966185499[29] = 0; + out_8585883916966185499[30] = 1; + out_8585883916966185499[31] = 0; + out_8585883916966185499[32] = 0; + out_8585883916966185499[33] = 0; + out_8585883916966185499[34] = 0; + out_8585883916966185499[35] = 0; + out_8585883916966185499[36] = 0; + out_8585883916966185499[37] = 0; + out_8585883916966185499[38] = 0; + out_8585883916966185499[39] = 0; + out_8585883916966185499[40] = 1; + out_8585883916966185499[41] = 0; + out_8585883916966185499[42] = 0; + out_8585883916966185499[43] = 0; + out_8585883916966185499[44] = 0; + out_8585883916966185499[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_8585883916966185499[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_8585883916966185499[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_8585883916966185499[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_8585883916966185499[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_8585883916966185499[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_8585883916966185499[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_8585883916966185499[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_8585883916966185499[53] = -9.8100000000000005*dt; + out_8585883916966185499[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_8585883916966185499[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_8585883916966185499[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8585883916966185499[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8585883916966185499[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_8585883916966185499[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_8585883916966185499[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_8585883916966185499[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_8585883916966185499[62] = 0; + out_8585883916966185499[63] = 0; + out_8585883916966185499[64] = 0; + out_8585883916966185499[65] = 0; + out_8585883916966185499[66] = 0; + out_8585883916966185499[67] = 0; + out_8585883916966185499[68] = 0; + out_8585883916966185499[69] = 0; + out_8585883916966185499[70] = 1; + out_8585883916966185499[71] = 0; + out_8585883916966185499[72] = 0; + out_8585883916966185499[73] = 0; + out_8585883916966185499[74] = 0; + out_8585883916966185499[75] = 0; + out_8585883916966185499[76] = 0; + out_8585883916966185499[77] = 0; + out_8585883916966185499[78] = 0; + out_8585883916966185499[79] = 0; + out_8585883916966185499[80] = 1; } -void h_25(double *state, double *unused, double *out_513824514853683092) { - out_513824514853683092[0] = state[6]; +void h_25(double *state, double *unused, double *out_1895365982627284096) { + out_1895365982627284096[0] = state[6]; } -void H_25(double *state, double *unused, double *out_4371119300233046035) { - out_4371119300233046035[0] = 0; - out_4371119300233046035[1] = 0; - out_4371119300233046035[2] = 0; - out_4371119300233046035[3] = 0; - out_4371119300233046035[4] = 0; - out_4371119300233046035[5] = 0; - out_4371119300233046035[6] = 1; - out_4371119300233046035[7] = 0; - out_4371119300233046035[8] = 0; +void H_25(double *state, double *unused, double *out_3357696062093255189) { + out_3357696062093255189[0] = 0; + out_3357696062093255189[1] = 0; + out_3357696062093255189[2] = 0; + out_3357696062093255189[3] = 0; + out_3357696062093255189[4] = 0; + out_3357696062093255189[5] = 0; + out_3357696062093255189[6] = 1; + out_3357696062093255189[7] = 0; + out_3357696062093255189[8] = 0; } -void h_24(double *state, double *unused, double *out_1763325166754368628) { - out_1763325166754368628[0] = state[4]; - out_1763325166754368628[1] = state[5]; +void h_24(double *state, double *unused, double *out_5360338391580302842) { + out_5360338391580302842[0] = state[4]; + out_5360338391580302842[1] = state[5]; } -void H_24(double *state, double *unused, double *out_813193701430722899) { - out_813193701430722899[0] = 0; - out_813193701430722899[1] = 0; - out_813193701430722899[2] = 0; - out_813193701430722899[3] = 0; - out_813193701430722899[4] = 1; - out_813193701430722899[5] = 0; - out_813193701430722899[6] = 0; - out_813193701430722899[7] = 0; - out_813193701430722899[8] = 0; - out_813193701430722899[9] = 0; - out_813193701430722899[10] = 0; - out_813193701430722899[11] = 0; - out_813193701430722899[12] = 0; - out_813193701430722899[13] = 0; - out_813193701430722899[14] = 1; - out_813193701430722899[15] = 0; - out_813193701430722899[16] = 0; - out_813193701430722899[17] = 0; +void H_24(double *state, double *unused, double *out_5530345661098754755) { + out_5530345661098754755[0] = 0; + out_5530345661098754755[1] = 0; + out_5530345661098754755[2] = 0; + out_5530345661098754755[3] = 0; + out_5530345661098754755[4] = 1; + out_5530345661098754755[5] = 0; + out_5530345661098754755[6] = 0; + out_5530345661098754755[7] = 0; + out_5530345661098754755[8] = 0; + out_5530345661098754755[9] = 0; + out_5530345661098754755[10] = 0; + out_5530345661098754755[11] = 0; + out_5530345661098754755[12] = 0; + out_5530345661098754755[13] = 0; + out_5530345661098754755[14] = 1; + out_5530345661098754755[15] = 0; + out_5530345661098754755[16] = 0; + out_5530345661098754755[17] = 0; } -void h_30(double *state, double *unused, double *out_2433901475501801869) { - out_2433901475501801869[0] = state[4]; +void h_30(double *state, double *unused, double *out_2052552650978413241) { + out_2052552650978413241[0] = state[4]; } -void H_30(double *state, double *unused, double *out_4500458247376286105) { - out_4500458247376286105[0] = 0; - out_4500458247376286105[1] = 0; - out_4500458247376286105[2] = 0; - out_4500458247376286105[3] = 0; - out_4500458247376286105[4] = 1; - out_4500458247376286105[5] = 0; - out_4500458247376286105[6] = 0; - out_4500458247376286105[7] = 0; - out_4500458247376286105[8] = 0; +void H_30(double *state, double *unused, double *out_839363103586006562) { + out_839363103586006562[0] = 0; + out_839363103586006562[1] = 0; + out_839363103586006562[2] = 0; + out_839363103586006562[3] = 0; + out_839363103586006562[4] = 1; + out_839363103586006562[5] = 0; + out_839363103586006562[6] = 0; + out_839363103586006562[7] = 0; + out_839363103586006562[8] = 0; } -void h_26(double *state, double *unused, double *out_1451817382166503757) { - out_1451817382166503757[0] = state[7]; +void h_26(double *state, double *unused, double *out_2655868535654890758) { + out_2655868535654890758[0] = state[7]; } -void H_26(double *state, double *unused, double *out_8112622619107102259) { - out_8112622619107102259[0] = 0; - out_8112622619107102259[1] = 0; - out_8112622619107102259[2] = 0; - out_8112622619107102259[3] = 0; - out_8112622619107102259[4] = 0; - out_8112622619107102259[5] = 0; - out_8112622619107102259[6] = 0; - out_8112622619107102259[7] = 1; - out_8112622619107102259[8] = 0; +void H_26(double *state, double *unused, double *out_53170092332454588) { + out_53170092332454588[0] = 0; + out_53170092332454588[1] = 0; + out_53170092332454588[2] = 0; + out_53170092332454588[3] = 0; + out_53170092332454588[4] = 0; + out_53170092332454588[5] = 0; + out_53170092332454588[6] = 0; + out_53170092332454588[7] = 1; + out_53170092332454588[8] = 0; } -void h_27(double *state, double *unused, double *out_8950950386849027759) { - out_8950950386849027759[0] = state[3]; +void h_27(double *state, double *unused, double *out_3774772722470275260) { + out_3774772722470275260[0] = state[3]; } -void H_27(double *state, double *unused, double *out_6675221559176711016) { - out_6675221559176711016[0] = 0; - out_6675221559176711016[1] = 0; - out_6675221559176711016[2] = 0; - out_6675221559176711016[3] = 1; - out_6675221559176711016[4] = 0; - out_6675221559176711016[5] = 0; - out_6675221559176711016[6] = 0; - out_6675221559176711016[7] = 0; - out_6675221559176711016[8] = 0; +void H_27(double *state, double *unused, double *out_1384230967597936655) { + out_1384230967597936655[0] = 0; + out_1384230967597936655[1] = 0; + out_1384230967597936655[2] = 0; + out_1384230967597936655[3] = 1; + out_1384230967597936655[4] = 0; + out_1384230967597936655[5] = 0; + out_1384230967597936655[6] = 0; + out_1384230967597936655[7] = 0; + out_1384230967597936655[8] = 0; } -void h_29(double *state, double *unused, double *out_7435574743162647514) { - out_7435574743162647514[0] = state[1]; +void h_29(double *state, double *unused, double *out_4656957971787110892) { + out_4656957971787110892[0] = state[1]; } -void H_29(double *state, double *unused, double *out_8388584286046262049) { - out_8388584286046262049[0] = 0; - out_8388584286046262049[1] = 1; - out_8388584286046262049[2] = 0; - out_8388584286046262049[3] = 0; - out_8388584286046262049[4] = 0; - out_8388584286046262049[5] = 0; - out_8388584286046262049[6] = 0; - out_8388584286046262049[7] = 0; - out_8388584286046262049[8] = 0; +void H_29(double *state, double *unused, double *out_329131759271614378) { + out_329131759271614378[0] = 0; + out_329131759271614378[1] = 1; + out_329131759271614378[2] = 0; + out_329131759271614378[3] = 0; + out_329131759271614378[4] = 0; + out_329131759271614378[5] = 0; + out_329131759271614378[6] = 0; + out_329131759271614378[7] = 0; + out_329131759271614378[8] = 0; } -void h_28(double *state, double *unused, double *out_4494120419199678212) { - out_4494120419199678212[0] = state[0]; +void h_28(double *state, double *unused, double *out_1170367401661577609) { + out_1170367401661577609[0] = state[0]; } -void H_28(double *state, double *unused, double *out_6424954014480935798) { - out_6424954014480935798[0] = 1; - out_6424954014480935798[1] = 0; - out_6424954014480935798[2] = 0; - out_6424954014480935798[3] = 0; - out_6424954014480935798[4] = 0; - out_6424954014480935798[5] = 0; - out_6424954014480935798[6] = 0; - out_6424954014480935798[7] = 0; - out_6424954014480935798[8] = 0; +void H_28(double *state, double *unused, double *out_5411530776341144952) { + out_5411530776341144952[0] = 1; + out_5411530776341144952[1] = 0; + out_5411530776341144952[2] = 0; + out_5411530776341144952[3] = 0; + out_5411530776341144952[4] = 0; + out_5411530776341144952[5] = 0; + out_5411530776341144952[6] = 0; + out_5411530776341144952[7] = 0; + out_5411530776341144952[8] = 0; } -void h_31(double *state, double *unused, double *out_6257010711496667844) { - out_6257010711496667844[0] = state[8]; +void h_31(double *state, double *unused, double *out_1437164620468722434) { + out_1437164620468722434[0] = state[8]; } -void H_31(double *state, double *unused, double *out_4340473338356085607) { - out_4340473338356085607[0] = 0; - out_4340473338356085607[1] = 0; - out_4340473338356085607[2] = 0; - out_4340473338356085607[3] = 0; - out_4340473338356085607[4] = 0; - out_4340473338356085607[5] = 0; - out_4340473338356085607[6] = 0; - out_4340473338356085607[7] = 0; - out_4340473338356085607[8] = 1; +void H_31(double *state, double *unused, double *out_7725407483200662889) { + out_7725407483200662889[0] = 0; + out_7725407483200662889[1] = 0; + out_7725407483200662889[2] = 0; + out_7725407483200662889[3] = 0; + out_7725407483200662889[4] = 0; + out_7725407483200662889[5] = 0; + out_7725407483200662889[6] = 0; + out_7725407483200662889[7] = 0; + out_7725407483200662889[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_7404715378150782082) { - err_fun(nom_x, delta_x, out_7404715378150782082); +void car_err_fun(double *nom_x, double *delta_x, double *out_8994217345304474551) { + err_fun(nom_x, delta_x, out_8994217345304474551); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_1067581222048175770) { - inv_err_fun(nom_x, true_x, out_1067581222048175770); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_8635883422281677209) { + inv_err_fun(nom_x, true_x, out_8635883422281677209); } -void car_H_mod_fun(double *state, double *out_7894099950222537712) { - H_mod_fun(state, out_7894099950222537712); +void car_H_mod_fun(double *state, double *out_5835492163818982048) { + H_mod_fun(state, out_5835492163818982048); } -void car_f_fun(double *state, double dt, double *out_18918990587173632) { - f_fun(state, dt, out_18918990587173632); +void car_f_fun(double *state, double dt, double *out_9046400166044584491) { + f_fun(state, dt, out_9046400166044584491); } -void car_F_fun(double *state, double dt, double *out_164532203047016632) { - F_fun(state, dt, out_164532203047016632); +void car_F_fun(double *state, double dt, double *out_8585883916966185499) { + F_fun(state, dt, out_8585883916966185499); } -void car_h_25(double *state, double *unused, double *out_513824514853683092) { - h_25(state, unused, out_513824514853683092); +void car_h_25(double *state, double *unused, double *out_1895365982627284096) { + h_25(state, unused, out_1895365982627284096); } -void car_H_25(double *state, double *unused, double *out_4371119300233046035) { - H_25(state, unused, out_4371119300233046035); +void car_H_25(double *state, double *unused, double *out_3357696062093255189) { + H_25(state, unused, out_3357696062093255189); } -void car_h_24(double *state, double *unused, double *out_1763325166754368628) { - h_24(state, unused, out_1763325166754368628); +void car_h_24(double *state, double *unused, double *out_5360338391580302842) { + h_24(state, unused, out_5360338391580302842); } -void car_H_24(double *state, double *unused, double *out_813193701430722899) { - H_24(state, unused, out_813193701430722899); +void car_H_24(double *state, double *unused, double *out_5530345661098754755) { + H_24(state, unused, out_5530345661098754755); } -void car_h_30(double *state, double *unused, double *out_2433901475501801869) { - h_30(state, unused, out_2433901475501801869); +void car_h_30(double *state, double *unused, double *out_2052552650978413241) { + h_30(state, unused, out_2052552650978413241); } -void car_H_30(double *state, double *unused, double *out_4500458247376286105) { - H_30(state, unused, out_4500458247376286105); +void car_H_30(double *state, double *unused, double *out_839363103586006562) { + H_30(state, unused, out_839363103586006562); } -void car_h_26(double *state, double *unused, double *out_1451817382166503757) { - h_26(state, unused, out_1451817382166503757); +void car_h_26(double *state, double *unused, double *out_2655868535654890758) { + h_26(state, unused, out_2655868535654890758); } -void car_H_26(double *state, double *unused, double *out_8112622619107102259) { - H_26(state, unused, out_8112622619107102259); +void car_H_26(double *state, double *unused, double *out_53170092332454588) { + H_26(state, unused, out_53170092332454588); } -void car_h_27(double *state, double *unused, double *out_8950950386849027759) { - h_27(state, unused, out_8950950386849027759); +void car_h_27(double *state, double *unused, double *out_3774772722470275260) { + h_27(state, unused, out_3774772722470275260); } -void car_H_27(double *state, double *unused, double *out_6675221559176711016) { - H_27(state, unused, out_6675221559176711016); +void car_H_27(double *state, double *unused, double *out_1384230967597936655) { + H_27(state, unused, out_1384230967597936655); } -void car_h_29(double *state, double *unused, double *out_7435574743162647514) { - h_29(state, unused, out_7435574743162647514); +void car_h_29(double *state, double *unused, double *out_4656957971787110892) { + h_29(state, unused, out_4656957971787110892); } -void car_H_29(double *state, double *unused, double *out_8388584286046262049) { - H_29(state, unused, out_8388584286046262049); +void car_H_29(double *state, double *unused, double *out_329131759271614378) { + H_29(state, unused, out_329131759271614378); } -void car_h_28(double *state, double *unused, double *out_4494120419199678212) { - h_28(state, unused, out_4494120419199678212); +void car_h_28(double *state, double *unused, double *out_1170367401661577609) { + h_28(state, unused, out_1170367401661577609); } -void car_H_28(double *state, double *unused, double *out_6424954014480935798) { - H_28(state, unused, out_6424954014480935798); +void car_H_28(double *state, double *unused, double *out_5411530776341144952) { + H_28(state, unused, out_5411530776341144952); } -void car_h_31(double *state, double *unused, double *out_6257010711496667844) { - h_31(state, unused, out_6257010711496667844); +void car_h_31(double *state, double *unused, double *out_1437164620468722434) { + h_31(state, unused, out_1437164620468722434); } -void car_H_31(double *state, double *unused, double *out_4340473338356085607) { - H_31(state, unused, out_4340473338356085607); +void car_H_31(double *state, double *unused, double *out_7725407483200662889) { + H_31(state, unused, out_7725407483200662889); } 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 629b2a102..a4aa6ece9 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_7404715378150782082); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_1067581222048175770); -void car_H_mod_fun(double *state, double *out_7894099950222537712); -void car_f_fun(double *state, double dt, double *out_18918990587173632); -void car_F_fun(double *state, double dt, double *out_164532203047016632); -void car_h_25(double *state, double *unused, double *out_513824514853683092); -void car_H_25(double *state, double *unused, double *out_4371119300233046035); -void car_h_24(double *state, double *unused, double *out_1763325166754368628); -void car_H_24(double *state, double *unused, double *out_813193701430722899); -void car_h_30(double *state, double *unused, double *out_2433901475501801869); -void car_H_30(double *state, double *unused, double *out_4500458247376286105); -void car_h_26(double *state, double *unused, double *out_1451817382166503757); -void car_H_26(double *state, double *unused, double *out_8112622619107102259); -void car_h_27(double *state, double *unused, double *out_8950950386849027759); -void car_H_27(double *state, double *unused, double *out_6675221559176711016); -void car_h_29(double *state, double *unused, double *out_7435574743162647514); -void car_H_29(double *state, double *unused, double *out_8388584286046262049); -void car_h_28(double *state, double *unused, double *out_4494120419199678212); -void car_H_28(double *state, double *unused, double *out_6424954014480935798); -void car_h_31(double *state, double *unused, double *out_6257010711496667844); -void car_H_31(double *state, double *unused, double *out_4340473338356085607); +void car_err_fun(double *nom_x, double *delta_x, double *out_8994217345304474551); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_8635883422281677209); +void car_H_mod_fun(double *state, double *out_5835492163818982048); +void car_f_fun(double *state, double dt, double *out_9046400166044584491); +void car_F_fun(double *state, double dt, double *out_8585883916966185499); +void car_h_25(double *state, double *unused, double *out_1895365982627284096); +void car_H_25(double *state, double *unused, double *out_3357696062093255189); +void car_h_24(double *state, double *unused, double *out_5360338391580302842); +void car_H_24(double *state, double *unused, double *out_5530345661098754755); +void car_h_30(double *state, double *unused, double *out_2052552650978413241); +void car_H_30(double *state, double *unused, double *out_839363103586006562); +void car_h_26(double *state, double *unused, double *out_2655868535654890758); +void car_H_26(double *state, double *unused, double *out_53170092332454588); +void car_h_27(double *state, double *unused, double *out_3774772722470275260); +void car_H_27(double *state, double *unused, double *out_1384230967597936655); +void car_h_29(double *state, double *unused, double *out_4656957971787110892); +void car_H_29(double *state, double *unused, double *out_329131759271614378); +void car_h_28(double *state, double *unused, double *out_1170367401661577609); +void car_H_28(double *state, double *unused, double *out_5411530776341144952); +void car_h_31(double *state, double *unused, double *out_1437164620468722434); +void car_H_31(double *state, double *unused, double *out_7725407483200662889); 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 11f3ae64f..088874416 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_5984861409348236479) { - out_5984861409348236479[0] = delta_x[0] + nom_x[0]; - out_5984861409348236479[1] = delta_x[1] + nom_x[1]; - out_5984861409348236479[2] = delta_x[2] + nom_x[2]; - out_5984861409348236479[3] = delta_x[3] + nom_x[3]; - out_5984861409348236479[4] = delta_x[4] + nom_x[4]; - out_5984861409348236479[5] = delta_x[5] + nom_x[5]; - out_5984861409348236479[6] = delta_x[6] + nom_x[6]; - out_5984861409348236479[7] = delta_x[7] + nom_x[7]; - out_5984861409348236479[8] = delta_x[8] + nom_x[8]; - out_5984861409348236479[9] = delta_x[9] + nom_x[9]; - out_5984861409348236479[10] = delta_x[10] + nom_x[10]; - out_5984861409348236479[11] = delta_x[11] + nom_x[11]; - out_5984861409348236479[12] = delta_x[12] + nom_x[12]; - out_5984861409348236479[13] = delta_x[13] + nom_x[13]; - out_5984861409348236479[14] = delta_x[14] + nom_x[14]; - out_5984861409348236479[15] = delta_x[15] + nom_x[15]; - out_5984861409348236479[16] = delta_x[16] + nom_x[16]; - out_5984861409348236479[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_5188089899736494345) { + out_5188089899736494345[0] = delta_x[0] + nom_x[0]; + out_5188089899736494345[1] = delta_x[1] + nom_x[1]; + out_5188089899736494345[2] = delta_x[2] + nom_x[2]; + out_5188089899736494345[3] = delta_x[3] + nom_x[3]; + out_5188089899736494345[4] = delta_x[4] + nom_x[4]; + out_5188089899736494345[5] = delta_x[5] + nom_x[5]; + out_5188089899736494345[6] = delta_x[6] + nom_x[6]; + out_5188089899736494345[7] = delta_x[7] + nom_x[7]; + out_5188089899736494345[8] = delta_x[8] + nom_x[8]; + out_5188089899736494345[9] = delta_x[9] + nom_x[9]; + out_5188089899736494345[10] = delta_x[10] + nom_x[10]; + out_5188089899736494345[11] = delta_x[11] + nom_x[11]; + out_5188089899736494345[12] = delta_x[12] + nom_x[12]; + out_5188089899736494345[13] = delta_x[13] + nom_x[13]; + out_5188089899736494345[14] = delta_x[14] + nom_x[14]; + out_5188089899736494345[15] = delta_x[15] + nom_x[15]; + out_5188089899736494345[16] = delta_x[16] + nom_x[16]; + out_5188089899736494345[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_1707565234864444406) { - out_1707565234864444406[0] = -nom_x[0] + true_x[0]; - out_1707565234864444406[1] = -nom_x[1] + true_x[1]; - out_1707565234864444406[2] = -nom_x[2] + true_x[2]; - out_1707565234864444406[3] = -nom_x[3] + true_x[3]; - out_1707565234864444406[4] = -nom_x[4] + true_x[4]; - out_1707565234864444406[5] = -nom_x[5] + true_x[5]; - out_1707565234864444406[6] = -nom_x[6] + true_x[6]; - out_1707565234864444406[7] = -nom_x[7] + true_x[7]; - out_1707565234864444406[8] = -nom_x[8] + true_x[8]; - out_1707565234864444406[9] = -nom_x[9] + true_x[9]; - out_1707565234864444406[10] = -nom_x[10] + true_x[10]; - out_1707565234864444406[11] = -nom_x[11] + true_x[11]; - out_1707565234864444406[12] = -nom_x[12] + true_x[12]; - out_1707565234864444406[13] = -nom_x[13] + true_x[13]; - out_1707565234864444406[14] = -nom_x[14] + true_x[14]; - out_1707565234864444406[15] = -nom_x[15] + true_x[15]; - out_1707565234864444406[16] = -nom_x[16] + true_x[16]; - out_1707565234864444406[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_516319331577825853) { + out_516319331577825853[0] = -nom_x[0] + true_x[0]; + out_516319331577825853[1] = -nom_x[1] + true_x[1]; + out_516319331577825853[2] = -nom_x[2] + true_x[2]; + out_516319331577825853[3] = -nom_x[3] + true_x[3]; + out_516319331577825853[4] = -nom_x[4] + true_x[4]; + out_516319331577825853[5] = -nom_x[5] + true_x[5]; + out_516319331577825853[6] = -nom_x[6] + true_x[6]; + out_516319331577825853[7] = -nom_x[7] + true_x[7]; + out_516319331577825853[8] = -nom_x[8] + true_x[8]; + out_516319331577825853[9] = -nom_x[9] + true_x[9]; + out_516319331577825853[10] = -nom_x[10] + true_x[10]; + out_516319331577825853[11] = -nom_x[11] + true_x[11]; + out_516319331577825853[12] = -nom_x[12] + true_x[12]; + out_516319331577825853[13] = -nom_x[13] + true_x[13]; + out_516319331577825853[14] = -nom_x[14] + true_x[14]; + out_516319331577825853[15] = -nom_x[15] + true_x[15]; + out_516319331577825853[16] = -nom_x[16] + true_x[16]; + out_516319331577825853[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_938651669609787352) { - out_938651669609787352[0] = 1.0; - out_938651669609787352[1] = 0.0; - out_938651669609787352[2] = 0.0; - out_938651669609787352[3] = 0.0; - out_938651669609787352[4] = 0.0; - out_938651669609787352[5] = 0.0; - out_938651669609787352[6] = 0.0; - out_938651669609787352[7] = 0.0; - out_938651669609787352[8] = 0.0; - out_938651669609787352[9] = 0.0; - out_938651669609787352[10] = 0.0; - out_938651669609787352[11] = 0.0; - out_938651669609787352[12] = 0.0; - out_938651669609787352[13] = 0.0; - out_938651669609787352[14] = 0.0; - out_938651669609787352[15] = 0.0; - out_938651669609787352[16] = 0.0; - out_938651669609787352[17] = 0.0; - out_938651669609787352[18] = 0.0; - out_938651669609787352[19] = 1.0; - out_938651669609787352[20] = 0.0; - out_938651669609787352[21] = 0.0; - out_938651669609787352[22] = 0.0; - out_938651669609787352[23] = 0.0; - out_938651669609787352[24] = 0.0; - out_938651669609787352[25] = 0.0; - out_938651669609787352[26] = 0.0; - out_938651669609787352[27] = 0.0; - out_938651669609787352[28] = 0.0; - out_938651669609787352[29] = 0.0; - out_938651669609787352[30] = 0.0; - out_938651669609787352[31] = 0.0; - out_938651669609787352[32] = 0.0; - out_938651669609787352[33] = 0.0; - out_938651669609787352[34] = 0.0; - out_938651669609787352[35] = 0.0; - out_938651669609787352[36] = 0.0; - out_938651669609787352[37] = 0.0; - out_938651669609787352[38] = 1.0; - out_938651669609787352[39] = 0.0; - out_938651669609787352[40] = 0.0; - out_938651669609787352[41] = 0.0; - out_938651669609787352[42] = 0.0; - out_938651669609787352[43] = 0.0; - out_938651669609787352[44] = 0.0; - out_938651669609787352[45] = 0.0; - out_938651669609787352[46] = 0.0; - out_938651669609787352[47] = 0.0; - out_938651669609787352[48] = 0.0; - out_938651669609787352[49] = 0.0; - out_938651669609787352[50] = 0.0; - out_938651669609787352[51] = 0.0; - out_938651669609787352[52] = 0.0; - out_938651669609787352[53] = 0.0; - out_938651669609787352[54] = 0.0; - out_938651669609787352[55] = 0.0; - out_938651669609787352[56] = 0.0; - out_938651669609787352[57] = 1.0; - out_938651669609787352[58] = 0.0; - out_938651669609787352[59] = 0.0; - out_938651669609787352[60] = 0.0; - out_938651669609787352[61] = 0.0; - out_938651669609787352[62] = 0.0; - out_938651669609787352[63] = 0.0; - out_938651669609787352[64] = 0.0; - out_938651669609787352[65] = 0.0; - out_938651669609787352[66] = 0.0; - out_938651669609787352[67] = 0.0; - out_938651669609787352[68] = 0.0; - out_938651669609787352[69] = 0.0; - out_938651669609787352[70] = 0.0; - out_938651669609787352[71] = 0.0; - out_938651669609787352[72] = 0.0; - out_938651669609787352[73] = 0.0; - out_938651669609787352[74] = 0.0; - out_938651669609787352[75] = 0.0; - out_938651669609787352[76] = 1.0; - out_938651669609787352[77] = 0.0; - out_938651669609787352[78] = 0.0; - out_938651669609787352[79] = 0.0; - out_938651669609787352[80] = 0.0; - out_938651669609787352[81] = 0.0; - out_938651669609787352[82] = 0.0; - out_938651669609787352[83] = 0.0; - out_938651669609787352[84] = 0.0; - out_938651669609787352[85] = 0.0; - out_938651669609787352[86] = 0.0; - out_938651669609787352[87] = 0.0; - out_938651669609787352[88] = 0.0; - out_938651669609787352[89] = 0.0; - out_938651669609787352[90] = 0.0; - out_938651669609787352[91] = 0.0; - out_938651669609787352[92] = 0.0; - out_938651669609787352[93] = 0.0; - out_938651669609787352[94] = 0.0; - out_938651669609787352[95] = 1.0; - out_938651669609787352[96] = 0.0; - out_938651669609787352[97] = 0.0; - out_938651669609787352[98] = 0.0; - out_938651669609787352[99] = 0.0; - out_938651669609787352[100] = 0.0; - out_938651669609787352[101] = 0.0; - out_938651669609787352[102] = 0.0; - out_938651669609787352[103] = 0.0; - out_938651669609787352[104] = 0.0; - out_938651669609787352[105] = 0.0; - out_938651669609787352[106] = 0.0; - out_938651669609787352[107] = 0.0; - out_938651669609787352[108] = 0.0; - out_938651669609787352[109] = 0.0; - out_938651669609787352[110] = 0.0; - out_938651669609787352[111] = 0.0; - out_938651669609787352[112] = 0.0; - out_938651669609787352[113] = 0.0; - out_938651669609787352[114] = 1.0; - out_938651669609787352[115] = 0.0; - out_938651669609787352[116] = 0.0; - out_938651669609787352[117] = 0.0; - out_938651669609787352[118] = 0.0; - out_938651669609787352[119] = 0.0; - out_938651669609787352[120] = 0.0; - out_938651669609787352[121] = 0.0; - out_938651669609787352[122] = 0.0; - out_938651669609787352[123] = 0.0; - out_938651669609787352[124] = 0.0; - out_938651669609787352[125] = 0.0; - out_938651669609787352[126] = 0.0; - out_938651669609787352[127] = 0.0; - out_938651669609787352[128] = 0.0; - out_938651669609787352[129] = 0.0; - out_938651669609787352[130] = 0.0; - out_938651669609787352[131] = 0.0; - out_938651669609787352[132] = 0.0; - out_938651669609787352[133] = 1.0; - out_938651669609787352[134] = 0.0; - out_938651669609787352[135] = 0.0; - out_938651669609787352[136] = 0.0; - out_938651669609787352[137] = 0.0; - out_938651669609787352[138] = 0.0; - out_938651669609787352[139] = 0.0; - out_938651669609787352[140] = 0.0; - out_938651669609787352[141] = 0.0; - out_938651669609787352[142] = 0.0; - out_938651669609787352[143] = 0.0; - out_938651669609787352[144] = 0.0; - out_938651669609787352[145] = 0.0; - out_938651669609787352[146] = 0.0; - out_938651669609787352[147] = 0.0; - out_938651669609787352[148] = 0.0; - out_938651669609787352[149] = 0.0; - out_938651669609787352[150] = 0.0; - out_938651669609787352[151] = 0.0; - out_938651669609787352[152] = 1.0; - out_938651669609787352[153] = 0.0; - out_938651669609787352[154] = 0.0; - out_938651669609787352[155] = 0.0; - out_938651669609787352[156] = 0.0; - out_938651669609787352[157] = 0.0; - out_938651669609787352[158] = 0.0; - out_938651669609787352[159] = 0.0; - out_938651669609787352[160] = 0.0; - out_938651669609787352[161] = 0.0; - out_938651669609787352[162] = 0.0; - out_938651669609787352[163] = 0.0; - out_938651669609787352[164] = 0.0; - out_938651669609787352[165] = 0.0; - out_938651669609787352[166] = 0.0; - out_938651669609787352[167] = 0.0; - out_938651669609787352[168] = 0.0; - out_938651669609787352[169] = 0.0; - out_938651669609787352[170] = 0.0; - out_938651669609787352[171] = 1.0; - out_938651669609787352[172] = 0.0; - out_938651669609787352[173] = 0.0; - out_938651669609787352[174] = 0.0; - out_938651669609787352[175] = 0.0; - out_938651669609787352[176] = 0.0; - out_938651669609787352[177] = 0.0; - out_938651669609787352[178] = 0.0; - out_938651669609787352[179] = 0.0; - out_938651669609787352[180] = 0.0; - out_938651669609787352[181] = 0.0; - out_938651669609787352[182] = 0.0; - out_938651669609787352[183] = 0.0; - out_938651669609787352[184] = 0.0; - out_938651669609787352[185] = 0.0; - out_938651669609787352[186] = 0.0; - out_938651669609787352[187] = 0.0; - out_938651669609787352[188] = 0.0; - out_938651669609787352[189] = 0.0; - out_938651669609787352[190] = 1.0; - out_938651669609787352[191] = 0.0; - out_938651669609787352[192] = 0.0; - out_938651669609787352[193] = 0.0; - out_938651669609787352[194] = 0.0; - out_938651669609787352[195] = 0.0; - out_938651669609787352[196] = 0.0; - out_938651669609787352[197] = 0.0; - out_938651669609787352[198] = 0.0; - out_938651669609787352[199] = 0.0; - out_938651669609787352[200] = 0.0; - out_938651669609787352[201] = 0.0; - out_938651669609787352[202] = 0.0; - out_938651669609787352[203] = 0.0; - out_938651669609787352[204] = 0.0; - out_938651669609787352[205] = 0.0; - out_938651669609787352[206] = 0.0; - out_938651669609787352[207] = 0.0; - out_938651669609787352[208] = 0.0; - out_938651669609787352[209] = 1.0; - out_938651669609787352[210] = 0.0; - out_938651669609787352[211] = 0.0; - out_938651669609787352[212] = 0.0; - out_938651669609787352[213] = 0.0; - out_938651669609787352[214] = 0.0; - out_938651669609787352[215] = 0.0; - out_938651669609787352[216] = 0.0; - out_938651669609787352[217] = 0.0; - out_938651669609787352[218] = 0.0; - out_938651669609787352[219] = 0.0; - out_938651669609787352[220] = 0.0; - out_938651669609787352[221] = 0.0; - out_938651669609787352[222] = 0.0; - out_938651669609787352[223] = 0.0; - out_938651669609787352[224] = 0.0; - out_938651669609787352[225] = 0.0; - out_938651669609787352[226] = 0.0; - out_938651669609787352[227] = 0.0; - out_938651669609787352[228] = 1.0; - out_938651669609787352[229] = 0.0; - out_938651669609787352[230] = 0.0; - out_938651669609787352[231] = 0.0; - out_938651669609787352[232] = 0.0; - out_938651669609787352[233] = 0.0; - out_938651669609787352[234] = 0.0; - out_938651669609787352[235] = 0.0; - out_938651669609787352[236] = 0.0; - out_938651669609787352[237] = 0.0; - out_938651669609787352[238] = 0.0; - out_938651669609787352[239] = 0.0; - out_938651669609787352[240] = 0.0; - out_938651669609787352[241] = 0.0; - out_938651669609787352[242] = 0.0; - out_938651669609787352[243] = 0.0; - out_938651669609787352[244] = 0.0; - out_938651669609787352[245] = 0.0; - out_938651669609787352[246] = 0.0; - out_938651669609787352[247] = 1.0; - out_938651669609787352[248] = 0.0; - out_938651669609787352[249] = 0.0; - out_938651669609787352[250] = 0.0; - out_938651669609787352[251] = 0.0; - out_938651669609787352[252] = 0.0; - out_938651669609787352[253] = 0.0; - out_938651669609787352[254] = 0.0; - out_938651669609787352[255] = 0.0; - out_938651669609787352[256] = 0.0; - out_938651669609787352[257] = 0.0; - out_938651669609787352[258] = 0.0; - out_938651669609787352[259] = 0.0; - out_938651669609787352[260] = 0.0; - out_938651669609787352[261] = 0.0; - out_938651669609787352[262] = 0.0; - out_938651669609787352[263] = 0.0; - out_938651669609787352[264] = 0.0; - out_938651669609787352[265] = 0.0; - out_938651669609787352[266] = 1.0; - out_938651669609787352[267] = 0.0; - out_938651669609787352[268] = 0.0; - out_938651669609787352[269] = 0.0; - out_938651669609787352[270] = 0.0; - out_938651669609787352[271] = 0.0; - out_938651669609787352[272] = 0.0; - out_938651669609787352[273] = 0.0; - out_938651669609787352[274] = 0.0; - out_938651669609787352[275] = 0.0; - out_938651669609787352[276] = 0.0; - out_938651669609787352[277] = 0.0; - out_938651669609787352[278] = 0.0; - out_938651669609787352[279] = 0.0; - out_938651669609787352[280] = 0.0; - out_938651669609787352[281] = 0.0; - out_938651669609787352[282] = 0.0; - out_938651669609787352[283] = 0.0; - out_938651669609787352[284] = 0.0; - out_938651669609787352[285] = 1.0; - out_938651669609787352[286] = 0.0; - out_938651669609787352[287] = 0.0; - out_938651669609787352[288] = 0.0; - out_938651669609787352[289] = 0.0; - out_938651669609787352[290] = 0.0; - out_938651669609787352[291] = 0.0; - out_938651669609787352[292] = 0.0; - out_938651669609787352[293] = 0.0; - out_938651669609787352[294] = 0.0; - out_938651669609787352[295] = 0.0; - out_938651669609787352[296] = 0.0; - out_938651669609787352[297] = 0.0; - out_938651669609787352[298] = 0.0; - out_938651669609787352[299] = 0.0; - out_938651669609787352[300] = 0.0; - out_938651669609787352[301] = 0.0; - out_938651669609787352[302] = 0.0; - out_938651669609787352[303] = 0.0; - out_938651669609787352[304] = 1.0; - out_938651669609787352[305] = 0.0; - out_938651669609787352[306] = 0.0; - out_938651669609787352[307] = 0.0; - out_938651669609787352[308] = 0.0; - out_938651669609787352[309] = 0.0; - out_938651669609787352[310] = 0.0; - out_938651669609787352[311] = 0.0; - out_938651669609787352[312] = 0.0; - out_938651669609787352[313] = 0.0; - out_938651669609787352[314] = 0.0; - out_938651669609787352[315] = 0.0; - out_938651669609787352[316] = 0.0; - out_938651669609787352[317] = 0.0; - out_938651669609787352[318] = 0.0; - out_938651669609787352[319] = 0.0; - out_938651669609787352[320] = 0.0; - out_938651669609787352[321] = 0.0; - out_938651669609787352[322] = 0.0; - out_938651669609787352[323] = 1.0; +void H_mod_fun(double *state, double *out_4951567132479851062) { + out_4951567132479851062[0] = 1.0; + out_4951567132479851062[1] = 0.0; + out_4951567132479851062[2] = 0.0; + out_4951567132479851062[3] = 0.0; + out_4951567132479851062[4] = 0.0; + out_4951567132479851062[5] = 0.0; + out_4951567132479851062[6] = 0.0; + out_4951567132479851062[7] = 0.0; + out_4951567132479851062[8] = 0.0; + out_4951567132479851062[9] = 0.0; + out_4951567132479851062[10] = 0.0; + out_4951567132479851062[11] = 0.0; + out_4951567132479851062[12] = 0.0; + out_4951567132479851062[13] = 0.0; + out_4951567132479851062[14] = 0.0; + out_4951567132479851062[15] = 0.0; + out_4951567132479851062[16] = 0.0; + out_4951567132479851062[17] = 0.0; + out_4951567132479851062[18] = 0.0; + out_4951567132479851062[19] = 1.0; + out_4951567132479851062[20] = 0.0; + out_4951567132479851062[21] = 0.0; + out_4951567132479851062[22] = 0.0; + out_4951567132479851062[23] = 0.0; + out_4951567132479851062[24] = 0.0; + out_4951567132479851062[25] = 0.0; + out_4951567132479851062[26] = 0.0; + out_4951567132479851062[27] = 0.0; + out_4951567132479851062[28] = 0.0; + out_4951567132479851062[29] = 0.0; + out_4951567132479851062[30] = 0.0; + out_4951567132479851062[31] = 0.0; + out_4951567132479851062[32] = 0.0; + out_4951567132479851062[33] = 0.0; + out_4951567132479851062[34] = 0.0; + out_4951567132479851062[35] = 0.0; + out_4951567132479851062[36] = 0.0; + out_4951567132479851062[37] = 0.0; + out_4951567132479851062[38] = 1.0; + out_4951567132479851062[39] = 0.0; + out_4951567132479851062[40] = 0.0; + out_4951567132479851062[41] = 0.0; + out_4951567132479851062[42] = 0.0; + out_4951567132479851062[43] = 0.0; + out_4951567132479851062[44] = 0.0; + out_4951567132479851062[45] = 0.0; + out_4951567132479851062[46] = 0.0; + out_4951567132479851062[47] = 0.0; + out_4951567132479851062[48] = 0.0; + out_4951567132479851062[49] = 0.0; + out_4951567132479851062[50] = 0.0; + out_4951567132479851062[51] = 0.0; + out_4951567132479851062[52] = 0.0; + out_4951567132479851062[53] = 0.0; + out_4951567132479851062[54] = 0.0; + out_4951567132479851062[55] = 0.0; + out_4951567132479851062[56] = 0.0; + out_4951567132479851062[57] = 1.0; + out_4951567132479851062[58] = 0.0; + out_4951567132479851062[59] = 0.0; + out_4951567132479851062[60] = 0.0; + out_4951567132479851062[61] = 0.0; + out_4951567132479851062[62] = 0.0; + out_4951567132479851062[63] = 0.0; + out_4951567132479851062[64] = 0.0; + out_4951567132479851062[65] = 0.0; + out_4951567132479851062[66] = 0.0; + out_4951567132479851062[67] = 0.0; + out_4951567132479851062[68] = 0.0; + out_4951567132479851062[69] = 0.0; + out_4951567132479851062[70] = 0.0; + out_4951567132479851062[71] = 0.0; + out_4951567132479851062[72] = 0.0; + out_4951567132479851062[73] = 0.0; + out_4951567132479851062[74] = 0.0; + out_4951567132479851062[75] = 0.0; + out_4951567132479851062[76] = 1.0; + out_4951567132479851062[77] = 0.0; + out_4951567132479851062[78] = 0.0; + out_4951567132479851062[79] = 0.0; + out_4951567132479851062[80] = 0.0; + out_4951567132479851062[81] = 0.0; + out_4951567132479851062[82] = 0.0; + out_4951567132479851062[83] = 0.0; + out_4951567132479851062[84] = 0.0; + out_4951567132479851062[85] = 0.0; + out_4951567132479851062[86] = 0.0; + out_4951567132479851062[87] = 0.0; + out_4951567132479851062[88] = 0.0; + out_4951567132479851062[89] = 0.0; + out_4951567132479851062[90] = 0.0; + out_4951567132479851062[91] = 0.0; + out_4951567132479851062[92] = 0.0; + out_4951567132479851062[93] = 0.0; + out_4951567132479851062[94] = 0.0; + out_4951567132479851062[95] = 1.0; + out_4951567132479851062[96] = 0.0; + out_4951567132479851062[97] = 0.0; + out_4951567132479851062[98] = 0.0; + out_4951567132479851062[99] = 0.0; + out_4951567132479851062[100] = 0.0; + out_4951567132479851062[101] = 0.0; + out_4951567132479851062[102] = 0.0; + out_4951567132479851062[103] = 0.0; + out_4951567132479851062[104] = 0.0; + out_4951567132479851062[105] = 0.0; + out_4951567132479851062[106] = 0.0; + out_4951567132479851062[107] = 0.0; + out_4951567132479851062[108] = 0.0; + out_4951567132479851062[109] = 0.0; + out_4951567132479851062[110] = 0.0; + out_4951567132479851062[111] = 0.0; + out_4951567132479851062[112] = 0.0; + out_4951567132479851062[113] = 0.0; + out_4951567132479851062[114] = 1.0; + out_4951567132479851062[115] = 0.0; + out_4951567132479851062[116] = 0.0; + out_4951567132479851062[117] = 0.0; + out_4951567132479851062[118] = 0.0; + out_4951567132479851062[119] = 0.0; + out_4951567132479851062[120] = 0.0; + out_4951567132479851062[121] = 0.0; + out_4951567132479851062[122] = 0.0; + out_4951567132479851062[123] = 0.0; + out_4951567132479851062[124] = 0.0; + out_4951567132479851062[125] = 0.0; + out_4951567132479851062[126] = 0.0; + out_4951567132479851062[127] = 0.0; + out_4951567132479851062[128] = 0.0; + out_4951567132479851062[129] = 0.0; + out_4951567132479851062[130] = 0.0; + out_4951567132479851062[131] = 0.0; + out_4951567132479851062[132] = 0.0; + out_4951567132479851062[133] = 1.0; + out_4951567132479851062[134] = 0.0; + out_4951567132479851062[135] = 0.0; + out_4951567132479851062[136] = 0.0; + out_4951567132479851062[137] = 0.0; + out_4951567132479851062[138] = 0.0; + out_4951567132479851062[139] = 0.0; + out_4951567132479851062[140] = 0.0; + out_4951567132479851062[141] = 0.0; + out_4951567132479851062[142] = 0.0; + out_4951567132479851062[143] = 0.0; + out_4951567132479851062[144] = 0.0; + out_4951567132479851062[145] = 0.0; + out_4951567132479851062[146] = 0.0; + out_4951567132479851062[147] = 0.0; + out_4951567132479851062[148] = 0.0; + out_4951567132479851062[149] = 0.0; + out_4951567132479851062[150] = 0.0; + out_4951567132479851062[151] = 0.0; + out_4951567132479851062[152] = 1.0; + out_4951567132479851062[153] = 0.0; + out_4951567132479851062[154] = 0.0; + out_4951567132479851062[155] = 0.0; + out_4951567132479851062[156] = 0.0; + out_4951567132479851062[157] = 0.0; + out_4951567132479851062[158] = 0.0; + out_4951567132479851062[159] = 0.0; + out_4951567132479851062[160] = 0.0; + out_4951567132479851062[161] = 0.0; + out_4951567132479851062[162] = 0.0; + out_4951567132479851062[163] = 0.0; + out_4951567132479851062[164] = 0.0; + out_4951567132479851062[165] = 0.0; + out_4951567132479851062[166] = 0.0; + out_4951567132479851062[167] = 0.0; + out_4951567132479851062[168] = 0.0; + out_4951567132479851062[169] = 0.0; + out_4951567132479851062[170] = 0.0; + out_4951567132479851062[171] = 1.0; + out_4951567132479851062[172] = 0.0; + out_4951567132479851062[173] = 0.0; + out_4951567132479851062[174] = 0.0; + out_4951567132479851062[175] = 0.0; + out_4951567132479851062[176] = 0.0; + out_4951567132479851062[177] = 0.0; + out_4951567132479851062[178] = 0.0; + out_4951567132479851062[179] = 0.0; + out_4951567132479851062[180] = 0.0; + out_4951567132479851062[181] = 0.0; + out_4951567132479851062[182] = 0.0; + out_4951567132479851062[183] = 0.0; + out_4951567132479851062[184] = 0.0; + out_4951567132479851062[185] = 0.0; + out_4951567132479851062[186] = 0.0; + out_4951567132479851062[187] = 0.0; + out_4951567132479851062[188] = 0.0; + out_4951567132479851062[189] = 0.0; + out_4951567132479851062[190] = 1.0; + out_4951567132479851062[191] = 0.0; + out_4951567132479851062[192] = 0.0; + out_4951567132479851062[193] = 0.0; + out_4951567132479851062[194] = 0.0; + out_4951567132479851062[195] = 0.0; + out_4951567132479851062[196] = 0.0; + out_4951567132479851062[197] = 0.0; + out_4951567132479851062[198] = 0.0; + out_4951567132479851062[199] = 0.0; + out_4951567132479851062[200] = 0.0; + out_4951567132479851062[201] = 0.0; + out_4951567132479851062[202] = 0.0; + out_4951567132479851062[203] = 0.0; + out_4951567132479851062[204] = 0.0; + out_4951567132479851062[205] = 0.0; + out_4951567132479851062[206] = 0.0; + out_4951567132479851062[207] = 0.0; + out_4951567132479851062[208] = 0.0; + out_4951567132479851062[209] = 1.0; + out_4951567132479851062[210] = 0.0; + out_4951567132479851062[211] = 0.0; + out_4951567132479851062[212] = 0.0; + out_4951567132479851062[213] = 0.0; + out_4951567132479851062[214] = 0.0; + out_4951567132479851062[215] = 0.0; + out_4951567132479851062[216] = 0.0; + out_4951567132479851062[217] = 0.0; + out_4951567132479851062[218] = 0.0; + out_4951567132479851062[219] = 0.0; + out_4951567132479851062[220] = 0.0; + out_4951567132479851062[221] = 0.0; + out_4951567132479851062[222] = 0.0; + out_4951567132479851062[223] = 0.0; + out_4951567132479851062[224] = 0.0; + out_4951567132479851062[225] = 0.0; + out_4951567132479851062[226] = 0.0; + out_4951567132479851062[227] = 0.0; + out_4951567132479851062[228] = 1.0; + out_4951567132479851062[229] = 0.0; + out_4951567132479851062[230] = 0.0; + out_4951567132479851062[231] = 0.0; + out_4951567132479851062[232] = 0.0; + out_4951567132479851062[233] = 0.0; + out_4951567132479851062[234] = 0.0; + out_4951567132479851062[235] = 0.0; + out_4951567132479851062[236] = 0.0; + out_4951567132479851062[237] = 0.0; + out_4951567132479851062[238] = 0.0; + out_4951567132479851062[239] = 0.0; + out_4951567132479851062[240] = 0.0; + out_4951567132479851062[241] = 0.0; + out_4951567132479851062[242] = 0.0; + out_4951567132479851062[243] = 0.0; + out_4951567132479851062[244] = 0.0; + out_4951567132479851062[245] = 0.0; + out_4951567132479851062[246] = 0.0; + out_4951567132479851062[247] = 1.0; + out_4951567132479851062[248] = 0.0; + out_4951567132479851062[249] = 0.0; + out_4951567132479851062[250] = 0.0; + out_4951567132479851062[251] = 0.0; + out_4951567132479851062[252] = 0.0; + out_4951567132479851062[253] = 0.0; + out_4951567132479851062[254] = 0.0; + out_4951567132479851062[255] = 0.0; + out_4951567132479851062[256] = 0.0; + out_4951567132479851062[257] = 0.0; + out_4951567132479851062[258] = 0.0; + out_4951567132479851062[259] = 0.0; + out_4951567132479851062[260] = 0.0; + out_4951567132479851062[261] = 0.0; + out_4951567132479851062[262] = 0.0; + out_4951567132479851062[263] = 0.0; + out_4951567132479851062[264] = 0.0; + out_4951567132479851062[265] = 0.0; + out_4951567132479851062[266] = 1.0; + out_4951567132479851062[267] = 0.0; + out_4951567132479851062[268] = 0.0; + out_4951567132479851062[269] = 0.0; + out_4951567132479851062[270] = 0.0; + out_4951567132479851062[271] = 0.0; + out_4951567132479851062[272] = 0.0; + out_4951567132479851062[273] = 0.0; + out_4951567132479851062[274] = 0.0; + out_4951567132479851062[275] = 0.0; + out_4951567132479851062[276] = 0.0; + out_4951567132479851062[277] = 0.0; + out_4951567132479851062[278] = 0.0; + out_4951567132479851062[279] = 0.0; + out_4951567132479851062[280] = 0.0; + out_4951567132479851062[281] = 0.0; + out_4951567132479851062[282] = 0.0; + out_4951567132479851062[283] = 0.0; + out_4951567132479851062[284] = 0.0; + out_4951567132479851062[285] = 1.0; + out_4951567132479851062[286] = 0.0; + out_4951567132479851062[287] = 0.0; + out_4951567132479851062[288] = 0.0; + out_4951567132479851062[289] = 0.0; + out_4951567132479851062[290] = 0.0; + out_4951567132479851062[291] = 0.0; + out_4951567132479851062[292] = 0.0; + out_4951567132479851062[293] = 0.0; + out_4951567132479851062[294] = 0.0; + out_4951567132479851062[295] = 0.0; + out_4951567132479851062[296] = 0.0; + out_4951567132479851062[297] = 0.0; + out_4951567132479851062[298] = 0.0; + out_4951567132479851062[299] = 0.0; + out_4951567132479851062[300] = 0.0; + out_4951567132479851062[301] = 0.0; + out_4951567132479851062[302] = 0.0; + out_4951567132479851062[303] = 0.0; + out_4951567132479851062[304] = 1.0; + out_4951567132479851062[305] = 0.0; + out_4951567132479851062[306] = 0.0; + out_4951567132479851062[307] = 0.0; + out_4951567132479851062[308] = 0.0; + out_4951567132479851062[309] = 0.0; + out_4951567132479851062[310] = 0.0; + out_4951567132479851062[311] = 0.0; + out_4951567132479851062[312] = 0.0; + out_4951567132479851062[313] = 0.0; + out_4951567132479851062[314] = 0.0; + out_4951567132479851062[315] = 0.0; + out_4951567132479851062[316] = 0.0; + out_4951567132479851062[317] = 0.0; + out_4951567132479851062[318] = 0.0; + out_4951567132479851062[319] = 0.0; + out_4951567132479851062[320] = 0.0; + out_4951567132479851062[321] = 0.0; + out_4951567132479851062[322] = 0.0; + out_4951567132479851062[323] = 1.0; } -void f_fun(double *state, double dt, double *out_336566407888546136) { - out_336566407888546136[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_336566407888546136[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_336566407888546136[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_336566407888546136[3] = dt*state[12] + state[3]; - out_336566407888546136[4] = dt*state[13] + state[4]; - out_336566407888546136[5] = dt*state[14] + state[5]; - out_336566407888546136[6] = state[6]; - out_336566407888546136[7] = state[7]; - out_336566407888546136[8] = state[8]; - out_336566407888546136[9] = state[9]; - out_336566407888546136[10] = state[10]; - out_336566407888546136[11] = state[11]; - out_336566407888546136[12] = state[12]; - out_336566407888546136[13] = state[13]; - out_336566407888546136[14] = state[14]; - out_336566407888546136[15] = state[15]; - out_336566407888546136[16] = state[16]; - out_336566407888546136[17] = state[17]; +void f_fun(double *state, double dt, double *out_898454825503606476) { + out_898454825503606476[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_898454825503606476[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_898454825503606476[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_898454825503606476[3] = dt*state[12] + state[3]; + out_898454825503606476[4] = dt*state[13] + state[4]; + out_898454825503606476[5] = dt*state[14] + state[5]; + out_898454825503606476[6] = state[6]; + out_898454825503606476[7] = state[7]; + out_898454825503606476[8] = state[8]; + out_898454825503606476[9] = state[9]; + out_898454825503606476[10] = state[10]; + out_898454825503606476[11] = state[11]; + out_898454825503606476[12] = state[12]; + out_898454825503606476[13] = state[13]; + out_898454825503606476[14] = state[14]; + out_898454825503606476[15] = state[15]; + out_898454825503606476[16] = state[16]; + out_898454825503606476[17] = state[17]; } -void F_fun(double *state, double dt, double *out_5996324224975557063) { - out_5996324224975557063[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_5996324224975557063[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_5996324224975557063[2] = 0; - out_5996324224975557063[3] = 0; - out_5996324224975557063[4] = 0; - out_5996324224975557063[5] = 0; - out_5996324224975557063[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_5996324224975557063[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_5996324224975557063[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_5996324224975557063[9] = 0; - out_5996324224975557063[10] = 0; - out_5996324224975557063[11] = 0; - out_5996324224975557063[12] = 0; - out_5996324224975557063[13] = 0; - out_5996324224975557063[14] = 0; - out_5996324224975557063[15] = 0; - out_5996324224975557063[16] = 0; - out_5996324224975557063[17] = 0; - out_5996324224975557063[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_5996324224975557063[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_5996324224975557063[20] = 0; - out_5996324224975557063[21] = 0; - out_5996324224975557063[22] = 0; - out_5996324224975557063[23] = 0; - out_5996324224975557063[24] = 0; - out_5996324224975557063[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_5996324224975557063[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_5996324224975557063[27] = 0; - out_5996324224975557063[28] = 0; - out_5996324224975557063[29] = 0; - out_5996324224975557063[30] = 0; - out_5996324224975557063[31] = 0; - out_5996324224975557063[32] = 0; - out_5996324224975557063[33] = 0; - out_5996324224975557063[34] = 0; - out_5996324224975557063[35] = 0; - out_5996324224975557063[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_5996324224975557063[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_5996324224975557063[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_5996324224975557063[39] = 0; - out_5996324224975557063[40] = 0; - out_5996324224975557063[41] = 0; - out_5996324224975557063[42] = 0; - out_5996324224975557063[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_5996324224975557063[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_5996324224975557063[45] = 0; - out_5996324224975557063[46] = 0; - out_5996324224975557063[47] = 0; - out_5996324224975557063[48] = 0; - out_5996324224975557063[49] = 0; - out_5996324224975557063[50] = 0; - out_5996324224975557063[51] = 0; - out_5996324224975557063[52] = 0; - out_5996324224975557063[53] = 0; - out_5996324224975557063[54] = 0; - out_5996324224975557063[55] = 0; - out_5996324224975557063[56] = 0; - out_5996324224975557063[57] = 1; - out_5996324224975557063[58] = 0; - out_5996324224975557063[59] = 0; - out_5996324224975557063[60] = 0; - out_5996324224975557063[61] = 0; - out_5996324224975557063[62] = 0; - out_5996324224975557063[63] = 0; - out_5996324224975557063[64] = 0; - out_5996324224975557063[65] = 0; - out_5996324224975557063[66] = dt; - out_5996324224975557063[67] = 0; - out_5996324224975557063[68] = 0; - out_5996324224975557063[69] = 0; - out_5996324224975557063[70] = 0; - out_5996324224975557063[71] = 0; - out_5996324224975557063[72] = 0; - out_5996324224975557063[73] = 0; - out_5996324224975557063[74] = 0; - out_5996324224975557063[75] = 0; - out_5996324224975557063[76] = 1; - out_5996324224975557063[77] = 0; - out_5996324224975557063[78] = 0; - out_5996324224975557063[79] = 0; - out_5996324224975557063[80] = 0; - out_5996324224975557063[81] = 0; - out_5996324224975557063[82] = 0; - out_5996324224975557063[83] = 0; - out_5996324224975557063[84] = 0; - out_5996324224975557063[85] = dt; - out_5996324224975557063[86] = 0; - out_5996324224975557063[87] = 0; - out_5996324224975557063[88] = 0; - out_5996324224975557063[89] = 0; - out_5996324224975557063[90] = 0; - out_5996324224975557063[91] = 0; - out_5996324224975557063[92] = 0; - out_5996324224975557063[93] = 0; - out_5996324224975557063[94] = 0; - out_5996324224975557063[95] = 1; - out_5996324224975557063[96] = 0; - out_5996324224975557063[97] = 0; - out_5996324224975557063[98] = 0; - out_5996324224975557063[99] = 0; - out_5996324224975557063[100] = 0; - out_5996324224975557063[101] = 0; - out_5996324224975557063[102] = 0; - out_5996324224975557063[103] = 0; - out_5996324224975557063[104] = dt; - out_5996324224975557063[105] = 0; - out_5996324224975557063[106] = 0; - out_5996324224975557063[107] = 0; - out_5996324224975557063[108] = 0; - out_5996324224975557063[109] = 0; - out_5996324224975557063[110] = 0; - out_5996324224975557063[111] = 0; - out_5996324224975557063[112] = 0; - out_5996324224975557063[113] = 0; - out_5996324224975557063[114] = 1; - out_5996324224975557063[115] = 0; - out_5996324224975557063[116] = 0; - out_5996324224975557063[117] = 0; - out_5996324224975557063[118] = 0; - out_5996324224975557063[119] = 0; - out_5996324224975557063[120] = 0; - out_5996324224975557063[121] = 0; - out_5996324224975557063[122] = 0; - out_5996324224975557063[123] = 0; - out_5996324224975557063[124] = 0; - out_5996324224975557063[125] = 0; - out_5996324224975557063[126] = 0; - out_5996324224975557063[127] = 0; - out_5996324224975557063[128] = 0; - out_5996324224975557063[129] = 0; - out_5996324224975557063[130] = 0; - out_5996324224975557063[131] = 0; - out_5996324224975557063[132] = 0; - out_5996324224975557063[133] = 1; - out_5996324224975557063[134] = 0; - out_5996324224975557063[135] = 0; - out_5996324224975557063[136] = 0; - out_5996324224975557063[137] = 0; - out_5996324224975557063[138] = 0; - out_5996324224975557063[139] = 0; - out_5996324224975557063[140] = 0; - out_5996324224975557063[141] = 0; - out_5996324224975557063[142] = 0; - out_5996324224975557063[143] = 0; - out_5996324224975557063[144] = 0; - out_5996324224975557063[145] = 0; - out_5996324224975557063[146] = 0; - out_5996324224975557063[147] = 0; - out_5996324224975557063[148] = 0; - out_5996324224975557063[149] = 0; - out_5996324224975557063[150] = 0; - out_5996324224975557063[151] = 0; - out_5996324224975557063[152] = 1; - out_5996324224975557063[153] = 0; - out_5996324224975557063[154] = 0; - out_5996324224975557063[155] = 0; - out_5996324224975557063[156] = 0; - out_5996324224975557063[157] = 0; - out_5996324224975557063[158] = 0; - out_5996324224975557063[159] = 0; - out_5996324224975557063[160] = 0; - out_5996324224975557063[161] = 0; - out_5996324224975557063[162] = 0; - out_5996324224975557063[163] = 0; - out_5996324224975557063[164] = 0; - out_5996324224975557063[165] = 0; - out_5996324224975557063[166] = 0; - out_5996324224975557063[167] = 0; - out_5996324224975557063[168] = 0; - out_5996324224975557063[169] = 0; - out_5996324224975557063[170] = 0; - out_5996324224975557063[171] = 1; - out_5996324224975557063[172] = 0; - out_5996324224975557063[173] = 0; - out_5996324224975557063[174] = 0; - out_5996324224975557063[175] = 0; - out_5996324224975557063[176] = 0; - out_5996324224975557063[177] = 0; - out_5996324224975557063[178] = 0; - out_5996324224975557063[179] = 0; - out_5996324224975557063[180] = 0; - out_5996324224975557063[181] = 0; - out_5996324224975557063[182] = 0; - out_5996324224975557063[183] = 0; - out_5996324224975557063[184] = 0; - out_5996324224975557063[185] = 0; - out_5996324224975557063[186] = 0; - out_5996324224975557063[187] = 0; - out_5996324224975557063[188] = 0; - out_5996324224975557063[189] = 0; - out_5996324224975557063[190] = 1; - out_5996324224975557063[191] = 0; - out_5996324224975557063[192] = 0; - out_5996324224975557063[193] = 0; - out_5996324224975557063[194] = 0; - out_5996324224975557063[195] = 0; - out_5996324224975557063[196] = 0; - out_5996324224975557063[197] = 0; - out_5996324224975557063[198] = 0; - out_5996324224975557063[199] = 0; - out_5996324224975557063[200] = 0; - out_5996324224975557063[201] = 0; - out_5996324224975557063[202] = 0; - out_5996324224975557063[203] = 0; - out_5996324224975557063[204] = 0; - out_5996324224975557063[205] = 0; - out_5996324224975557063[206] = 0; - out_5996324224975557063[207] = 0; - out_5996324224975557063[208] = 0; - out_5996324224975557063[209] = 1; - out_5996324224975557063[210] = 0; - out_5996324224975557063[211] = 0; - out_5996324224975557063[212] = 0; - out_5996324224975557063[213] = 0; - out_5996324224975557063[214] = 0; - out_5996324224975557063[215] = 0; - out_5996324224975557063[216] = 0; - out_5996324224975557063[217] = 0; - out_5996324224975557063[218] = 0; - out_5996324224975557063[219] = 0; - out_5996324224975557063[220] = 0; - out_5996324224975557063[221] = 0; - out_5996324224975557063[222] = 0; - out_5996324224975557063[223] = 0; - out_5996324224975557063[224] = 0; - out_5996324224975557063[225] = 0; - out_5996324224975557063[226] = 0; - out_5996324224975557063[227] = 0; - out_5996324224975557063[228] = 1; - out_5996324224975557063[229] = 0; - out_5996324224975557063[230] = 0; - out_5996324224975557063[231] = 0; - out_5996324224975557063[232] = 0; - out_5996324224975557063[233] = 0; - out_5996324224975557063[234] = 0; - out_5996324224975557063[235] = 0; - out_5996324224975557063[236] = 0; - out_5996324224975557063[237] = 0; - out_5996324224975557063[238] = 0; - out_5996324224975557063[239] = 0; - out_5996324224975557063[240] = 0; - out_5996324224975557063[241] = 0; - out_5996324224975557063[242] = 0; - out_5996324224975557063[243] = 0; - out_5996324224975557063[244] = 0; - out_5996324224975557063[245] = 0; - out_5996324224975557063[246] = 0; - out_5996324224975557063[247] = 1; - out_5996324224975557063[248] = 0; - out_5996324224975557063[249] = 0; - out_5996324224975557063[250] = 0; - out_5996324224975557063[251] = 0; - out_5996324224975557063[252] = 0; - out_5996324224975557063[253] = 0; - out_5996324224975557063[254] = 0; - out_5996324224975557063[255] = 0; - out_5996324224975557063[256] = 0; - out_5996324224975557063[257] = 0; - out_5996324224975557063[258] = 0; - out_5996324224975557063[259] = 0; - out_5996324224975557063[260] = 0; - out_5996324224975557063[261] = 0; - out_5996324224975557063[262] = 0; - out_5996324224975557063[263] = 0; - out_5996324224975557063[264] = 0; - out_5996324224975557063[265] = 0; - out_5996324224975557063[266] = 1; - out_5996324224975557063[267] = 0; - out_5996324224975557063[268] = 0; - out_5996324224975557063[269] = 0; - out_5996324224975557063[270] = 0; - out_5996324224975557063[271] = 0; - out_5996324224975557063[272] = 0; - out_5996324224975557063[273] = 0; - out_5996324224975557063[274] = 0; - out_5996324224975557063[275] = 0; - out_5996324224975557063[276] = 0; - out_5996324224975557063[277] = 0; - out_5996324224975557063[278] = 0; - out_5996324224975557063[279] = 0; - out_5996324224975557063[280] = 0; - out_5996324224975557063[281] = 0; - out_5996324224975557063[282] = 0; - out_5996324224975557063[283] = 0; - out_5996324224975557063[284] = 0; - out_5996324224975557063[285] = 1; - out_5996324224975557063[286] = 0; - out_5996324224975557063[287] = 0; - out_5996324224975557063[288] = 0; - out_5996324224975557063[289] = 0; - out_5996324224975557063[290] = 0; - out_5996324224975557063[291] = 0; - out_5996324224975557063[292] = 0; - out_5996324224975557063[293] = 0; - out_5996324224975557063[294] = 0; - out_5996324224975557063[295] = 0; - out_5996324224975557063[296] = 0; - out_5996324224975557063[297] = 0; - out_5996324224975557063[298] = 0; - out_5996324224975557063[299] = 0; - out_5996324224975557063[300] = 0; - out_5996324224975557063[301] = 0; - out_5996324224975557063[302] = 0; - out_5996324224975557063[303] = 0; - out_5996324224975557063[304] = 1; - out_5996324224975557063[305] = 0; - out_5996324224975557063[306] = 0; - out_5996324224975557063[307] = 0; - out_5996324224975557063[308] = 0; - out_5996324224975557063[309] = 0; - out_5996324224975557063[310] = 0; - out_5996324224975557063[311] = 0; - out_5996324224975557063[312] = 0; - out_5996324224975557063[313] = 0; - out_5996324224975557063[314] = 0; - out_5996324224975557063[315] = 0; - out_5996324224975557063[316] = 0; - out_5996324224975557063[317] = 0; - out_5996324224975557063[318] = 0; - out_5996324224975557063[319] = 0; - out_5996324224975557063[320] = 0; - out_5996324224975557063[321] = 0; - out_5996324224975557063[322] = 0; - out_5996324224975557063[323] = 1; +void F_fun(double *state, double dt, double *out_906698228237069812) { + out_906698228237069812[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_906698228237069812[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_906698228237069812[2] = 0; + out_906698228237069812[3] = 0; + out_906698228237069812[4] = 0; + out_906698228237069812[5] = 0; + out_906698228237069812[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_906698228237069812[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_906698228237069812[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_906698228237069812[9] = 0; + out_906698228237069812[10] = 0; + out_906698228237069812[11] = 0; + out_906698228237069812[12] = 0; + out_906698228237069812[13] = 0; + out_906698228237069812[14] = 0; + out_906698228237069812[15] = 0; + out_906698228237069812[16] = 0; + out_906698228237069812[17] = 0; + out_906698228237069812[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_906698228237069812[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_906698228237069812[20] = 0; + out_906698228237069812[21] = 0; + out_906698228237069812[22] = 0; + out_906698228237069812[23] = 0; + out_906698228237069812[24] = 0; + out_906698228237069812[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_906698228237069812[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_906698228237069812[27] = 0; + out_906698228237069812[28] = 0; + out_906698228237069812[29] = 0; + out_906698228237069812[30] = 0; + out_906698228237069812[31] = 0; + out_906698228237069812[32] = 0; + out_906698228237069812[33] = 0; + out_906698228237069812[34] = 0; + out_906698228237069812[35] = 0; + out_906698228237069812[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_906698228237069812[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_906698228237069812[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_906698228237069812[39] = 0; + out_906698228237069812[40] = 0; + out_906698228237069812[41] = 0; + out_906698228237069812[42] = 0; + out_906698228237069812[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_906698228237069812[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_906698228237069812[45] = 0; + out_906698228237069812[46] = 0; + out_906698228237069812[47] = 0; + out_906698228237069812[48] = 0; + out_906698228237069812[49] = 0; + out_906698228237069812[50] = 0; + out_906698228237069812[51] = 0; + out_906698228237069812[52] = 0; + out_906698228237069812[53] = 0; + out_906698228237069812[54] = 0; + out_906698228237069812[55] = 0; + out_906698228237069812[56] = 0; + out_906698228237069812[57] = 1; + out_906698228237069812[58] = 0; + out_906698228237069812[59] = 0; + out_906698228237069812[60] = 0; + out_906698228237069812[61] = 0; + out_906698228237069812[62] = 0; + out_906698228237069812[63] = 0; + out_906698228237069812[64] = 0; + out_906698228237069812[65] = 0; + out_906698228237069812[66] = dt; + out_906698228237069812[67] = 0; + out_906698228237069812[68] = 0; + out_906698228237069812[69] = 0; + out_906698228237069812[70] = 0; + out_906698228237069812[71] = 0; + out_906698228237069812[72] = 0; + out_906698228237069812[73] = 0; + out_906698228237069812[74] = 0; + out_906698228237069812[75] = 0; + out_906698228237069812[76] = 1; + out_906698228237069812[77] = 0; + out_906698228237069812[78] = 0; + out_906698228237069812[79] = 0; + out_906698228237069812[80] = 0; + out_906698228237069812[81] = 0; + out_906698228237069812[82] = 0; + out_906698228237069812[83] = 0; + out_906698228237069812[84] = 0; + out_906698228237069812[85] = dt; + out_906698228237069812[86] = 0; + out_906698228237069812[87] = 0; + out_906698228237069812[88] = 0; + out_906698228237069812[89] = 0; + out_906698228237069812[90] = 0; + out_906698228237069812[91] = 0; + out_906698228237069812[92] = 0; + out_906698228237069812[93] = 0; + out_906698228237069812[94] = 0; + out_906698228237069812[95] = 1; + out_906698228237069812[96] = 0; + out_906698228237069812[97] = 0; + out_906698228237069812[98] = 0; + out_906698228237069812[99] = 0; + out_906698228237069812[100] = 0; + out_906698228237069812[101] = 0; + out_906698228237069812[102] = 0; + out_906698228237069812[103] = 0; + out_906698228237069812[104] = dt; + out_906698228237069812[105] = 0; + out_906698228237069812[106] = 0; + out_906698228237069812[107] = 0; + out_906698228237069812[108] = 0; + out_906698228237069812[109] = 0; + out_906698228237069812[110] = 0; + out_906698228237069812[111] = 0; + out_906698228237069812[112] = 0; + out_906698228237069812[113] = 0; + out_906698228237069812[114] = 1; + out_906698228237069812[115] = 0; + out_906698228237069812[116] = 0; + out_906698228237069812[117] = 0; + out_906698228237069812[118] = 0; + out_906698228237069812[119] = 0; + out_906698228237069812[120] = 0; + out_906698228237069812[121] = 0; + out_906698228237069812[122] = 0; + out_906698228237069812[123] = 0; + out_906698228237069812[124] = 0; + out_906698228237069812[125] = 0; + out_906698228237069812[126] = 0; + out_906698228237069812[127] = 0; + out_906698228237069812[128] = 0; + out_906698228237069812[129] = 0; + out_906698228237069812[130] = 0; + out_906698228237069812[131] = 0; + out_906698228237069812[132] = 0; + out_906698228237069812[133] = 1; + out_906698228237069812[134] = 0; + out_906698228237069812[135] = 0; + out_906698228237069812[136] = 0; + out_906698228237069812[137] = 0; + out_906698228237069812[138] = 0; + out_906698228237069812[139] = 0; + out_906698228237069812[140] = 0; + out_906698228237069812[141] = 0; + out_906698228237069812[142] = 0; + out_906698228237069812[143] = 0; + out_906698228237069812[144] = 0; + out_906698228237069812[145] = 0; + out_906698228237069812[146] = 0; + out_906698228237069812[147] = 0; + out_906698228237069812[148] = 0; + out_906698228237069812[149] = 0; + out_906698228237069812[150] = 0; + out_906698228237069812[151] = 0; + out_906698228237069812[152] = 1; + out_906698228237069812[153] = 0; + out_906698228237069812[154] = 0; + out_906698228237069812[155] = 0; + out_906698228237069812[156] = 0; + out_906698228237069812[157] = 0; + out_906698228237069812[158] = 0; + out_906698228237069812[159] = 0; + out_906698228237069812[160] = 0; + out_906698228237069812[161] = 0; + out_906698228237069812[162] = 0; + out_906698228237069812[163] = 0; + out_906698228237069812[164] = 0; + out_906698228237069812[165] = 0; + out_906698228237069812[166] = 0; + out_906698228237069812[167] = 0; + out_906698228237069812[168] = 0; + out_906698228237069812[169] = 0; + out_906698228237069812[170] = 0; + out_906698228237069812[171] = 1; + out_906698228237069812[172] = 0; + out_906698228237069812[173] = 0; + out_906698228237069812[174] = 0; + out_906698228237069812[175] = 0; + out_906698228237069812[176] = 0; + out_906698228237069812[177] = 0; + out_906698228237069812[178] = 0; + out_906698228237069812[179] = 0; + out_906698228237069812[180] = 0; + out_906698228237069812[181] = 0; + out_906698228237069812[182] = 0; + out_906698228237069812[183] = 0; + out_906698228237069812[184] = 0; + out_906698228237069812[185] = 0; + out_906698228237069812[186] = 0; + out_906698228237069812[187] = 0; + out_906698228237069812[188] = 0; + out_906698228237069812[189] = 0; + out_906698228237069812[190] = 1; + out_906698228237069812[191] = 0; + out_906698228237069812[192] = 0; + out_906698228237069812[193] = 0; + out_906698228237069812[194] = 0; + out_906698228237069812[195] = 0; + out_906698228237069812[196] = 0; + out_906698228237069812[197] = 0; + out_906698228237069812[198] = 0; + out_906698228237069812[199] = 0; + out_906698228237069812[200] = 0; + out_906698228237069812[201] = 0; + out_906698228237069812[202] = 0; + out_906698228237069812[203] = 0; + out_906698228237069812[204] = 0; + out_906698228237069812[205] = 0; + out_906698228237069812[206] = 0; + out_906698228237069812[207] = 0; + out_906698228237069812[208] = 0; + out_906698228237069812[209] = 1; + out_906698228237069812[210] = 0; + out_906698228237069812[211] = 0; + out_906698228237069812[212] = 0; + out_906698228237069812[213] = 0; + out_906698228237069812[214] = 0; + out_906698228237069812[215] = 0; + out_906698228237069812[216] = 0; + out_906698228237069812[217] = 0; + out_906698228237069812[218] = 0; + out_906698228237069812[219] = 0; + out_906698228237069812[220] = 0; + out_906698228237069812[221] = 0; + out_906698228237069812[222] = 0; + out_906698228237069812[223] = 0; + out_906698228237069812[224] = 0; + out_906698228237069812[225] = 0; + out_906698228237069812[226] = 0; + out_906698228237069812[227] = 0; + out_906698228237069812[228] = 1; + out_906698228237069812[229] = 0; + out_906698228237069812[230] = 0; + out_906698228237069812[231] = 0; + out_906698228237069812[232] = 0; + out_906698228237069812[233] = 0; + out_906698228237069812[234] = 0; + out_906698228237069812[235] = 0; + out_906698228237069812[236] = 0; + out_906698228237069812[237] = 0; + out_906698228237069812[238] = 0; + out_906698228237069812[239] = 0; + out_906698228237069812[240] = 0; + out_906698228237069812[241] = 0; + out_906698228237069812[242] = 0; + out_906698228237069812[243] = 0; + out_906698228237069812[244] = 0; + out_906698228237069812[245] = 0; + out_906698228237069812[246] = 0; + out_906698228237069812[247] = 1; + out_906698228237069812[248] = 0; + out_906698228237069812[249] = 0; + out_906698228237069812[250] = 0; + out_906698228237069812[251] = 0; + out_906698228237069812[252] = 0; + out_906698228237069812[253] = 0; + out_906698228237069812[254] = 0; + out_906698228237069812[255] = 0; + out_906698228237069812[256] = 0; + out_906698228237069812[257] = 0; + out_906698228237069812[258] = 0; + out_906698228237069812[259] = 0; + out_906698228237069812[260] = 0; + out_906698228237069812[261] = 0; + out_906698228237069812[262] = 0; + out_906698228237069812[263] = 0; + out_906698228237069812[264] = 0; + out_906698228237069812[265] = 0; + out_906698228237069812[266] = 1; + out_906698228237069812[267] = 0; + out_906698228237069812[268] = 0; + out_906698228237069812[269] = 0; + out_906698228237069812[270] = 0; + out_906698228237069812[271] = 0; + out_906698228237069812[272] = 0; + out_906698228237069812[273] = 0; + out_906698228237069812[274] = 0; + out_906698228237069812[275] = 0; + out_906698228237069812[276] = 0; + out_906698228237069812[277] = 0; + out_906698228237069812[278] = 0; + out_906698228237069812[279] = 0; + out_906698228237069812[280] = 0; + out_906698228237069812[281] = 0; + out_906698228237069812[282] = 0; + out_906698228237069812[283] = 0; + out_906698228237069812[284] = 0; + out_906698228237069812[285] = 1; + out_906698228237069812[286] = 0; + out_906698228237069812[287] = 0; + out_906698228237069812[288] = 0; + out_906698228237069812[289] = 0; + out_906698228237069812[290] = 0; + out_906698228237069812[291] = 0; + out_906698228237069812[292] = 0; + out_906698228237069812[293] = 0; + out_906698228237069812[294] = 0; + out_906698228237069812[295] = 0; + out_906698228237069812[296] = 0; + out_906698228237069812[297] = 0; + out_906698228237069812[298] = 0; + out_906698228237069812[299] = 0; + out_906698228237069812[300] = 0; + out_906698228237069812[301] = 0; + out_906698228237069812[302] = 0; + out_906698228237069812[303] = 0; + out_906698228237069812[304] = 1; + out_906698228237069812[305] = 0; + out_906698228237069812[306] = 0; + out_906698228237069812[307] = 0; + out_906698228237069812[308] = 0; + out_906698228237069812[309] = 0; + out_906698228237069812[310] = 0; + out_906698228237069812[311] = 0; + out_906698228237069812[312] = 0; + out_906698228237069812[313] = 0; + out_906698228237069812[314] = 0; + out_906698228237069812[315] = 0; + out_906698228237069812[316] = 0; + out_906698228237069812[317] = 0; + out_906698228237069812[318] = 0; + out_906698228237069812[319] = 0; + out_906698228237069812[320] = 0; + out_906698228237069812[321] = 0; + out_906698228237069812[322] = 0; + out_906698228237069812[323] = 1; } -void h_4(double *state, double *unused, double *out_2660879566845649022) { - out_2660879566845649022[0] = state[6] + state[9]; - out_2660879566845649022[1] = state[7] + state[10]; - out_2660879566845649022[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_6577940567349030778) { + out_6577940567349030778[0] = state[6] + state[9]; + out_6577940567349030778[1] = state[7] + state[10]; + out_6577940567349030778[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_6786733476361746073) { - out_6786733476361746073[0] = 0; - out_6786733476361746073[1] = 0; - out_6786733476361746073[2] = 0; - out_6786733476361746073[3] = 0; - out_6786733476361746073[4] = 0; - out_6786733476361746073[5] = 0; - out_6786733476361746073[6] = 1; - out_6786733476361746073[7] = 0; - out_6786733476361746073[8] = 0; - out_6786733476361746073[9] = 1; - out_6786733476361746073[10] = 0; - out_6786733476361746073[11] = 0; - out_6786733476361746073[12] = 0; - out_6786733476361746073[13] = 0; - out_6786733476361746073[14] = 0; - out_6786733476361746073[15] = 0; - out_6786733476361746073[16] = 0; - out_6786733476361746073[17] = 0; - out_6786733476361746073[18] = 0; - out_6786733476361746073[19] = 0; - out_6786733476361746073[20] = 0; - out_6786733476361746073[21] = 0; - out_6786733476361746073[22] = 0; - out_6786733476361746073[23] = 0; - out_6786733476361746073[24] = 0; - out_6786733476361746073[25] = 1; - out_6786733476361746073[26] = 0; - out_6786733476361746073[27] = 0; - out_6786733476361746073[28] = 1; - out_6786733476361746073[29] = 0; - out_6786733476361746073[30] = 0; - out_6786733476361746073[31] = 0; - out_6786733476361746073[32] = 0; - out_6786733476361746073[33] = 0; - out_6786733476361746073[34] = 0; - out_6786733476361746073[35] = 0; - out_6786733476361746073[36] = 0; - out_6786733476361746073[37] = 0; - out_6786733476361746073[38] = 0; - out_6786733476361746073[39] = 0; - out_6786733476361746073[40] = 0; - out_6786733476361746073[41] = 0; - out_6786733476361746073[42] = 0; - out_6786733476361746073[43] = 0; - out_6786733476361746073[44] = 1; - out_6786733476361746073[45] = 0; - out_6786733476361746073[46] = 0; - out_6786733476361746073[47] = 1; - out_6786733476361746073[48] = 0; - out_6786733476361746073[49] = 0; - out_6786733476361746073[50] = 0; - out_6786733476361746073[51] = 0; - out_6786733476361746073[52] = 0; - out_6786733476361746073[53] = 0; +void H_4(double *state, double *unused, double *out_913158878120160673) { + out_913158878120160673[0] = 0; + out_913158878120160673[1] = 0; + out_913158878120160673[2] = 0; + out_913158878120160673[3] = 0; + out_913158878120160673[4] = 0; + out_913158878120160673[5] = 0; + out_913158878120160673[6] = 1; + out_913158878120160673[7] = 0; + out_913158878120160673[8] = 0; + out_913158878120160673[9] = 1; + out_913158878120160673[10] = 0; + out_913158878120160673[11] = 0; + out_913158878120160673[12] = 0; + out_913158878120160673[13] = 0; + out_913158878120160673[14] = 0; + out_913158878120160673[15] = 0; + out_913158878120160673[16] = 0; + out_913158878120160673[17] = 0; + out_913158878120160673[18] = 0; + out_913158878120160673[19] = 0; + out_913158878120160673[20] = 0; + out_913158878120160673[21] = 0; + out_913158878120160673[22] = 0; + out_913158878120160673[23] = 0; + out_913158878120160673[24] = 0; + out_913158878120160673[25] = 1; + out_913158878120160673[26] = 0; + out_913158878120160673[27] = 0; + out_913158878120160673[28] = 1; + out_913158878120160673[29] = 0; + out_913158878120160673[30] = 0; + out_913158878120160673[31] = 0; + out_913158878120160673[32] = 0; + out_913158878120160673[33] = 0; + out_913158878120160673[34] = 0; + out_913158878120160673[35] = 0; + out_913158878120160673[36] = 0; + out_913158878120160673[37] = 0; + out_913158878120160673[38] = 0; + out_913158878120160673[39] = 0; + out_913158878120160673[40] = 0; + out_913158878120160673[41] = 0; + out_913158878120160673[42] = 0; + out_913158878120160673[43] = 0; + out_913158878120160673[44] = 1; + out_913158878120160673[45] = 0; + out_913158878120160673[46] = 0; + out_913158878120160673[47] = 1; + out_913158878120160673[48] = 0; + out_913158878120160673[49] = 0; + out_913158878120160673[50] = 0; + out_913158878120160673[51] = 0; + out_913158878120160673[52] = 0; + out_913158878120160673[53] = 0; } -void h_10(double *state, double *unused, double *out_323021619404120390) { - out_323021619404120390[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_323021619404120390[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_323021619404120390[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_3373051515203641771) { + out_3373051515203641771[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_3373051515203641771[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_3373051515203641771[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_4140593631986057126) { - out_4140593631986057126[0] = 0; - out_4140593631986057126[1] = 9.8100000000000005*cos(state[1]); - out_4140593631986057126[2] = 0; - out_4140593631986057126[3] = 0; - out_4140593631986057126[4] = -state[8]; - out_4140593631986057126[5] = state[7]; - out_4140593631986057126[6] = 0; - out_4140593631986057126[7] = state[5]; - out_4140593631986057126[8] = -state[4]; - out_4140593631986057126[9] = 0; - out_4140593631986057126[10] = 0; - out_4140593631986057126[11] = 0; - out_4140593631986057126[12] = 1; - out_4140593631986057126[13] = 0; - out_4140593631986057126[14] = 0; - out_4140593631986057126[15] = 1; - out_4140593631986057126[16] = 0; - out_4140593631986057126[17] = 0; - out_4140593631986057126[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_4140593631986057126[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_4140593631986057126[20] = 0; - out_4140593631986057126[21] = state[8]; - out_4140593631986057126[22] = 0; - out_4140593631986057126[23] = -state[6]; - out_4140593631986057126[24] = -state[5]; - out_4140593631986057126[25] = 0; - out_4140593631986057126[26] = state[3]; - out_4140593631986057126[27] = 0; - out_4140593631986057126[28] = 0; - out_4140593631986057126[29] = 0; - out_4140593631986057126[30] = 0; - out_4140593631986057126[31] = 1; - out_4140593631986057126[32] = 0; - out_4140593631986057126[33] = 0; - out_4140593631986057126[34] = 1; - out_4140593631986057126[35] = 0; - out_4140593631986057126[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_4140593631986057126[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_4140593631986057126[38] = 0; - out_4140593631986057126[39] = -state[7]; - out_4140593631986057126[40] = state[6]; - out_4140593631986057126[41] = 0; - out_4140593631986057126[42] = state[4]; - out_4140593631986057126[43] = -state[3]; - out_4140593631986057126[44] = 0; - out_4140593631986057126[45] = 0; - out_4140593631986057126[46] = 0; - out_4140593631986057126[47] = 0; - out_4140593631986057126[48] = 0; - out_4140593631986057126[49] = 0; - out_4140593631986057126[50] = 1; - out_4140593631986057126[51] = 0; - out_4140593631986057126[52] = 0; - out_4140593631986057126[53] = 1; +void H_10(double *state, double *unused, double *out_7288760212554836769) { + out_7288760212554836769[0] = 0; + out_7288760212554836769[1] = 9.8100000000000005*cos(state[1]); + out_7288760212554836769[2] = 0; + out_7288760212554836769[3] = 0; + out_7288760212554836769[4] = -state[8]; + out_7288760212554836769[5] = state[7]; + out_7288760212554836769[6] = 0; + out_7288760212554836769[7] = state[5]; + out_7288760212554836769[8] = -state[4]; + out_7288760212554836769[9] = 0; + out_7288760212554836769[10] = 0; + out_7288760212554836769[11] = 0; + out_7288760212554836769[12] = 1; + out_7288760212554836769[13] = 0; + out_7288760212554836769[14] = 0; + out_7288760212554836769[15] = 1; + out_7288760212554836769[16] = 0; + out_7288760212554836769[17] = 0; + out_7288760212554836769[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_7288760212554836769[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_7288760212554836769[20] = 0; + out_7288760212554836769[21] = state[8]; + out_7288760212554836769[22] = 0; + out_7288760212554836769[23] = -state[6]; + out_7288760212554836769[24] = -state[5]; + out_7288760212554836769[25] = 0; + out_7288760212554836769[26] = state[3]; + out_7288760212554836769[27] = 0; + out_7288760212554836769[28] = 0; + out_7288760212554836769[29] = 0; + out_7288760212554836769[30] = 0; + out_7288760212554836769[31] = 1; + out_7288760212554836769[32] = 0; + out_7288760212554836769[33] = 0; + out_7288760212554836769[34] = 1; + out_7288760212554836769[35] = 0; + out_7288760212554836769[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_7288760212554836769[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_7288760212554836769[38] = 0; + out_7288760212554836769[39] = -state[7]; + out_7288760212554836769[40] = state[6]; + out_7288760212554836769[41] = 0; + out_7288760212554836769[42] = state[4]; + out_7288760212554836769[43] = -state[3]; + out_7288760212554836769[44] = 0; + out_7288760212554836769[45] = 0; + out_7288760212554836769[46] = 0; + out_7288760212554836769[47] = 0; + out_7288760212554836769[48] = 0; + out_7288760212554836769[49] = 0; + out_7288760212554836769[50] = 1; + out_7288760212554836769[51] = 0; + out_7288760212554836769[52] = 0; + out_7288760212554836769[53] = 1; } -void h_13(double *state, double *unused, double *out_3801171022701447110) { - out_3801171022701447110[0] = state[3]; - out_3801171022701447110[1] = state[4]; - out_3801171022701447110[2] = state[5]; +void h_13(double *state, double *unused, double *out_413669578140119338) { + out_413669578140119338[0] = state[3]; + out_413669578140119338[1] = state[4]; + out_413669578140119338[2] = state[5]; } -void H_13(double *state, double *unused, double *out_3574459651029413272) { - out_3574459651029413272[0] = 0; - out_3574459651029413272[1] = 0; - out_3574459651029413272[2] = 0; - out_3574459651029413272[3] = 1; - out_3574459651029413272[4] = 0; - out_3574459651029413272[5] = 0; - out_3574459651029413272[6] = 0; - out_3574459651029413272[7] = 0; - out_3574459651029413272[8] = 0; - out_3574459651029413272[9] = 0; - out_3574459651029413272[10] = 0; - out_3574459651029413272[11] = 0; - out_3574459651029413272[12] = 0; - out_3574459651029413272[13] = 0; - out_3574459651029413272[14] = 0; - out_3574459651029413272[15] = 0; - out_3574459651029413272[16] = 0; - out_3574459651029413272[17] = 0; - out_3574459651029413272[18] = 0; - out_3574459651029413272[19] = 0; - out_3574459651029413272[20] = 0; - out_3574459651029413272[21] = 0; - out_3574459651029413272[22] = 1; - out_3574459651029413272[23] = 0; - out_3574459651029413272[24] = 0; - out_3574459651029413272[25] = 0; - out_3574459651029413272[26] = 0; - out_3574459651029413272[27] = 0; - out_3574459651029413272[28] = 0; - out_3574459651029413272[29] = 0; - out_3574459651029413272[30] = 0; - out_3574459651029413272[31] = 0; - out_3574459651029413272[32] = 0; - out_3574459651029413272[33] = 0; - out_3574459651029413272[34] = 0; - out_3574459651029413272[35] = 0; - out_3574459651029413272[36] = 0; - out_3574459651029413272[37] = 0; - out_3574459651029413272[38] = 0; - out_3574459651029413272[39] = 0; - out_3574459651029413272[40] = 0; - out_3574459651029413272[41] = 1; - out_3574459651029413272[42] = 0; - out_3574459651029413272[43] = 0; - out_3574459651029413272[44] = 0; - out_3574459651029413272[45] = 0; - out_3574459651029413272[46] = 0; - out_3574459651029413272[47] = 0; - out_3574459651029413272[48] = 0; - out_3574459651029413272[49] = 0; - out_3574459651029413272[50] = 0; - out_3574459651029413272[51] = 0; - out_3574459651029413272[52] = 0; - out_3574459651029413272[53] = 0; +void H_13(double *state, double *unused, double *out_8523790086436861602) { + out_8523790086436861602[0] = 0; + out_8523790086436861602[1] = 0; + out_8523790086436861602[2] = 0; + out_8523790086436861602[3] = 1; + out_8523790086436861602[4] = 0; + out_8523790086436861602[5] = 0; + out_8523790086436861602[6] = 0; + out_8523790086436861602[7] = 0; + out_8523790086436861602[8] = 0; + out_8523790086436861602[9] = 0; + out_8523790086436861602[10] = 0; + out_8523790086436861602[11] = 0; + out_8523790086436861602[12] = 0; + out_8523790086436861602[13] = 0; + out_8523790086436861602[14] = 0; + out_8523790086436861602[15] = 0; + out_8523790086436861602[16] = 0; + out_8523790086436861602[17] = 0; + out_8523790086436861602[18] = 0; + out_8523790086436861602[19] = 0; + out_8523790086436861602[20] = 0; + out_8523790086436861602[21] = 0; + out_8523790086436861602[22] = 1; + out_8523790086436861602[23] = 0; + out_8523790086436861602[24] = 0; + out_8523790086436861602[25] = 0; + out_8523790086436861602[26] = 0; + out_8523790086436861602[27] = 0; + out_8523790086436861602[28] = 0; + out_8523790086436861602[29] = 0; + out_8523790086436861602[30] = 0; + out_8523790086436861602[31] = 0; + out_8523790086436861602[32] = 0; + out_8523790086436861602[33] = 0; + out_8523790086436861602[34] = 0; + out_8523790086436861602[35] = 0; + out_8523790086436861602[36] = 0; + out_8523790086436861602[37] = 0; + out_8523790086436861602[38] = 0; + out_8523790086436861602[39] = 0; + out_8523790086436861602[40] = 0; + out_8523790086436861602[41] = 1; + out_8523790086436861602[42] = 0; + out_8523790086436861602[43] = 0; + out_8523790086436861602[44] = 0; + out_8523790086436861602[45] = 0; + out_8523790086436861602[46] = 0; + out_8523790086436861602[47] = 0; + out_8523790086436861602[48] = 0; + out_8523790086436861602[49] = 0; + out_8523790086436861602[50] = 0; + out_8523790086436861602[51] = 0; + out_8523790086436861602[52] = 0; + out_8523790086436861602[53] = 0; } -void h_14(double *state, double *unused, double *out_7703097660121677315) { - out_7703097660121677315[0] = state[6]; - out_7703097660121677315[1] = state[7]; - out_7703097660121677315[2] = state[8]; +void h_14(double *state, double *unused, double *out_8770101800505378255) { + out_8770101800505378255[0] = state[6]; + out_8770101800505378255[1] = state[7]; + out_8770101800505378255[2] = state[8]; } -void H_14(double *state, double *unused, double *out_2823492620022261544) { - out_2823492620022261544[0] = 0; - out_2823492620022261544[1] = 0; - out_2823492620022261544[2] = 0; - out_2823492620022261544[3] = 0; - out_2823492620022261544[4] = 0; - out_2823492620022261544[5] = 0; - out_2823492620022261544[6] = 1; - out_2823492620022261544[7] = 0; - out_2823492620022261544[8] = 0; - out_2823492620022261544[9] = 0; - out_2823492620022261544[10] = 0; - out_2823492620022261544[11] = 0; - out_2823492620022261544[12] = 0; - out_2823492620022261544[13] = 0; - out_2823492620022261544[14] = 0; - out_2823492620022261544[15] = 0; - out_2823492620022261544[16] = 0; - out_2823492620022261544[17] = 0; - out_2823492620022261544[18] = 0; - out_2823492620022261544[19] = 0; - out_2823492620022261544[20] = 0; - out_2823492620022261544[21] = 0; - out_2823492620022261544[22] = 0; - out_2823492620022261544[23] = 0; - out_2823492620022261544[24] = 0; - out_2823492620022261544[25] = 1; - out_2823492620022261544[26] = 0; - out_2823492620022261544[27] = 0; - out_2823492620022261544[28] = 0; - out_2823492620022261544[29] = 0; - out_2823492620022261544[30] = 0; - out_2823492620022261544[31] = 0; - out_2823492620022261544[32] = 0; - out_2823492620022261544[33] = 0; - out_2823492620022261544[34] = 0; - out_2823492620022261544[35] = 0; - out_2823492620022261544[36] = 0; - out_2823492620022261544[37] = 0; - out_2823492620022261544[38] = 0; - out_2823492620022261544[39] = 0; - out_2823492620022261544[40] = 0; - out_2823492620022261544[41] = 0; - out_2823492620022261544[42] = 0; - out_2823492620022261544[43] = 0; - out_2823492620022261544[44] = 1; - out_2823492620022261544[45] = 0; - out_2823492620022261544[46] = 0; - out_2823492620022261544[47] = 0; - out_2823492620022261544[48] = 0; - out_2823492620022261544[49] = 0; - out_2823492620022261544[50] = 0; - out_2823492620022261544[51] = 0; - out_2823492620022261544[52] = 0; - out_2823492620022261544[53] = 0; +void H_14(double *state, double *unused, double *out_4876399734459645202) { + out_4876399734459645202[0] = 0; + out_4876399734459645202[1] = 0; + out_4876399734459645202[2] = 0; + out_4876399734459645202[3] = 0; + out_4876399734459645202[4] = 0; + out_4876399734459645202[5] = 0; + out_4876399734459645202[6] = 1; + out_4876399734459645202[7] = 0; + out_4876399734459645202[8] = 0; + out_4876399734459645202[9] = 0; + out_4876399734459645202[10] = 0; + out_4876399734459645202[11] = 0; + out_4876399734459645202[12] = 0; + out_4876399734459645202[13] = 0; + out_4876399734459645202[14] = 0; + out_4876399734459645202[15] = 0; + out_4876399734459645202[16] = 0; + out_4876399734459645202[17] = 0; + out_4876399734459645202[18] = 0; + out_4876399734459645202[19] = 0; + out_4876399734459645202[20] = 0; + out_4876399734459645202[21] = 0; + out_4876399734459645202[22] = 0; + out_4876399734459645202[23] = 0; + out_4876399734459645202[24] = 0; + out_4876399734459645202[25] = 1; + out_4876399734459645202[26] = 0; + out_4876399734459645202[27] = 0; + out_4876399734459645202[28] = 0; + out_4876399734459645202[29] = 0; + out_4876399734459645202[30] = 0; + out_4876399734459645202[31] = 0; + out_4876399734459645202[32] = 0; + out_4876399734459645202[33] = 0; + out_4876399734459645202[34] = 0; + out_4876399734459645202[35] = 0; + out_4876399734459645202[36] = 0; + out_4876399734459645202[37] = 0; + out_4876399734459645202[38] = 0; + out_4876399734459645202[39] = 0; + out_4876399734459645202[40] = 0; + out_4876399734459645202[41] = 0; + out_4876399734459645202[42] = 0; + out_4876399734459645202[43] = 0; + out_4876399734459645202[44] = 1; + out_4876399734459645202[45] = 0; + out_4876399734459645202[46] = 0; + out_4876399734459645202[47] = 0; + out_4876399734459645202[48] = 0; + out_4876399734459645202[49] = 0; + out_4876399734459645202[50] = 0; + out_4876399734459645202[51] = 0; + out_4876399734459645202[52] = 0; + out_4876399734459645202[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_5984861409348236479) { - err_fun(nom_x, delta_x, out_5984861409348236479); +void pose_err_fun(double *nom_x, double *delta_x, double *out_5188089899736494345) { + err_fun(nom_x, delta_x, out_5188089899736494345); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1707565234864444406) { - inv_err_fun(nom_x, true_x, out_1707565234864444406); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_516319331577825853) { + inv_err_fun(nom_x, true_x, out_516319331577825853); } -void pose_H_mod_fun(double *state, double *out_938651669609787352) { - H_mod_fun(state, out_938651669609787352); +void pose_H_mod_fun(double *state, double *out_4951567132479851062) { + H_mod_fun(state, out_4951567132479851062); } -void pose_f_fun(double *state, double dt, double *out_336566407888546136) { - f_fun(state, dt, out_336566407888546136); +void pose_f_fun(double *state, double dt, double *out_898454825503606476) { + f_fun(state, dt, out_898454825503606476); } -void pose_F_fun(double *state, double dt, double *out_5996324224975557063) { - F_fun(state, dt, out_5996324224975557063); +void pose_F_fun(double *state, double dt, double *out_906698228237069812) { + F_fun(state, dt, out_906698228237069812); } -void pose_h_4(double *state, double *unused, double *out_2660879566845649022) { - h_4(state, unused, out_2660879566845649022); +void pose_h_4(double *state, double *unused, double *out_6577940567349030778) { + h_4(state, unused, out_6577940567349030778); } -void pose_H_4(double *state, double *unused, double *out_6786733476361746073) { - H_4(state, unused, out_6786733476361746073); +void pose_H_4(double *state, double *unused, double *out_913158878120160673) { + H_4(state, unused, out_913158878120160673); } -void pose_h_10(double *state, double *unused, double *out_323021619404120390) { - h_10(state, unused, out_323021619404120390); +void pose_h_10(double *state, double *unused, double *out_3373051515203641771) { + h_10(state, unused, out_3373051515203641771); } -void pose_H_10(double *state, double *unused, double *out_4140593631986057126) { - H_10(state, unused, out_4140593631986057126); +void pose_H_10(double *state, double *unused, double *out_7288760212554836769) { + H_10(state, unused, out_7288760212554836769); } -void pose_h_13(double *state, double *unused, double *out_3801171022701447110) { - h_13(state, unused, out_3801171022701447110); +void pose_h_13(double *state, double *unused, double *out_413669578140119338) { + h_13(state, unused, out_413669578140119338); } -void pose_H_13(double *state, double *unused, double *out_3574459651029413272) { - H_13(state, unused, out_3574459651029413272); +void pose_H_13(double *state, double *unused, double *out_8523790086436861602) { + H_13(state, unused, out_8523790086436861602); } -void pose_h_14(double *state, double *unused, double *out_7703097660121677315) { - h_14(state, unused, out_7703097660121677315); +void pose_h_14(double *state, double *unused, double *out_8770101800505378255) { + h_14(state, unused, out_8770101800505378255); } -void pose_H_14(double *state, double *unused, double *out_2823492620022261544) { - H_14(state, unused, out_2823492620022261544); +void pose_H_14(double *state, double *unused, double *out_4876399734459645202) { + H_14(state, unused, out_4876399734459645202); } 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 c6b128c72..4a1d716f9 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_5984861409348236479); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_1707565234864444406); -void pose_H_mod_fun(double *state, double *out_938651669609787352); -void pose_f_fun(double *state, double dt, double *out_336566407888546136); -void pose_F_fun(double *state, double dt, double *out_5996324224975557063); -void pose_h_4(double *state, double *unused, double *out_2660879566845649022); -void pose_H_4(double *state, double *unused, double *out_6786733476361746073); -void pose_h_10(double *state, double *unused, double *out_323021619404120390); -void pose_H_10(double *state, double *unused, double *out_4140593631986057126); -void pose_h_13(double *state, double *unused, double *out_3801171022701447110); -void pose_H_13(double *state, double *unused, double *out_3574459651029413272); -void pose_h_14(double *state, double *unused, double *out_7703097660121677315); -void pose_H_14(double *state, double *unused, double *out_2823492620022261544); +void pose_err_fun(double *nom_x, double *delta_x, double *out_5188089899736494345); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_516319331577825853); +void pose_H_mod_fun(double *state, double *out_4951567132479851062); +void pose_f_fun(double *state, double dt, double *out_898454825503606476); +void pose_F_fun(double *state, double dt, double *out_906698228237069812); +void pose_h_4(double *state, double *unused, double *out_6577940567349030778); +void pose_H_4(double *state, double *unused, double *out_913158878120160673); +void pose_h_10(double *state, double *unused, double *out_3373051515203641771); +void pose_H_10(double *state, double *unused, double *out_7288760212554836769); +void pose_h_13(double *state, double *unused, double *out_413669578140119338); +void pose_H_13(double *state, double *unused, double *out_8523790086436861602); +void pose_h_14(double *state, double *unused, double *out_8770101800505378255); +void pose_H_14(double *state, double *unused, double *out_4876399734459645202); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index 3a0aeef22..7c17b3d68 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py index d33f06982..42dbdc898 100644 --- a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py +++ b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py @@ -686,12 +686,7 @@ class StarPilotLongitudinalTuneLayout(_SettingsPage): set_state=lambda s: self._params.put_bool("CoastUpToLeads", s), visible=self._longitudinal_enabled), ], column_pair="human_driving", row_height=UTILITY_ROW_HEIGHT), - SettingSection(tr_noop("Following"), [ - SettingRow("HumanFollowing", "toggle", tr_noop("Human-Like Following"), - subtitle=tr_noop("More natural following behavior behind other vehicles."), - get_state=lambda: self._params.get_bool("HumanFollowing"), - set_state=lambda s: self._params.put_bool("HumanFollowing", s), - visible=self._longitudinal_enabled), + SettingSection(tr_noop("Lane Changes"), [ SettingRow("HumanLaneChanges", "toggle", tr_noop("Human-Like Lane Changes"), subtitle=tr_noop("Radar-informed behavior during lane changes."), get_state=lambda: self._params.get_bool("HumanLaneChanges"), diff --git a/selfdrive/ui/translations/main_uk.qm b/selfdrive/ui/translations/main_uk.qm index 06fa6b097..34371fc7b 100644 Binary files a/selfdrive/ui/translations/main_uk.qm and b/selfdrive/ui/translations/main_uk.qm differ diff --git a/selfdrive/ui/translations/main_uk.ts b/selfdrive/ui/translations/main_uk.ts index 2ff25ae02..5d7770ad4 100644 --- a/selfdrive/ui/translations/main_uk.ts +++ b/selfdrive/ui/translations/main_uk.ts @@ -2246,14 +2246,6 @@ <b>Acceleration that mimics human behavior</b> by easing the throttle at low speeds and adding extra power when taking off from a stop. <b>Прискорення, що імітує поведінку людини</b>, шляхом послаблення дросельної заслінки на низьких швидкостях і додавання додаткової потужності при рушанні з місця. - - Human-Like Following - Людиноподібне слідування - - - <b>Following behavior that mimics human drivers</b> by closing gaps behind faster vehicles for quicker takeoffs and dynamically adjusting the desired following distance for gentler, more efficient braking. - <b>Поведінка, що імітує поведінку водіїв-людей</b>, шляхом скорочення відстані за швидшими транспортними засобами для швидшого старту та динамічного регулювання бажаної відстані для більш плавного та ефективного гальмування. - Lead Detection Sensitivity Чутливість виявлення лідера diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 8549c99dd..a2e997d1a 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/starpilot/common/safe_mode.py b/starpilot/common/safe_mode.py index 45c823166..5e8d82b70 100644 --- a/starpilot/common/safe_mode.py +++ b/starpilot/common/safe_mode.py @@ -67,7 +67,6 @@ SAFE_MODE_MANAGED_KEYS = ( "AccelerationProfile", "DecelerationProfile", "HumanAcceleration", - "HumanFollowing", "CoastUpToLeads", "HumanLaneChanges", "LeadDetectionThreshold", diff --git a/starpilot/common/starpilot_variables.py b/starpilot/common/starpilot_variables.py index f9f7a1bb1..7ca7d79d2 100644 --- a/starpilot/common/starpilot_variables.py +++ b/starpilot/common/starpilot_variables.py @@ -946,7 +946,6 @@ class StarPilotVariables: else: toggle.custom_accel_profile_values = [custom_accel_defaults[key] for key in CUSTOM_ACCEL_PROFILE_PARAM_KEYS] toggle.human_acceleration = self.get_value("HumanAcceleration", condition=longitudinal_tuning) - toggle.human_following = self.get_value("HumanFollowing", condition=longitudinal_tuning) toggle.coast_up_to_leads = self.get_value("CoastUpToLeads", condition=longitudinal_tuning) if longitudinal_tuning and self.params.get("CoastUpToLeads") is None: toggle.coast_up_to_leads = True diff --git a/starpilot/controls/lib/starpilot_following.py b/starpilot/controls/lib/starpilot_following.py index baee7bddb..bdc47c9fc 100644 --- a/starpilot/controls/lib/starpilot_following.py +++ b/starpilot/controls/lib/starpilot_following.py @@ -99,23 +99,7 @@ class StarPilotFollowing: self.desired_follow_distance = 0 def update_follow_values(self, lead_distance, v_ego, v_lead, starpilot_toggles): - if starpilot_toggles.human_following and v_lead > v_ego: - distance_factor = max(lead_distance - (v_ego * self.t_follow), 1) - from starpilot.common.starpilot_variables import get_starpilot_toggles - fp_toggles = get_starpilot_toggles() - accelerating_offset = float(np.clip(fp_toggles.stop_distance - v_ego, 1, distance_factor)) - - self.acceleration_jerk /= accelerating_offset - self.speed_jerk /= accelerating_offset - self.t_follow /= accelerating_offset - - if (starpilot_toggles.conditional_slower_lead or starpilot_toggles.human_following) and v_lead < v_ego: + if starpilot_toggles.conditional_slower_lead and v_lead < v_ego: distance_factor = max(lead_distance - (v_lead * self.t_follow), 1) braking_offset = float(np.clip(min(v_ego - v_lead, v_lead) - COMFORT_BRAKE, 1, distance_factor)) - - if starpilot_toggles.human_following: - from starpilot.common.starpilot_variables import get_starpilot_toggles - fp_toggles = get_starpilot_toggles() - far_lead_offset = max(lead_distance - (v_ego * self.t_follow) - fp_toggles.stop_distance, 0) - self.t_follow /= braking_offset + far_lead_offset self.slower_lead = braking_offset > 1 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 fe9a2f535..cf5763ad9 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 @@ -832,7 +832,7 @@ { "key": "RelaxedFollow", "label": "Following Distance", - "description": "How many seconds openpilot follows behind lead vehicles when using the \"Relaxed\" profile. Increase for more space; decrease for tighter gaps.\n\nDefault: 1.75 seconds.", + "description": "How many seconds openpilot follows behind lead vehicles when using the \"Relaxed\" profile. Increase for more space; decrease for tighter gaps.\n\nDefault: 1.6 seconds.", "data_type": "float", "ui_type": "numeric", "min": 0.5, @@ -970,14 +970,6 @@ "ui_type": "toggle", "parent_key": "LongitudinalTune" }, - { - "key": "HumanFollowing", - "label": "Human-Like Following", - "description": "Following behavior that mimics human drivers by closing gaps behind faster vehicles for quicker takeoffs and dynamically adjusting the desired following distance for gentler, more efficient braking.", - "data_type": "bool", - "ui_type": "toggle", - "parent_key": "LongitudinalTune" - }, { "key": "CoastUpToLeads", "label": "Coast Up To Leads", diff --git a/starpilot/ui/qt/offroad/longitudinal_settings.cc b/starpilot/ui/qt/offroad/longitudinal_settings.cc index 472535bd2..ddf4ee3fd 100644 --- a/starpilot/ui/qt/offroad/longitudinal_settings.cc +++ b/starpilot/ui/qt/offroad/longitudinal_settings.cc @@ -134,7 +134,7 @@ StarPilotLongitudinalPanel::StarPilotLongitudinalPanel(StarPilotSettingsWindow * {"ResetStandardPersonality", tr("Reset to Defaults"), tr("Reset the \"Standard\" profile to defaults."), ""}, {"RelaxedPersonalityProfile", tr("Relaxed"), tr("Customize the \"Relaxed\" personality profile. Designed for smoother, more comfortable driving with larger gaps."), "../../starpilot/assets/stock_theme/distance_icons/relaxed.png"}, - {"RelaxedFollow", tr("Following Distance"), tr("How many seconds openpilot follows behind lead vehicles when using the \"Relaxed\" profile. Increase for more space; decrease for tighter gaps.

Default: 1.75 seconds."), ""}, + {"RelaxedFollow", tr("Following Distance"), tr("How many seconds openpilot follows behind lead vehicles when using the \"Relaxed\" profile. Increase for more space; decrease for tighter gaps.

Default: 1.6 seconds."), ""}, {"RelaxedFollowHigh", tr("High Speed Following Distance"), tr("Following distance for higher speeds in the \"Relaxed\" profile. openpilot smoothly blends from the base value to this value as speed rises."), ""}, {"RelaxedJerkAcceleration", tr("Acceleration Smoothness"), tr("How smoothly openpilot accelerates with the \"Relaxed\" profile. Increase for gentler starts; decrease for faster but more abrupt takeoffs."), ""}, {"RelaxedJerkDeceleration", tr("Braking Smoothness"), tr("How smoothly openpilot brakes with the \"Relaxed\" profile. Increase for gentler stops; decrease for quicker but sharper braking."), ""}, @@ -147,7 +147,6 @@ StarPilotLongitudinalPanel::StarPilotLongitudinalPanel(StarPilotSettingsWindow * {"AccelerationProfile", tr("Acceleration Profile"), tr("How quickly openpilot speeds up. \"Eco\" is gentle and efficient, \"Sport\" is firmer and more responsive, and \"Sport+\" accelerates at the maximum rate allowed."), ""}, {"DecelerationProfile", tr("Deceleration Profile"), tr("How firmly openpilot slows down. \"Eco\" favors coasting, \"Sport\" applies stronger braking."), ""}, {"HumanAcceleration", tr("Human-Like Acceleration"), tr("Acceleration that mimics human behavior by easing the throttle at low speeds and adding extra power when taking off from a stop."), ""}, - {"HumanFollowing", tr("Human-Like Following"), tr("Following behavior that mimics human drivers by closing gaps behind faster vehicles for quicker takeoffs and dynamically adjusting the desired following distance for gentler, more efficient braking."), ""}, {"CoastUpToLeads", tr("Coast Up To Leads"), tr("Allow openpilot to coast toward far leads before resuming normal throttle. Disable this if your vehicle shows noticeable gas/brake alternation while approaching distant traffic."), ""}, {"HumanLaneChanges", tr("Human-Like Lane Changes"), tr("Lane-change behavior that mimics human drivers by anticipating and tracking adjacent vehicles during lane changes."), ""}, {"LeadDetectionThreshold", tr("Lead Detection Sensitivity"), tr("How sensitive openpilot is to detecting vehicles. Higher sensitivity allows quicker detection at longer distances but may react to non-vehicle objects; lower sensitivity is more conservative and reduces false detections."), ""}, diff --git a/starpilot/ui/qt/offroad/longitudinal_settings.h b/starpilot/ui/qt/offroad/longitudinal_settings.h index eea510c15..daab66f0d 100644 --- a/starpilot/ui/qt/offroad/longitudinal_settings.h +++ b/starpilot/ui/qt/offroad/longitudinal_settings.h @@ -33,7 +33,7 @@ private: QSet conditionalExperimentalKeys = {"PersistExperimentalState", "CESpeed", "CESpeedLead", "CECurves", "CELead", "CEModelStopTime", "CESignalSpeed", "CEStopLights", "ShowCEMStatus"}; QSet curveSpeedKeys = {"CalibratedLateralAcceleration", "CalibrationProgress", "ResetCurveData", "ShowCSCStatus"}; QSet customDrivingPersonalityKeys = {"AggressivePersonalityProfile", "RelaxedPersonalityProfile", "StandardPersonalityProfile", "TrafficPersonalityProfile"}; - QSet longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "HumanAcceleration", "HumanFollowing", "CoastUpToLeads", "HumanLaneChanges", "LeadDetectionThreshold", "TacoTune"}; + QSet longitudinalTuneKeys = {"AccelerationProfile", "DecelerationProfile", "HumanAcceleration", "CoastUpToLeads", "HumanLaneChanges", "LeadDetectionThreshold", "TacoTune"}; QSet qolKeys = {"CustomCruise", "CustomCruiseLong", "ForceStops", "ForceStopDistanceOffset", "ForceStandstill", "IncreasedStoppedDistance", "MapGears", "ReverseCruise", "SetSpeedOffset", "WeatherPresets"}; QSet relaxedPersonalityKeys = {"RelaxedFollow", "RelaxedFollowHigh", "RelaxedJerkAcceleration", "RelaxedJerkDeceleration", "RelaxedJerkDanger", "RelaxedJerkSpeed", "RelaxedJerkSpeedDecrease", "ResetRelaxedPersonality"}; QSet speedLimitControllerKeys = {"SLCOffsets", "SLCFallback", "SLCOverride", "SLCPriority", "SLCQOL", "SLCVisuals"}; diff --git a/system/camerad/camerad b/system/camerad/camerad index e74e76b05..aa0173fba 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 33308c61f..8c7366acf 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 879107871..cf9361e06 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index cba93fcf2..403b84224 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ diff --git a/system/manager/launch_param_migrations.py b/system/manager/launch_param_migrations.py index ad45cf3a7..184e675e6 100644 --- a/system/manager/launch_param_migrations.py +++ b/system/manager/launch_param_migrations.py @@ -15,6 +15,39 @@ LEGACY_STEER_KP = 0.7 QT_STEER_KP_PLACEHOLDER = 1.0 LAUNCH_PARAM_MIGRATION_MARKER = ".starpilot_launch_param_migrations_v2" +BRANCH_DEFAULTS_MIGRATION_MARKER = ".starpilot_branch_defaults_migrations_v1" + +BRANCH_BOOL_DEFAULTS = { + "ConditionalExperimental": True, + "CELead": True, + "CESlowerLead": True, + "CEStoppedLead": False, + "ForceStops": True, +} + +BRANCH_FLOAT_DEFAULTS = { + "AggressiveFollow": 1.25, + "AggressiveFollowHigh": 1.0, + "AggressiveJerkAcceleration": 50.0, + "AggressiveJerkDanger": 100.0, + "AggressiveJerkDeceleration": 50.0, + "AggressiveJerkSpeed": 50.0, + "AggressiveJerkSpeedDecrease": 50.0, + "StandardFollow": 1.45, + "StandardFollowHigh": 1.2, + "StandardJerkAcceleration": 100.0, + "StandardJerkDanger": 100.0, + "StandardJerkDeceleration": 100.0, + "StandardJerkSpeed": 100.0, + "StandardJerkSpeedDecrease": 100.0, + "RelaxedFollow": 1.6, + "RelaxedFollowHigh": 1.4, + "RelaxedJerkAcceleration": 100.0, + "RelaxedJerkDanger": 100.0, + "RelaxedJerkDeceleration": 100.0, + "RelaxedJerkSpeed": 100.0, + "RelaxedJerkSpeedDecrease": 100.0, +} class ParamsLike(Protocol): @@ -33,8 +66,11 @@ def _default_marker_path(params: ParamsLike) -> Path: return Path(params.get_param_path()) / LAUNCH_PARAM_MIGRATION_MARKER -def apply_launch_param_migrations(params: ParamsLike, marker_path: Path | None = None) -> None: - marker = marker_path or _default_marker_path(params) +def _branch_defaults_marker_path(params: ParamsLike) -> Path: + return Path(params.get_param_path()) / BRANCH_DEFAULTS_MIGRATION_MARKER + + +def _apply_legacy_launch_param_migrations(params: ParamsLike, marker: Path) -> None: if marker.exists(): return @@ -61,6 +97,29 @@ def apply_launch_param_migrations(params: ParamsLike, marker_path: Path | None = marker.touch() +def _apply_branch_default_migration(params: ParamsLike, marker: Path) -> None: + if marker.exists(): + return + + marker.parent.mkdir(parents=True, exist_ok=True) + + for key, value in BRANCH_BOOL_DEFAULTS.items(): + params.put_bool(key, value) + + for key, value in BRANCH_FLOAT_DEFAULTS.items(): + params.put_float(key, value) + + marker.touch() + + +def apply_launch_param_migrations(params: ParamsLike, marker_path: Path | None = None, + branch_defaults_marker_path: Path | None = None) -> None: + _apply_legacy_launch_param_migrations(params, marker_path or _default_marker_path(params)) + # Keep branch-default rollout on its own marker so older installs that already + # have the legacy marker still receive this one-time param reset. + _apply_branch_default_migration(params, branch_defaults_marker_path or _branch_defaults_marker_path(params)) + + def main() -> int: try: from openpilot.common.params import Params diff --git a/system/manager/manager.py b/system/manager/manager.py index f324dd7bc..0512b5957 100755 --- a/system/manager/manager.py +++ b/system/manager/manager.py @@ -40,6 +40,7 @@ STARPILOT_COAST_UP_TO_LEADS_MIGRATION_FLAG = Path("/data") / "starpilot_coast_up STARPILOT_PARAM_RENAME_MIGRATION_FLAG = Path("/data") / "starpilot_param_rename_v1" STARPILOT_PARAM_CANONICALIZATION_MIGRATION_FLAG = Path("/data") / "starpilot_param_canonicalization_v1" STARPILOT_PC_ROOT_MIGRATION_FLAG = Path("/data") / "starpilot_pc_root_v1" +STARPILOT_REMOVED_PARAM_KEYS = ("HumanFollowing",) LEGACY_CARMODEL_MIGRATIONS = { "CHEVROLET_BOLT_CC_2019_2021": "CHEVROLET_BOLT_CC_2018_2021", } @@ -148,6 +149,35 @@ def _has_persisted_param_file(params: Params, key: str | bytes) -> bool: return bool(path) and os.path.isfile(path) +def _remove_persisted_param_file(params: Params, key: str | bytes) -> bool: + try: + path = params.get_param_path(key) + except Exception: + return False + + if not path or not os.path.isfile(path): + return False + + try: + os.remove(path) + return True + except Exception: + cloudlog.exception(f"Failed to remove deprecated param file: {key}") + return False + + +def cleanup_removed_starpilot_params(params: Params, params_cache: Params) -> None: + removed_keys = [] + for key in STARPILOT_REMOVED_PARAM_KEYS: + removed = _remove_persisted_param_file(params, key) + removed = _remove_persisted_param_file(params_cache, key) or removed + if removed: + removed_keys.append(key) + + if removed_keys: + cloudlog.warning(f"Removed deprecated StarPilot params: {removed_keys}") + + def migrate_starpilot_param_renames(params: Params, params_cache: Params) -> None: if STARPILOT_PARAM_RENAME_MIGRATION_FLAG.exists(): return @@ -337,7 +367,6 @@ def migrate_starpilot_default_parity(params: Params, params_cache: Params) -> No "AdvancedLateralTune": True, "ForceAutoTuneOff": True, "HumanAcceleration": False, - "HumanFollowing": False, "NNFF": False, "NNFFLite": False, } @@ -385,7 +414,7 @@ def migrate_disable_humanlike_defaults(params: Params, params_cache: Params) -> disabled_keys: list[str] = [] - for key in ("HumanAcceleration", "HumanFollowing", "HumanLaneChanges"): + for key in ("HumanAcceleration", "HumanLaneChanges"): if not (params.get_bool(key) or params_cache.get_bool(key)): continue @@ -615,6 +644,7 @@ def manager_init() -> None: # Canonicalize legacy string encodings (e.g. INT params stored as "26.000000") # before bulk reads below to avoid repeated cast warnings and UI-side churn. migrate_param_type_canonicalization(params) + cleanup_removed_starpilot_params(params, params_cache) migrate_starpilot_default_parity(params, params_cache) migrate_disable_humanlike_defaults(params, params_cache) migrate_cluster_offset_default(params, params_cache) diff --git a/system/manager/test/test_launch_param_migrations.py b/system/manager/test/test_launch_param_migrations.py index 18f03f562..491a6defa 100644 --- a/system/manager/test/test_launch_param_migrations.py +++ b/system/manager/test/test_launch_param_migrations.py @@ -1,6 +1,7 @@ from pathlib import Path from openpilot.system.manager.launch_param_migrations import ( + BRANCH_DEFAULTS_MIGRATION_MARKER, DEFAULT_STEER_KP, LAUNCH_PARAM_MIGRATION_MARKER, apply_launch_param_migrations, @@ -84,3 +85,59 @@ def test_apply_launch_param_migrations_does_not_reapply_after_marker(tmp_path): assert not params.get_bool("LongPitch") assert params.get_float("SteerKP") == 0.65 assert params.get_float("SteerKPStock") == DEFAULT_STEER_KP + + +def test_apply_launch_param_migrations_applies_branch_defaults_for_existing_installs(tmp_path): + params = FileBackedFakeParams(tmp_path / "params") + + params.put_bool("LongPitch", False) + params.put_bool("ConditionalExperimental", False) + params.put_bool("CELead", False) + params.put_bool("CESlowerLead", False) + params.put_bool("CEStoppedLead", True) + params.put_bool("ForceStops", False) + params.put_float("AggressiveFollowHigh", 1.25) + params.put_float("StandardFollowHigh", 1.45) + params.put_float("StandardJerkAcceleration", 50.0) + params.put_float("RelaxedFollow", 1.75) + params.put_float("RelaxedFollowHigh", 1.75) + params.put_float("RelaxedJerkSpeed", 50.0) + (tmp_path / "params" / LAUNCH_PARAM_MIGRATION_MARKER).touch() + + apply_launch_param_migrations(params) + + assert not params.get_bool("LongPitch") + assert params.get_bool("ConditionalExperimental") + assert params.get_bool("CELead") + assert params.get_bool("CESlowerLead") + assert not params.get_bool("CEStoppedLead") + assert params.get_bool("ForceStops") + assert params.get_float("AggressiveFollowHigh") == 1.0 + assert params.get_float("StandardFollowHigh") == 1.2 + assert params.get_float("StandardJerkAcceleration") == 100.0 + assert params.get_float("RelaxedFollow") == 1.6 + assert params.get_float("RelaxedFollowHigh") == 1.4 + assert params.get_float("RelaxedJerkSpeed") == 100.0 + assert (tmp_path / "params" / BRANCH_DEFAULTS_MIGRATION_MARKER).is_file() + + +def test_apply_launch_param_migrations_does_not_reapply_branch_defaults_after_marker(tmp_path): + params = FileBackedFakeParams(tmp_path / "params") + branch_defaults_marker = tmp_path / "params" / BRANCH_DEFAULTS_MIGRATION_MARKER + + params.put_bool("ConditionalExperimental", False) + params.put_bool("CEStoppedLead", True) + params.put_bool("ForceStops", False) + params.put_float("AggressiveFollowHigh", 2.0) + params.put_float("StandardJerkAcceleration", 25.0) + params.put_float("RelaxedFollow", 2.0) + branch_defaults_marker.touch() + + apply_launch_param_migrations(params) + + assert not params.get_bool("ConditionalExperimental") + assert params.get_bool("CEStoppedLead") + assert not params.get_bool("ForceStops") + assert params.get_float("AggressiveFollowHigh") == 2.0 + assert params.get_float("StandardJerkAcceleration") == 25.0 + assert params.get_float("RelaxedFollow") == 2.0 diff --git a/system/manager/test/test_manager.py b/system/manager/test/test_manager.py index 202d9f1f4..3e49b77b5 100644 --- a/system/manager/test/test_manager.py +++ b/system/manager/test/test_manager.py @@ -168,15 +168,11 @@ class TestManager: assert params.get("CEModelStopTime") == "3.5" assert params_cache.get_bool("NNFF") - assert Path(params.get_param_path("HumanFollowing")).is_file() - assert not params.get_bool("HumanFollowing") - def test_migrate_disable_humanlike_defaults(self, tmp_path, monkeypatch): monkeypatch.setattr(manager, "STARPILOT_HUMANLIKE_DISABLE_MIGRATION_FLAG", tmp_path / "starpilot_humanlike_disable_v1") params = FileBackedFakeParams(tmp_path / "params", { "HumanAcceleration": True, - "HumanFollowing": True, }) params_cache = FileBackedFakeParams(tmp_path / "cache", { "HumanLaneChanges": True, @@ -185,12 +181,23 @@ class TestManager: manager.migrate_disable_humanlike_defaults(params, params_cache) assert not params.get_bool("HumanAcceleration") - assert not params.get_bool("HumanFollowing") assert not params.get_bool("HumanLaneChanges") assert not params_cache.get_bool("HumanAcceleration") - assert not params_cache.get_bool("HumanFollowing") assert not params_cache.get_bool("HumanLaneChanges") + def test_cleanup_removed_starpilot_params(self, tmp_path): + params = FileBackedFakeParams(tmp_path / "params", { + "HumanFollowing": True, + }) + params_cache = FileBackedFakeParams(tmp_path / "cache", { + "HumanFollowing": False, + }) + + manager.cleanup_removed_starpilot_params(params, params_cache) + + assert not Path(params.get_param_path("HumanFollowing")).exists() + assert not Path(params_cache.get_param_path("HumanFollowing")).exists() + def test_migrate_cluster_offset_default_resets_legacy_default_only(self, tmp_path, monkeypatch): monkeypatch.setattr(manager, "STARPILOT_CLUSTER_OFFSET_MIGRATION_FLAG", tmp_path / "starpilot_cluster_offset_v1") diff --git a/tools/StarPilot/feasibleparams.txt b/tools/StarPilot/feasibleparams.txt index 857e0f32e..db13a23a4 100644 --- a/tools/StarPilot/feasibleparams.txt +++ b/tools/StarPilot/feasibleparams.txt @@ -132,7 +132,6 @@ HideSpeedLimit HigherBitrate HolidayThemes HumanAcceleration -HumanFollowing IncreaseThermalLimits IncreasedStoppedDistance IsDriverViewEnabled