diff --git a/common/libcommon.a b/common/libcommon.a index cb615e03b..1b8cde78c 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 23ff8b9bf..9d91299f1 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -382,6 +382,7 @@ inline static std::unordered_map keys = { {"NavDesiresAllowed", {PERSISTENT, BOOL, "1", "0", 2}}, {"NavLongitudinalAllowed", {PERSISTENT, BOOL, "1", "0", 2}}, {"NavDestination", {PERSISTENT, STRING, "", ""}}, + {"NavInstructionCollapsed", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL, "0", "0"}}, {"NavInstructionState", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, JSON, "{}", "{}"}}, {"NextMapSpeedLimit", {CLEAR_ON_MANAGER_START, JSON, "{}", "{}"}}, {"VisionSpeedLimit", {CLEAR_ON_MANAGER_START, FLOAT, "0.0", "0.0"}}, diff --git a/common/params_pyx.so b/common/params_pyx.so index cc3fbfc93..c0b20e50c 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 42cd4c773..45fb5860c 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -297,6 +297,7 @@ class CarController(CarControllerBase): copies_xp = REDNECK_BUTTON_COPIES_TIME_METRIC if CS.is_metric else REDNECK_BUTTON_COPIES_TIME_IMPERIAL copies = int(np.interp(REDNECK_BUTTON_COPIES_TIME, copies_xp, [1, REDNECK_BUTTON_COPIES])) can_sends = [hyundaican.create_clu11(self.packer, self.frame, CS.clu11, send_button, self.CP)] * copies + CS.redneck_last_sent_button = getattr(CS, "redneck_send_button", 0) if (self.frame - self.last_button_frame) * DT_CTRL >= 0.15: self.last_button_frame = self.frame @@ -318,6 +319,7 @@ class CarController(CarControllerBase): hyundaicanfd.create_buttons(self.packer, self.CP, self.CAN, (CS.buttons_counter + button_counter_offset) % 0xF, send_button) for _ in range(20) ] + CS.redneck_last_sent_button = getattr(CS, "redneck_send_button", 0) self.last_button_frame = self.frame return can_sends @@ -376,6 +378,7 @@ class CarController(CarControllerBase): accel = accel_cmd stopping = actuators.longControlState == LongCtrlState.stopping set_speed_in_units = hud_control.setSpeed * (CV.MS_TO_KPH if CS.is_metric else CV.MS_TO_MPH) + CS.redneck_last_sent_button = 0 can_sends = [] diff --git a/opendbc_repo/opendbc/car/hyundai/radar_interface.py b/opendbc_repo/opendbc/car/hyundai/radar_interface.py index 9ca71d468..bad511f47 100644 --- a/opendbc_repo/opendbc/car/hyundai/radar_interface.py +++ b/opendbc_repo/opendbc/car/hyundai/radar_interface.py @@ -2,6 +2,8 @@ import math from dataclasses import dataclass from opendbc.can import CANParser +from opendbc.can.dbc import DBC as DBCReader +from opendbc.can.parser import get_raw_value from opendbc.car import Bus, structs from opendbc.car.interfaces import RadarInterfaceBase from opendbc.car.hyundai.values import CAR, DBC, HYUNDAI_MANDO_FRONT_RADAR_DBC, HYUNDAI_MRR30_RADAR_DBC, \ @@ -10,6 +12,7 @@ from openpilot.common.swaglog import cloudlog RADAR_START_ADDR = 0x500 RADAR_MSG_COUNT = 32 +G90_RADAR_MSG_COUNT = 64 MRR30_RADAR_START_ADDR = 0x210 MRR30_RADAR_MSG_COUNT = 16 MRR35_RADAR_START_ADDR = 0x3A5 @@ -23,6 +26,11 @@ class RadarTrackConfig: radar_type: str bus: int = 1 frequency: int = 50 + parser_msg_count: int | None = None + + @property + def can_parser_msg_count(self) -> int: + return self.parser_msg_count if self.parser_msg_count is not None else self.msg_count RADAR_TRACK_CONFIGS = { @@ -36,6 +44,8 @@ RADAR_TRACK_CONFIGS = { def get_radar_track_config(car_fingerprint) -> RadarTrackConfig | None: radar_dbc = DBC[car_fingerprint].get(Bus.radar) + if car_fingerprint == CAR.GENESIS_G90 and radar_dbc == HYUNDAI_MANDO_FRONT_RADAR_DBC: + return RadarTrackConfig(RADAR_START_ADDR, G90_RADAR_MSG_COUNT, "mando", parser_msg_count=RADAR_MSG_COUNT) return RADAR_TRACK_CONFIGS.get(radar_dbc) @@ -43,7 +53,8 @@ def get_radar_can_parser(CP, radar_config): if radar_config is None: return None - messages = [(f"RADAR_TRACK_{addr:x}", radar_config.frequency) for addr in range(radar_config.start_addr, radar_config.start_addr + radar_config.msg_count)] + messages = [(f"RADAR_TRACK_{addr:x}", radar_config.frequency) + for addr in range(radar_config.start_addr, radar_config.start_addr + radar_config.can_parser_msg_count)] return CANParser(DBC[CP.carFingerprint][Bus.radar], messages, radar_config.bus) @@ -52,8 +63,15 @@ class RadarInterface(RadarInterfaceBase): super().__init__(CP) self.radar_config = get_radar_track_config(CP.carFingerprint) self.updated_messages = set() - self.trigger_msg = (self.radar_config.start_addr + self.radar_config.msg_count - 1) if self.radar_config is not None else RADAR_START_ADDR + self.trigger_msg = (self.radar_config.start_addr + self.radar_config.can_parser_msg_count - 1 + if self.radar_config is not None else RADAR_START_ADDR) self.track_id = 0 + self.g90_extended_mando = (CP.carFingerprint == CAR.GENESIS_G90 and self.radar_config is not None and + self.radar_config.msg_count > self.radar_config.can_parser_msg_count) + self.g90_mando_signals = [] + if self.g90_extended_mando: + radar_dbc = DBCReader(DBC[CP.carFingerprint][Bus.radar]) + self.g90_mando_signals = list(radar_dbc.addr_to_msg[RADAR_START_ADDR].sigs.values()) self.radar_off_can = CP.radarUnavailable # Probe whether radar tracks still exist on the Ioniq 6 while OP long is active, @@ -70,7 +88,7 @@ class RadarInterface(RadarInterfaceBase): if self.radar_config is not None: self.track_addrs = [(addr, f"RADAR_TRACK_{addr:x}") for addr in range(self.radar_config.start_addr, - self.radar_config.start_addr + self.radar_config.msg_count)] + self.radar_config.start_addr + self.radar_config.can_parser_msg_count)] def update(self, can_strings): if self.ioniq_6_radar_probe and self.rcp is not None and not self.ioniq_6_radar_probe_logged: @@ -93,6 +111,8 @@ class RadarInterface(RadarInterfaceBase): vls = self.rcp.update(can_strings) self.updated_messages.update(vls) + if self.g90_extended_mando: + self._update_g90_extended_mando_tracks(can_strings) if self.trigger_msg not in self.updated_messages: return None @@ -102,6 +122,46 @@ class RadarInterface(RadarInterfaceBase): return rr + def _decode_g90_mando_values(self, dat: bytes): + vals = {} + for sig in self.g90_mando_signals: + raw = get_raw_value(dat, sig) + if sig.is_signed: + raw -= ((raw >> (sig.size - 1)) & 1) * (1 << sig.size) + vals[sig.name] = raw * sig.factor + sig.offset + return vals + + def _update_g90_extended_mando_tracks(self, can_strings): + if self.radar_config is None: + return + + start_addr = self.radar_config.start_addr + self.radar_config.can_parser_msg_count + end_addr = self.radar_config.start_addr + self.radar_config.msg_count + + for _, frames in can_strings: + for address, dat, src in frames: + if src != self.radar_config.bus or not (start_addr <= address < end_addr) or len(dat) < 8: + continue + + self.updated_messages.add(address) + msg = self._decode_g90_mando_values(dat) + valid = msg["STATE"] in (3, 4) + if valid: + if address not in self.pts: + self.pts[address] = structs.RadarData.RadarPoint() + self.pts[address].trackId = self.track_id + self.track_id += 1 + + azimuth = math.radians(msg["AZIMUTH"]) + self.pts[address].measured = True + self.pts[address].dRel = math.cos(azimuth) * msg["LONG_DIST"] + self.pts[address].yRel = 0.5 * -math.sin(azimuth) * msg["LONG_DIST"] + self.pts[address].vRel = msg["REL_SPEED"] + self.pts[address].aRel = msg["REL_ACCEL"] + self.pts[address].yvRel = float("nan") + elif address in self.pts: + del self.pts[address] + def _update(self, updated_messages): ret = structs.RadarData() if self.rcp is None: diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 160a8097d..6d8fe1bb8 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -120,7 +120,7 @@ class TestHyundaiFingerprint: assert bool(CP.flags & HyundaiFlags.CANFD_LKA_STEERING) == lka_steering # radar available - for candidate in (CAR.HYUNDAI_SONATA, CAR.GENESIS_G90): + for candidate in (CAR.HYUNDAI_SONATA, CAR.HYUNDAI_SONATA_HYBRID, CAR.GENESIS_G90): assert get_radar_track_config(candidate).start_addr == RADAR_START_ADDR for radar in (True, False): fingerprint = gen_empty_fingerprint() @@ -129,11 +129,16 @@ class TestHyundaiFingerprint: CP = CarInterface.get_params(candidate, fingerprint, [], False, False, False, None) assert CP.radarUnavailable != radar + assert get_radar_track_config(CAR.HYUNDAI_SONATA_HYBRID).msg_count == 32 + assert get_radar_track_config(CAR.GENESIS_G90).msg_count == 64 + assert get_radar_track_config(CAR.GENESIS_G90).can_parser_msg_count == 32 + fingerprint = gen_empty_fingerprint() fingerprint[1][RADAR_START_ADDR] = 8 - CP = CarInterface.get_params(CAR.GENESIS_G90, fingerprint, [], True, False, False, None) - assert CP.openpilotLongitudinalControl - assert not CP.radarUnavailable + for candidate in (CAR.HYUNDAI_SONATA, CAR.HYUNDAI_SONATA_HYBRID, CAR.GENESIS_G90): + CP = CarInterface.get_params(candidate, fingerprint, [], True, False, False, None) + assert CP.openpilotLongitudinalControl + assert not CP.radarUnavailable for candidate, radar_addr in ( (CAR.HYUNDAI_IONIQ_5, MRR30_RADAR_START_ADDR), diff --git a/opendbc_repo/opendbc/car/hyundai/values.py b/opendbc_repo/opendbc/car/hyundai/values.py index 3b475e0e7..a9806b43b 100644 --- a/opendbc_repo/opendbc/car/hyundai/values.py +++ b/opendbc_repo/opendbc/car/hyundai/values.py @@ -1111,7 +1111,11 @@ CANFD_RADAR_SCC_CAR = CAR.with_flags(HyundaiFlags.RADAR_SCC) # TODO: merge with CANFD_SECURITYACCESS_CAR = {CAR.HYUNDAI_IONIQ_5, CAR.HYUNDAI_IONIQ_6, CAR.HYUNDAI_KONA_EV_2ND_GEN} CANFD_UNSUPPORTED_LONGITUDINAL_CAR = CAR.with_flags(HyundaiFlags.CANFD_NO_RADAR_DISABLE) - CANFD_SECURITYACCESS_CAR # TODO: merge with UNSUPPORTED_LONGITUDINAL_CAR CANFD_RADAR_LIVE_LONGITUDINAL_CAR = {CAR.HYUNDAI_IONIQ_5, CAR.HYUNDAI_IONIQ_6} -RADAR_LIVE_LONGITUDINAL_CAR = CANFD_RADAR_LIVE_LONGITUDINAL_CAR | {CAR.GENESIS_G90} +RADAR_LIVE_LONGITUDINAL_CAR = CANFD_RADAR_LIVE_LONGITUDINAL_CAR | { + CAR.HYUNDAI_SONATA, + CAR.HYUNDAI_SONATA_HYBRID, + CAR.GENESIS_G90, +} CAMERA_SCC_CAR = CAR.with_flags(HyundaiFlags.CAMERA_SCC) diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index ca37b2f7e..5f31d0433 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 2cb052921..168429ea1 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 2346a69c4..0caed1cfd 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 740be5291..6cc0ed669 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 0dcb72e5d..d211448a7 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 bbdab3d90..a138772ca 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 2e7abf19a..951baa2dd 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 2e7abf19a..951baa2dd 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 d67210bd8..f2893d116 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 bbdab3d90..a138772ca 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 e6de3dcfb..2ab19458a 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-f06c82b2-DEBUG"; +const uint8_t gitversion[19] = "DEV-75b1deaf-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 10b6b5621..c000b33f2 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 588fdde8c..7dc5b351a 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 8a0d03426..20b1dba04 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 dfd62a550..58c48c214 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 3787b1934..249a83673 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 e524f33d4..993350f79 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 9d166adb9..66cdc12f7 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 e58c580ac..f54021f44 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 130234dd4..7bb56a3d5 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 9c98a0b5f..1da26caed 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 731997681..d5677e1cc 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 44ae680fc..6ebeb95f1 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 42207f1a2..1580ce7bf 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 4d0c0d58f..f4344bd3f 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 4b78eaf50..0c4213d82 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 5240b9da6..ee3719332 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 17540bacc..ea8a06741 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 5ecc83522..23bc91172 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 5529997d7..2172e9ccb 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 a26636ade..959a36457 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 61eedf377..caa24c9c8 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-f06c82b2-DEBUG \ No newline at end of file +DEV-75b1deaf-DEBUG \ No newline at end of file diff --git a/selfdrive/car/card.py b/selfdrive/car/card.py index 1fd0d3eb2..cc8e9eaa6 100644 --- a/selfdrive/car/card.py +++ b/selfdrive/car/card.py @@ -22,7 +22,7 @@ from opendbc.safety import ALTERNATIVE_EXPERIENCE from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp from openpilot.common.constants import CV from openpilot.selfdrive.car.cruise import VCruiseHelper, IMPERIAL_INCREMENT, V_CRUISE_MAX, V_CRUISE_MIN -from openpilot.selfdrive.car.redneck_cruise import RedneckCruise +from openpilot.selfdrive.car.redneck_cruise import RedneckCruise, SEND_BUTTON_DECREASE, SEND_BUTTON_INCREASE from openpilot.selfdrive.car.car_specific import MockCarState from openpilot.starpilot.common.starpilot_variables import get_starpilot_toggles, update_starpilot_toggles @@ -30,6 +30,8 @@ from openpilot.starpilot.controls.starpilot_card import StarPilotCard REPLAY = "REPLAY" in os.environ OPENPILOT_LEAD_MIN_DISTANCE = 0.1 +REDNECK_DECREASE_LOOKAHEAD_POINTS = 10 +REDNECK_AUTO_BUTTON_FILTER_FRAMES = int(0.3 / DT_CTRL) EventName = log.OnroadEvent.EventName @@ -165,8 +167,12 @@ class Car: self.params.put_nonblocking("CarParamsPersistent", cp_bytes) self.mock_carstate = MockCarState() - self.v_cruise_helper = VCruiseHelper(self.CP) + self.v_cruise_helper = VCruiseHelper(self.CP, self.FPCP) self.redneck_cruise = RedneckCruise(self.CP, self.FPCP) if self.CP.brand == "hyundai" else None + self.redneck_button_event_filter_frames = { + int(ButtonType.accelCruise): 0, + int(ButtonType.decelCruise): 0, + } self.is_metric = self.params.get_bool("IsMetric") self.safe_mode = self.params.get_bool("SafeMode") @@ -210,6 +216,8 @@ class Car: if self.CP.brand == 'mock': CS, FPCS = self.mock_carstate.update(CS, FPCS) + self._filter_redneck_button_events(CS) + # Update radar tracks from CAN RD: structs.RadarDataT | None = self.RI.update(can_list) @@ -345,6 +353,7 @@ class Car: self._update_redneck_cruise(CS, CC) self._update_openpilot_lead_state(CC) self.last_actuators_output, can_sends = self.CI.apply(CC, now_nanos, self.starpilot_toggles) + self._record_redneck_button_feedback_filter() self.pm.send('sendcan', can_list_to_can_capnp(can_sends, msgtype='sendcan', valid=CS.canValid)) self.CC_prev = CC @@ -373,19 +382,59 @@ class Car: if self.redneck_cruise is None: return - send_button, v_target = self.redneck_cruise.run(CS, CC, self._get_redneck_target_speed(), self.is_metric) + send_button, v_target = self.redneck_cruise.run(CS, CC, self._get_redneck_target_speed(CS), self.is_metric) self.CI.CS.redneck_send_button = send_button self.CI.CS.redneck_v_target = v_target - def _get_redneck_target_speed(self) -> float: - if self.sm.seen['longitudinalPlan'] and self.sm.valid['longitudinalPlan']: - speeds = self.sm['longitudinalPlan'].speeds - if len(speeds) > 0: - target_speed = float(speeds[0]) - if math.isfinite(target_speed): - return target_speed + def _get_redneck_target_speed(self, CS: car.CarState) -> float: + fallback_target_speed = float(CS.cruiseState.speedCluster) - return float(self.sm['starpilotPlan'].vCruise) + if self.sm.seen['starpilotPlan'] and self.sm.valid['starpilotPlan']: + starpilot_target_speed = float(self.sm['starpilotPlan'].vCruise) + if starpilot_target_speed > 0.0: + fallback_target_speed = starpilot_target_speed + + if self.sm.seen['longitudinalPlan'] and self.sm.valid['longitudinalPlan']: + plan_speeds = [float(speed) for speed in self.sm['longitudinalPlan'].speeds if math.isfinite(float(speed))] + if len(plan_speeds) > 0: + target_speed = plan_speeds[0] + decrease_target_speed = min(plan_speeds[:REDNECK_DECREASE_LOOKAHEAD_POINTS]) + if decrease_target_speed < min(target_speed, float(CS.cruiseState.speedCluster)): + return decrease_target_speed + return target_speed + + return fallback_target_speed + + def _filter_redneck_button_events(self, CS: car.CarState) -> None: + if self.redneck_cruise is None: + return + + for button_type in self.redneck_button_event_filter_frames: + self.redneck_button_event_filter_frames[button_type] = max(0, self.redneck_button_event_filter_frames[button_type] - 1) + + if len(CS.buttonEvents) == 0: + return + + filtered_button_events = [] + for event in CS.buttonEvents: + button_type = event.type.raw if hasattr(event.type, "raw") else int(event.type) + if self.redneck_button_event_filter_frames.get(button_type, 0) > 0: + continue + filtered_button_events.append(event) + + CS.buttonEvents = filtered_button_events + + def _record_redneck_button_feedback_filter(self) -> None: + if self.redneck_cruise is None: + return + + sent_button = int(getattr(self.CI.CS, "redneck_last_sent_button", 0) or 0) + if sent_button == SEND_BUTTON_INCREASE: + self.redneck_button_event_filter_frames[int(ButtonType.accelCruise)] = REDNECK_AUTO_BUTTON_FILTER_FRAMES + elif sent_button == SEND_BUTTON_DECREASE: + self.redneck_button_event_filter_frames[int(ButtonType.decelCruise)] = REDNECK_AUTO_BUTTON_FILTER_FRAMES + + self.CI.CS.redneck_last_sent_button = 0 def step(self): CS, RD, FPCS = self.state_update() diff --git a/selfdrive/car/cruise.py b/selfdrive/car/cruise.py index 76b338d11..32f9367c6 100644 --- a/selfdrive/car/cruise.py +++ b/selfdrive/car/cruise.py @@ -32,7 +32,7 @@ CRUISE_INTERVAL_SIGN = { class VCruiseHelper: - def __init__(self, CP): + def __init__(self, CP, FPCP=None): self.CP = CP self.v_cruise_kph = V_CRUISE_UNSET self.v_cruise_cluster_kph = V_CRUISE_UNSET @@ -41,6 +41,7 @@ class VCruiseHelper: self.button_change_states = {btn: {"standstill": False, "enabled": False} for btn in self.button_timers} self.gm_cc_only = self.CP.carFingerprint in CC_ONLY_CAR and self.CP.flags & GMFlags.CC_LONG.value + self.redneck_non_pcm = bool(FPCP is not None and not getattr(FPCP, "pcmCruiseSpeed", True)) def _get_short_press_delta(self, is_metric, starpilot_toggles: SimpleNamespace) -> float: base_delta = 1. if is_metric else IMPERIAL_INCREMENT @@ -58,7 +59,7 @@ class VCruiseHelper: self.v_cruise_kph_last = self.v_cruise_kph if CS.cruiseState.available: - if self.gm_cc_only or not self.CP.pcmCruise: + if self.gm_cc_only or self.redneck_non_pcm or not self.CP.pcmCruise: # if stock cruise is completely disabled, then we can use our own set speed logic self._update_v_cruise_non_pcm(CS, enabled, is_metric, speed_limit_changed, starpilot_toggles) self.v_cruise_cluster_kph = self.v_cruise_kph @@ -145,7 +146,7 @@ class VCruiseHelper: def initialize_v_cruise(self, CS, experimental_mode: bool, resume_prev_button: bool, starpilot_toggles: SimpleNamespace, desired_speed_limit: float = 0.0) -> None: # initializing is handled by the PCM - if self.CP.pcmCruise and not self.gm_cc_only: + if self.CP.pcmCruise and not (self.gm_cc_only or self.redneck_non_pcm): return engage_floor_kph = max(V_CRUISE_MIN, 7.0 * CV.MPH_TO_KPH) @@ -158,6 +159,8 @@ class VCruiseHelper: # the custom cruise-button interval. initialized_speed_limit_kph = round(desired_speed_limit * CV.MS_TO_KPH, 1) self.v_cruise_kph = float(np.clip(initialized_speed_limit_kph, V_CRUISE_MIN, V_CRUISE_MAX)) + elif self.redneck_non_pcm and CS.cruiseState.speedCluster > 0: + self.v_cruise_kph = float(np.clip(CS.cruiseState.speedCluster * CV.MS_TO_KPH, V_CRUISE_MIN, V_CRUISE_MAX)) else: self.v_cruise_kph = int(round(np.clip(CS.vEgo * CV.MS_TO_KPH, engage_floor_kph, V_CRUISE_MAX))) diff --git a/selfdrive/car/redneck_cruise.py b/selfdrive/car/redneck_cruise.py index 9b2408f12..ac5efe1ac 100644 --- a/selfdrive/car/redneck_cruise.py +++ b/selfdrive/car/redneck_cruise.py @@ -10,7 +10,8 @@ SEND_BUTTON_INCREASE = 1 SEND_BUTTON_DECREASE = 2 HYST_GAP = 0.0 -INACTIVE_TIMER = 0.4 +INCREASE_INACTIVE_TIMER = 0.4 +DECREASE_INACTIVE_TIMER = 0.1 CRUISE_BUTTON_TIMERS = { int(ButtonType.decelCruise): 0, @@ -80,32 +81,53 @@ class RedneckCruise: button_pressed = any(timer > 0 for timer in self.cruise_button_timers.values()) self.is_ready = CC.enabled and not CC.cruiseControl.override and not CC.cruiseControl.cancel and not CC.cruiseControl.resume and not button_pressed - def _update_state_machine(self) -> int: - self.pre_active_timer = max(0, self.pre_active_timer - 1) + def _desired_state(self) -> str: + if self.v_target > self.v_cruise_cluster: + return "increasing" + if self.v_target < self.v_cruise_cluster and self.v_cruise_cluster > self.v_cruise_min: + return "decreasing" + return "holding" - if self.state != "inactive": - if not self.is_ready: - self.state = "inactive" - elif self.state == "preActive": + @staticmethod + def _get_pre_active_frames(state: str) -> int: + timer = DECREASE_INACTIVE_TIMER if state == "decreasing" else INCREASE_INACTIVE_TIMER + return int(timer / DT_CTRL) + + def _arm_pre_active(self, desired_state: str) -> None: + if desired_state == "holding": + self.state = "holding" + self.pre_active_timer = 0 + return + + self.state = "preActive" + self.pre_active_timer = self._get_pre_active_frames(desired_state) + + def _update_state_machine(self) -> int: + desired_state = self._desired_state() + + if not self.is_ready: + self.state = "inactive" + self.pre_active_timer = 0 + elif self.state == "inactive": + if not self.is_ready_prev: + self._arm_pre_active(desired_state) + elif self.state == "preActive": + if desired_state == "holding": + self.state = "holding" + self.pre_active_timer = 0 + else: + desired_frames = self._get_pre_active_frames(desired_state) + self.pre_active_timer = max(0, min(self.pre_active_timer, desired_frames) - 1) if self.pre_active_timer <= 0: - if self.v_target == self.v_cruise_cluster: - self.state = "holding" - elif self.v_target > self.v_cruise_cluster: - self.state = "increasing" - elif self.v_target < self.v_cruise_cluster and self.v_cruise_cluster > self.v_cruise_min: - self.state = "decreasing" - elif self.state == "holding": - if self.v_target != self.v_cruise_cluster: - self.state = "preActive" - elif self.state == "increasing": - if self.v_target <= self.v_cruise_cluster: - self.state = "holding" - elif self.state == "decreasing": - if self.v_target >= self.v_cruise_cluster or self.v_cruise_cluster <= self.v_cruise_min: - self.state = "holding" - elif self.is_ready and not self.is_ready_prev: - self.pre_active_timer = int(INACTIVE_TIMER / DT_CTRL) - self.state = "preActive" + self.state = desired_state + elif self.state == "holding": + if desired_state != "holding": + self._arm_pre_active(desired_state) + elif self.state != desired_state: + if desired_state == "holding": + self.state = "holding" + else: + self._arm_pre_active(desired_state) return self._send_button_for_state(self.state) diff --git a/selfdrive/car/tests/test_cruise_speed.py b/selfdrive/car/tests/test_cruise_speed.py index ab138f34c..9fe8251b7 100644 --- a/selfdrive/car/tests/test_cruise_speed.py +++ b/selfdrive/car/tests/test_cruise_speed.py @@ -304,3 +304,43 @@ class TestVCruiseHelper: ) assert self.v_cruise_helper.v_cruise_kph == pytest.approx(initial_v_cruise_kph + IMPERIAL_INCREMENT) + + +class TestVCruiseHelperRedneck: + def setup_method(self): + self.CP = car.CarParams(pcmCruise=True) + self.FPCP = SimpleNamespace(pcmCruiseSpeed=False) + self.v_cruise_helper = VCruiseHelper(self.CP, self.FPCP) + self.starpilot_toggles = SimpleNamespace( + cruise_increase=1, + cruise_increase_long=5, + is_metric=False, + set_speed_limit=False, + ) + + def test_initialize_v_cruise_uses_cluster_speed(self): + cs = car.CarState( + vEgo=55 * CV.MPH_TO_MS, + cruiseState={"speedCluster": 62 * CV.MPH_TO_MS}, + ) + self.v_cruise_helper.initialize_v_cruise(cs, experimental_mode=False, resume_prev_button=False, + starpilot_toggles=self.starpilot_toggles) + assert self.v_cruise_helper.v_cruise_kph == pytest.approx(62 * CV.MPH_TO_KPH) + assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(62 * CV.MPH_TO_KPH) + + def test_update_v_cruise_does_not_follow_stock_pcm_speed(self): + cs = car.CarState( + vEgo=55 * CV.MPH_TO_MS, + cruiseState={"speedCluster": 62 * CV.MPH_TO_MS}, + ) + self.v_cruise_helper.initialize_v_cruise(cs, experimental_mode=False, resume_prev_button=False, + starpilot_toggles=self.starpilot_toggles) + + update_cs = car.CarState( + cruiseState={"available": True, "speed": 50 * CV.MPH_TO_MS, "speedCluster": 50 * CV.MPH_TO_MS}, + ) + self.v_cruise_helper.update_v_cruise(update_cs, enabled=True, is_metric=False, + speed_limit_changed=False, starpilot_toggles=self.starpilot_toggles) + + assert self.v_cruise_helper.v_cruise_kph == pytest.approx(62 * CV.MPH_TO_KPH) + assert self.v_cruise_helper.v_cruise_cluster_kph == pytest.approx(62 * CV.MPH_TO_KPH) diff --git a/selfdrive/car/tests/test_redneck_cruise.py b/selfdrive/car/tests/test_redneck_cruise.py index cb9a554c7..0228ea4f3 100644 --- a/selfdrive/car/tests/test_redneck_cruise.py +++ b/selfdrive/car/tests/test_redneck_cruise.py @@ -5,7 +5,8 @@ from cereal import car from openpilot.common.constants import CV from openpilot.common.realtime import DT_CTRL from openpilot.selfdrive.car.redneck_cruise import ( - INACTIVE_TIMER, + DECREASE_INACTIVE_TIMER, + INCREASE_INACTIVE_TIMER, RedneckCruise, SEND_BUTTON_DECREASE, SEND_BUTTON_INCREASE, @@ -40,7 +41,7 @@ class TestRedneckCruise(unittest.TestCase): return SimpleNamespace(type=button_type, pressed=pressed) def _run_until_active(self, target_mph, speed_cluster_mph=20.0, button_events=None, override=False, cancel=False, resume=False): - frames = int(INACTIVE_TIMER / DT_CTRL) + 2 + frames = int(max(INCREASE_INACTIVE_TIMER, DECREASE_INACTIVE_TIMER) / DT_CTRL) + 2 send_button = SEND_BUTTON_NONE v_target = 0 for _ in range(frames): @@ -53,6 +54,19 @@ class TestRedneckCruise(unittest.TestCase): button_events = None return send_button, v_target + def _frames_until_button(self, target_mph, speed_cluster_mph): + frames = int(INCREASE_INACTIVE_TIMER / DT_CTRL) + 4 + for frame in range(frames): + send_button, _ = self.redneck.run( + self._new_state(speed_cluster_mph=speed_cluster_mph), + self._new_control(), + target_mph * CV.MPH_TO_MS, + is_metric=False, + ) + if send_button != SEND_BUTTON_NONE: + return frame + return None + def test_increases_cluster_speed_toward_target(self): send_button, v_target = self._run_until_active(target_mph=25.0, speed_cluster_mph=20.0) self.assertEqual(SEND_BUTTON_INCREASE, send_button) @@ -63,6 +77,15 @@ class TestRedneckCruise(unittest.TestCase): self.assertEqual(SEND_BUTTON_DECREASE, send_button) self.assertEqual(20, v_target) + def test_decrease_activates_faster_than_increase(self): + decrease_frame = self._frames_until_button(target_mph=20.0, speed_cluster_mph=25.0) + self.redneck = RedneckCruise(self.CP, self.FPCP) + increase_frame = self._frames_until_button(target_mph=25.0, speed_cluster_mph=20.0) + + self.assertIsNotNone(decrease_frame) + self.assertIsNotNone(increase_frame) + self.assertLess(decrease_frame, increase_frame) + def test_suppresses_output_during_manual_cruise_button_use(self): button_event = self._button_event(ButtonType.accelCruise, True) send_button, _ = self._run_until_active(target_mph=25.0, speed_cluster_mph=20.0, button_events=[button_event]) diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index c9e4ebed6..45a0025e8 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -113,6 +113,10 @@ VOLT_STANDARD_CARS = ( GENESIS_G90_CARS = ( HYUNDAI_CAR.GENESIS_G90, ) +PALISADE_CARS = ( + HYUNDAI_CAR.HYUNDAI_PALISADE, + HYUNDAI_CAR.HYUNDAI_PALISADE_2023, +) IONIQ_5_CARS = ( HYUNDAI_CAR.HYUNDAI_IONIQ_5, ) @@ -300,6 +304,31 @@ KIA_FORTE_CENTER_TAPER_LAT_WIDTH = 0.03 KIA_FORTE_CENTER_TAPER_SPEED = 24.0 KIA_FORTE_CENTER_TAPER_SPEED_WIDTH = 2.5 +PALISADE_BASE_LAT_ACCEL_FACTOR_MULT = 0.98 +PALISADE_FF_GAIN_LEFT = 0.14 +PALISADE_FF_GAIN_RIGHT = 0.12 +PALISADE_FF_ONSET = 0.08 +PALISADE_FF_ONSET_WIDTH = 0.04 +PALISADE_FF_CUTOFF = 1.25 +PALISADE_FF_CUTOFF_WIDTH = 0.36 +PALISADE_TRANSITION_SPEED = 9.0 +PALISADE_PHASE_SCALE = 0.11 +PALISADE_TURN_IN_BOOST_LEFT = 0.34 +PALISADE_TURN_IN_BOOST_RIGHT = 0.24 +PALISADE_UNWIND_TAPER_LEFT = 0.18 +PALISADE_UNWIND_TAPER_RIGHT = 0.30 +PALISADE_FRICTION_MULT = 1.02 +PALISADE_FRICTION_LAT_RISE = 0.20 +PALISADE_FRICTION_JERK_RISE = 0.24 +PALISADE_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.18 +PALISADE_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.14 +PALISADE_UNWIND_THRESHOLD_INCREASE_LEFT = 0.14 +PALISADE_UNWIND_THRESHOLD_INCREASE_RIGHT = 0.22 +PALISADE_TURN_IN_FRICTION_BOOST_LEFT = 0.08 +PALISADE_TURN_IN_FRICTION_BOOST_RIGHT = 0.06 +PALISADE_UNWIND_FRICTION_REDUCTION_LEFT = 0.12 +PALISADE_UNWIND_FRICTION_REDUCTION_RIGHT = 0.20 + GENESIS_G90_LATERAL_TESTING_GROUND_ID = testing_ground.id_4 GENESIS_G90_FF_GAIN_LEFT = 0.32 GENESIS_G90_FF_GAIN_RIGHT = 0.16 @@ -1077,6 +1106,74 @@ def get_kia_forte_center_taper_scale(desired_lateral_accel: float, v_ego: float) return 1.0 - reduction +def _palisade_sigmoid(x: float) -> float: + return _sigmoid(x) + + +def _palisade_low_speed_factor(v_ego: float) -> float: + return 1.0 / (1.0 + (max(v_ego, 0.0) / PALISADE_TRANSITION_SPEED) ** 2) + + +def _palisade_transition_phase(desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + return math.tanh((desired_lateral_accel * desired_lateral_jerk) / PALISADE_PHASE_SCALE) + + +def _palisade_side_value(desired_lateral_accel: float, left_value: float, right_value: float) -> float: + return left_value if desired_lateral_accel >= 0.0 else right_value + + +def _palisade_transition_envelope(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + lat_factor = 1.0 - math.exp(-abs(desired_lateral_accel) / PALISADE_FRICTION_LAT_RISE) + jerk_factor = 1.0 - math.exp(-abs(desired_lateral_jerk) / PALISADE_FRICTION_JERK_RISE) + return _palisade_low_speed_factor(v_ego) * lat_factor * jerk_factor + + +def get_palisade_ff_scale(desired_lateral_accel: float, desired_lateral_jerk: float, v_ego: float) -> float: + if desired_lateral_accel == 0.0: + return 1.0 + + gain = _palisade_side_value(desired_lateral_accel, PALISADE_FF_GAIN_LEFT, PALISADE_FF_GAIN_RIGHT) + abs_lateral_accel = abs(desired_lateral_accel) + onset = _palisade_sigmoid((abs_lateral_accel - PALISADE_FF_ONSET) / PALISADE_FF_ONSET_WIDTH) + cutoff = _palisade_sigmoid((PALISADE_FF_CUTOFF - abs_lateral_accel) / PALISADE_FF_CUTOFF_WIDTH) + extra_scale = gain * onset * cutoff + phase = _palisade_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + low_speed_factor = _palisade_low_speed_factor(v_ego) + turn_in_boost = 1.0 + (_palisade_side_value(desired_lateral_accel, PALISADE_TURN_IN_BOOST_LEFT, PALISADE_TURN_IN_BOOST_RIGHT) * + turn_in_weight * (0.35 + 0.65 * low_speed_factor)) + unwind_taper = 1.0 - (_palisade_side_value(desired_lateral_accel, PALISADE_UNWIND_TAPER_LEFT, PALISADE_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.35 + 0.65 * low_speed_factor)) + return 1.0 + (extra_scale * turn_in_boost * max(unwind_taper, 0.0)) + + +def get_palisade_friction_threshold(v_ego: float, desired_lateral_accel: float = 0.0, desired_lateral_jerk: float = 0.0) -> float: + base_threshold = get_friction_threshold(v_ego) + transition_envelope = _palisade_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _palisade_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + threshold_scale = 1.0 - (_palisade_side_value(desired_lateral_accel, PALISADE_TURN_IN_THRESHOLD_REDUCTION_LEFT, PALISADE_TURN_IN_THRESHOLD_REDUCTION_RIGHT) * + transition_envelope * turn_in_weight) + threshold_scale += (_palisade_side_value(desired_lateral_accel, PALISADE_UNWIND_THRESHOLD_INCREASE_LEFT, PALISADE_UNWIND_THRESHOLD_INCREASE_RIGHT) * + transition_envelope * unwind_weight) + return base_threshold * min(max(threshold_scale, 0.84), 1.14) + + +def get_palisade_friction_scale(v_ego: float, desired_lateral_accel: float, desired_lateral_jerk: float) -> float: + transition_envelope = _palisade_transition_envelope(v_ego, desired_lateral_accel, desired_lateral_jerk) + phase = _palisade_transition_phase(desired_lateral_accel, desired_lateral_jerk) + turn_in_weight = max(phase, 0.0) + unwind_weight = max(-phase, 0.0) + friction_scale = PALISADE_FRICTION_MULT + friction_scale += (_palisade_side_value(desired_lateral_accel, PALISADE_TURN_IN_FRICTION_BOOST_LEFT, PALISADE_TURN_IN_FRICTION_BOOST_RIGHT) * + transition_envelope * turn_in_weight) + friction_scale -= (_palisade_side_value(desired_lateral_accel, PALISADE_UNWIND_FRICTION_REDUCTION_LEFT, PALISADE_UNWIND_FRICTION_REDUCTION_RIGHT) * + transition_envelope * unwind_weight) + return min(max(friction_scale, 0.92), 1.12) + + def genesis_g90_lateral_testing_ground_active() -> bool: return testing_ground.use(GENESIS_G90_LATERAL_TESTING_GROUND_ID) @@ -1577,6 +1674,7 @@ class LatControlTorque(LatControl): self.is_bolt_2017 = CP.carFingerprint in BOLT_2017_CARS self.is_volt_standard = CP.carFingerprint in VOLT_STANDARD_CARS self.is_genesis_g90 = CP.carFingerprint in GENESIS_G90_CARS + self.is_palisade = CP.carFingerprint in PALISADE_CARS self.is_ioniq_5 = CP.carFingerprint in IONIQ_5_CARS self.is_ioniq_ev_old = CP.carFingerprint in IONIQ_EV_OLD_CARS self.is_ioniq_6 = CP.carFingerprint in IONIQ_6_CARS @@ -1593,6 +1691,8 @@ class LatControlTorque(LatControl): self.torque_ff_scale_neg = 1.0 self.torque_deadzone_boost = float(getattr(self.torque_params, "kfDEPRECATED", 0.0)) self.torque_ki_mult = 1.0 + if self.is_palisade: + self.torque_params.latAccelFactor *= PALISADE_BASE_LAT_ACCEL_FACTOR_MULT if self.is_ioniq_5: self.torque_params.latAccelFactor *= IONIQ_5_BASE_LAT_ACCEL_FACTOR_MULT if self.is_ioniq_ev_old: @@ -1620,6 +1720,8 @@ class LatControlTorque(LatControl): self.pid._k_i = [self.pid._k_i[0], [k * self.torque_ki_mult for k in self.pid._k_i[1]]] def update_live_torque_params(self, latAccelFactor, latAccelOffset, friction): + if self.is_palisade: + latAccelFactor *= PALISADE_BASE_LAT_ACCEL_FACTOR_MULT if self.is_ioniq_5: latAccelFactor *= IONIQ_5_BASE_LAT_ACCEL_FACTOR_MULT if self.is_ioniq_ev_old: @@ -1703,6 +1805,7 @@ class LatControlTorque(LatControl): bolt_2018_2021_tuned_path_active = self.is_bolt_2018_2021 volt_standard_test_active = self.is_volt_standard and volt_standard_lateral_testing_ground_active() genesis_g90_test_active = self.is_genesis_g90 and genesis_g90_lateral_testing_ground_active() + palisade_active = self.is_palisade ioniq_5_active = self.is_ioniq_5 ioniq_ev_old_active = self.is_ioniq_ev_old ioniq_6_active = self.is_ioniq_6 @@ -1739,6 +1842,10 @@ class LatControlTorque(LatControl): ff *= get_genesis_g90_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) friction_threshold = get_genesis_g90_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) friction_scale = get_genesis_g90_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) + elif palisade_active: + ff *= get_palisade_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) + friction_threshold = get_palisade_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) + friction_scale = get_palisade_friction_scale(CS.vEgo, setpoint, desired_lateral_jerk) elif ioniq_5_active: ff *= get_ioniq_5_ff_scale(setpoint, desired_lateral_jerk, CS.vEgo) * ioniq_5_center_taper friction_threshold = get_ioniq_5_friction_threshold(CS.vEgo, setpoint, desired_lateral_jerk) diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py index 5ed72f8b4..038628de6 100755 --- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py +++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py @@ -590,7 +590,8 @@ class LongitudinalMpc: self.max_a = max_a def update(self, radarstate, v_cruise, x, v, a, j, danger_factor, t_follow, - personality=log.LongitudinalPersonality.standard, tracking_lead=True): + personality=log.LongitudinalPersonality.standard, tracking_lead=True, + optional_far_lead_comfort=True): v_ego = self.x0[1] lead_one = radarstate.leadOne lead_two = radarstate.leadTwo @@ -622,11 +623,12 @@ class LongitudinalMpc: v_upper) cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow) prev_source = self.source - if prev_source == 'lead0': - cruise_obstacle += self.get_radarless_matched_follow_cruise_hysteresis(lead_one, v_ego, t_follow) - elif prev_source == 'lead1': - cruise_obstacle += self.get_radarless_matched_follow_cruise_hysteresis(lead_two, v_ego, t_follow) - if tracking_lead and lead_one.status: + if optional_far_lead_comfort: + if prev_source == 'lead0': + cruise_obstacle += self.get_radarless_matched_follow_cruise_hysteresis(lead_one, v_ego, t_follow) + elif prev_source == 'lead1': + cruise_obstacle += self.get_radarless_matched_follow_cruise_hysteresis(lead_two, v_ego, t_follow) + if optional_far_lead_comfort and tracking_lead and lead_one.status: desired_gap = desired_follow_distance(v_ego, lead_one.vLead, t_follow) closing_speed = max(0.0, v_ego - lead_one.vLead) cruise_obstacle += get_tracked_lead_catchup_bias(v_ego, lead_one.dRel, desired_gap, closing_speed, v_cruise=v_cruise) diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index 424f09f99..c65489f9b 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -1086,17 +1086,15 @@ class LongitudinalPlanner: return self.lead_two return None - def get_follow_control_lead(self, lead_control_active, v_ego, t_follow): - matched_follow_lead = self.get_matched_follow_control_lead(v_ego, t_follow) - if matched_follow_lead is not None: - return matched_follow_lead + def get_follow_control_lead(self, lead_control_active, v_ego, t_follow, *, allow_optional_far_lead_logic=True): + if allow_optional_far_lead_logic: + matched_follow_lead = self.get_matched_follow_control_lead(v_ego, t_follow) + if matched_follow_lead is not None: + return matched_follow_lead if not lead_control_active: return None - if self.mpc.source == 'lead1' and self.lead_is_matched_follow_window(self.lead_two, v_ego, t_follow): - return self.lead_two - if self.lead_one.status: return self.lead_one if self.lead_two.status: @@ -1560,9 +1558,11 @@ class LongitudinalPlanner: dec_mpc_mode = self.get_mpc_mode() if not self.mlsim: self.mpc.mode = dec_mpc_mode + optional_far_lead_comfort = getattr(starpilot_toggles, "coast_up_to_leads", True) self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, sm['starpilotPlan'].dangerFactor, effective_t_follow, - personality=personality, tracking_lead=lead_control_active) + personality=personality, tracking_lead=lead_control_active, + optional_far_lead_comfort=optional_far_lead_comfort) self.a_desired_trajectory_full = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution) self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution) @@ -1831,7 +1831,12 @@ class LongitudinalPlanner: if vision_brake_cap_active: output_accel_min = min(output_accel_min, vision_cap_accel_min) - follow_control_lead = self.get_follow_control_lead(lead_control_active, scene_v_ego, effective_t_follow) + follow_control_lead = self.get_follow_control_lead( + lead_control_active, + scene_v_ego, + effective_t_follow, + allow_optional_far_lead_logic=optional_far_lead_comfort, + ) if follow_control_lead is not None and not panic_bypass: if not output_should_stop and not vision_low_speed_stop_active: tracked_vision_model_brake_floor = self.get_tracked_vision_model_brake_floor( @@ -1845,10 +1850,11 @@ class LongitudinalPlanner: self.a_desired = min(self.a_desired, tracked_vision_model_brake_floor) output_a_target = min(output_a_target, tracked_vision_model_brake_floor) - matched_follow_brake_cap = self.get_matched_follow_brake_cap(follow_control_lead, scene_v_ego, effective_t_follow) - if matched_follow_brake_cap is not None: - self.a_desired = max(self.a_desired, matched_follow_brake_cap) - output_a_target = max(output_a_target, matched_follow_brake_cap) + if optional_far_lead_comfort: + matched_follow_brake_cap = self.get_matched_follow_brake_cap(follow_control_lead, scene_v_ego, effective_t_follow) + if matched_follow_brake_cap is not None: + self.a_desired = max(self.a_desired, matched_follow_brake_cap) + output_a_target = max(output_a_target, matched_follow_brake_cap) if not close_lead_caps and not output_should_stop and not vision_low_speed_stop_active: low_speed_transition_brake_cap = self.get_low_speed_follow_transition_brake_cap( @@ -1863,7 +1869,7 @@ class LongitudinalPlanner: output_a_target = max(output_a_target, low_speed_transition_brake_cap) comfort_lead = self.lead_two if self.mpc.source == 'lead1' and self.lead_two.status else self.lead_one - if comfort_lead is not None and not panic_bypass: + if optional_far_lead_comfort and comfort_lead is not None and not panic_bypass: far_lead_brake_cap = self.get_far_lead_brake_cap(comfort_lead, scene_v_ego, effective_t_follow) if far_lead_brake_cap is not None: self.a_desired = max(self.a_desired, far_lead_brake_cap) diff --git a/selfdrive/controls/radard.py b/selfdrive/controls/radard.py index baf0fc7cc..bc32b657f 100644 --- a/selfdrive/controls/radard.py +++ b/selfdrive/controls/radard.py @@ -27,6 +27,8 @@ V_EGO_STATIONARY = 4. # no stationary object flag below this speed RADAR_TO_CENTER = 2.7 # (deprecated) RADAR is ~ 2.7m ahead from center of car RADAR_TO_CAMERA = 1.52 # RADAR is ~ 1.5m ahead from center of mesh frame +G90_RADAR_LOW_SPEED_MAX_DIST = 12.0 +G90_RADAR_LOW_SPEED_MAX_Y = 0.6 class KalmanParams: @@ -127,7 +129,19 @@ def laplacian_pdf(x: float, mu: float, b: float): return math.exp(-abs(x-mu)/b) -def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_data: capnp._DynamicStructReader, tracks: dict[int, Track], starpilot_toggles: SimpleNamespace): +def g90_radar_lead_lateral_sane(track: Track) -> bool: + max_y = min(6.0, max(2.5, 1.5 + 0.08 * max(track.dRel, 0.0))) + return abs(track.yRel) <= max_y + + +def g90_low_speed_radar_lead_sane(track: Track, v_ego: float) -> bool: + return (track.cnt >= 3 and v_ego < 3.0 and + 0.75 < track.dRel < G90_RADAR_LOW_SPEED_MAX_DIST and + abs(track.yRel) < G90_RADAR_LOW_SPEED_MAX_Y) + + +def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_data: capnp._DynamicStructReader, tracks: dict[int, Track], + starpilot_toggles: SimpleNamespace, g90_radar_filter: bool = False): if model_data.meta.laneChangeState == LaneChangeState.laneChangeStarting and getattr(starpilot_toggles, "human_lane_changes", False): direction = model_data.meta.laneChangeDirection if direction == LaneChangeDirection.left: @@ -135,6 +149,9 @@ def match_vision_to_track(v_ego: float, lead: capnp._DynamicStructReader, model_ elif direction == LaneChangeDirection.right: tracks = {k: v for k, v in tracks.items() if v.yRel < 0} + if g90_radar_filter: + tracks = {k: v for k, v in tracks.items() if g90_radar_lead_lateral_sane(v)} + if not tracks: return None @@ -180,12 +197,12 @@ def get_RadarState_from_vision(lead_msg: capnp._DynamicStructReader, v_ego: floa def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capnp._DynamicStructReader, model_v_ego: float, model_data: capnp._DynamicStructReader, standstill: bool, starpilot_plan: capnp._DynamicStructReader, starpilot_toggles: SimpleNamespace, - low_speed_override: bool = True) -> dict[str, Any]: + low_speed_override: bool = True, g90_radar_filter: bool = False) -> dict[str, Any]: lead_detection_probability = float(getattr(starpilot_toggles, "lead_detection_probability", 0.35)) # Determine leads, this is where the essential logic happens if len(tracks) > 0 and ready and lead_msg.prob > lead_detection_probability: - track = match_vision_to_track(v_ego, lead_msg, model_data, tracks, starpilot_toggles) + track = match_vision_to_track(v_ego, lead_msg, model_data, tracks, starpilot_toggles, g90_radar_filter) else: track = None @@ -196,7 +213,10 @@ def get_lead(v_ego: float, ready: bool, tracks: dict[int, Track], lead_msg: capn lead_dict = get_RadarState_from_vision(lead_msg, v_ego, model_v_ego) if low_speed_override: - low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)] + if g90_radar_filter: + low_speed_tracks = [c for c in tracks.values() if g90_low_speed_radar_lead_sane(c, v_ego)] + else: + low_speed_tracks = [c for c in tracks.values() if c.potential_low_speed_lead(v_ego)] if len(low_speed_tracks) > 0: closest_track = min(low_speed_tracks, key=lambda c: c.dRel) @@ -225,11 +245,12 @@ def get_adjacent_lead(tracks: dict[int, Track], standstill: bool, model_data: ca class RadarD: - def __init__(self, radar_ts: float = DT_MDL, delay: float = 0.0): + def __init__(self, radar_ts: float = DT_MDL, delay: float = 0.0, g90_radar_filter: bool = False): self.current_time = 0.0 self.tracks: dict[int, Track] = {} self.kalman_params = KalmanParams(radar_ts) + self.g90_radar_filter = g90_radar_filter self.v_ego = 0.0 self.v_ego_hist = deque([0.0], maxlen=int(round(delay / DT_MDL)) + 1) @@ -286,9 +307,11 @@ class RadarD: leads_v3 = sm['modelV2'].leadsV3 if len(leads_v3) > 1: self.radar_state.leadOne = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[0], model_v_ego, sm['modelV2'], - sm['carState'].standstill, sm['starpilotPlan'], self.starpilot_toggles, low_speed_override=True) + sm['carState'].standstill, sm['starpilotPlan'], self.starpilot_toggles, low_speed_override=True, + g90_radar_filter=self.g90_radar_filter) self.radar_state.leadTwo = get_lead(self.v_ego, self.ready, self.tracks, leads_v3[1], model_v_ego, sm['modelV2'], - sm['carState'].standstill, sm['starpilotPlan'], self.starpilot_toggles, low_speed_override=False) + sm['carState'].standstill, sm['starpilotPlan'], self.starpilot_toggles, low_speed_override=False, + g90_radar_filter=self.g90_radar_filter) if self.ready and (self.starpilot_toggles.adjacent_lead_tracking or self.starpilot_toggles.human_lane_changes): self.starpilot_radar_state.leadLeft = get_adjacent_lead(self.tracks, sm['carState'].standstill, sm['modelV2'], left=True) @@ -328,7 +351,8 @@ def main() -> None: if not 0.01 < radar_ts < 0.2: radar_ts = DT_MDL - RD = RadarD(radar_ts=radar_ts, delay=CP.radarDelay) + g90_radar_filter = CP.brand == "hyundai" and CP.carFingerprint == "GENESIS_G90" + RD = RadarD(radar_ts=radar_ts, delay=CP.radarDelay, g90_radar_filter=g90_radar_filter) sm = sm.extend(['starpilotPlan']) pm = pm.extend(['starpilotRadarState']) diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 8984de667..1e504df5c 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -40,6 +40,9 @@ from openpilot.selfdrive.controls.lib.latcontrol_torque import ( get_genesis_g90_friction_scale, get_genesis_g90_friction_threshold, get_elantra_non_scc_ff_scale, + get_palisade_ff_scale, + get_palisade_friction_scale, + get_palisade_friction_threshold, get_ioniq_5_ff_scale, get_ioniq_5_friction_scale, get_ioniq_5_friction_threshold, @@ -305,6 +308,39 @@ class TestLatControl: assert left_turn_in > right_turn_in > base assert base > left_unwind > right_unwind + def test_palisade_ff_scale_curve(self): + assert get_palisade_ff_scale(0.0, 0.0, 20.0) == 1.0 + steady_left = get_palisade_ff_scale(0.6, 0.0, 8.0) + steady_right = get_palisade_ff_scale(-0.6, 0.0, 8.0) + turn_in_left = get_palisade_ff_scale(0.6, 0.8, 8.0) + turn_in_right = get_palisade_ff_scale(-0.6, -0.8, 8.0) + unwind_left = get_palisade_ff_scale(0.6, -0.8, 8.0) + unwind_right = get_palisade_ff_scale(-0.6, 0.8, 8.0) + assert steady_left > 1.0 + assert steady_right > 1.0 + assert turn_in_left > steady_left + assert turn_in_right > steady_right + assert unwind_left < steady_left + assert unwind_right < steady_right + assert unwind_right < unwind_left + + def test_palisade_friction_threshold_curve(self): + base = get_friction_threshold(6.0) + left_turn_in = get_palisade_friction_threshold(6.0, 0.7, 0.8) + right_turn_in = get_palisade_friction_threshold(6.0, -0.7, -0.8) + left_unwind = get_palisade_friction_threshold(6.0, 0.7, -0.8) + right_unwind = get_palisade_friction_threshold(6.0, -0.7, 0.8) + assert left_turn_in < right_turn_in < base < left_unwind < right_unwind + + def test_palisade_friction_scale_curve(self): + base = get_palisade_friction_scale(25.0, 0.7, 0.8) + left_turn_in = get_palisade_friction_scale(6.0, 0.7, 0.8) + right_turn_in = get_palisade_friction_scale(6.0, -0.7, -0.8) + left_unwind = get_palisade_friction_scale(6.0, 0.7, -0.8) + right_unwind = get_palisade_friction_scale(6.0, -0.7, 0.8) + assert left_turn_in > right_turn_in > base + assert base > left_unwind > right_unwind + def test_ioniq_5_ff_scale_curve(self): assert get_ioniq_5_ff_scale(0.0, 0.0, 20.0) == 1.0 steady_left = get_ioniq_5_ff_scale(0.7, 0.0, 12.0) @@ -491,6 +527,16 @@ class TestLatControl: assert lac_log.active + def test_palisade_default_update_path(self): + controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_PALISADE_2023) + CarInterface = interfaces[HYUNDAI.HYUNDAI_PALISADE_2023] + CP = CarInterface.get_non_essential_params(HYUNDAI.HYUNDAI_PALISADE_2023) + + _, _, lac_log = controller.update(True, CS, VM, params, False, 0.0025, False, 0.2, None, None, starpilot_toggles) + + assert lac_log.active + assert controller.torque_params.latAccelFactor == pytest.approx(CP.lateralTuning.torque.latAccelFactor * 0.98) + def test_ioniq_5_default_update_path(self): controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.HYUNDAI_IONIQ_5) CarInterface = interfaces[HYUNDAI.HYUNDAI_IONIQ_5] diff --git a/selfdrive/controls/tests/test_leads.py b/selfdrive/controls/tests/test_leads.py index 77384fea2..ce836c807 100644 --- a/selfdrive/controls/tests/test_leads.py +++ b/selfdrive/controls/tests/test_leads.py @@ -1,10 +1,23 @@ +from types import SimpleNamespace + import cereal.messaging as messaging from opendbc.car.toyota.values import CAR as TOYOTA from openpilot.selfdrive.test.process_replay import replay_process_with_name +from openpilot.selfdrive.controls.radard import g90_low_speed_radar_lead_sane, g90_radar_lead_lateral_sane class TestLeads: + def test_g90_radar_filters_side_tracks(self): + side_track = SimpleNamespace(dRel=13.0, yRel=-10.38, cnt=10) + centered_track = SimpleNamespace(dRel=10.8, yRel=-0.21, cnt=5) + far_low_speed_track = SimpleNamespace(dRel=15.5, yRel=0.58, cnt=5) + + assert not g90_radar_lead_lateral_sane(side_track) + assert g90_radar_lead_lateral_sane(centered_track) + assert g90_low_speed_radar_lead_sane(centered_track, 2.0) + assert not g90_low_speed_radar_lead_sane(far_low_speed_track, 3.5) + def test_radar_fault(self): # if there's no radar-related can traffic, radard should either not respond or respond with an error # this is tightly coupled with underlying car radar_interface implementation, but it's a good sanity check diff --git a/selfdrive/controls/tests/test_longitudinal_planner.py b/selfdrive/controls/tests/test_longitudinal_planner.py index 9ed9f2864..26335950e 100644 --- a/selfdrive/controls/tests/test_longitudinal_planner.py +++ b/selfdrive/controls/tests/test_longitudinal_planner.py @@ -1702,6 +1702,19 @@ def test_follow_control_lead_prefers_active_lead1_for_matched_follow(): assert follow_lead is planner.lead_two +def test_follow_control_lead_disables_optional_matched_follow_override(): + v_ego = 23.3 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + planner.lead_one = make_lead(status=True, d_rel=80.0, v_lead=20.0, radar=False, model_prob=0.6) + planner.lead_two = make_lead(status=True, d_rel=49.9, v_lead=21.9, radar=False, model_prob=0.98) + planner.mpc.source = "lead1" + + follow_lead = planner.get_follow_control_lead(True, v_ego, 1.45, allow_optional_far_lead_logic=False) + + assert follow_lead is planner.lead_one + + def test_follow_control_lead_keeps_matched_follow_lead_without_tracking_latch(): v_ego = 27.5 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) @@ -1713,6 +1726,17 @@ def test_follow_control_lead_keeps_matched_follow_lead_without_tracking_latch(): assert follow_lead is planner.lead_one +def test_follow_control_lead_requires_real_lead_control_when_optional_logic_disabled(): + v_ego = 27.5 + CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) + planner = LongitudinalPlanner(CP, init_v=v_ego) + planner.lead_one = make_lead(status=True, d_rel=61.99, v_lead=27.63, radar=False, model_prob=0.99) + + follow_lead = planner.get_follow_control_lead(False, v_ego, 1.45, allow_optional_far_lead_logic=False) + + assert follow_lead is None + + def test_far_lead_soft_brake_cap_limits_high_confidence_distant_vision_lead(): v_ego = 32.37 CP = CarInterface.get_non_essential_params(CAR.HONDA_CIVIC) diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 56108aa8b..8fa2168b1 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_4387195160971747702) { - out_4387195160971747702[0] = delta_x[0] + nom_x[0]; - out_4387195160971747702[1] = delta_x[1] + nom_x[1]; - out_4387195160971747702[2] = delta_x[2] + nom_x[2]; - out_4387195160971747702[3] = delta_x[3] + nom_x[3]; - out_4387195160971747702[4] = delta_x[4] + nom_x[4]; - out_4387195160971747702[5] = delta_x[5] + nom_x[5]; - out_4387195160971747702[6] = delta_x[6] + nom_x[6]; - out_4387195160971747702[7] = delta_x[7] + nom_x[7]; - out_4387195160971747702[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_7214711627163091402) { + out_7214711627163091402[0] = delta_x[0] + nom_x[0]; + out_7214711627163091402[1] = delta_x[1] + nom_x[1]; + out_7214711627163091402[2] = delta_x[2] + nom_x[2]; + out_7214711627163091402[3] = delta_x[3] + nom_x[3]; + out_7214711627163091402[4] = delta_x[4] + nom_x[4]; + out_7214711627163091402[5] = delta_x[5] + nom_x[5]; + out_7214711627163091402[6] = delta_x[6] + nom_x[6]; + out_7214711627163091402[7] = delta_x[7] + nom_x[7]; + out_7214711627163091402[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_7707264064834051115) { - out_7707264064834051115[0] = -nom_x[0] + true_x[0]; - out_7707264064834051115[1] = -nom_x[1] + true_x[1]; - out_7707264064834051115[2] = -nom_x[2] + true_x[2]; - out_7707264064834051115[3] = -nom_x[3] + true_x[3]; - out_7707264064834051115[4] = -nom_x[4] + true_x[4]; - out_7707264064834051115[5] = -nom_x[5] + true_x[5]; - out_7707264064834051115[6] = -nom_x[6] + true_x[6]; - out_7707264064834051115[7] = -nom_x[7] + true_x[7]; - out_7707264064834051115[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_911828672798028438) { + out_911828672798028438[0] = -nom_x[0] + true_x[0]; + out_911828672798028438[1] = -nom_x[1] + true_x[1]; + out_911828672798028438[2] = -nom_x[2] + true_x[2]; + out_911828672798028438[3] = -nom_x[3] + true_x[3]; + out_911828672798028438[4] = -nom_x[4] + true_x[4]; + out_911828672798028438[5] = -nom_x[5] + true_x[5]; + out_911828672798028438[6] = -nom_x[6] + true_x[6]; + out_911828672798028438[7] = -nom_x[7] + true_x[7]; + out_911828672798028438[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_7178605602671138983) { - out_7178605602671138983[0] = 1.0; - out_7178605602671138983[1] = 0.0; - out_7178605602671138983[2] = 0.0; - out_7178605602671138983[3] = 0.0; - out_7178605602671138983[4] = 0.0; - out_7178605602671138983[5] = 0.0; - out_7178605602671138983[6] = 0.0; - out_7178605602671138983[7] = 0.0; - out_7178605602671138983[8] = 0.0; - out_7178605602671138983[9] = 0.0; - out_7178605602671138983[10] = 1.0; - out_7178605602671138983[11] = 0.0; - out_7178605602671138983[12] = 0.0; - out_7178605602671138983[13] = 0.0; - out_7178605602671138983[14] = 0.0; - out_7178605602671138983[15] = 0.0; - out_7178605602671138983[16] = 0.0; - out_7178605602671138983[17] = 0.0; - out_7178605602671138983[18] = 0.0; - out_7178605602671138983[19] = 0.0; - out_7178605602671138983[20] = 1.0; - out_7178605602671138983[21] = 0.0; - out_7178605602671138983[22] = 0.0; - out_7178605602671138983[23] = 0.0; - out_7178605602671138983[24] = 0.0; - out_7178605602671138983[25] = 0.0; - out_7178605602671138983[26] = 0.0; - out_7178605602671138983[27] = 0.0; - out_7178605602671138983[28] = 0.0; - out_7178605602671138983[29] = 0.0; - out_7178605602671138983[30] = 1.0; - out_7178605602671138983[31] = 0.0; - out_7178605602671138983[32] = 0.0; - out_7178605602671138983[33] = 0.0; - out_7178605602671138983[34] = 0.0; - out_7178605602671138983[35] = 0.0; - out_7178605602671138983[36] = 0.0; - out_7178605602671138983[37] = 0.0; - out_7178605602671138983[38] = 0.0; - out_7178605602671138983[39] = 0.0; - out_7178605602671138983[40] = 1.0; - out_7178605602671138983[41] = 0.0; - out_7178605602671138983[42] = 0.0; - out_7178605602671138983[43] = 0.0; - out_7178605602671138983[44] = 0.0; - out_7178605602671138983[45] = 0.0; - out_7178605602671138983[46] = 0.0; - out_7178605602671138983[47] = 0.0; - out_7178605602671138983[48] = 0.0; - out_7178605602671138983[49] = 0.0; - out_7178605602671138983[50] = 1.0; - out_7178605602671138983[51] = 0.0; - out_7178605602671138983[52] = 0.0; - out_7178605602671138983[53] = 0.0; - out_7178605602671138983[54] = 0.0; - out_7178605602671138983[55] = 0.0; - out_7178605602671138983[56] = 0.0; - out_7178605602671138983[57] = 0.0; - out_7178605602671138983[58] = 0.0; - out_7178605602671138983[59] = 0.0; - out_7178605602671138983[60] = 1.0; - out_7178605602671138983[61] = 0.0; - out_7178605602671138983[62] = 0.0; - out_7178605602671138983[63] = 0.0; - out_7178605602671138983[64] = 0.0; - out_7178605602671138983[65] = 0.0; - out_7178605602671138983[66] = 0.0; - out_7178605602671138983[67] = 0.0; - out_7178605602671138983[68] = 0.0; - out_7178605602671138983[69] = 0.0; - out_7178605602671138983[70] = 1.0; - out_7178605602671138983[71] = 0.0; - out_7178605602671138983[72] = 0.0; - out_7178605602671138983[73] = 0.0; - out_7178605602671138983[74] = 0.0; - out_7178605602671138983[75] = 0.0; - out_7178605602671138983[76] = 0.0; - out_7178605602671138983[77] = 0.0; - out_7178605602671138983[78] = 0.0; - out_7178605602671138983[79] = 0.0; - out_7178605602671138983[80] = 1.0; +void H_mod_fun(double *state, double *out_149552472482575511) { + out_149552472482575511[0] = 1.0; + out_149552472482575511[1] = 0.0; + out_149552472482575511[2] = 0.0; + out_149552472482575511[3] = 0.0; + out_149552472482575511[4] = 0.0; + out_149552472482575511[5] = 0.0; + out_149552472482575511[6] = 0.0; + out_149552472482575511[7] = 0.0; + out_149552472482575511[8] = 0.0; + out_149552472482575511[9] = 0.0; + out_149552472482575511[10] = 1.0; + out_149552472482575511[11] = 0.0; + out_149552472482575511[12] = 0.0; + out_149552472482575511[13] = 0.0; + out_149552472482575511[14] = 0.0; + out_149552472482575511[15] = 0.0; + out_149552472482575511[16] = 0.0; + out_149552472482575511[17] = 0.0; + out_149552472482575511[18] = 0.0; + out_149552472482575511[19] = 0.0; + out_149552472482575511[20] = 1.0; + out_149552472482575511[21] = 0.0; + out_149552472482575511[22] = 0.0; + out_149552472482575511[23] = 0.0; + out_149552472482575511[24] = 0.0; + out_149552472482575511[25] = 0.0; + out_149552472482575511[26] = 0.0; + out_149552472482575511[27] = 0.0; + out_149552472482575511[28] = 0.0; + out_149552472482575511[29] = 0.0; + out_149552472482575511[30] = 1.0; + out_149552472482575511[31] = 0.0; + out_149552472482575511[32] = 0.0; + out_149552472482575511[33] = 0.0; + out_149552472482575511[34] = 0.0; + out_149552472482575511[35] = 0.0; + out_149552472482575511[36] = 0.0; + out_149552472482575511[37] = 0.0; + out_149552472482575511[38] = 0.0; + out_149552472482575511[39] = 0.0; + out_149552472482575511[40] = 1.0; + out_149552472482575511[41] = 0.0; + out_149552472482575511[42] = 0.0; + out_149552472482575511[43] = 0.0; + out_149552472482575511[44] = 0.0; + out_149552472482575511[45] = 0.0; + out_149552472482575511[46] = 0.0; + out_149552472482575511[47] = 0.0; + out_149552472482575511[48] = 0.0; + out_149552472482575511[49] = 0.0; + out_149552472482575511[50] = 1.0; + out_149552472482575511[51] = 0.0; + out_149552472482575511[52] = 0.0; + out_149552472482575511[53] = 0.0; + out_149552472482575511[54] = 0.0; + out_149552472482575511[55] = 0.0; + out_149552472482575511[56] = 0.0; + out_149552472482575511[57] = 0.0; + out_149552472482575511[58] = 0.0; + out_149552472482575511[59] = 0.0; + out_149552472482575511[60] = 1.0; + out_149552472482575511[61] = 0.0; + out_149552472482575511[62] = 0.0; + out_149552472482575511[63] = 0.0; + out_149552472482575511[64] = 0.0; + out_149552472482575511[65] = 0.0; + out_149552472482575511[66] = 0.0; + out_149552472482575511[67] = 0.0; + out_149552472482575511[68] = 0.0; + out_149552472482575511[69] = 0.0; + out_149552472482575511[70] = 1.0; + out_149552472482575511[71] = 0.0; + out_149552472482575511[72] = 0.0; + out_149552472482575511[73] = 0.0; + out_149552472482575511[74] = 0.0; + out_149552472482575511[75] = 0.0; + out_149552472482575511[76] = 0.0; + out_149552472482575511[77] = 0.0; + out_149552472482575511[78] = 0.0; + out_149552472482575511[79] = 0.0; + out_149552472482575511[80] = 1.0; } -void f_fun(double *state, double dt, double *out_2416599425795193412) { - out_2416599425795193412[0] = state[0]; - out_2416599425795193412[1] = state[1]; - out_2416599425795193412[2] = state[2]; - out_2416599425795193412[3] = state[3]; - out_2416599425795193412[4] = state[4]; - out_2416599425795193412[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_2416599425795193412[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_2416599425795193412[7] = state[7]; - out_2416599425795193412[8] = state[8]; +void f_fun(double *state, double dt, double *out_4547528931834134293) { + out_4547528931834134293[0] = state[0]; + out_4547528931834134293[1] = state[1]; + out_4547528931834134293[2] = state[2]; + out_4547528931834134293[3] = state[3]; + out_4547528931834134293[4] = state[4]; + out_4547528931834134293[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_4547528931834134293[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_4547528931834134293[7] = state[7]; + out_4547528931834134293[8] = state[8]; } -void F_fun(double *state, double dt, double *out_6761986232654706930) { - out_6761986232654706930[0] = 1; - out_6761986232654706930[1] = 0; - out_6761986232654706930[2] = 0; - out_6761986232654706930[3] = 0; - out_6761986232654706930[4] = 0; - out_6761986232654706930[5] = 0; - out_6761986232654706930[6] = 0; - out_6761986232654706930[7] = 0; - out_6761986232654706930[8] = 0; - out_6761986232654706930[9] = 0; - out_6761986232654706930[10] = 1; - out_6761986232654706930[11] = 0; - out_6761986232654706930[12] = 0; - out_6761986232654706930[13] = 0; - out_6761986232654706930[14] = 0; - out_6761986232654706930[15] = 0; - out_6761986232654706930[16] = 0; - out_6761986232654706930[17] = 0; - out_6761986232654706930[18] = 0; - out_6761986232654706930[19] = 0; - out_6761986232654706930[20] = 1; - out_6761986232654706930[21] = 0; - out_6761986232654706930[22] = 0; - out_6761986232654706930[23] = 0; - out_6761986232654706930[24] = 0; - out_6761986232654706930[25] = 0; - out_6761986232654706930[26] = 0; - out_6761986232654706930[27] = 0; - out_6761986232654706930[28] = 0; - out_6761986232654706930[29] = 0; - out_6761986232654706930[30] = 1; - out_6761986232654706930[31] = 0; - out_6761986232654706930[32] = 0; - out_6761986232654706930[33] = 0; - out_6761986232654706930[34] = 0; - out_6761986232654706930[35] = 0; - out_6761986232654706930[36] = 0; - out_6761986232654706930[37] = 0; - out_6761986232654706930[38] = 0; - out_6761986232654706930[39] = 0; - out_6761986232654706930[40] = 1; - out_6761986232654706930[41] = 0; - out_6761986232654706930[42] = 0; - out_6761986232654706930[43] = 0; - out_6761986232654706930[44] = 0; - out_6761986232654706930[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_6761986232654706930[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_6761986232654706930[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_6761986232654706930[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_6761986232654706930[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_6761986232654706930[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_6761986232654706930[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_6761986232654706930[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_6761986232654706930[53] = -9.8100000000000005*dt; - out_6761986232654706930[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_6761986232654706930[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_6761986232654706930[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6761986232654706930[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6761986232654706930[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_6761986232654706930[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_6761986232654706930[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_6761986232654706930[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6761986232654706930[62] = 0; - out_6761986232654706930[63] = 0; - out_6761986232654706930[64] = 0; - out_6761986232654706930[65] = 0; - out_6761986232654706930[66] = 0; - out_6761986232654706930[67] = 0; - out_6761986232654706930[68] = 0; - out_6761986232654706930[69] = 0; - out_6761986232654706930[70] = 1; - out_6761986232654706930[71] = 0; - out_6761986232654706930[72] = 0; - out_6761986232654706930[73] = 0; - out_6761986232654706930[74] = 0; - out_6761986232654706930[75] = 0; - out_6761986232654706930[76] = 0; - out_6761986232654706930[77] = 0; - out_6761986232654706930[78] = 0; - out_6761986232654706930[79] = 0; - out_6761986232654706930[80] = 1; +void F_fun(double *state, double dt, double *out_3560058468708809121) { + out_3560058468708809121[0] = 1; + out_3560058468708809121[1] = 0; + out_3560058468708809121[2] = 0; + out_3560058468708809121[3] = 0; + out_3560058468708809121[4] = 0; + out_3560058468708809121[5] = 0; + out_3560058468708809121[6] = 0; + out_3560058468708809121[7] = 0; + out_3560058468708809121[8] = 0; + out_3560058468708809121[9] = 0; + out_3560058468708809121[10] = 1; + out_3560058468708809121[11] = 0; + out_3560058468708809121[12] = 0; + out_3560058468708809121[13] = 0; + out_3560058468708809121[14] = 0; + out_3560058468708809121[15] = 0; + out_3560058468708809121[16] = 0; + out_3560058468708809121[17] = 0; + out_3560058468708809121[18] = 0; + out_3560058468708809121[19] = 0; + out_3560058468708809121[20] = 1; + out_3560058468708809121[21] = 0; + out_3560058468708809121[22] = 0; + out_3560058468708809121[23] = 0; + out_3560058468708809121[24] = 0; + out_3560058468708809121[25] = 0; + out_3560058468708809121[26] = 0; + out_3560058468708809121[27] = 0; + out_3560058468708809121[28] = 0; + out_3560058468708809121[29] = 0; + out_3560058468708809121[30] = 1; + out_3560058468708809121[31] = 0; + out_3560058468708809121[32] = 0; + out_3560058468708809121[33] = 0; + out_3560058468708809121[34] = 0; + out_3560058468708809121[35] = 0; + out_3560058468708809121[36] = 0; + out_3560058468708809121[37] = 0; + out_3560058468708809121[38] = 0; + out_3560058468708809121[39] = 0; + out_3560058468708809121[40] = 1; + out_3560058468708809121[41] = 0; + out_3560058468708809121[42] = 0; + out_3560058468708809121[43] = 0; + out_3560058468708809121[44] = 0; + out_3560058468708809121[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_3560058468708809121[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_3560058468708809121[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_3560058468708809121[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_3560058468708809121[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_3560058468708809121[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_3560058468708809121[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_3560058468708809121[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_3560058468708809121[53] = -9.8100000000000005*dt; + out_3560058468708809121[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_3560058468708809121[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_3560058468708809121[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_3560058468708809121[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_3560058468708809121[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_3560058468708809121[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_3560058468708809121[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_3560058468708809121[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_3560058468708809121[62] = 0; + out_3560058468708809121[63] = 0; + out_3560058468708809121[64] = 0; + out_3560058468708809121[65] = 0; + out_3560058468708809121[66] = 0; + out_3560058468708809121[67] = 0; + out_3560058468708809121[68] = 0; + out_3560058468708809121[69] = 0; + out_3560058468708809121[70] = 1; + out_3560058468708809121[71] = 0; + out_3560058468708809121[72] = 0; + out_3560058468708809121[73] = 0; + out_3560058468708809121[74] = 0; + out_3560058468708809121[75] = 0; + out_3560058468708809121[76] = 0; + out_3560058468708809121[77] = 0; + out_3560058468708809121[78] = 0; + out_3560058468708809121[79] = 0; + out_3560058468708809121[80] = 1; } -void h_25(double *state, double *unused, double *out_319074315832850169) { - out_319074315832850169[0] = state[6]; +void h_25(double *state, double *unused, double *out_1132582605868503102) { + out_1132582605868503102[0] = state[6]; } -void H_25(double *state, double *unused, double *out_3655624952681647306) { - out_3655624952681647306[0] = 0; - out_3655624952681647306[1] = 0; - out_3655624952681647306[2] = 0; - out_3655624952681647306[3] = 0; - out_3655624952681647306[4] = 0; - out_3655624952681647306[5] = 0; - out_3655624952681647306[6] = 1; - out_3655624952681647306[7] = 0; - out_3655624952681647306[8] = 0; +void H_25(double *state, double *unused, double *out_9104003375314738868) { + out_9104003375314738868[0] = 0; + out_9104003375314738868[1] = 0; + out_9104003375314738868[2] = 0; + out_9104003375314738868[3] = 0; + out_9104003375314738868[4] = 0; + out_9104003375314738868[5] = 0; + out_9104003375314738868[6] = 1; + out_9104003375314738868[7] = 0; + out_9104003375314738868[8] = 0; } -void h_24(double *state, double *unused, double *out_5734853090591072423) { - out_5734853090591072423[0] = state[4]; - out_5734853090591072423[1] = state[5]; +void h_24(double *state, double *unused, double *out_8381470974523360630) { + out_8381470974523360630[0] = state[4]; + out_8381470974523360630[1] = state[5]; } -void H_24(double *state, double *unused, double *out_1429917168702778744) { - out_1429917168702778744[0] = 0; - out_1429917168702778744[1] = 0; - out_1429917168702778744[2] = 0; - out_1429917168702778744[3] = 0; - out_1429917168702778744[4] = 1; - out_1429917168702778744[5] = 0; - out_1429917168702778744[6] = 0; - out_1429917168702778744[7] = 0; - out_1429917168702778744[8] = 0; - out_1429917168702778744[9] = 0; - out_1429917168702778744[10] = 0; - out_1429917168702778744[11] = 0; - out_1429917168702778744[12] = 0; - out_1429917168702778744[13] = 0; - out_1429917168702778744[14] = 1; - out_1429917168702778744[15] = 0; - out_1429917168702778744[16] = 0; - out_1429917168702778744[17] = 0; +void H_24(double *state, double *unused, double *out_4357494048348229834) { + out_4357494048348229834[0] = 0; + out_4357494048348229834[1] = 0; + out_4357494048348229834[2] = 0; + out_4357494048348229834[3] = 0; + out_4357494048348229834[4] = 1; + out_4357494048348229834[5] = 0; + out_4357494048348229834[6] = 0; + out_4357494048348229834[7] = 0; + out_4357494048348229834[8] = 0; + out_4357494048348229834[9] = 0; + out_4357494048348229834[10] = 0; + out_4357494048348229834[11] = 0; + out_4357494048348229834[12] = 0; + out_4357494048348229834[13] = 0; + out_4357494048348229834[14] = 1; + out_4357494048348229834[15] = 0; + out_4357494048348229834[16] = 0; + out_4357494048348229834[17] = 0; } -void h_30(double *state, double *unused, double *out_8212013810717373528) { - out_8212013810717373528[0] = state[4]; +void h_30(double *state, double *unused, double *out_975395937517373957) { + out_975395937517373957[0] = state[4]; } -void H_30(double *state, double *unused, double *out_3784963899824887376) { - out_3784963899824887376[0] = 0; - out_3784963899824887376[1] = 0; - out_3784963899824887376[2] = 0; - out_3784963899824887376[3] = 0; - out_3784963899824887376[4] = 1; - out_3784963899824887376[5] = 0; - out_3784963899824887376[6] = 0; - out_3784963899824887376[7] = 0; - out_3784963899824887376[8] = 0; +void H_30(double *state, double *unused, double *out_2426050356903195993) { + out_2426050356903195993[0] = 0; + out_2426050356903195993[1] = 0; + out_2426050356903195993[2] = 0; + out_2426050356903195993[3] = 0; + out_2426050356903195993[4] = 1; + out_2426050356903195993[5] = 0; + out_2426050356903195993[6] = 0; + out_2426050356903195993[7] = 0; + out_2426050356903195993[8] = 0; } -void h_26(double *state, double *unused, double *out_5233056995331332443) { - out_5233056995331332443[0] = state[7]; +void h_26(double *state, double *unused, double *out_4026277330143471688) { + out_4026277330143471688[0] = state[7]; } -void H_26(double *state, double *unused, double *out_2998770888571335402) { - out_2998770888571335402[0] = 0; - out_2998770888571335402[1] = 0; - out_2998770888571335402[2] = 0; - out_2998770888571335402[3] = 0; - out_2998770888571335402[4] = 0; - out_2998770888571335402[5] = 0; - out_2998770888571335402[6] = 0; - out_2998770888571335402[7] = 1; - out_2998770888571335402[8] = 0; +void H_26(double *state, double *unused, double *out_5362500056440682644) { + out_5362500056440682644[0] = 0; + out_5362500056440682644[1] = 0; + out_5362500056440682644[2] = 0; + out_5362500056440682644[3] = 0; + out_5362500056440682644[4] = 0; + out_5362500056440682644[5] = 0; + out_5362500056440682644[6] = 0; + out_5362500056440682644[7] = 1; + out_5362500056440682644[8] = 0; } -void h_27(double *state, double *unused, double *out_3355860367277182854) { - out_3355860367277182854[0] = state[3]; +void h_27(double *state, double *unused, double *out_990245959502812892) { + out_990245959502812892[0] = state[3]; } -void H_27(double *state, double *unused, double *out_5959727211625312287) { - out_5959727211625312287[0] = 0; - out_5959727211625312287[1] = 0; - out_5959727211625312287[2] = 0; - out_5959727211625312287[3] = 1; - out_5959727211625312287[4] = 0; - out_5959727211625312287[5] = 0; - out_5959727211625312287[6] = 0; - out_5959727211625312287[7] = 0; - out_5959727211625312287[8] = 0; +void H_27(double *state, double *unused, double *out_4600813668703620904) { + out_4600813668703620904[0] = 0; + out_4600813668703620904[1] = 0; + out_4600813668703620904[2] = 0; + out_4600813668703620904[3] = 1; + out_4600813668703620904[4] = 0; + out_4600813668703620904[5] = 0; + out_4600813668703620904[6] = 0; + out_4600813668703620904[7] = 0; + out_4600813668703620904[8] = 0; } -void h_29(double *state, double *unused, double *out_7630324942183480437) { - out_7630324942183480437[0] = state[1]; +void h_29(double *state, double *unused, double *out_8450804723751701217) { + out_8450804723751701217[0] = state[1]; } -void H_29(double *state, double *unused, double *out_3274732555510495192) { - out_3274732555510495192[0] = 0; - out_3274732555510495192[1] = 1; - out_3274732555510495192[2] = 0; - out_3274732555510495192[3] = 0; - out_3274732555510495192[4] = 0; - out_3274732555510495192[5] = 0; - out_3274732555510495192[6] = 0; - out_3274732555510495192[7] = 0; - out_3274732555510495192[8] = 0; +void H_29(double *state, double *unused, double *out_1915819012588803809) { + out_1915819012588803809[0] = 0; + out_1915819012588803809[1] = 1; + out_1915819012588803809[2] = 0; + out_1915819012588803809[3] = 0; + out_1915819012588803809[4] = 0; + out_1915819012588803809[5] = 0; + out_1915819012588803809[6] = 0; + out_1915819012588803809[7] = 0; + out_1915819012588803809[8] = 0; } -void h_28(double *state, double *unused, double *out_4299370220178845289) { - out_4299370220178845289[0] = state[0]; +void h_28(double *state, double *unused, double *out_2847713298477492018) { + out_2847713298477492018[0] = state[0]; } -void H_28(double *state, double *unused, double *out_1311102283945168941) { - out_1311102283945168941[0] = 1; - out_1311102283945168941[1] = 0; - out_1311102283945168941[2] = 0; - out_1311102283945168941[3] = 0; - out_1311102283945168941[4] = 0; - out_1311102283945168941[5] = 0; - out_1311102283945168941[6] = 0; - out_1311102283945168941[7] = 0; - out_1311102283945168941[8] = 0; +void H_28(double *state, double *unused, double *out_7050168661066849105) { + out_7050168661066849105[0] = 1; + out_7050168661066849105[1] = 0; + out_7050168661066849105[2] = 0; + out_7050168661066849105[3] = 0; + out_7050168661066849105[4] = 0; + out_7050168661066849105[5] = 0; + out_7050168661066849105[6] = 0; + out_7050168661066849105[7] = 0; + out_7050168661066849105[8] = 0; } -void h_31(double *state, double *unused, double *out_594268378117356058) { - out_594268378117356058[0] = state[8]; +void h_31(double *state, double *unused, double *out_857388543583997213) { + out_857388543583997213[0] = state[8]; } -void H_31(double *state, double *unused, double *out_3624978990804686878) { - out_3624978990804686878[0] = 0; - out_3624978990804686878[1] = 0; - out_3624978990804686878[2] = 0; - out_3624978990804686878[3] = 0; - out_3624978990804686878[4] = 0; - out_3624978990804686878[5] = 0; - out_3624978990804686878[6] = 0; - out_3624978990804686878[7] = 0; - out_3624978990804686878[8] = 1; +void H_31(double *state, double *unused, double *out_9134649337191699296) { + out_9134649337191699296[0] = 0; + out_9134649337191699296[1] = 0; + out_9134649337191699296[2] = 0; + out_9134649337191699296[3] = 0; + out_9134649337191699296[4] = 0; + out_9134649337191699296[5] = 0; + out_9134649337191699296[6] = 0; + out_9134649337191699296[7] = 0; + out_9134649337191699296[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_4387195160971747702) { - err_fun(nom_x, delta_x, out_4387195160971747702); +void car_err_fun(double *nom_x, double *delta_x, double *out_7214711627163091402) { + err_fun(nom_x, delta_x, out_7214711627163091402); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_7707264064834051115) { - inv_err_fun(nom_x, true_x, out_7707264064834051115); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_911828672798028438) { + inv_err_fun(nom_x, true_x, out_911828672798028438); } -void car_H_mod_fun(double *state, double *out_7178605602671138983) { - H_mod_fun(state, out_7178605602671138983); +void car_H_mod_fun(double *state, double *out_149552472482575511) { + H_mod_fun(state, out_149552472482575511); } -void car_f_fun(double *state, double dt, double *out_2416599425795193412) { - f_fun(state, dt, out_2416599425795193412); +void car_f_fun(double *state, double dt, double *out_4547528931834134293) { + f_fun(state, dt, out_4547528931834134293); } -void car_F_fun(double *state, double dt, double *out_6761986232654706930) { - F_fun(state, dt, out_6761986232654706930); +void car_F_fun(double *state, double dt, double *out_3560058468708809121) { + F_fun(state, dt, out_3560058468708809121); } -void car_h_25(double *state, double *unused, double *out_319074315832850169) { - h_25(state, unused, out_319074315832850169); +void car_h_25(double *state, double *unused, double *out_1132582605868503102) { + h_25(state, unused, out_1132582605868503102); } -void car_H_25(double *state, double *unused, double *out_3655624952681647306) { - H_25(state, unused, out_3655624952681647306); +void car_H_25(double *state, double *unused, double *out_9104003375314738868) { + H_25(state, unused, out_9104003375314738868); } -void car_h_24(double *state, double *unused, double *out_5734853090591072423) { - h_24(state, unused, out_5734853090591072423); +void car_h_24(double *state, double *unused, double *out_8381470974523360630) { + h_24(state, unused, out_8381470974523360630); } -void car_H_24(double *state, double *unused, double *out_1429917168702778744) { - H_24(state, unused, out_1429917168702778744); +void car_H_24(double *state, double *unused, double *out_4357494048348229834) { + H_24(state, unused, out_4357494048348229834); } -void car_h_30(double *state, double *unused, double *out_8212013810717373528) { - h_30(state, unused, out_8212013810717373528); +void car_h_30(double *state, double *unused, double *out_975395937517373957) { + h_30(state, unused, out_975395937517373957); } -void car_H_30(double *state, double *unused, double *out_3784963899824887376) { - H_30(state, unused, out_3784963899824887376); +void car_H_30(double *state, double *unused, double *out_2426050356903195993) { + H_30(state, unused, out_2426050356903195993); } -void car_h_26(double *state, double *unused, double *out_5233056995331332443) { - h_26(state, unused, out_5233056995331332443); +void car_h_26(double *state, double *unused, double *out_4026277330143471688) { + h_26(state, unused, out_4026277330143471688); } -void car_H_26(double *state, double *unused, double *out_2998770888571335402) { - H_26(state, unused, out_2998770888571335402); +void car_H_26(double *state, double *unused, double *out_5362500056440682644) { + H_26(state, unused, out_5362500056440682644); } -void car_h_27(double *state, double *unused, double *out_3355860367277182854) { - h_27(state, unused, out_3355860367277182854); +void car_h_27(double *state, double *unused, double *out_990245959502812892) { + h_27(state, unused, out_990245959502812892); } -void car_H_27(double *state, double *unused, double *out_5959727211625312287) { - H_27(state, unused, out_5959727211625312287); +void car_H_27(double *state, double *unused, double *out_4600813668703620904) { + H_27(state, unused, out_4600813668703620904); } -void car_h_29(double *state, double *unused, double *out_7630324942183480437) { - h_29(state, unused, out_7630324942183480437); +void car_h_29(double *state, double *unused, double *out_8450804723751701217) { + h_29(state, unused, out_8450804723751701217); } -void car_H_29(double *state, double *unused, double *out_3274732555510495192) { - H_29(state, unused, out_3274732555510495192); +void car_H_29(double *state, double *unused, double *out_1915819012588803809) { + H_29(state, unused, out_1915819012588803809); } -void car_h_28(double *state, double *unused, double *out_4299370220178845289) { - h_28(state, unused, out_4299370220178845289); +void car_h_28(double *state, double *unused, double *out_2847713298477492018) { + h_28(state, unused, out_2847713298477492018); } -void car_H_28(double *state, double *unused, double *out_1311102283945168941) { - H_28(state, unused, out_1311102283945168941); +void car_H_28(double *state, double *unused, double *out_7050168661066849105) { + H_28(state, unused, out_7050168661066849105); } -void car_h_31(double *state, double *unused, double *out_594268378117356058) { - h_31(state, unused, out_594268378117356058); +void car_h_31(double *state, double *unused, double *out_857388543583997213) { + h_31(state, unused, out_857388543583997213); } -void car_H_31(double *state, double *unused, double *out_3624978990804686878) { - H_31(state, unused, out_3624978990804686878); +void car_H_31(double *state, double *unused, double *out_9134649337191699296) { + H_31(state, unused, out_9134649337191699296); } 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 73987e5d2..c9d533c8a 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_4387195160971747702); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_7707264064834051115); -void car_H_mod_fun(double *state, double *out_7178605602671138983); -void car_f_fun(double *state, double dt, double *out_2416599425795193412); -void car_F_fun(double *state, double dt, double *out_6761986232654706930); -void car_h_25(double *state, double *unused, double *out_319074315832850169); -void car_H_25(double *state, double *unused, double *out_3655624952681647306); -void car_h_24(double *state, double *unused, double *out_5734853090591072423); -void car_H_24(double *state, double *unused, double *out_1429917168702778744); -void car_h_30(double *state, double *unused, double *out_8212013810717373528); -void car_H_30(double *state, double *unused, double *out_3784963899824887376); -void car_h_26(double *state, double *unused, double *out_5233056995331332443); -void car_H_26(double *state, double *unused, double *out_2998770888571335402); -void car_h_27(double *state, double *unused, double *out_3355860367277182854); -void car_H_27(double *state, double *unused, double *out_5959727211625312287); -void car_h_29(double *state, double *unused, double *out_7630324942183480437); -void car_H_29(double *state, double *unused, double *out_3274732555510495192); -void car_h_28(double *state, double *unused, double *out_4299370220178845289); -void car_H_28(double *state, double *unused, double *out_1311102283945168941); -void car_h_31(double *state, double *unused, double *out_594268378117356058); -void car_H_31(double *state, double *unused, double *out_3624978990804686878); +void car_err_fun(double *nom_x, double *delta_x, double *out_7214711627163091402); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_911828672798028438); +void car_H_mod_fun(double *state, double *out_149552472482575511); +void car_f_fun(double *state, double dt, double *out_4547528931834134293); +void car_F_fun(double *state, double dt, double *out_3560058468708809121); +void car_h_25(double *state, double *unused, double *out_1132582605868503102); +void car_H_25(double *state, double *unused, double *out_9104003375314738868); +void car_h_24(double *state, double *unused, double *out_8381470974523360630); +void car_H_24(double *state, double *unused, double *out_4357494048348229834); +void car_h_30(double *state, double *unused, double *out_975395937517373957); +void car_H_30(double *state, double *unused, double *out_2426050356903195993); +void car_h_26(double *state, double *unused, double *out_4026277330143471688); +void car_H_26(double *state, double *unused, double *out_5362500056440682644); +void car_h_27(double *state, double *unused, double *out_990245959502812892); +void car_H_27(double *state, double *unused, double *out_4600813668703620904); +void car_h_29(double *state, double *unused, double *out_8450804723751701217); +void car_H_29(double *state, double *unused, double *out_1915819012588803809); +void car_h_28(double *state, double *unused, double *out_2847713298477492018); +void car_H_28(double *state, double *unused, double *out_7050168661066849105); +void car_h_31(double *state, double *unused, double *out_857388543583997213); +void car_H_31(double *state, double *unused, double *out_9134649337191699296); 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 08ba23662..e106e2659 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_8207359146711228947) { - out_8207359146711228947[0] = delta_x[0] + nom_x[0]; - out_8207359146711228947[1] = delta_x[1] + nom_x[1]; - out_8207359146711228947[2] = delta_x[2] + nom_x[2]; - out_8207359146711228947[3] = delta_x[3] + nom_x[3]; - out_8207359146711228947[4] = delta_x[4] + nom_x[4]; - out_8207359146711228947[5] = delta_x[5] + nom_x[5]; - out_8207359146711228947[6] = delta_x[6] + nom_x[6]; - out_8207359146711228947[7] = delta_x[7] + nom_x[7]; - out_8207359146711228947[8] = delta_x[8] + nom_x[8]; - out_8207359146711228947[9] = delta_x[9] + nom_x[9]; - out_8207359146711228947[10] = delta_x[10] + nom_x[10]; - out_8207359146711228947[11] = delta_x[11] + nom_x[11]; - out_8207359146711228947[12] = delta_x[12] + nom_x[12]; - out_8207359146711228947[13] = delta_x[13] + nom_x[13]; - out_8207359146711228947[14] = delta_x[14] + nom_x[14]; - out_8207359146711228947[15] = delta_x[15] + nom_x[15]; - out_8207359146711228947[16] = delta_x[16] + nom_x[16]; - out_8207359146711228947[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_6169503642516981710) { + out_6169503642516981710[0] = delta_x[0] + nom_x[0]; + out_6169503642516981710[1] = delta_x[1] + nom_x[1]; + out_6169503642516981710[2] = delta_x[2] + nom_x[2]; + out_6169503642516981710[3] = delta_x[3] + nom_x[3]; + out_6169503642516981710[4] = delta_x[4] + nom_x[4]; + out_6169503642516981710[5] = delta_x[5] + nom_x[5]; + out_6169503642516981710[6] = delta_x[6] + nom_x[6]; + out_6169503642516981710[7] = delta_x[7] + nom_x[7]; + out_6169503642516981710[8] = delta_x[8] + nom_x[8]; + out_6169503642516981710[9] = delta_x[9] + nom_x[9]; + out_6169503642516981710[10] = delta_x[10] + nom_x[10]; + out_6169503642516981710[11] = delta_x[11] + nom_x[11]; + out_6169503642516981710[12] = delta_x[12] + nom_x[12]; + out_6169503642516981710[13] = delta_x[13] + nom_x[13]; + out_6169503642516981710[14] = delta_x[14] + nom_x[14]; + out_6169503642516981710[15] = delta_x[15] + nom_x[15]; + out_6169503642516981710[16] = delta_x[16] + nom_x[16]; + out_6169503642516981710[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_8369219426901598353) { - out_8369219426901598353[0] = -nom_x[0] + true_x[0]; - out_8369219426901598353[1] = -nom_x[1] + true_x[1]; - out_8369219426901598353[2] = -nom_x[2] + true_x[2]; - out_8369219426901598353[3] = -nom_x[3] + true_x[3]; - out_8369219426901598353[4] = -nom_x[4] + true_x[4]; - out_8369219426901598353[5] = -nom_x[5] + true_x[5]; - out_8369219426901598353[6] = -nom_x[6] + true_x[6]; - out_8369219426901598353[7] = -nom_x[7] + true_x[7]; - out_8369219426901598353[8] = -nom_x[8] + true_x[8]; - out_8369219426901598353[9] = -nom_x[9] + true_x[9]; - out_8369219426901598353[10] = -nom_x[10] + true_x[10]; - out_8369219426901598353[11] = -nom_x[11] + true_x[11]; - out_8369219426901598353[12] = -nom_x[12] + true_x[12]; - out_8369219426901598353[13] = -nom_x[13] + true_x[13]; - out_8369219426901598353[14] = -nom_x[14] + true_x[14]; - out_8369219426901598353[15] = -nom_x[15] + true_x[15]; - out_8369219426901598353[16] = -nom_x[16] + true_x[16]; - out_8369219426901598353[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_2781558835044378693) { + out_2781558835044378693[0] = -nom_x[0] + true_x[0]; + out_2781558835044378693[1] = -nom_x[1] + true_x[1]; + out_2781558835044378693[2] = -nom_x[2] + true_x[2]; + out_2781558835044378693[3] = -nom_x[3] + true_x[3]; + out_2781558835044378693[4] = -nom_x[4] + true_x[4]; + out_2781558835044378693[5] = -nom_x[5] + true_x[5]; + out_2781558835044378693[6] = -nom_x[6] + true_x[6]; + out_2781558835044378693[7] = -nom_x[7] + true_x[7]; + out_2781558835044378693[8] = -nom_x[8] + true_x[8]; + out_2781558835044378693[9] = -nom_x[9] + true_x[9]; + out_2781558835044378693[10] = -nom_x[10] + true_x[10]; + out_2781558835044378693[11] = -nom_x[11] + true_x[11]; + out_2781558835044378693[12] = -nom_x[12] + true_x[12]; + out_2781558835044378693[13] = -nom_x[13] + true_x[13]; + out_2781558835044378693[14] = -nom_x[14] + true_x[14]; + out_2781558835044378693[15] = -nom_x[15] + true_x[15]; + out_2781558835044378693[16] = -nom_x[16] + true_x[16]; + out_2781558835044378693[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_7202306802530511060) { - out_7202306802530511060[0] = 1.0; - out_7202306802530511060[1] = 0.0; - out_7202306802530511060[2] = 0.0; - out_7202306802530511060[3] = 0.0; - out_7202306802530511060[4] = 0.0; - out_7202306802530511060[5] = 0.0; - out_7202306802530511060[6] = 0.0; - out_7202306802530511060[7] = 0.0; - out_7202306802530511060[8] = 0.0; - out_7202306802530511060[9] = 0.0; - out_7202306802530511060[10] = 0.0; - out_7202306802530511060[11] = 0.0; - out_7202306802530511060[12] = 0.0; - out_7202306802530511060[13] = 0.0; - out_7202306802530511060[14] = 0.0; - out_7202306802530511060[15] = 0.0; - out_7202306802530511060[16] = 0.0; - out_7202306802530511060[17] = 0.0; - out_7202306802530511060[18] = 0.0; - out_7202306802530511060[19] = 1.0; - out_7202306802530511060[20] = 0.0; - out_7202306802530511060[21] = 0.0; - out_7202306802530511060[22] = 0.0; - out_7202306802530511060[23] = 0.0; - out_7202306802530511060[24] = 0.0; - out_7202306802530511060[25] = 0.0; - out_7202306802530511060[26] = 0.0; - out_7202306802530511060[27] = 0.0; - out_7202306802530511060[28] = 0.0; - out_7202306802530511060[29] = 0.0; - out_7202306802530511060[30] = 0.0; - out_7202306802530511060[31] = 0.0; - out_7202306802530511060[32] = 0.0; - out_7202306802530511060[33] = 0.0; - out_7202306802530511060[34] = 0.0; - out_7202306802530511060[35] = 0.0; - out_7202306802530511060[36] = 0.0; - out_7202306802530511060[37] = 0.0; - out_7202306802530511060[38] = 1.0; - out_7202306802530511060[39] = 0.0; - out_7202306802530511060[40] = 0.0; - out_7202306802530511060[41] = 0.0; - out_7202306802530511060[42] = 0.0; - out_7202306802530511060[43] = 0.0; - out_7202306802530511060[44] = 0.0; - out_7202306802530511060[45] = 0.0; - out_7202306802530511060[46] = 0.0; - out_7202306802530511060[47] = 0.0; - out_7202306802530511060[48] = 0.0; - out_7202306802530511060[49] = 0.0; - out_7202306802530511060[50] = 0.0; - out_7202306802530511060[51] = 0.0; - out_7202306802530511060[52] = 0.0; - out_7202306802530511060[53] = 0.0; - out_7202306802530511060[54] = 0.0; - out_7202306802530511060[55] = 0.0; - out_7202306802530511060[56] = 0.0; - out_7202306802530511060[57] = 1.0; - out_7202306802530511060[58] = 0.0; - out_7202306802530511060[59] = 0.0; - out_7202306802530511060[60] = 0.0; - out_7202306802530511060[61] = 0.0; - out_7202306802530511060[62] = 0.0; - out_7202306802530511060[63] = 0.0; - out_7202306802530511060[64] = 0.0; - out_7202306802530511060[65] = 0.0; - out_7202306802530511060[66] = 0.0; - out_7202306802530511060[67] = 0.0; - out_7202306802530511060[68] = 0.0; - out_7202306802530511060[69] = 0.0; - out_7202306802530511060[70] = 0.0; - out_7202306802530511060[71] = 0.0; - out_7202306802530511060[72] = 0.0; - out_7202306802530511060[73] = 0.0; - out_7202306802530511060[74] = 0.0; - out_7202306802530511060[75] = 0.0; - out_7202306802530511060[76] = 1.0; - out_7202306802530511060[77] = 0.0; - out_7202306802530511060[78] = 0.0; - out_7202306802530511060[79] = 0.0; - out_7202306802530511060[80] = 0.0; - out_7202306802530511060[81] = 0.0; - out_7202306802530511060[82] = 0.0; - out_7202306802530511060[83] = 0.0; - out_7202306802530511060[84] = 0.0; - out_7202306802530511060[85] = 0.0; - out_7202306802530511060[86] = 0.0; - out_7202306802530511060[87] = 0.0; - out_7202306802530511060[88] = 0.0; - out_7202306802530511060[89] = 0.0; - out_7202306802530511060[90] = 0.0; - out_7202306802530511060[91] = 0.0; - out_7202306802530511060[92] = 0.0; - out_7202306802530511060[93] = 0.0; - out_7202306802530511060[94] = 0.0; - out_7202306802530511060[95] = 1.0; - out_7202306802530511060[96] = 0.0; - out_7202306802530511060[97] = 0.0; - out_7202306802530511060[98] = 0.0; - out_7202306802530511060[99] = 0.0; - out_7202306802530511060[100] = 0.0; - out_7202306802530511060[101] = 0.0; - out_7202306802530511060[102] = 0.0; - out_7202306802530511060[103] = 0.0; - out_7202306802530511060[104] = 0.0; - out_7202306802530511060[105] = 0.0; - out_7202306802530511060[106] = 0.0; - out_7202306802530511060[107] = 0.0; - out_7202306802530511060[108] = 0.0; - out_7202306802530511060[109] = 0.0; - out_7202306802530511060[110] = 0.0; - out_7202306802530511060[111] = 0.0; - out_7202306802530511060[112] = 0.0; - out_7202306802530511060[113] = 0.0; - out_7202306802530511060[114] = 1.0; - out_7202306802530511060[115] = 0.0; - out_7202306802530511060[116] = 0.0; - out_7202306802530511060[117] = 0.0; - out_7202306802530511060[118] = 0.0; - out_7202306802530511060[119] = 0.0; - out_7202306802530511060[120] = 0.0; - out_7202306802530511060[121] = 0.0; - out_7202306802530511060[122] = 0.0; - out_7202306802530511060[123] = 0.0; - out_7202306802530511060[124] = 0.0; - out_7202306802530511060[125] = 0.0; - out_7202306802530511060[126] = 0.0; - out_7202306802530511060[127] = 0.0; - out_7202306802530511060[128] = 0.0; - out_7202306802530511060[129] = 0.0; - out_7202306802530511060[130] = 0.0; - out_7202306802530511060[131] = 0.0; - out_7202306802530511060[132] = 0.0; - out_7202306802530511060[133] = 1.0; - out_7202306802530511060[134] = 0.0; - out_7202306802530511060[135] = 0.0; - out_7202306802530511060[136] = 0.0; - out_7202306802530511060[137] = 0.0; - out_7202306802530511060[138] = 0.0; - out_7202306802530511060[139] = 0.0; - out_7202306802530511060[140] = 0.0; - out_7202306802530511060[141] = 0.0; - out_7202306802530511060[142] = 0.0; - out_7202306802530511060[143] = 0.0; - out_7202306802530511060[144] = 0.0; - out_7202306802530511060[145] = 0.0; - out_7202306802530511060[146] = 0.0; - out_7202306802530511060[147] = 0.0; - out_7202306802530511060[148] = 0.0; - out_7202306802530511060[149] = 0.0; - out_7202306802530511060[150] = 0.0; - out_7202306802530511060[151] = 0.0; - out_7202306802530511060[152] = 1.0; - out_7202306802530511060[153] = 0.0; - out_7202306802530511060[154] = 0.0; - out_7202306802530511060[155] = 0.0; - out_7202306802530511060[156] = 0.0; - out_7202306802530511060[157] = 0.0; - out_7202306802530511060[158] = 0.0; - out_7202306802530511060[159] = 0.0; - out_7202306802530511060[160] = 0.0; - out_7202306802530511060[161] = 0.0; - out_7202306802530511060[162] = 0.0; - out_7202306802530511060[163] = 0.0; - out_7202306802530511060[164] = 0.0; - out_7202306802530511060[165] = 0.0; - out_7202306802530511060[166] = 0.0; - out_7202306802530511060[167] = 0.0; - out_7202306802530511060[168] = 0.0; - out_7202306802530511060[169] = 0.0; - out_7202306802530511060[170] = 0.0; - out_7202306802530511060[171] = 1.0; - out_7202306802530511060[172] = 0.0; - out_7202306802530511060[173] = 0.0; - out_7202306802530511060[174] = 0.0; - out_7202306802530511060[175] = 0.0; - out_7202306802530511060[176] = 0.0; - out_7202306802530511060[177] = 0.0; - out_7202306802530511060[178] = 0.0; - out_7202306802530511060[179] = 0.0; - out_7202306802530511060[180] = 0.0; - out_7202306802530511060[181] = 0.0; - out_7202306802530511060[182] = 0.0; - out_7202306802530511060[183] = 0.0; - out_7202306802530511060[184] = 0.0; - out_7202306802530511060[185] = 0.0; - out_7202306802530511060[186] = 0.0; - out_7202306802530511060[187] = 0.0; - out_7202306802530511060[188] = 0.0; - out_7202306802530511060[189] = 0.0; - out_7202306802530511060[190] = 1.0; - out_7202306802530511060[191] = 0.0; - out_7202306802530511060[192] = 0.0; - out_7202306802530511060[193] = 0.0; - out_7202306802530511060[194] = 0.0; - out_7202306802530511060[195] = 0.0; - out_7202306802530511060[196] = 0.0; - out_7202306802530511060[197] = 0.0; - out_7202306802530511060[198] = 0.0; - out_7202306802530511060[199] = 0.0; - out_7202306802530511060[200] = 0.0; - out_7202306802530511060[201] = 0.0; - out_7202306802530511060[202] = 0.0; - out_7202306802530511060[203] = 0.0; - out_7202306802530511060[204] = 0.0; - out_7202306802530511060[205] = 0.0; - out_7202306802530511060[206] = 0.0; - out_7202306802530511060[207] = 0.0; - out_7202306802530511060[208] = 0.0; - out_7202306802530511060[209] = 1.0; - out_7202306802530511060[210] = 0.0; - out_7202306802530511060[211] = 0.0; - out_7202306802530511060[212] = 0.0; - out_7202306802530511060[213] = 0.0; - out_7202306802530511060[214] = 0.0; - out_7202306802530511060[215] = 0.0; - out_7202306802530511060[216] = 0.0; - out_7202306802530511060[217] = 0.0; - out_7202306802530511060[218] = 0.0; - out_7202306802530511060[219] = 0.0; - out_7202306802530511060[220] = 0.0; - out_7202306802530511060[221] = 0.0; - out_7202306802530511060[222] = 0.0; - out_7202306802530511060[223] = 0.0; - out_7202306802530511060[224] = 0.0; - out_7202306802530511060[225] = 0.0; - out_7202306802530511060[226] = 0.0; - out_7202306802530511060[227] = 0.0; - out_7202306802530511060[228] = 1.0; - out_7202306802530511060[229] = 0.0; - out_7202306802530511060[230] = 0.0; - out_7202306802530511060[231] = 0.0; - out_7202306802530511060[232] = 0.0; - out_7202306802530511060[233] = 0.0; - out_7202306802530511060[234] = 0.0; - out_7202306802530511060[235] = 0.0; - out_7202306802530511060[236] = 0.0; - out_7202306802530511060[237] = 0.0; - out_7202306802530511060[238] = 0.0; - out_7202306802530511060[239] = 0.0; - out_7202306802530511060[240] = 0.0; - out_7202306802530511060[241] = 0.0; - out_7202306802530511060[242] = 0.0; - out_7202306802530511060[243] = 0.0; - out_7202306802530511060[244] = 0.0; - out_7202306802530511060[245] = 0.0; - out_7202306802530511060[246] = 0.0; - out_7202306802530511060[247] = 1.0; - out_7202306802530511060[248] = 0.0; - out_7202306802530511060[249] = 0.0; - out_7202306802530511060[250] = 0.0; - out_7202306802530511060[251] = 0.0; - out_7202306802530511060[252] = 0.0; - out_7202306802530511060[253] = 0.0; - out_7202306802530511060[254] = 0.0; - out_7202306802530511060[255] = 0.0; - out_7202306802530511060[256] = 0.0; - out_7202306802530511060[257] = 0.0; - out_7202306802530511060[258] = 0.0; - out_7202306802530511060[259] = 0.0; - out_7202306802530511060[260] = 0.0; - out_7202306802530511060[261] = 0.0; - out_7202306802530511060[262] = 0.0; - out_7202306802530511060[263] = 0.0; - out_7202306802530511060[264] = 0.0; - out_7202306802530511060[265] = 0.0; - out_7202306802530511060[266] = 1.0; - out_7202306802530511060[267] = 0.0; - out_7202306802530511060[268] = 0.0; - out_7202306802530511060[269] = 0.0; - out_7202306802530511060[270] = 0.0; - out_7202306802530511060[271] = 0.0; - out_7202306802530511060[272] = 0.0; - out_7202306802530511060[273] = 0.0; - out_7202306802530511060[274] = 0.0; - out_7202306802530511060[275] = 0.0; - out_7202306802530511060[276] = 0.0; - out_7202306802530511060[277] = 0.0; - out_7202306802530511060[278] = 0.0; - out_7202306802530511060[279] = 0.0; - out_7202306802530511060[280] = 0.0; - out_7202306802530511060[281] = 0.0; - out_7202306802530511060[282] = 0.0; - out_7202306802530511060[283] = 0.0; - out_7202306802530511060[284] = 0.0; - out_7202306802530511060[285] = 1.0; - out_7202306802530511060[286] = 0.0; - out_7202306802530511060[287] = 0.0; - out_7202306802530511060[288] = 0.0; - out_7202306802530511060[289] = 0.0; - out_7202306802530511060[290] = 0.0; - out_7202306802530511060[291] = 0.0; - out_7202306802530511060[292] = 0.0; - out_7202306802530511060[293] = 0.0; - out_7202306802530511060[294] = 0.0; - out_7202306802530511060[295] = 0.0; - out_7202306802530511060[296] = 0.0; - out_7202306802530511060[297] = 0.0; - out_7202306802530511060[298] = 0.0; - out_7202306802530511060[299] = 0.0; - out_7202306802530511060[300] = 0.0; - out_7202306802530511060[301] = 0.0; - out_7202306802530511060[302] = 0.0; - out_7202306802530511060[303] = 0.0; - out_7202306802530511060[304] = 1.0; - out_7202306802530511060[305] = 0.0; - out_7202306802530511060[306] = 0.0; - out_7202306802530511060[307] = 0.0; - out_7202306802530511060[308] = 0.0; - out_7202306802530511060[309] = 0.0; - out_7202306802530511060[310] = 0.0; - out_7202306802530511060[311] = 0.0; - out_7202306802530511060[312] = 0.0; - out_7202306802530511060[313] = 0.0; - out_7202306802530511060[314] = 0.0; - out_7202306802530511060[315] = 0.0; - out_7202306802530511060[316] = 0.0; - out_7202306802530511060[317] = 0.0; - out_7202306802530511060[318] = 0.0; - out_7202306802530511060[319] = 0.0; - out_7202306802530511060[320] = 0.0; - out_7202306802530511060[321] = 0.0; - out_7202306802530511060[322] = 0.0; - out_7202306802530511060[323] = 1.0; +void H_mod_fun(double *state, double *out_4498457996676133449) { + out_4498457996676133449[0] = 1.0; + out_4498457996676133449[1] = 0.0; + out_4498457996676133449[2] = 0.0; + out_4498457996676133449[3] = 0.0; + out_4498457996676133449[4] = 0.0; + out_4498457996676133449[5] = 0.0; + out_4498457996676133449[6] = 0.0; + out_4498457996676133449[7] = 0.0; + out_4498457996676133449[8] = 0.0; + out_4498457996676133449[9] = 0.0; + out_4498457996676133449[10] = 0.0; + out_4498457996676133449[11] = 0.0; + out_4498457996676133449[12] = 0.0; + out_4498457996676133449[13] = 0.0; + out_4498457996676133449[14] = 0.0; + out_4498457996676133449[15] = 0.0; + out_4498457996676133449[16] = 0.0; + out_4498457996676133449[17] = 0.0; + out_4498457996676133449[18] = 0.0; + out_4498457996676133449[19] = 1.0; + out_4498457996676133449[20] = 0.0; + out_4498457996676133449[21] = 0.0; + out_4498457996676133449[22] = 0.0; + out_4498457996676133449[23] = 0.0; + out_4498457996676133449[24] = 0.0; + out_4498457996676133449[25] = 0.0; + out_4498457996676133449[26] = 0.0; + out_4498457996676133449[27] = 0.0; + out_4498457996676133449[28] = 0.0; + out_4498457996676133449[29] = 0.0; + out_4498457996676133449[30] = 0.0; + out_4498457996676133449[31] = 0.0; + out_4498457996676133449[32] = 0.0; + out_4498457996676133449[33] = 0.0; + out_4498457996676133449[34] = 0.0; + out_4498457996676133449[35] = 0.0; + out_4498457996676133449[36] = 0.0; + out_4498457996676133449[37] = 0.0; + out_4498457996676133449[38] = 1.0; + out_4498457996676133449[39] = 0.0; + out_4498457996676133449[40] = 0.0; + out_4498457996676133449[41] = 0.0; + out_4498457996676133449[42] = 0.0; + out_4498457996676133449[43] = 0.0; + out_4498457996676133449[44] = 0.0; + out_4498457996676133449[45] = 0.0; + out_4498457996676133449[46] = 0.0; + out_4498457996676133449[47] = 0.0; + out_4498457996676133449[48] = 0.0; + out_4498457996676133449[49] = 0.0; + out_4498457996676133449[50] = 0.0; + out_4498457996676133449[51] = 0.0; + out_4498457996676133449[52] = 0.0; + out_4498457996676133449[53] = 0.0; + out_4498457996676133449[54] = 0.0; + out_4498457996676133449[55] = 0.0; + out_4498457996676133449[56] = 0.0; + out_4498457996676133449[57] = 1.0; + out_4498457996676133449[58] = 0.0; + out_4498457996676133449[59] = 0.0; + out_4498457996676133449[60] = 0.0; + out_4498457996676133449[61] = 0.0; + out_4498457996676133449[62] = 0.0; + out_4498457996676133449[63] = 0.0; + out_4498457996676133449[64] = 0.0; + out_4498457996676133449[65] = 0.0; + out_4498457996676133449[66] = 0.0; + out_4498457996676133449[67] = 0.0; + out_4498457996676133449[68] = 0.0; + out_4498457996676133449[69] = 0.0; + out_4498457996676133449[70] = 0.0; + out_4498457996676133449[71] = 0.0; + out_4498457996676133449[72] = 0.0; + out_4498457996676133449[73] = 0.0; + out_4498457996676133449[74] = 0.0; + out_4498457996676133449[75] = 0.0; + out_4498457996676133449[76] = 1.0; + out_4498457996676133449[77] = 0.0; + out_4498457996676133449[78] = 0.0; + out_4498457996676133449[79] = 0.0; + out_4498457996676133449[80] = 0.0; + out_4498457996676133449[81] = 0.0; + out_4498457996676133449[82] = 0.0; + out_4498457996676133449[83] = 0.0; + out_4498457996676133449[84] = 0.0; + out_4498457996676133449[85] = 0.0; + out_4498457996676133449[86] = 0.0; + out_4498457996676133449[87] = 0.0; + out_4498457996676133449[88] = 0.0; + out_4498457996676133449[89] = 0.0; + out_4498457996676133449[90] = 0.0; + out_4498457996676133449[91] = 0.0; + out_4498457996676133449[92] = 0.0; + out_4498457996676133449[93] = 0.0; + out_4498457996676133449[94] = 0.0; + out_4498457996676133449[95] = 1.0; + out_4498457996676133449[96] = 0.0; + out_4498457996676133449[97] = 0.0; + out_4498457996676133449[98] = 0.0; + out_4498457996676133449[99] = 0.0; + out_4498457996676133449[100] = 0.0; + out_4498457996676133449[101] = 0.0; + out_4498457996676133449[102] = 0.0; + out_4498457996676133449[103] = 0.0; + out_4498457996676133449[104] = 0.0; + out_4498457996676133449[105] = 0.0; + out_4498457996676133449[106] = 0.0; + out_4498457996676133449[107] = 0.0; + out_4498457996676133449[108] = 0.0; + out_4498457996676133449[109] = 0.0; + out_4498457996676133449[110] = 0.0; + out_4498457996676133449[111] = 0.0; + out_4498457996676133449[112] = 0.0; + out_4498457996676133449[113] = 0.0; + out_4498457996676133449[114] = 1.0; + out_4498457996676133449[115] = 0.0; + out_4498457996676133449[116] = 0.0; + out_4498457996676133449[117] = 0.0; + out_4498457996676133449[118] = 0.0; + out_4498457996676133449[119] = 0.0; + out_4498457996676133449[120] = 0.0; + out_4498457996676133449[121] = 0.0; + out_4498457996676133449[122] = 0.0; + out_4498457996676133449[123] = 0.0; + out_4498457996676133449[124] = 0.0; + out_4498457996676133449[125] = 0.0; + out_4498457996676133449[126] = 0.0; + out_4498457996676133449[127] = 0.0; + out_4498457996676133449[128] = 0.0; + out_4498457996676133449[129] = 0.0; + out_4498457996676133449[130] = 0.0; + out_4498457996676133449[131] = 0.0; + out_4498457996676133449[132] = 0.0; + out_4498457996676133449[133] = 1.0; + out_4498457996676133449[134] = 0.0; + out_4498457996676133449[135] = 0.0; + out_4498457996676133449[136] = 0.0; + out_4498457996676133449[137] = 0.0; + out_4498457996676133449[138] = 0.0; + out_4498457996676133449[139] = 0.0; + out_4498457996676133449[140] = 0.0; + out_4498457996676133449[141] = 0.0; + out_4498457996676133449[142] = 0.0; + out_4498457996676133449[143] = 0.0; + out_4498457996676133449[144] = 0.0; + out_4498457996676133449[145] = 0.0; + out_4498457996676133449[146] = 0.0; + out_4498457996676133449[147] = 0.0; + out_4498457996676133449[148] = 0.0; + out_4498457996676133449[149] = 0.0; + out_4498457996676133449[150] = 0.0; + out_4498457996676133449[151] = 0.0; + out_4498457996676133449[152] = 1.0; + out_4498457996676133449[153] = 0.0; + out_4498457996676133449[154] = 0.0; + out_4498457996676133449[155] = 0.0; + out_4498457996676133449[156] = 0.0; + out_4498457996676133449[157] = 0.0; + out_4498457996676133449[158] = 0.0; + out_4498457996676133449[159] = 0.0; + out_4498457996676133449[160] = 0.0; + out_4498457996676133449[161] = 0.0; + out_4498457996676133449[162] = 0.0; + out_4498457996676133449[163] = 0.0; + out_4498457996676133449[164] = 0.0; + out_4498457996676133449[165] = 0.0; + out_4498457996676133449[166] = 0.0; + out_4498457996676133449[167] = 0.0; + out_4498457996676133449[168] = 0.0; + out_4498457996676133449[169] = 0.0; + out_4498457996676133449[170] = 0.0; + out_4498457996676133449[171] = 1.0; + out_4498457996676133449[172] = 0.0; + out_4498457996676133449[173] = 0.0; + out_4498457996676133449[174] = 0.0; + out_4498457996676133449[175] = 0.0; + out_4498457996676133449[176] = 0.0; + out_4498457996676133449[177] = 0.0; + out_4498457996676133449[178] = 0.0; + out_4498457996676133449[179] = 0.0; + out_4498457996676133449[180] = 0.0; + out_4498457996676133449[181] = 0.0; + out_4498457996676133449[182] = 0.0; + out_4498457996676133449[183] = 0.0; + out_4498457996676133449[184] = 0.0; + out_4498457996676133449[185] = 0.0; + out_4498457996676133449[186] = 0.0; + out_4498457996676133449[187] = 0.0; + out_4498457996676133449[188] = 0.0; + out_4498457996676133449[189] = 0.0; + out_4498457996676133449[190] = 1.0; + out_4498457996676133449[191] = 0.0; + out_4498457996676133449[192] = 0.0; + out_4498457996676133449[193] = 0.0; + out_4498457996676133449[194] = 0.0; + out_4498457996676133449[195] = 0.0; + out_4498457996676133449[196] = 0.0; + out_4498457996676133449[197] = 0.0; + out_4498457996676133449[198] = 0.0; + out_4498457996676133449[199] = 0.0; + out_4498457996676133449[200] = 0.0; + out_4498457996676133449[201] = 0.0; + out_4498457996676133449[202] = 0.0; + out_4498457996676133449[203] = 0.0; + out_4498457996676133449[204] = 0.0; + out_4498457996676133449[205] = 0.0; + out_4498457996676133449[206] = 0.0; + out_4498457996676133449[207] = 0.0; + out_4498457996676133449[208] = 0.0; + out_4498457996676133449[209] = 1.0; + out_4498457996676133449[210] = 0.0; + out_4498457996676133449[211] = 0.0; + out_4498457996676133449[212] = 0.0; + out_4498457996676133449[213] = 0.0; + out_4498457996676133449[214] = 0.0; + out_4498457996676133449[215] = 0.0; + out_4498457996676133449[216] = 0.0; + out_4498457996676133449[217] = 0.0; + out_4498457996676133449[218] = 0.0; + out_4498457996676133449[219] = 0.0; + out_4498457996676133449[220] = 0.0; + out_4498457996676133449[221] = 0.0; + out_4498457996676133449[222] = 0.0; + out_4498457996676133449[223] = 0.0; + out_4498457996676133449[224] = 0.0; + out_4498457996676133449[225] = 0.0; + out_4498457996676133449[226] = 0.0; + out_4498457996676133449[227] = 0.0; + out_4498457996676133449[228] = 1.0; + out_4498457996676133449[229] = 0.0; + out_4498457996676133449[230] = 0.0; + out_4498457996676133449[231] = 0.0; + out_4498457996676133449[232] = 0.0; + out_4498457996676133449[233] = 0.0; + out_4498457996676133449[234] = 0.0; + out_4498457996676133449[235] = 0.0; + out_4498457996676133449[236] = 0.0; + out_4498457996676133449[237] = 0.0; + out_4498457996676133449[238] = 0.0; + out_4498457996676133449[239] = 0.0; + out_4498457996676133449[240] = 0.0; + out_4498457996676133449[241] = 0.0; + out_4498457996676133449[242] = 0.0; + out_4498457996676133449[243] = 0.0; + out_4498457996676133449[244] = 0.0; + out_4498457996676133449[245] = 0.0; + out_4498457996676133449[246] = 0.0; + out_4498457996676133449[247] = 1.0; + out_4498457996676133449[248] = 0.0; + out_4498457996676133449[249] = 0.0; + out_4498457996676133449[250] = 0.0; + out_4498457996676133449[251] = 0.0; + out_4498457996676133449[252] = 0.0; + out_4498457996676133449[253] = 0.0; + out_4498457996676133449[254] = 0.0; + out_4498457996676133449[255] = 0.0; + out_4498457996676133449[256] = 0.0; + out_4498457996676133449[257] = 0.0; + out_4498457996676133449[258] = 0.0; + out_4498457996676133449[259] = 0.0; + out_4498457996676133449[260] = 0.0; + out_4498457996676133449[261] = 0.0; + out_4498457996676133449[262] = 0.0; + out_4498457996676133449[263] = 0.0; + out_4498457996676133449[264] = 0.0; + out_4498457996676133449[265] = 0.0; + out_4498457996676133449[266] = 1.0; + out_4498457996676133449[267] = 0.0; + out_4498457996676133449[268] = 0.0; + out_4498457996676133449[269] = 0.0; + out_4498457996676133449[270] = 0.0; + out_4498457996676133449[271] = 0.0; + out_4498457996676133449[272] = 0.0; + out_4498457996676133449[273] = 0.0; + out_4498457996676133449[274] = 0.0; + out_4498457996676133449[275] = 0.0; + out_4498457996676133449[276] = 0.0; + out_4498457996676133449[277] = 0.0; + out_4498457996676133449[278] = 0.0; + out_4498457996676133449[279] = 0.0; + out_4498457996676133449[280] = 0.0; + out_4498457996676133449[281] = 0.0; + out_4498457996676133449[282] = 0.0; + out_4498457996676133449[283] = 0.0; + out_4498457996676133449[284] = 0.0; + out_4498457996676133449[285] = 1.0; + out_4498457996676133449[286] = 0.0; + out_4498457996676133449[287] = 0.0; + out_4498457996676133449[288] = 0.0; + out_4498457996676133449[289] = 0.0; + out_4498457996676133449[290] = 0.0; + out_4498457996676133449[291] = 0.0; + out_4498457996676133449[292] = 0.0; + out_4498457996676133449[293] = 0.0; + out_4498457996676133449[294] = 0.0; + out_4498457996676133449[295] = 0.0; + out_4498457996676133449[296] = 0.0; + out_4498457996676133449[297] = 0.0; + out_4498457996676133449[298] = 0.0; + out_4498457996676133449[299] = 0.0; + out_4498457996676133449[300] = 0.0; + out_4498457996676133449[301] = 0.0; + out_4498457996676133449[302] = 0.0; + out_4498457996676133449[303] = 0.0; + out_4498457996676133449[304] = 1.0; + out_4498457996676133449[305] = 0.0; + out_4498457996676133449[306] = 0.0; + out_4498457996676133449[307] = 0.0; + out_4498457996676133449[308] = 0.0; + out_4498457996676133449[309] = 0.0; + out_4498457996676133449[310] = 0.0; + out_4498457996676133449[311] = 0.0; + out_4498457996676133449[312] = 0.0; + out_4498457996676133449[313] = 0.0; + out_4498457996676133449[314] = 0.0; + out_4498457996676133449[315] = 0.0; + out_4498457996676133449[316] = 0.0; + out_4498457996676133449[317] = 0.0; + out_4498457996676133449[318] = 0.0; + out_4498457996676133449[319] = 0.0; + out_4498457996676133449[320] = 0.0; + out_4498457996676133449[321] = 0.0; + out_4498457996676133449[322] = 0.0; + out_4498457996676133449[323] = 1.0; } -void f_fun(double *state, double dt, double *out_4961001892384611064) { - out_4961001892384611064[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_4961001892384611064[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_4961001892384611064[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_4961001892384611064[3] = dt*state[12] + state[3]; - out_4961001892384611064[4] = dt*state[13] + state[4]; - out_4961001892384611064[5] = dt*state[14] + state[5]; - out_4961001892384611064[6] = state[6]; - out_4961001892384611064[7] = state[7]; - out_4961001892384611064[8] = state[8]; - out_4961001892384611064[9] = state[9]; - out_4961001892384611064[10] = state[10]; - out_4961001892384611064[11] = state[11]; - out_4961001892384611064[12] = state[12]; - out_4961001892384611064[13] = state[13]; - out_4961001892384611064[14] = state[14]; - out_4961001892384611064[15] = state[15]; - out_4961001892384611064[16] = state[16]; - out_4961001892384611064[17] = state[17]; +void f_fun(double *state, double dt, double *out_3471362747861957887) { + out_3471362747861957887[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_3471362747861957887[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_3471362747861957887[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_3471362747861957887[3] = dt*state[12] + state[3]; + out_3471362747861957887[4] = dt*state[13] + state[4]; + out_3471362747861957887[5] = dt*state[14] + state[5]; + out_3471362747861957887[6] = state[6]; + out_3471362747861957887[7] = state[7]; + out_3471362747861957887[8] = state[8]; + out_3471362747861957887[9] = state[9]; + out_3471362747861957887[10] = state[10]; + out_3471362747861957887[11] = state[11]; + out_3471362747861957887[12] = state[12]; + out_3471362747861957887[13] = state[13]; + out_3471362747861957887[14] = state[14]; + out_3471362747861957887[15] = state[15]; + out_3471362747861957887[16] = state[16]; + out_3471362747861957887[17] = state[17]; } -void F_fun(double *state, double dt, double *out_4383751150204030030) { - out_4383751150204030030[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_4383751150204030030[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_4383751150204030030[2] = 0; - out_4383751150204030030[3] = 0; - out_4383751150204030030[4] = 0; - out_4383751150204030030[5] = 0; - out_4383751150204030030[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_4383751150204030030[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_4383751150204030030[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_4383751150204030030[9] = 0; - out_4383751150204030030[10] = 0; - out_4383751150204030030[11] = 0; - out_4383751150204030030[12] = 0; - out_4383751150204030030[13] = 0; - out_4383751150204030030[14] = 0; - out_4383751150204030030[15] = 0; - out_4383751150204030030[16] = 0; - out_4383751150204030030[17] = 0; - out_4383751150204030030[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_4383751150204030030[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_4383751150204030030[20] = 0; - out_4383751150204030030[21] = 0; - out_4383751150204030030[22] = 0; - out_4383751150204030030[23] = 0; - out_4383751150204030030[24] = 0; - out_4383751150204030030[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_4383751150204030030[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_4383751150204030030[27] = 0; - out_4383751150204030030[28] = 0; - out_4383751150204030030[29] = 0; - out_4383751150204030030[30] = 0; - out_4383751150204030030[31] = 0; - out_4383751150204030030[32] = 0; - out_4383751150204030030[33] = 0; - out_4383751150204030030[34] = 0; - out_4383751150204030030[35] = 0; - out_4383751150204030030[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_4383751150204030030[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_4383751150204030030[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_4383751150204030030[39] = 0; - out_4383751150204030030[40] = 0; - out_4383751150204030030[41] = 0; - out_4383751150204030030[42] = 0; - out_4383751150204030030[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_4383751150204030030[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_4383751150204030030[45] = 0; - out_4383751150204030030[46] = 0; - out_4383751150204030030[47] = 0; - out_4383751150204030030[48] = 0; - out_4383751150204030030[49] = 0; - out_4383751150204030030[50] = 0; - out_4383751150204030030[51] = 0; - out_4383751150204030030[52] = 0; - out_4383751150204030030[53] = 0; - out_4383751150204030030[54] = 0; - out_4383751150204030030[55] = 0; - out_4383751150204030030[56] = 0; - out_4383751150204030030[57] = 1; - out_4383751150204030030[58] = 0; - out_4383751150204030030[59] = 0; - out_4383751150204030030[60] = 0; - out_4383751150204030030[61] = 0; - out_4383751150204030030[62] = 0; - out_4383751150204030030[63] = 0; - out_4383751150204030030[64] = 0; - out_4383751150204030030[65] = 0; - out_4383751150204030030[66] = dt; - out_4383751150204030030[67] = 0; - out_4383751150204030030[68] = 0; - out_4383751150204030030[69] = 0; - out_4383751150204030030[70] = 0; - out_4383751150204030030[71] = 0; - out_4383751150204030030[72] = 0; - out_4383751150204030030[73] = 0; - out_4383751150204030030[74] = 0; - out_4383751150204030030[75] = 0; - out_4383751150204030030[76] = 1; - out_4383751150204030030[77] = 0; - out_4383751150204030030[78] = 0; - out_4383751150204030030[79] = 0; - out_4383751150204030030[80] = 0; - out_4383751150204030030[81] = 0; - out_4383751150204030030[82] = 0; - out_4383751150204030030[83] = 0; - out_4383751150204030030[84] = 0; - out_4383751150204030030[85] = dt; - out_4383751150204030030[86] = 0; - out_4383751150204030030[87] = 0; - out_4383751150204030030[88] = 0; - out_4383751150204030030[89] = 0; - out_4383751150204030030[90] = 0; - out_4383751150204030030[91] = 0; - out_4383751150204030030[92] = 0; - out_4383751150204030030[93] = 0; - out_4383751150204030030[94] = 0; - out_4383751150204030030[95] = 1; - out_4383751150204030030[96] = 0; - out_4383751150204030030[97] = 0; - out_4383751150204030030[98] = 0; - out_4383751150204030030[99] = 0; - out_4383751150204030030[100] = 0; - out_4383751150204030030[101] = 0; - out_4383751150204030030[102] = 0; - out_4383751150204030030[103] = 0; - out_4383751150204030030[104] = dt; - out_4383751150204030030[105] = 0; - out_4383751150204030030[106] = 0; - out_4383751150204030030[107] = 0; - out_4383751150204030030[108] = 0; - out_4383751150204030030[109] = 0; - out_4383751150204030030[110] = 0; - out_4383751150204030030[111] = 0; - out_4383751150204030030[112] = 0; - out_4383751150204030030[113] = 0; - out_4383751150204030030[114] = 1; - out_4383751150204030030[115] = 0; - out_4383751150204030030[116] = 0; - out_4383751150204030030[117] = 0; - out_4383751150204030030[118] = 0; - out_4383751150204030030[119] = 0; - out_4383751150204030030[120] = 0; - out_4383751150204030030[121] = 0; - out_4383751150204030030[122] = 0; - out_4383751150204030030[123] = 0; - out_4383751150204030030[124] = 0; - out_4383751150204030030[125] = 0; - out_4383751150204030030[126] = 0; - out_4383751150204030030[127] = 0; - out_4383751150204030030[128] = 0; - out_4383751150204030030[129] = 0; - out_4383751150204030030[130] = 0; - out_4383751150204030030[131] = 0; - out_4383751150204030030[132] = 0; - out_4383751150204030030[133] = 1; - out_4383751150204030030[134] = 0; - out_4383751150204030030[135] = 0; - out_4383751150204030030[136] = 0; - out_4383751150204030030[137] = 0; - out_4383751150204030030[138] = 0; - out_4383751150204030030[139] = 0; - out_4383751150204030030[140] = 0; - out_4383751150204030030[141] = 0; - out_4383751150204030030[142] = 0; - out_4383751150204030030[143] = 0; - out_4383751150204030030[144] = 0; - out_4383751150204030030[145] = 0; - out_4383751150204030030[146] = 0; - out_4383751150204030030[147] = 0; - out_4383751150204030030[148] = 0; - out_4383751150204030030[149] = 0; - out_4383751150204030030[150] = 0; - out_4383751150204030030[151] = 0; - out_4383751150204030030[152] = 1; - out_4383751150204030030[153] = 0; - out_4383751150204030030[154] = 0; - out_4383751150204030030[155] = 0; - out_4383751150204030030[156] = 0; - out_4383751150204030030[157] = 0; - out_4383751150204030030[158] = 0; - out_4383751150204030030[159] = 0; - out_4383751150204030030[160] = 0; - out_4383751150204030030[161] = 0; - out_4383751150204030030[162] = 0; - out_4383751150204030030[163] = 0; - out_4383751150204030030[164] = 0; - out_4383751150204030030[165] = 0; - out_4383751150204030030[166] = 0; - out_4383751150204030030[167] = 0; - out_4383751150204030030[168] = 0; - out_4383751150204030030[169] = 0; - out_4383751150204030030[170] = 0; - out_4383751150204030030[171] = 1; - out_4383751150204030030[172] = 0; - out_4383751150204030030[173] = 0; - out_4383751150204030030[174] = 0; - out_4383751150204030030[175] = 0; - out_4383751150204030030[176] = 0; - out_4383751150204030030[177] = 0; - out_4383751150204030030[178] = 0; - out_4383751150204030030[179] = 0; - out_4383751150204030030[180] = 0; - out_4383751150204030030[181] = 0; - out_4383751150204030030[182] = 0; - out_4383751150204030030[183] = 0; - out_4383751150204030030[184] = 0; - out_4383751150204030030[185] = 0; - out_4383751150204030030[186] = 0; - out_4383751150204030030[187] = 0; - out_4383751150204030030[188] = 0; - out_4383751150204030030[189] = 0; - out_4383751150204030030[190] = 1; - out_4383751150204030030[191] = 0; - out_4383751150204030030[192] = 0; - out_4383751150204030030[193] = 0; - out_4383751150204030030[194] = 0; - out_4383751150204030030[195] = 0; - out_4383751150204030030[196] = 0; - out_4383751150204030030[197] = 0; - out_4383751150204030030[198] = 0; - out_4383751150204030030[199] = 0; - out_4383751150204030030[200] = 0; - out_4383751150204030030[201] = 0; - out_4383751150204030030[202] = 0; - out_4383751150204030030[203] = 0; - out_4383751150204030030[204] = 0; - out_4383751150204030030[205] = 0; - out_4383751150204030030[206] = 0; - out_4383751150204030030[207] = 0; - out_4383751150204030030[208] = 0; - out_4383751150204030030[209] = 1; - out_4383751150204030030[210] = 0; - out_4383751150204030030[211] = 0; - out_4383751150204030030[212] = 0; - out_4383751150204030030[213] = 0; - out_4383751150204030030[214] = 0; - out_4383751150204030030[215] = 0; - out_4383751150204030030[216] = 0; - out_4383751150204030030[217] = 0; - out_4383751150204030030[218] = 0; - out_4383751150204030030[219] = 0; - out_4383751150204030030[220] = 0; - out_4383751150204030030[221] = 0; - out_4383751150204030030[222] = 0; - out_4383751150204030030[223] = 0; - out_4383751150204030030[224] = 0; - out_4383751150204030030[225] = 0; - out_4383751150204030030[226] = 0; - out_4383751150204030030[227] = 0; - out_4383751150204030030[228] = 1; - out_4383751150204030030[229] = 0; - out_4383751150204030030[230] = 0; - out_4383751150204030030[231] = 0; - out_4383751150204030030[232] = 0; - out_4383751150204030030[233] = 0; - out_4383751150204030030[234] = 0; - out_4383751150204030030[235] = 0; - out_4383751150204030030[236] = 0; - out_4383751150204030030[237] = 0; - out_4383751150204030030[238] = 0; - out_4383751150204030030[239] = 0; - out_4383751150204030030[240] = 0; - out_4383751150204030030[241] = 0; - out_4383751150204030030[242] = 0; - out_4383751150204030030[243] = 0; - out_4383751150204030030[244] = 0; - out_4383751150204030030[245] = 0; - out_4383751150204030030[246] = 0; - out_4383751150204030030[247] = 1; - out_4383751150204030030[248] = 0; - out_4383751150204030030[249] = 0; - out_4383751150204030030[250] = 0; - out_4383751150204030030[251] = 0; - out_4383751150204030030[252] = 0; - out_4383751150204030030[253] = 0; - out_4383751150204030030[254] = 0; - out_4383751150204030030[255] = 0; - out_4383751150204030030[256] = 0; - out_4383751150204030030[257] = 0; - out_4383751150204030030[258] = 0; - out_4383751150204030030[259] = 0; - out_4383751150204030030[260] = 0; - out_4383751150204030030[261] = 0; - out_4383751150204030030[262] = 0; - out_4383751150204030030[263] = 0; - out_4383751150204030030[264] = 0; - out_4383751150204030030[265] = 0; - out_4383751150204030030[266] = 1; - out_4383751150204030030[267] = 0; - out_4383751150204030030[268] = 0; - out_4383751150204030030[269] = 0; - out_4383751150204030030[270] = 0; - out_4383751150204030030[271] = 0; - out_4383751150204030030[272] = 0; - out_4383751150204030030[273] = 0; - out_4383751150204030030[274] = 0; - out_4383751150204030030[275] = 0; - out_4383751150204030030[276] = 0; - out_4383751150204030030[277] = 0; - out_4383751150204030030[278] = 0; - out_4383751150204030030[279] = 0; - out_4383751150204030030[280] = 0; - out_4383751150204030030[281] = 0; - out_4383751150204030030[282] = 0; - out_4383751150204030030[283] = 0; - out_4383751150204030030[284] = 0; - out_4383751150204030030[285] = 1; - out_4383751150204030030[286] = 0; - out_4383751150204030030[287] = 0; - out_4383751150204030030[288] = 0; - out_4383751150204030030[289] = 0; - out_4383751150204030030[290] = 0; - out_4383751150204030030[291] = 0; - out_4383751150204030030[292] = 0; - out_4383751150204030030[293] = 0; - out_4383751150204030030[294] = 0; - out_4383751150204030030[295] = 0; - out_4383751150204030030[296] = 0; - out_4383751150204030030[297] = 0; - out_4383751150204030030[298] = 0; - out_4383751150204030030[299] = 0; - out_4383751150204030030[300] = 0; - out_4383751150204030030[301] = 0; - out_4383751150204030030[302] = 0; - out_4383751150204030030[303] = 0; - out_4383751150204030030[304] = 1; - out_4383751150204030030[305] = 0; - out_4383751150204030030[306] = 0; - out_4383751150204030030[307] = 0; - out_4383751150204030030[308] = 0; - out_4383751150204030030[309] = 0; - out_4383751150204030030[310] = 0; - out_4383751150204030030[311] = 0; - out_4383751150204030030[312] = 0; - out_4383751150204030030[313] = 0; - out_4383751150204030030[314] = 0; - out_4383751150204030030[315] = 0; - out_4383751150204030030[316] = 0; - out_4383751150204030030[317] = 0; - out_4383751150204030030[318] = 0; - out_4383751150204030030[319] = 0; - out_4383751150204030030[320] = 0; - out_4383751150204030030[321] = 0; - out_4383751150204030030[322] = 0; - out_4383751150204030030[323] = 1; +void F_fun(double *state, double dt, double *out_2324287403941072643) { + out_2324287403941072643[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_2324287403941072643[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_2324287403941072643[2] = 0; + out_2324287403941072643[3] = 0; + out_2324287403941072643[4] = 0; + out_2324287403941072643[5] = 0; + out_2324287403941072643[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_2324287403941072643[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_2324287403941072643[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_2324287403941072643[9] = 0; + out_2324287403941072643[10] = 0; + out_2324287403941072643[11] = 0; + out_2324287403941072643[12] = 0; + out_2324287403941072643[13] = 0; + out_2324287403941072643[14] = 0; + out_2324287403941072643[15] = 0; + out_2324287403941072643[16] = 0; + out_2324287403941072643[17] = 0; + out_2324287403941072643[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_2324287403941072643[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_2324287403941072643[20] = 0; + out_2324287403941072643[21] = 0; + out_2324287403941072643[22] = 0; + out_2324287403941072643[23] = 0; + out_2324287403941072643[24] = 0; + out_2324287403941072643[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_2324287403941072643[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_2324287403941072643[27] = 0; + out_2324287403941072643[28] = 0; + out_2324287403941072643[29] = 0; + out_2324287403941072643[30] = 0; + out_2324287403941072643[31] = 0; + out_2324287403941072643[32] = 0; + out_2324287403941072643[33] = 0; + out_2324287403941072643[34] = 0; + out_2324287403941072643[35] = 0; + out_2324287403941072643[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_2324287403941072643[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_2324287403941072643[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_2324287403941072643[39] = 0; + out_2324287403941072643[40] = 0; + out_2324287403941072643[41] = 0; + out_2324287403941072643[42] = 0; + out_2324287403941072643[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_2324287403941072643[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_2324287403941072643[45] = 0; + out_2324287403941072643[46] = 0; + out_2324287403941072643[47] = 0; + out_2324287403941072643[48] = 0; + out_2324287403941072643[49] = 0; + out_2324287403941072643[50] = 0; + out_2324287403941072643[51] = 0; + out_2324287403941072643[52] = 0; + out_2324287403941072643[53] = 0; + out_2324287403941072643[54] = 0; + out_2324287403941072643[55] = 0; + out_2324287403941072643[56] = 0; + out_2324287403941072643[57] = 1; + out_2324287403941072643[58] = 0; + out_2324287403941072643[59] = 0; + out_2324287403941072643[60] = 0; + out_2324287403941072643[61] = 0; + out_2324287403941072643[62] = 0; + out_2324287403941072643[63] = 0; + out_2324287403941072643[64] = 0; + out_2324287403941072643[65] = 0; + out_2324287403941072643[66] = dt; + out_2324287403941072643[67] = 0; + out_2324287403941072643[68] = 0; + out_2324287403941072643[69] = 0; + out_2324287403941072643[70] = 0; + out_2324287403941072643[71] = 0; + out_2324287403941072643[72] = 0; + out_2324287403941072643[73] = 0; + out_2324287403941072643[74] = 0; + out_2324287403941072643[75] = 0; + out_2324287403941072643[76] = 1; + out_2324287403941072643[77] = 0; + out_2324287403941072643[78] = 0; + out_2324287403941072643[79] = 0; + out_2324287403941072643[80] = 0; + out_2324287403941072643[81] = 0; + out_2324287403941072643[82] = 0; + out_2324287403941072643[83] = 0; + out_2324287403941072643[84] = 0; + out_2324287403941072643[85] = dt; + out_2324287403941072643[86] = 0; + out_2324287403941072643[87] = 0; + out_2324287403941072643[88] = 0; + out_2324287403941072643[89] = 0; + out_2324287403941072643[90] = 0; + out_2324287403941072643[91] = 0; + out_2324287403941072643[92] = 0; + out_2324287403941072643[93] = 0; + out_2324287403941072643[94] = 0; + out_2324287403941072643[95] = 1; + out_2324287403941072643[96] = 0; + out_2324287403941072643[97] = 0; + out_2324287403941072643[98] = 0; + out_2324287403941072643[99] = 0; + out_2324287403941072643[100] = 0; + out_2324287403941072643[101] = 0; + out_2324287403941072643[102] = 0; + out_2324287403941072643[103] = 0; + out_2324287403941072643[104] = dt; + out_2324287403941072643[105] = 0; + out_2324287403941072643[106] = 0; + out_2324287403941072643[107] = 0; + out_2324287403941072643[108] = 0; + out_2324287403941072643[109] = 0; + out_2324287403941072643[110] = 0; + out_2324287403941072643[111] = 0; + out_2324287403941072643[112] = 0; + out_2324287403941072643[113] = 0; + out_2324287403941072643[114] = 1; + out_2324287403941072643[115] = 0; + out_2324287403941072643[116] = 0; + out_2324287403941072643[117] = 0; + out_2324287403941072643[118] = 0; + out_2324287403941072643[119] = 0; + out_2324287403941072643[120] = 0; + out_2324287403941072643[121] = 0; + out_2324287403941072643[122] = 0; + out_2324287403941072643[123] = 0; + out_2324287403941072643[124] = 0; + out_2324287403941072643[125] = 0; + out_2324287403941072643[126] = 0; + out_2324287403941072643[127] = 0; + out_2324287403941072643[128] = 0; + out_2324287403941072643[129] = 0; + out_2324287403941072643[130] = 0; + out_2324287403941072643[131] = 0; + out_2324287403941072643[132] = 0; + out_2324287403941072643[133] = 1; + out_2324287403941072643[134] = 0; + out_2324287403941072643[135] = 0; + out_2324287403941072643[136] = 0; + out_2324287403941072643[137] = 0; + out_2324287403941072643[138] = 0; + out_2324287403941072643[139] = 0; + out_2324287403941072643[140] = 0; + out_2324287403941072643[141] = 0; + out_2324287403941072643[142] = 0; + out_2324287403941072643[143] = 0; + out_2324287403941072643[144] = 0; + out_2324287403941072643[145] = 0; + out_2324287403941072643[146] = 0; + out_2324287403941072643[147] = 0; + out_2324287403941072643[148] = 0; + out_2324287403941072643[149] = 0; + out_2324287403941072643[150] = 0; + out_2324287403941072643[151] = 0; + out_2324287403941072643[152] = 1; + out_2324287403941072643[153] = 0; + out_2324287403941072643[154] = 0; + out_2324287403941072643[155] = 0; + out_2324287403941072643[156] = 0; + out_2324287403941072643[157] = 0; + out_2324287403941072643[158] = 0; + out_2324287403941072643[159] = 0; + out_2324287403941072643[160] = 0; + out_2324287403941072643[161] = 0; + out_2324287403941072643[162] = 0; + out_2324287403941072643[163] = 0; + out_2324287403941072643[164] = 0; + out_2324287403941072643[165] = 0; + out_2324287403941072643[166] = 0; + out_2324287403941072643[167] = 0; + out_2324287403941072643[168] = 0; + out_2324287403941072643[169] = 0; + out_2324287403941072643[170] = 0; + out_2324287403941072643[171] = 1; + out_2324287403941072643[172] = 0; + out_2324287403941072643[173] = 0; + out_2324287403941072643[174] = 0; + out_2324287403941072643[175] = 0; + out_2324287403941072643[176] = 0; + out_2324287403941072643[177] = 0; + out_2324287403941072643[178] = 0; + out_2324287403941072643[179] = 0; + out_2324287403941072643[180] = 0; + out_2324287403941072643[181] = 0; + out_2324287403941072643[182] = 0; + out_2324287403941072643[183] = 0; + out_2324287403941072643[184] = 0; + out_2324287403941072643[185] = 0; + out_2324287403941072643[186] = 0; + out_2324287403941072643[187] = 0; + out_2324287403941072643[188] = 0; + out_2324287403941072643[189] = 0; + out_2324287403941072643[190] = 1; + out_2324287403941072643[191] = 0; + out_2324287403941072643[192] = 0; + out_2324287403941072643[193] = 0; + out_2324287403941072643[194] = 0; + out_2324287403941072643[195] = 0; + out_2324287403941072643[196] = 0; + out_2324287403941072643[197] = 0; + out_2324287403941072643[198] = 0; + out_2324287403941072643[199] = 0; + out_2324287403941072643[200] = 0; + out_2324287403941072643[201] = 0; + out_2324287403941072643[202] = 0; + out_2324287403941072643[203] = 0; + out_2324287403941072643[204] = 0; + out_2324287403941072643[205] = 0; + out_2324287403941072643[206] = 0; + out_2324287403941072643[207] = 0; + out_2324287403941072643[208] = 0; + out_2324287403941072643[209] = 1; + out_2324287403941072643[210] = 0; + out_2324287403941072643[211] = 0; + out_2324287403941072643[212] = 0; + out_2324287403941072643[213] = 0; + out_2324287403941072643[214] = 0; + out_2324287403941072643[215] = 0; + out_2324287403941072643[216] = 0; + out_2324287403941072643[217] = 0; + out_2324287403941072643[218] = 0; + out_2324287403941072643[219] = 0; + out_2324287403941072643[220] = 0; + out_2324287403941072643[221] = 0; + out_2324287403941072643[222] = 0; + out_2324287403941072643[223] = 0; + out_2324287403941072643[224] = 0; + out_2324287403941072643[225] = 0; + out_2324287403941072643[226] = 0; + out_2324287403941072643[227] = 0; + out_2324287403941072643[228] = 1; + out_2324287403941072643[229] = 0; + out_2324287403941072643[230] = 0; + out_2324287403941072643[231] = 0; + out_2324287403941072643[232] = 0; + out_2324287403941072643[233] = 0; + out_2324287403941072643[234] = 0; + out_2324287403941072643[235] = 0; + out_2324287403941072643[236] = 0; + out_2324287403941072643[237] = 0; + out_2324287403941072643[238] = 0; + out_2324287403941072643[239] = 0; + out_2324287403941072643[240] = 0; + out_2324287403941072643[241] = 0; + out_2324287403941072643[242] = 0; + out_2324287403941072643[243] = 0; + out_2324287403941072643[244] = 0; + out_2324287403941072643[245] = 0; + out_2324287403941072643[246] = 0; + out_2324287403941072643[247] = 1; + out_2324287403941072643[248] = 0; + out_2324287403941072643[249] = 0; + out_2324287403941072643[250] = 0; + out_2324287403941072643[251] = 0; + out_2324287403941072643[252] = 0; + out_2324287403941072643[253] = 0; + out_2324287403941072643[254] = 0; + out_2324287403941072643[255] = 0; + out_2324287403941072643[256] = 0; + out_2324287403941072643[257] = 0; + out_2324287403941072643[258] = 0; + out_2324287403941072643[259] = 0; + out_2324287403941072643[260] = 0; + out_2324287403941072643[261] = 0; + out_2324287403941072643[262] = 0; + out_2324287403941072643[263] = 0; + out_2324287403941072643[264] = 0; + out_2324287403941072643[265] = 0; + out_2324287403941072643[266] = 1; + out_2324287403941072643[267] = 0; + out_2324287403941072643[268] = 0; + out_2324287403941072643[269] = 0; + out_2324287403941072643[270] = 0; + out_2324287403941072643[271] = 0; + out_2324287403941072643[272] = 0; + out_2324287403941072643[273] = 0; + out_2324287403941072643[274] = 0; + out_2324287403941072643[275] = 0; + out_2324287403941072643[276] = 0; + out_2324287403941072643[277] = 0; + out_2324287403941072643[278] = 0; + out_2324287403941072643[279] = 0; + out_2324287403941072643[280] = 0; + out_2324287403941072643[281] = 0; + out_2324287403941072643[282] = 0; + out_2324287403941072643[283] = 0; + out_2324287403941072643[284] = 0; + out_2324287403941072643[285] = 1; + out_2324287403941072643[286] = 0; + out_2324287403941072643[287] = 0; + out_2324287403941072643[288] = 0; + out_2324287403941072643[289] = 0; + out_2324287403941072643[290] = 0; + out_2324287403941072643[291] = 0; + out_2324287403941072643[292] = 0; + out_2324287403941072643[293] = 0; + out_2324287403941072643[294] = 0; + out_2324287403941072643[295] = 0; + out_2324287403941072643[296] = 0; + out_2324287403941072643[297] = 0; + out_2324287403941072643[298] = 0; + out_2324287403941072643[299] = 0; + out_2324287403941072643[300] = 0; + out_2324287403941072643[301] = 0; + out_2324287403941072643[302] = 0; + out_2324287403941072643[303] = 0; + out_2324287403941072643[304] = 1; + out_2324287403941072643[305] = 0; + out_2324287403941072643[306] = 0; + out_2324287403941072643[307] = 0; + out_2324287403941072643[308] = 0; + out_2324287403941072643[309] = 0; + out_2324287403941072643[310] = 0; + out_2324287403941072643[311] = 0; + out_2324287403941072643[312] = 0; + out_2324287403941072643[313] = 0; + out_2324287403941072643[314] = 0; + out_2324287403941072643[315] = 0; + out_2324287403941072643[316] = 0; + out_2324287403941072643[317] = 0; + out_2324287403941072643[318] = 0; + out_2324287403941072643[319] = 0; + out_2324287403941072643[320] = 0; + out_2324287403941072643[321] = 0; + out_2324287403941072643[322] = 0; + out_2324287403941072643[323] = 1; } -void h_4(double *state, double *unused, double *out_878843833946471253) { - out_878843833946471253[0] = state[6] + state[9]; - out_878843833946471253[1] = state[7] + state[10]; - out_878843833946471253[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_3295127809911230665) { + out_3295127809911230665[0] = state[6] + state[9]; + out_3295127809911230665[1] = state[7] + state[10]; + out_3295127809911230665[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_7970215192388091303) { - out_7970215192388091303[0] = 0; - out_7970215192388091303[1] = 0; - out_7970215192388091303[2] = 0; - out_7970215192388091303[3] = 0; - out_7970215192388091303[4] = 0; - out_7970215192388091303[5] = 0; - out_7970215192388091303[6] = 1; - out_7970215192388091303[7] = 0; - out_7970215192388091303[8] = 0; - out_7970215192388091303[9] = 1; - out_7970215192388091303[10] = 0; - out_7970215192388091303[11] = 0; - out_7970215192388091303[12] = 0; - out_7970215192388091303[13] = 0; - out_7970215192388091303[14] = 0; - out_7970215192388091303[15] = 0; - out_7970215192388091303[16] = 0; - out_7970215192388091303[17] = 0; - out_7970215192388091303[18] = 0; - out_7970215192388091303[19] = 0; - out_7970215192388091303[20] = 0; - out_7970215192388091303[21] = 0; - out_7970215192388091303[22] = 0; - out_7970215192388091303[23] = 0; - out_7970215192388091303[24] = 0; - out_7970215192388091303[25] = 1; - out_7970215192388091303[26] = 0; - out_7970215192388091303[27] = 0; - out_7970215192388091303[28] = 1; - out_7970215192388091303[29] = 0; - out_7970215192388091303[30] = 0; - out_7970215192388091303[31] = 0; - out_7970215192388091303[32] = 0; - out_7970215192388091303[33] = 0; - out_7970215192388091303[34] = 0; - out_7970215192388091303[35] = 0; - out_7970215192388091303[36] = 0; - out_7970215192388091303[37] = 0; - out_7970215192388091303[38] = 0; - out_7970215192388091303[39] = 0; - out_7970215192388091303[40] = 0; - out_7970215192388091303[41] = 0; - out_7970215192388091303[42] = 0; - out_7970215192388091303[43] = 0; - out_7970215192388091303[44] = 1; - out_7970215192388091303[45] = 0; - out_7970215192388091303[46] = 0; - out_7970215192388091303[47] = 1; - out_7970215192388091303[48] = 0; - out_7970215192388091303[49] = 0; - out_7970215192388091303[50] = 0; - out_7970215192388091303[51] = 0; - out_7970215192388091303[52] = 0; - out_7970215192388091303[53] = 0; +void H_4(double *state, double *unused, double *out_8270265206520041021) { + out_8270265206520041021[0] = 0; + out_8270265206520041021[1] = 0; + out_8270265206520041021[2] = 0; + out_8270265206520041021[3] = 0; + out_8270265206520041021[4] = 0; + out_8270265206520041021[5] = 0; + out_8270265206520041021[6] = 1; + out_8270265206520041021[7] = 0; + out_8270265206520041021[8] = 0; + out_8270265206520041021[9] = 1; + out_8270265206520041021[10] = 0; + out_8270265206520041021[11] = 0; + out_8270265206520041021[12] = 0; + out_8270265206520041021[13] = 0; + out_8270265206520041021[14] = 0; + out_8270265206520041021[15] = 0; + out_8270265206520041021[16] = 0; + out_8270265206520041021[17] = 0; + out_8270265206520041021[18] = 0; + out_8270265206520041021[19] = 0; + out_8270265206520041021[20] = 0; + out_8270265206520041021[21] = 0; + out_8270265206520041021[22] = 0; + out_8270265206520041021[23] = 0; + out_8270265206520041021[24] = 0; + out_8270265206520041021[25] = 1; + out_8270265206520041021[26] = 0; + out_8270265206520041021[27] = 0; + out_8270265206520041021[28] = 1; + out_8270265206520041021[29] = 0; + out_8270265206520041021[30] = 0; + out_8270265206520041021[31] = 0; + out_8270265206520041021[32] = 0; + out_8270265206520041021[33] = 0; + out_8270265206520041021[34] = 0; + out_8270265206520041021[35] = 0; + out_8270265206520041021[36] = 0; + out_8270265206520041021[37] = 0; + out_8270265206520041021[38] = 0; + out_8270265206520041021[39] = 0; + out_8270265206520041021[40] = 0; + out_8270265206520041021[41] = 0; + out_8270265206520041021[42] = 0; + out_8270265206520041021[43] = 0; + out_8270265206520041021[44] = 1; + out_8270265206520041021[45] = 0; + out_8270265206520041021[46] = 0; + out_8270265206520041021[47] = 1; + out_8270265206520041021[48] = 0; + out_8270265206520041021[49] = 0; + out_8270265206520041021[50] = 0; + out_8270265206520041021[51] = 0; + out_8270265206520041021[52] = 0; + out_8270265206520041021[53] = 0; } -void h_10(double *state, double *unused, double *out_1191517594273352576) { - out_1191517594273352576[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_1191517594273352576[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_1191517594273352576[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_3863591942336910664) { + out_3863591942336910664[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_3863591942336910664[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_3863591942336910664[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_7914634342859917414) { - out_7914634342859917414[0] = 0; - out_7914634342859917414[1] = 9.8100000000000005*cos(state[1]); - out_7914634342859917414[2] = 0; - out_7914634342859917414[3] = 0; - out_7914634342859917414[4] = -state[8]; - out_7914634342859917414[5] = state[7]; - out_7914634342859917414[6] = 0; - out_7914634342859917414[7] = state[5]; - out_7914634342859917414[8] = -state[4]; - out_7914634342859917414[9] = 0; - out_7914634342859917414[10] = 0; - out_7914634342859917414[11] = 0; - out_7914634342859917414[12] = 1; - out_7914634342859917414[13] = 0; - out_7914634342859917414[14] = 0; - out_7914634342859917414[15] = 1; - out_7914634342859917414[16] = 0; - out_7914634342859917414[17] = 0; - out_7914634342859917414[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_7914634342859917414[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_7914634342859917414[20] = 0; - out_7914634342859917414[21] = state[8]; - out_7914634342859917414[22] = 0; - out_7914634342859917414[23] = -state[6]; - out_7914634342859917414[24] = -state[5]; - out_7914634342859917414[25] = 0; - out_7914634342859917414[26] = state[3]; - out_7914634342859917414[27] = 0; - out_7914634342859917414[28] = 0; - out_7914634342859917414[29] = 0; - out_7914634342859917414[30] = 0; - out_7914634342859917414[31] = 1; - out_7914634342859917414[32] = 0; - out_7914634342859917414[33] = 0; - out_7914634342859917414[34] = 1; - out_7914634342859917414[35] = 0; - out_7914634342859917414[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_7914634342859917414[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_7914634342859917414[38] = 0; - out_7914634342859917414[39] = -state[7]; - out_7914634342859917414[40] = state[6]; - out_7914634342859917414[41] = 0; - out_7914634342859917414[42] = state[4]; - out_7914634342859917414[43] = -state[3]; - out_7914634342859917414[44] = 0; - out_7914634342859917414[45] = 0; - out_7914634342859917414[46] = 0; - out_7914634342859917414[47] = 0; - out_7914634342859917414[48] = 0; - out_7914634342859917414[49] = 0; - out_7914634342859917414[50] = 1; - out_7914634342859917414[51] = 0; - out_7914634342859917414[52] = 0; - out_7914634342859917414[53] = 1; +void H_10(double *state, double *unused, double *out_6343727255885996060) { + out_6343727255885996060[0] = 0; + out_6343727255885996060[1] = 9.8100000000000005*cos(state[1]); + out_6343727255885996060[2] = 0; + out_6343727255885996060[3] = 0; + out_6343727255885996060[4] = -state[8]; + out_6343727255885996060[5] = state[7]; + out_6343727255885996060[6] = 0; + out_6343727255885996060[7] = state[5]; + out_6343727255885996060[8] = -state[4]; + out_6343727255885996060[9] = 0; + out_6343727255885996060[10] = 0; + out_6343727255885996060[11] = 0; + out_6343727255885996060[12] = 1; + out_6343727255885996060[13] = 0; + out_6343727255885996060[14] = 0; + out_6343727255885996060[15] = 1; + out_6343727255885996060[16] = 0; + out_6343727255885996060[17] = 0; + out_6343727255885996060[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_6343727255885996060[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_6343727255885996060[20] = 0; + out_6343727255885996060[21] = state[8]; + out_6343727255885996060[22] = 0; + out_6343727255885996060[23] = -state[6]; + out_6343727255885996060[24] = -state[5]; + out_6343727255885996060[25] = 0; + out_6343727255885996060[26] = state[3]; + out_6343727255885996060[27] = 0; + out_6343727255885996060[28] = 0; + out_6343727255885996060[29] = 0; + out_6343727255885996060[30] = 0; + out_6343727255885996060[31] = 1; + out_6343727255885996060[32] = 0; + out_6343727255885996060[33] = 0; + out_6343727255885996060[34] = 1; + out_6343727255885996060[35] = 0; + out_6343727255885996060[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_6343727255885996060[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_6343727255885996060[38] = 0; + out_6343727255885996060[39] = -state[7]; + out_6343727255885996060[40] = state[6]; + out_6343727255885996060[41] = 0; + out_6343727255885996060[42] = state[4]; + out_6343727255885996060[43] = -state[3]; + out_6343727255885996060[44] = 0; + out_6343727255885996060[45] = 0; + out_6343727255885996060[46] = 0; + out_6343727255885996060[47] = 0; + out_6343727255885996060[48] = 0; + out_6343727255885996060[49] = 0; + out_6343727255885996060[50] = 1; + out_6343727255885996060[51] = 0; + out_6343727255885996060[52] = 0; + out_6343727255885996060[53] = 1; } -void h_13(double *state, double *unused, double *out_5300060524255047032) { - out_5300060524255047032[0] = state[3]; - out_5300060524255047032[1] = state[4]; - out_5300060524255047032[2] = state[5]; +void h_13(double *state, double *unused, double *out_3880954183827223653) { + out_3880954183827223653[0] = state[3]; + out_3880954183827223653[1] = state[4]; + out_3880954183827223653[2] = state[5]; } -void H_13(double *state, double *unused, double *out_7264255055989127512) { - out_7264255055989127512[0] = 0; - out_7264255055989127512[1] = 0; - out_7264255055989127512[2] = 0; - out_7264255055989127512[3] = 1; - out_7264255055989127512[4] = 0; - out_7264255055989127512[5] = 0; - out_7264255055989127512[6] = 0; - out_7264255055989127512[7] = 0; - out_7264255055989127512[8] = 0; - out_7264255055989127512[9] = 0; - out_7264255055989127512[10] = 0; - out_7264255055989127512[11] = 0; - out_7264255055989127512[12] = 0; - out_7264255055989127512[13] = 0; - out_7264255055989127512[14] = 0; - out_7264255055989127512[15] = 0; - out_7264255055989127512[16] = 0; - out_7264255055989127512[17] = 0; - out_7264255055989127512[18] = 0; - out_7264255055989127512[19] = 0; - out_7264255055989127512[20] = 0; - out_7264255055989127512[21] = 0; - out_7264255055989127512[22] = 1; - out_7264255055989127512[23] = 0; - out_7264255055989127512[24] = 0; - out_7264255055989127512[25] = 0; - out_7264255055989127512[26] = 0; - out_7264255055989127512[27] = 0; - out_7264255055989127512[28] = 0; - out_7264255055989127512[29] = 0; - out_7264255055989127512[30] = 0; - out_7264255055989127512[31] = 0; - out_7264255055989127512[32] = 0; - out_7264255055989127512[33] = 0; - out_7264255055989127512[34] = 0; - out_7264255055989127512[35] = 0; - out_7264255055989127512[36] = 0; - out_7264255055989127512[37] = 0; - out_7264255055989127512[38] = 0; - out_7264255055989127512[39] = 0; - out_7264255055989127512[40] = 0; - out_7264255055989127512[41] = 1; - out_7264255055989127512[42] = 0; - out_7264255055989127512[43] = 0; - out_7264255055989127512[44] = 0; - out_7264255055989127512[45] = 0; - out_7264255055989127512[46] = 0; - out_7264255055989127512[47] = 0; - out_7264255055989127512[48] = 0; - out_7264255055989127512[49] = 0; - out_7264255055989127512[50] = 0; - out_7264255055989127512[51] = 0; - out_7264255055989127512[52] = 0; - out_7264255055989127512[53] = 0; +void H_13(double *state, double *unused, double *out_6964205041857177794) { + out_6964205041857177794[0] = 0; + out_6964205041857177794[1] = 0; + out_6964205041857177794[2] = 0; + out_6964205041857177794[3] = 1; + out_6964205041857177794[4] = 0; + out_6964205041857177794[5] = 0; + out_6964205041857177794[6] = 0; + out_6964205041857177794[7] = 0; + out_6964205041857177794[8] = 0; + out_6964205041857177794[9] = 0; + out_6964205041857177794[10] = 0; + out_6964205041857177794[11] = 0; + out_6964205041857177794[12] = 0; + out_6964205041857177794[13] = 0; + out_6964205041857177794[14] = 0; + out_6964205041857177794[15] = 0; + out_6964205041857177794[16] = 0; + out_6964205041857177794[17] = 0; + out_6964205041857177794[18] = 0; + out_6964205041857177794[19] = 0; + out_6964205041857177794[20] = 0; + out_6964205041857177794[21] = 0; + out_6964205041857177794[22] = 1; + out_6964205041857177794[23] = 0; + out_6964205041857177794[24] = 0; + out_6964205041857177794[25] = 0; + out_6964205041857177794[26] = 0; + out_6964205041857177794[27] = 0; + out_6964205041857177794[28] = 0; + out_6964205041857177794[29] = 0; + out_6964205041857177794[30] = 0; + out_6964205041857177794[31] = 0; + out_6964205041857177794[32] = 0; + out_6964205041857177794[33] = 0; + out_6964205041857177794[34] = 0; + out_6964205041857177794[35] = 0; + out_6964205041857177794[36] = 0; + out_6964205041857177794[37] = 0; + out_6964205041857177794[38] = 0; + out_6964205041857177794[39] = 0; + out_6964205041857177794[40] = 0; + out_6964205041857177794[41] = 1; + out_6964205041857177794[42] = 0; + out_6964205041857177794[43] = 0; + out_6964205041857177794[44] = 0; + out_6964205041857177794[45] = 0; + out_6964205041857177794[46] = 0; + out_6964205041857177794[47] = 0; + out_6964205041857177794[48] = 0; + out_6964205041857177794[49] = 0; + out_6964205041857177794[50] = 0; + out_6964205041857177794[51] = 0; + out_6964205041857177794[52] = 0; + out_6964205041857177794[53] = 0; } -void h_14(double *state, double *unused, double *out_3965460404426586337) { - out_3965460404426586337[0] = state[6]; - out_3965460404426586337[1] = state[7]; - out_3965460404426586337[2] = state[8]; +void h_14(double *state, double *unused, double *out_8209348319732836111) { + out_8209348319732836111[0] = state[6]; + out_8209348319732836111[1] = state[7]; + out_8209348319732836111[2] = state[8]; } -void H_14(double *state, double *unused, double *out_6513288024981975784) { - out_6513288024981975784[0] = 0; - out_6513288024981975784[1] = 0; - out_6513288024981975784[2] = 0; - out_6513288024981975784[3] = 0; - out_6513288024981975784[4] = 0; - out_6513288024981975784[5] = 0; - out_6513288024981975784[6] = 1; - out_6513288024981975784[7] = 0; - out_6513288024981975784[8] = 0; - out_6513288024981975784[9] = 0; - out_6513288024981975784[10] = 0; - out_6513288024981975784[11] = 0; - out_6513288024981975784[12] = 0; - out_6513288024981975784[13] = 0; - out_6513288024981975784[14] = 0; - out_6513288024981975784[15] = 0; - out_6513288024981975784[16] = 0; - out_6513288024981975784[17] = 0; - out_6513288024981975784[18] = 0; - out_6513288024981975784[19] = 0; - out_6513288024981975784[20] = 0; - out_6513288024981975784[21] = 0; - out_6513288024981975784[22] = 0; - out_6513288024981975784[23] = 0; - out_6513288024981975784[24] = 0; - out_6513288024981975784[25] = 1; - out_6513288024981975784[26] = 0; - out_6513288024981975784[27] = 0; - out_6513288024981975784[28] = 0; - out_6513288024981975784[29] = 0; - out_6513288024981975784[30] = 0; - out_6513288024981975784[31] = 0; - out_6513288024981975784[32] = 0; - out_6513288024981975784[33] = 0; - out_6513288024981975784[34] = 0; - out_6513288024981975784[35] = 0; - out_6513288024981975784[36] = 0; - out_6513288024981975784[37] = 0; - out_6513288024981975784[38] = 0; - out_6513288024981975784[39] = 0; - out_6513288024981975784[40] = 0; - out_6513288024981975784[41] = 0; - out_6513288024981975784[42] = 0; - out_6513288024981975784[43] = 0; - out_6513288024981975784[44] = 1; - out_6513288024981975784[45] = 0; - out_6513288024981975784[46] = 0; - out_6513288024981975784[47] = 0; - out_6513288024981975784[48] = 0; - out_6513288024981975784[49] = 0; - out_6513288024981975784[50] = 0; - out_6513288024981975784[51] = 0; - out_6513288024981975784[52] = 0; - out_6513288024981975784[53] = 0; +void H_14(double *state, double *unused, double *out_5187476774224668725) { + out_5187476774224668725[0] = 0; + out_5187476774224668725[1] = 0; + out_5187476774224668725[2] = 0; + out_5187476774224668725[3] = 0; + out_5187476774224668725[4] = 0; + out_5187476774224668725[5] = 0; + out_5187476774224668725[6] = 1; + out_5187476774224668725[7] = 0; + out_5187476774224668725[8] = 0; + out_5187476774224668725[9] = 0; + out_5187476774224668725[10] = 0; + out_5187476774224668725[11] = 0; + out_5187476774224668725[12] = 0; + out_5187476774224668725[13] = 0; + out_5187476774224668725[14] = 0; + out_5187476774224668725[15] = 0; + out_5187476774224668725[16] = 0; + out_5187476774224668725[17] = 0; + out_5187476774224668725[18] = 0; + out_5187476774224668725[19] = 0; + out_5187476774224668725[20] = 0; + out_5187476774224668725[21] = 0; + out_5187476774224668725[22] = 0; + out_5187476774224668725[23] = 0; + out_5187476774224668725[24] = 0; + out_5187476774224668725[25] = 1; + out_5187476774224668725[26] = 0; + out_5187476774224668725[27] = 0; + out_5187476774224668725[28] = 0; + out_5187476774224668725[29] = 0; + out_5187476774224668725[30] = 0; + out_5187476774224668725[31] = 0; + out_5187476774224668725[32] = 0; + out_5187476774224668725[33] = 0; + out_5187476774224668725[34] = 0; + out_5187476774224668725[35] = 0; + out_5187476774224668725[36] = 0; + out_5187476774224668725[37] = 0; + out_5187476774224668725[38] = 0; + out_5187476774224668725[39] = 0; + out_5187476774224668725[40] = 0; + out_5187476774224668725[41] = 0; + out_5187476774224668725[42] = 0; + out_5187476774224668725[43] = 0; + out_5187476774224668725[44] = 1; + out_5187476774224668725[45] = 0; + out_5187476774224668725[46] = 0; + out_5187476774224668725[47] = 0; + out_5187476774224668725[48] = 0; + out_5187476774224668725[49] = 0; + out_5187476774224668725[50] = 0; + out_5187476774224668725[51] = 0; + out_5187476774224668725[52] = 0; + out_5187476774224668725[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_8207359146711228947) { - err_fun(nom_x, delta_x, out_8207359146711228947); +void pose_err_fun(double *nom_x, double *delta_x, double *out_6169503642516981710) { + err_fun(nom_x, delta_x, out_6169503642516981710); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8369219426901598353) { - inv_err_fun(nom_x, true_x, out_8369219426901598353); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2781558835044378693) { + inv_err_fun(nom_x, true_x, out_2781558835044378693); } -void pose_H_mod_fun(double *state, double *out_7202306802530511060) { - H_mod_fun(state, out_7202306802530511060); +void pose_H_mod_fun(double *state, double *out_4498457996676133449) { + H_mod_fun(state, out_4498457996676133449); } -void pose_f_fun(double *state, double dt, double *out_4961001892384611064) { - f_fun(state, dt, out_4961001892384611064); +void pose_f_fun(double *state, double dt, double *out_3471362747861957887) { + f_fun(state, dt, out_3471362747861957887); } -void pose_F_fun(double *state, double dt, double *out_4383751150204030030) { - F_fun(state, dt, out_4383751150204030030); +void pose_F_fun(double *state, double dt, double *out_2324287403941072643) { + F_fun(state, dt, out_2324287403941072643); } -void pose_h_4(double *state, double *unused, double *out_878843833946471253) { - h_4(state, unused, out_878843833946471253); +void pose_h_4(double *state, double *unused, double *out_3295127809911230665) { + h_4(state, unused, out_3295127809911230665); } -void pose_H_4(double *state, double *unused, double *out_7970215192388091303) { - H_4(state, unused, out_7970215192388091303); +void pose_H_4(double *state, double *unused, double *out_8270265206520041021) { + H_4(state, unused, out_8270265206520041021); } -void pose_h_10(double *state, double *unused, double *out_1191517594273352576) { - h_10(state, unused, out_1191517594273352576); +void pose_h_10(double *state, double *unused, double *out_3863591942336910664) { + h_10(state, unused, out_3863591942336910664); } -void pose_H_10(double *state, double *unused, double *out_7914634342859917414) { - H_10(state, unused, out_7914634342859917414); +void pose_H_10(double *state, double *unused, double *out_6343727255885996060) { + H_10(state, unused, out_6343727255885996060); } -void pose_h_13(double *state, double *unused, double *out_5300060524255047032) { - h_13(state, unused, out_5300060524255047032); +void pose_h_13(double *state, double *unused, double *out_3880954183827223653) { + h_13(state, unused, out_3880954183827223653); } -void pose_H_13(double *state, double *unused, double *out_7264255055989127512) { - H_13(state, unused, out_7264255055989127512); +void pose_H_13(double *state, double *unused, double *out_6964205041857177794) { + H_13(state, unused, out_6964205041857177794); } -void pose_h_14(double *state, double *unused, double *out_3965460404426586337) { - h_14(state, unused, out_3965460404426586337); +void pose_h_14(double *state, double *unused, double *out_8209348319732836111) { + h_14(state, unused, out_8209348319732836111); } -void pose_H_14(double *state, double *unused, double *out_6513288024981975784) { - H_14(state, unused, out_6513288024981975784); +void pose_H_14(double *state, double *unused, double *out_5187476774224668725) { + H_14(state, unused, out_5187476774224668725); } 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 106fa2f23..2d797114d 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_8207359146711228947); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8369219426901598353); -void pose_H_mod_fun(double *state, double *out_7202306802530511060); -void pose_f_fun(double *state, double dt, double *out_4961001892384611064); -void pose_F_fun(double *state, double dt, double *out_4383751150204030030); -void pose_h_4(double *state, double *unused, double *out_878843833946471253); -void pose_H_4(double *state, double *unused, double *out_7970215192388091303); -void pose_h_10(double *state, double *unused, double *out_1191517594273352576); -void pose_H_10(double *state, double *unused, double *out_7914634342859917414); -void pose_h_13(double *state, double *unused, double *out_5300060524255047032); -void pose_H_13(double *state, double *unused, double *out_7264255055989127512); -void pose_h_14(double *state, double *unused, double *out_3965460404426586337); -void pose_H_14(double *state, double *unused, double *out_6513288024981975784); +void pose_err_fun(double *nom_x, double *delta_x, double *out_6169503642516981710); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_2781558835044378693); +void pose_H_mod_fun(double *state, double *out_4498457996676133449); +void pose_f_fun(double *state, double dt, double *out_3471362747861957887); +void pose_F_fun(double *state, double dt, double *out_2324287403941072643); +void pose_h_4(double *state, double *unused, double *out_3295127809911230665); +void pose_H_4(double *state, double *unused, double *out_8270265206520041021); +void pose_h_10(double *state, double *unused, double *out_3863591942336910664); +void pose_H_10(double *state, double *unused, double *out_6343727255885996060); +void pose_h_13(double *state, double *unused, double *out_3880954183827223653); +void pose_H_13(double *state, double *unused, double *out_6964205041857177794); +void pose_h_14(double *state, double *unused, double *out_8209348319732836111); +void pose_H_14(double *state, double *unused, double *out_5187476774224668725); 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 f5726b822..d2ae9e694 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 8635e897e..5e348f461 100644 --- a/selfdrive/ui/layouts/settings/starpilot/longitudinal.py +++ b/selfdrive/ui/layouts/settings/starpilot/longitudinal.py @@ -357,7 +357,7 @@ class StarPilotLongitudinalTuneLayout(_SettingsPage): set_state=lambda s: self._params.put_bool("HumanAcceleration", s), visible=self._longitudinal_enabled), SettingRow("CoastUpToLeads", "toggle", tr_noop("Coast Up To Leads"), - subtitle=tr_noop("Briefly coast toward far leads before applying normal throttle again."), + subtitle=tr_noop("Keep optional far-lead comfort logic on. Recommended unless your car shows lead-follow stutter or springy gas/brake behavior."), get_state=lambda: self._params.get_bool("CoastUpToLeads"), set_state=lambda s: self._params.put_bool("CoastUpToLeads", s), visible=self._longitudinal_enabled), diff --git a/selfdrive/ui/mici/onroad/augmented_road_view.py b/selfdrive/ui/mici/onroad/augmented_road_view.py index e1221e57f..90bb39fc0 100644 --- a/selfdrive/ui/mici/onroad/augmented_road_view.py +++ b/selfdrive/ui/mici/onroad/augmented_road_view.py @@ -414,8 +414,8 @@ class AugmentedRoadView(CameraView): self._offroad_label.set_text("start the car to\nuse openpilot") def _handle_mouse_release(self, mouse_pos: MousePos): - # Don't trigger click callback if bookmark was triggered - if not self._bookmark_icon.interacting(): + # Don't trigger click callback if bookmark or HUD widgets consumed the tap. + if not self._bookmark_icon.interacting() and not self._hud_renderer.user_interacting(): super()._handle_mouse_release(mouse_pos) def _render(self, _): diff --git a/selfdrive/ui/mici/onroad/hud_renderer.py b/selfdrive/ui/mici/onroad/hud_renderer.py index a2b37ab5f..832b6a999 100644 --- a/selfdrive/ui/mici/onroad/hud_renderer.py +++ b/selfdrive/ui/mici/onroad/hud_renderer.py @@ -260,6 +260,9 @@ class HudRenderer(Widget): self._draw_steering_wheel(self._rect) self._draw_speed_limit_prompt(self._rect) + def user_interacting(self) -> bool: + return self._navigation_card.is_pressed + def _render(self, rect: rl.Rectangle) -> None: """Render HUD elements to the screen.""" self.prepare(rect) diff --git a/selfdrive/ui/onroad/hud_renderer.py b/selfdrive/ui/onroad/hud_renderer.py index 307bf9cec..e2f77df9c 100644 --- a/selfdrive/ui/onroad/hud_renderer.py +++ b/selfdrive/ui/onroad/hud_renderer.py @@ -128,7 +128,7 @@ class HudRenderer(Widget): self._exp_button.render(rl.Rectangle(button_x, button_y, UI_CONFIG.button_size, UI_CONFIG.button_size)) def user_interacting(self) -> bool: - return self._exp_button.is_pressed + return self._exp_button.is_pressed or self._navigation_card.is_pressed def _draw_set_speed(self, rect: rl.Rectangle) -> None: """Draw the MAX speed indicator box.""" diff --git a/selfdrive/ui/onroad/starpilot/navigation_card.py b/selfdrive/ui/onroad/starpilot/navigation_card.py index f8446d9b2..501220d4b 100644 --- a/selfdrive/ui/onroad/starpilot/navigation_card.py +++ b/selfdrive/ui/onroad/starpilot/navigation_card.py @@ -6,6 +6,7 @@ from pathlib import Path import pyray as rl +from openpilot.common.params import UnknownKeyName from openpilot.selfdrive.ui.ui_state import ui_state from openpilot.system.ui.lib.application import FontWeight, gui_app from openpilot.system.ui.lib.text_measure import measure_text_cached @@ -71,6 +72,31 @@ class NavigationCardRenderer(Widget): self._has_next = False self._next_maneuver_type = "turn" self._next_modifier = "straight" + self._collapsed = False + self._collapsed_fallback = False + self._collapsed_param_supported: bool | None = None + self._interactive_rect = rl.Rectangle(0, 0, 0, 0) + self._click_delay = 0.15 + + self.set_click_callback(self._toggle_collapsed) + + @property + def _hit_rect(self) -> rl.Rectangle: + if self._interactive_rect.width > 0 and self._interactive_rect.height > 0: + return self._interactive_rect + return rl.Rectangle(-1, -1, 0, 0) + + def _toggle_collapsed(self) -> None: + if not self._valid: + return + new_state = not self._collapsed + try: + ui_state.params_memory.put_bool("NavInstructionCollapsed", new_state) + self._collapsed_param_supported = True + except UnknownKeyName: + self._collapsed_param_supported = False + self._collapsed_fallback = new_state + self._collapsed = new_state def _icon_filename(self, maneuver_type: str, modifier: str) -> str: normalized_type = _normalize_maneuver_type(maneuver_type) @@ -98,6 +124,7 @@ class NavigationCardRenderer(Widget): def _update_state(self) -> None: self._enabled = ui_state.params.get_bool("NavigationUI") self._valid = False + self._interactive_rect = rl.Rectangle(0, 0, 0, 0) if not self._enabled: return @@ -127,6 +154,10 @@ class NavigationCardRenderer(Widget): self._next_maneuver_type = str(nav_state.get("nextManeuverType") or "turn") self._next_modifier = str(nav_state.get("nextManeuverModifier") or "straight") self._has_next = bool(nav_state.get("nextManeuverType") or nav_state.get("nextManeuverModifier")) + if self._collapsed_param_supported is False: + self._collapsed = self._collapsed_fallback + else: + self._collapsed = ui_state.params_memory.get_bool("NavInstructionCollapsed") self._valid = True @@ -220,6 +251,7 @@ class NavigationCardRenderer(Widget): distance_font_size = 48 if container_height == 225 else 40 container = rl.Rectangle(container_x, container_y, container_width, container_height) + self._interactive_rect = container rl.draw_rectangle_rounded(container, border_radius / container_height, 10, rl.Color(0, 0, 0, 192)) icon_x = container_x + icon_padding @@ -284,6 +316,29 @@ class NavigationCardRenderer(Widget): rl.WHITE, ) + def _render_collapsed_default(self, rect: rl.Rectangle) -> None: + chip_size = 94 + chip_x = int(rect.x + rect.width - chip_size - 32) + chip_y = int(rect.y + 82) + chip_rect = rl.Rectangle(chip_x, chip_y, chip_size, chip_size) + self._interactive_rect = chip_rect + + rl.draw_rectangle_rounded(chip_rect, 0.34, 10, rl.Color(7, 11, 18, 228)) + rl.draw_rectangle_rounded_lines_ex(chip_rect, 0.34, 10, 2, rl.Color(255, 255, 255, 40)) + + icon = self._get_icon(self._maneuver_type, self._modifier) + icon_size = 58 + icon_x = chip_x + (chip_size - icon_size) / 2 + icon_y = chip_y + (chip_size - icon_size) / 2 + rl.draw_texture_pro( + icon, + rl.Rectangle(0, 0, icon.width, icon.height), + rl.Rectangle(icon_x, icon_y, icon_size, icon_size), + rl.Vector2(0, 0), + 0, + rl.WHITE, + ) + def _render_mici(self, rect: rl.Rectangle) -> None: left_safe = 96 right_margin = 12 @@ -293,6 +348,7 @@ class NavigationCardRenderer(Widget): container_x = int(rect.x + left_safe) container_y = int(rect.y + 16) container = rl.Rectangle(container_x, container_y, container_width, container_height) + self._interactive_rect = container rl.draw_rectangle_rounded(container, 0.2, 10, rl.Color(7, 11, 18, 228)) rl.draw_rectangle_rounded_lines_ex(container, 0.2, 10, 2, rl.Color(255, 255, 255, 34)) @@ -378,10 +434,40 @@ class NavigationCardRenderer(Widget): rl.WHITE, ) + def _render_collapsed_mici(self, rect: rl.Rectangle) -> None: + chip_size = 72 + right_reserved = 160 + chip_x = int(rect.x + rect.width - chip_size - right_reserved) + chip_y = int(rect.y + 18) + chip_rect = rl.Rectangle(chip_x, chip_y, chip_size, chip_size) + self._interactive_rect = chip_rect + + rl.draw_rectangle_rounded(chip_rect, 0.34, 10, rl.Color(7, 11, 18, 232)) + rl.draw_rectangle_rounded_lines_ex(chip_rect, 0.34, 10, 2, rl.Color(255, 255, 255, 40)) + + icon = self._get_icon(self._maneuver_type, self._modifier) + icon_size = 44 + icon_x = chip_x + (chip_size - icon_size) / 2 + icon_y = chip_y + (chip_size - icon_size) / 2 + rl.draw_texture_pro( + icon, + rl.Rectangle(0, 0, icon.width, icon.height), + rl.Rectangle(icon_x, icon_y, icon_size, icon_size), + rl.Vector2(0, 0), + 0, + rl.WHITE, + ) + def _render(self, rect: rl.Rectangle) -> None: self._update_state() if not self._valid: return + if self._collapsed: + if self._layout_variant == "mici": + self._render_collapsed_mici(rect) + else: + self._render_collapsed_default(rect) + return if self._layout_variant == "mici": self._render_mici(rect) else: diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc index 38f80d96b..76708a998 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.cc +++ b/selfdrive/ui/qt/onroad/annotated_camera.cc @@ -27,6 +27,10 @@ AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget *par screen_recorder->setVisible(false); } +bool AnnotatedCameraWidget::handleHudTap(const QPoint &pos) { + return hud.handleNavigationTap(pos); +} + void AnnotatedCameraWidget::updateState(const UIState &s, const StarPilotUIState &fs) { // update engageability/experimental mode button experimental_btn->updateState(s, fs); diff --git a/selfdrive/ui/qt/onroad/annotated_camera.h b/selfdrive/ui/qt/onroad/annotated_camera.h index 64c9563de..1c14756b5 100644 --- a/selfdrive/ui/qt/onroad/annotated_camera.h +++ b/selfdrive/ui/qt/onroad/annotated_camera.h @@ -17,6 +17,7 @@ class AnnotatedCameraWidget : public CameraWidget { public: explicit AnnotatedCameraWidget(VisionStreamType type, QWidget* parent = 0); void updateState(const UIState &s, const StarPilotUIState &fs); + bool handleHudTap(const QPoint &pos); double fps; diff --git a/selfdrive/ui/qt/onroad/hud.cc b/selfdrive/ui/qt/onroad/hud.cc index cada93c87..7125722b8 100644 --- a/selfdrive/ui/qt/onroad/hud.cc +++ b/selfdrive/ui/qt/onroad/hud.cc @@ -12,6 +12,15 @@ constexpr int SET_SPEED_NA = 255; HudRenderer::HudRenderer() {} +bool HudRenderer::handleNavigationTap(const QPoint &pos) { + if (!navigation_valid || !nav_hit_rect.contains(pos)) { + return false; + } + + params_memory.putBool("NavInstructionCollapsed", !navigation_collapsed); + return true; +} + void HudRenderer::updateState(const UIState &s) { is_metric = s.scene.is_metric; status = s.status; @@ -48,6 +57,8 @@ void HudRenderer::updateState(const UIState &s) { navigation_enabled = params.getBool("NavigationUI"); navigation_valid = false; navigation_has_next = false; + navigation_collapsed = false; + nav_hit_rect = QRect(); nav_primary_text.clear(); nav_secondary_text.clear(); nav_distance.clear(); @@ -88,6 +99,7 @@ void HudRenderer::updateState(const UIState &s) { nav_next_maneuver_type = nav.value("nextManeuverType").toString().trimmed(); nav_next_modifier = nav.value("nextManeuverModifier").toString().trimmed(); navigation_has_next = !nav_next_maneuver_type.isEmpty() || !nav_next_modifier.isEmpty(); + navigation_collapsed = params_memory.getBool("NavInstructionCollapsed"); } void HudRenderer::draw(QPainter &p, const QRect &surface_rect) { @@ -273,6 +285,27 @@ QStringList HudRenderer::wrapNavigationText(int max_width, int font_size, const } void HudRenderer::drawNavigationCard(QPainter &p, const QRect &surface_rect) { + if (navigation_collapsed) { + const int chip_size = 94; + const int chip_x = surface_rect.right() - chip_size - 32; + const int chip_y = surface_rect.width() >= 1200 ? 232 : 204; + nav_hit_rect = QRect(chip_x, chip_y, chip_size, chip_size); + + p.setPen(Qt::NoPen); + p.setBrush(QColor(7, 11, 18, 228)); + p.drawRoundedRect(nav_hit_rect, 28, 28); + p.setPen(QPen(QColor(255, 255, 255, 40), 2)); + p.setBrush(Qt::NoBrush); + p.drawRoundedRect(nav_hit_rect, 28, 28); + + const int icon_size = 58; + const QPixmap icon = getNavigationIcon(nav_maneuver_type, nav_modifier, icon_size); + const int icon_x = chip_x + (chip_size - icon_size) / 2; + const int icon_y = chip_y + (chip_size - icon_size) / 2; + p.drawPixmap(icon_x, icon_y, icon); + return; + } + const int container_width = std::clamp(surface_rect.width() - 120, 760, 1080); const int container_height = surface_rect.width() >= 1200 ? 238 : 206; const int container_x = (surface_rect.width() - container_width) / 2; @@ -291,6 +324,7 @@ void HudRenderer::drawNavigationCard(QPainter &p, const QRect &surface_rect) { const int secondary_gap = container_height == 238 ? 10 : 8; QRect container(container_x, container_y, container_width, container_height); + nav_hit_rect = container; p.setPen(Qt::NoPen); p.setBrush(QColor(0, 0, 0, 180)); p.drawRoundedRect(container, border_radius, border_radius); diff --git a/selfdrive/ui/qt/onroad/hud.h b/selfdrive/ui/qt/onroad/hud.h index 5ca6d9d1a..92cbb51b0 100644 --- a/selfdrive/ui/qt/onroad/hud.h +++ b/selfdrive/ui/qt/onroad/hud.h @@ -15,6 +15,7 @@ public: HudRenderer(); void updateState(const UIState &s); void draw(QPainter &p, const QRect &surface_rect); + bool handleNavigationTap(const QPoint &pos); StarPilotAnnotatedCameraWidget *starpilot_nvg; @@ -39,6 +40,7 @@ private: bool navigation_enabled = false; bool navigation_valid = false; bool navigation_has_next = false; + bool navigation_collapsed = false; int status = STATUS_DISENGAGED; QString nav_distance; QString nav_primary_text; @@ -47,6 +49,7 @@ private: QString nav_modifier; QString nav_next_maneuver_type; QString nav_next_modifier; + QRect nav_hit_rect; QHash nav_icon_cache; Params params; Params params_memory{"", true}; diff --git a/selfdrive/ui/qt/onroad/onroad_home.cc b/selfdrive/ui/qt/onroad/onroad_home.cc index 17e8b0322..366479d8a 100644 --- a/selfdrive/ui/qt/onroad/onroad_home.cc +++ b/selfdrive/ui/qt/onroad/onroad_home.cc @@ -113,6 +113,11 @@ void OnroadWindow::paintEvent(QPaintEvent *event) { } void OnroadWindow::mousePressEvent(QMouseEvent* mouseEvent) { + if (nvg->handleHudTap(mouseEvent->pos())) { + mouseEvent->accept(); + return; + } + starpilot_nvg->mousePressEvent(mouseEvent); if (mouseEvent->isAccepted()) { diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 02a479a0a..6f0a8f9a2 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/starpilot/navigation/navigationd.py b/starpilot/navigation/navigationd.py index cf3b9b863..18c7a7ba2 100644 --- a/starpilot/navigation/navigationd.py +++ b/starpilot/navigation/navigationd.py @@ -14,7 +14,7 @@ from openpilot.starpilot.navigation.destination_store import parse_destination_j from openpilot.starpilot.navigation.route_engine import Coordinate, MapboxRouteEngine, NavigationRoute, RouteProgress NAVIGATIOND_HZ = 1 -REROUTE_TRIGGER_SECONDS = 3.0 +REROUTE_TRIGGER_SECONDS = 2.0 ARRIVAL_CLEAR_SECONDS = 5.0 LOCATION_STATE_STALE_SECONDS = 2.5 @@ -71,6 +71,7 @@ class Navigationd: self._bearing_misaligned_started_at = None self._arrival_started_at = None self._publish_nav_state(None, None, False) + self.params_memory.remove("NavInstructionCollapsed") if remove_destination: self.params.remove("NavDestination") @@ -180,7 +181,7 @@ class Navigationd: now = monotonic() off_route = route.off_route_distance_exceeded(progress, v_ego) - misaligned = route.route_bearing_misaligned(progress.closest_index, self._last_bearing, v_ego) + misaligned = route.route_bearing_misaligned(progress.closest_segment_index, self._last_bearing, v_ego) arrived = route.arrived(progress, v_ego) self._off_route_started_at = self._bump_timer(self._off_route_started_at, off_route and not arrived, now) diff --git a/starpilot/navigation/route_engine.py b/starpilot/navigation/route_engine.py index 71e905414..c019eb028 100644 --- a/starpilot/navigation/route_engine.py +++ b/starpilot/navigation/route_engine.py @@ -20,7 +20,7 @@ SPEED_CONVERSIONS = { } OFF_ROUTE_SPEED_BREAKPOINTS = [0.0, 5.0, 10.0, 20.0, 40.0] -OFF_ROUTE_DISTANCE_BREAKPOINTS = [100.0, 125.0, 150.0, 200.0, 250.0] +OFF_ROUTE_DISTANCE_BREAKPOINTS = [40.0, 50.0, 60.0, 80.0, 100.0] UPCOMING_TURN_SPEED_BREAKPOINTS = [0.0, 5.0, 10.0, 15.0, 20.0, 25.0, 30.0, 35.0, 40.0] UPCOMING_TURN_DISTANCE_BREAKPOINTS = [20.0, 25.0, 30.0, 45.0, 60.0, 75.0, 90.0, 105.0, 120.0] UTURN_MODIFIER = "uturn" @@ -79,6 +79,7 @@ class RouteStep: @dataclass(frozen=True) class RouteProgress: closest_index: int + closest_segment_index: int distance_from_route: float current_step: RouteStep next_step: RouteStep | None @@ -112,23 +113,24 @@ def minimum_distance(a: Coordinate, b: Coordinate, p: Coordinate) -> float: projection = a + ab * t return projection.distance_to(p) +def project_onto_segment(a: Coordinate, b: Coordinate, p: Coordinate) -> tuple[float, float]: + lat_scale = EARTH_MEAN_RADIUS * math.pi / 180.0 + ref_lat = math.radians((a.latitude + b.latitude + p.latitude) / 3.0) + lon_scale = lat_scale * math.cos(ref_lat) -def distance_along_geometry(geometry: list[Coordinate], position: Coordinate) -> float: - if len(geometry) <= 2: - return geometry[0].distance_to(position) + ab_x = (b.longitude - a.longitude) * lon_scale + ab_y = (b.latitude - a.latitude) * lat_scale + ap_x = (p.longitude - a.longitude) * lon_scale + ap_y = (p.latitude - a.latitude) * lat_scale - total_distance = 0.0 - closest_total_distance = 0.0 - closest_distance = 1e9 + segment_length_sq = ab_x * ab_x + ab_y * ab_y + if segment_length_sq <= 1e-6: + return 0.0, math.hypot(ap_x, ap_y) - for index in range(len(geometry) - 1): - segment_distance = minimum_distance(geometry[index], geometry[index + 1], position) - if segment_distance < closest_distance: - closest_distance = segment_distance - closest_total_distance = total_distance + geometry[index].distance_to(position) - total_distance += geometry[index].distance_to(geometry[index + 1]) - - return closest_total_distance + t = float(np.clip((ap_x * ab_x + ap_y * ab_y) / segment_length_sq, 0.0, 1.0)) + proj_x = ab_x * t + proj_y = ab_y * t + return t, math.hypot(ap_x - proj_x, ap_y - proj_y) def string_to_direction(direction: str) -> str: @@ -207,6 +209,7 @@ def parse_banner_instructions(banners: Any, distance_to_maneuver: float = 0.0) - @dataclass class NavigationRoute: geometry: list[Coordinate] + geometry_cumulative_distances: list[float] bearings: list[float] steps: list[RouteStep] total_distance: float @@ -243,38 +246,54 @@ class NavigationRoute: instruction=str(step.get("instruction", "")), )) - bearings = [bearing_between_two_points(geometry[index], geometry[index + 2]) for index in range(len(geometry) - 2)] + bearings = [bearing_between_two_points(geometry[index], geometry[index + 1]) for index in range(len(geometry) - 1)] return cls( geometry=geometry, + geometry_cumulative_distances=cumulative_distances, bearings=bearings, steps=steps, total_distance=float(route_data.get("totalDistance", 0.0)), total_duration=float(route_data.get("totalDuration", 0.0)), ) - def route_bearing_misaligned(self, closest_index: int, current_bearing: float | None, v_ego: float) -> bool: - if current_bearing is None or v_ego < 5.0 or closest_index <= 0 or closest_index >= len(self.geometry) - 1: - return False - if closest_index - 1 >= len(self.bearings): + def route_bearing_misaligned(self, closest_segment_index: int, current_bearing: float | None, v_ego: float) -> bool: + if current_bearing is None or v_ego < 2.5 or closest_segment_index < 0 or closest_segment_index >= len(self.bearings): return False - route_bearing = self.bearings[closest_index - 1] + route_bearing = self.bearings[closest_segment_index] normalized_bearing = (current_bearing + 360.0) % 360.0 bearing_difference = abs(normalized_bearing - route_bearing) - return min(bearing_difference, 360.0 - bearing_difference) > 110.0 + return min(bearing_difference, 360.0 - bearing_difference) > 75.0 def get_progress(self, position: Coordinate) -> RouteProgress | None: if not self.geometry or not self.steps: return None + if len(self.geometry) == 1: + closest_index = 0 + closest_segment_index = 0 + min_distance = position.distance_to(self.geometry[0]) + closest_cumulative = 0.0 + else: + best_segment_index = 0 + best_distance = float("inf") + best_t = 0.0 - closest_index, min_distance = min( - ((idx, position.distance_to(coord)) for idx, coord in enumerate(self.geometry)), - key=lambda item: item[1], - ) - closest_cumulative = distance_along_geometry(self.geometry, position) + for index in range(len(self.geometry) - 1): + t, segment_distance = project_onto_segment(self.geometry[index], self.geometry[index + 1], position) + if segment_distance < best_distance: + best_distance = segment_distance + best_segment_index = index + best_t = t + + closest_segment_index = best_segment_index + segment_start = self.geometry_cumulative_distances[closest_segment_index] + segment_end = self.geometry_cumulative_distances[closest_segment_index + 1] + closest_cumulative = segment_start + (segment_end - segment_start) * best_t + min_distance = best_distance + closest_index = min(closest_segment_index + (1 if best_t >= 0.5 else 0), len(self.geometry) - 1) current_step_index = max( - (idx for idx, step in enumerate(self.steps) if step.cumulative_distance <= closest_cumulative), + (idx for idx, step in enumerate(self.steps) if step.cumulative_distance <= (closest_cumulative + 1e-3)), default=-1, ) current_step = self.steps[current_step_index if current_step_index >= 0 else 0] @@ -303,6 +322,7 @@ class NavigationRoute: return RouteProgress( closest_index=closest_index, + closest_segment_index=closest_segment_index, distance_from_route=min_distance, current_step=current_step, next_step=next_step, @@ -328,10 +348,13 @@ class NavigationRoute: return progress.distance_from_route > distance_threshold def arrived(self, progress: RouteProgress, v_ego: float) -> bool: - if v_ego >= 1.0 or not progress.all_maneuvers: + if v_ego >= 2.0 or not progress.all_maneuvers: return False current = progress.all_maneuvers[0] - return current["type"] == "arrive" or progress.current_step.instruction.startswith("Your destination") + destination_step = current["type"] == "arrive" or progress.current_step.maneuver == "arrive" or progress.current_step.instruction.startswith("Your destination") + if not destination_step and progress.next_step is not None: + destination_step = progress.next_step.maneuver == "arrive" and progress.distance_to_end_of_step <= max(15.0, v_ego * 8.0) + return destination_step and progress.distance_remaining <= 40.0 def build_instruction_payload(self, progress: RouteProgress, *, use_vienna_sign: bool = False) -> dict[str, Any]: parsed = parse_banner_instructions(progress.current_step.banner_instructions, progress.distance_to_end_of_step) or {} diff --git a/starpilot/navigation/test_route_engine.py b/starpilot/navigation/test_route_engine.py index bb6b7133f..2b791eea6 100644 --- a/starpilot/navigation/test_route_engine.py +++ b/starpilot/navigation/test_route_engine.py @@ -90,10 +90,29 @@ def test_route_off_route_and_arrival_detection(): assert misaligned_progress is not None assert arrive_progress is not None assert route.off_route_distance_exceeded(off_route_progress, 10.0) - assert route.route_bearing_misaligned(misaligned_progress.closest_index, 270.0, 10.0) + assert route.route_bearing_misaligned(misaligned_progress.closest_segment_index, 270.0, 10.0) assert route.arrived(arrive_progress, 0.5) +def test_route_progress_projects_onto_segments_for_destination_step(): + route = make_route() + progress = route.get_progress(Coordinate(0.00098, 0.0020)) + + assert progress is not None + assert progress.next_step is not None + assert progress.next_step.maneuver == "arrive" + assert progress.distance_remaining <= 5.0 + assert route.arrived(progress, 0.5) + + +def test_route_bearing_misaligned_catches_ninety_degree_wrong_turn(): + route = make_route() + progress = route.get_progress(Coordinate(0.0, 0.0015)) + + assert progress is not None + assert route.route_bearing_misaligned(progress.closest_segment_index, 0.0, 6.0) + + def test_lane_payload_uses_capnp_enum_names(): route_data = { "geometry": [ 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 1b6e77e3b..3f73f085c 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 @@ -1000,7 +1000,7 @@ { "key": "CoastUpToLeads", "label": "Coast Up To Leads", - "description": "Allow openpilot to briefly coast toward far leads before resuming normal throttle. Disable this if your vehicle shows gas/brake alternation while approaching distant traffic.", + "description": "Keep the optional far-lead comfort logic enabled, including gentle coasting and matched-follow smoothing for distant leads. Recommended unless your car shows lead-follow stutter or springy gas/brake behavior.", "data_type": "bool", "ui_type": "toggle", "parent_key": "LongitudinalTune" diff --git a/starpilot/ui/qt/offroad/longitudinal_settings.cc b/starpilot/ui/qt/offroad/longitudinal_settings.cc index 2b70e3a2d..bd5415a18 100644 --- a/starpilot/ui/qt/offroad/longitudinal_settings.cc +++ b/starpilot/ui/qt/offroad/longitudinal_settings.cc @@ -147,7 +147,7 @@ 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."), ""}, - {"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."), ""}, + {"CoastUpToLeads", tr("Coast Up To Leads"), tr("Keep the optional far-lead comfort logic enabled, including gentle coasting and matched-follow smoothing for distant leads. Recommended to leave on unless your vehicle shows lead-follow stutter or springy gas/brake behavior."), ""}, {"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."), ""}, {"TacoTune", tr("\"Taco Bell Run\" Turn Speed Hack"), tr("The turn-speed hack from comma's 2022 \"Taco Bell Run\". Designed to slow down for left and right turns."), ""}, diff --git a/system/camerad/camerad b/system/camerad/camerad index 8316c7cf6..cd4ccb33d 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 17fc208b1..2ed729f17 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 6551b9a75..8b0fa344b 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index 0dee1b642..63ba52575 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ diff --git a/tools/replay/fake_nav_demo.py b/tools/replay/fake_nav_demo.py index 9424577f3..03b3a032d 100644 --- a/tools/replay/fake_nav_demo.py +++ b/tools/replay/fake_nav_demo.py @@ -5,6 +5,7 @@ import time from itertools import cycle from cereal import log, messaging +from openpilot.common.params import Params, UnknownKeyName ROUTE_COORDS = [ @@ -108,6 +109,20 @@ SCENARIOS = [ ] +def build_nav_state(params_memory: Params, scenario: dict) -> None: + params_memory.put_nonblocking("NavInstructionState", { + "valid": True, + "maneuverModifier": str(scenario["modifier"]), + "maneuverType": str(scenario["type"]), + "maneuverPrimaryText": str(scenario["primary"]), + "maneuverSecondaryText": str(scenario["secondary"]), + "maneuverDistance": float(scenario["distance"]), + "nextManeuverType": str(scenario["next_type"]), + "nextManeuverModifier": str(scenario["next_modifier"]), + "nextManeuverDistance": float(scenario["distance"] + 220.0), + }) + + def build_instruction(pm: messaging.PubMaster, scenario: dict) -> None: msg = messaging.new_message("navInstruction") msg.valid = True @@ -146,22 +161,35 @@ def main() -> None: args = parser.parse_args() pm = messaging.PubMaster(["navInstruction", "navRoute"]) + params_memory = Params(memory=True) + try: + params_memory.put_bool("NavInstructionCollapsed", False) + except UnknownKeyName: + pass scenario_cycle = cycle(SCENARIOS) current = next(scenario_cycle) next_switch = time.monotonic() + args.hold_seconds print(f"showing: {current['type']} / {current['modifier']} -> {current['next_type']} / {current['next_modifier']}", flush=True) - while True: - now = time.monotonic() - if now >= next_switch: - current = next(scenario_cycle) - next_switch = now + args.hold_seconds - print(f"showing: {current['type']} / {current['modifier']} -> {current['next_type']} / {current['next_modifier']}", flush=True) + try: + while True: + now = time.monotonic() + if now >= next_switch: + current = next(scenario_cycle) + next_switch = now + args.hold_seconds + print(f"showing: {current['type']} / {current['modifier']} -> {current['next_type']} / {current['next_modifier']}", flush=True) - build_instruction(pm, current) - build_route(pm) - time.sleep(args.publish_interval) + build_nav_state(params_memory, current) + build_instruction(pm, current) + build_route(pm) + time.sleep(args.publish_interval) + finally: + params_memory.remove("NavInstructionState") + try: + params_memory.remove("NavInstructionCollapsed") + except UnknownKeyName: + pass if __name__ == "__main__":