diff --git a/opendbc_repo/opendbc/car/honda/stock_aol.py b/opendbc_repo/opendbc/car/honda/stock_aol.py index fbdded8ff2..cc7e115941 100644 --- a/opendbc_repo/opendbc/car/honda/stock_aol.py +++ b/opendbc_repo/opendbc/car/honda/stock_aol.py @@ -70,11 +70,16 @@ def temporary_restriction(cp, state): (state.gearShifter not in (gears.drive, gears.sport, gears.low, gears.brake) or state.steerFaultTemporary)) -def native_observation(native, *, latched, panda_ready, restricted, now_ns, pending_since_ns): +def native_observation(native, *, latched, panda_ready, restricted, now_ns, pending_since_ns, + previously_acknowledged=False, host_requested_lateral=False): lost = not panda_ready or native is None if not latched or lost or restricted: return 0, lost, False if native.requestedLateral and native.lateralAllowed: return 0, False, False + if (previously_acknowledged and host_requested_lateral and + not native.requestedLateral and not native.lateralAllowed): + # A current host request lost an established grant, not a cold handshake. + return 0, False, True pending_since_ns = pending_since_ns or now_ns return pending_since_ns, False, now_ns - pending_since_ns >= 200_000_000 diff --git a/opendbc_repo/opendbc/car/honda/tests/test_stock_aol.py b/opendbc_repo/opendbc/car/honda/tests/test_stock_aol.py index c2a8a47cc9..2f950d72f2 100644 --- a/opendbc_repo/opendbc/car/honda/tests/test_stock_aol.py +++ b/opendbc_repo/opendbc/car/honda/tests/test_stock_aol.py @@ -5,7 +5,7 @@ import pytest from opendbc.car import gen_empty_fingerprint, structs from opendbc.car.honda.interface import CarInterface -from opendbc.car.honda.stock_aol import CLASSIC_BOSCH, NIDEC, RADARLESS, qualified, temporary_restriction +from opendbc.car.honda.stock_aol import CLASSIC_BOSCH, NIDEC, RADARLESS, qualified, temporary_restriction, native_observation from opendbc.car.honda.values import CAR, HondaFlags from openpilot.starpilot.aol.intent import AolSettings, AOL_TOGGLE from openpilot.starpilot.car.honda.aol import policy_for, create_intent, native_accepts_cp @@ -347,6 +347,37 @@ def test_actual_card_active_observation_requires_fresh_applied_command(monkeypat assert tick(previous=True, long=True)["standard_active"] assert intent.allowed_latch # Actual normal long activity also proves active engagement. + # The actual Card caller consumes the acknowledged native permission. Its + # immediate rejection boundary remains strict; no host grace is extended. + tick(enabled=False) + cs = state(True) + native = replace(native, requestedLateral=True, lateralAllowed=True, + observedMonoTime=now + 10_000_000, validUntilMonoTime=now + 210_000_000) + tick(enabled=False) + assert not intent.allowed_latch # The previously active latch toggles OFF. + cs = state(False) + native = replace(native, observedMonoTime=now + 10_000_000) + tick(enabled=False) + cs = state(True) + native = replace(native, observedMonoTime=now + 10_000_000) + tick(enabled=False) + assert intent.allowed_latch # Release then a new physical press toggles ON. + cs = state(False) + native = replace(native, observedMonoTime=now + 10_000_000) + assert tick(enabled=False)["native_rejection_ns"] == 0 + assert intent.allowed_latch + native = replace(native, lateralAllowed=False, observedMonoTime=now + 10_000_000) + observed_native = native.observedMonoTime + assert tick(enabled=False)["native_rejection_ns"] == observed_native + assert not intent.allowed_latch # Native denial immediately revokes independent intent. + native = replace(native, lateralAllowed=True, observedMonoTime=now + 10_000_000) + assert tick(enabled=False)["native_rejection_ns"] == 0 + assert not intent.allowed_latch # Permission recovery alone cannot recreate a gesture. + cs = state(True) + native = replace(native, observedMonoTime=now + 10_000_000) + tick(enabled=False) + assert intent.allowed_latch # A genuinely new LKAS edge can arm again. + generic = AolCardIntent(intent.settings) host.aol_card_intent = generic @@ -357,3 +388,97 @@ def test_actual_card_active_observation_requires_fresh_applied_command(monkeypat monkeypatch.setattr(generic, "update", generic_boundary) with pytest.raises(BoundaryReached): host.state_update() + + +@pytest.mark.parametrize("acknowledged,requested,reset", ((False, True, False), (True, True, True), + (True, False, False), (False, False, False))) +def test_native_cold_pending_vs_requested_ack_loss(acknowledged, requested, reset): + native = SimpleNamespace(requestedLateral=False, lateralAllowed=False) + result = native_observation(native, latched=True, panda_ready=True, restricted=False, + now_ns=1_000_000_000, pending_since_ns=0, + previously_acknowledged=acknowledged, host_requested_lateral=requested) + assert result == ((0, False, True) if reset else (1_000_000_000, False, False)) + if not reset: + assert native_observation(native, latched=True, panda_ready=True, restricted=False, + now_ns=1_200_000_000, pending_since_ns=result[0], + previously_acknowledged=acknowledged, host_requested_lateral=requested) == (result[0], False, True) + + +@pytest.mark.parametrize("changed", ("cold", "session", "panda", "cp", "manual_off", "restriction", + "software_pause", "host_withdrawal", "invalid_axis", "stale_axis", + "replaced_producer", "older_native", "resuming_pause", "pending_pause", "requested_denial", "unpaired_event", + "fault_event", "invalid_event", "stale_event", "none")) +def test_actual_card_ack_loss_requires_current_requested_same_identity(changed): + from openpilot.selfdrive.car.card import Car + from openpilot.starpilot.aol.transport_pause import TransportPauseFeedback + from openpilot.starpilot.aol.wire import SafetyState + + cp = final_cp(CAR.HONDA_CIVIC_2022, alpha=True, hybrid=True) + cp.alternativeExperience |= 32 + now = 2_000_000_000 + native = SafetyState(1, True, now, now + 200_000_000, int(cp.safetyConfigs[0].safetyModel.raw), + cp.safetyConfigs[0].safetyParam, True, False, True, False, "panda", "axis") + axis = SimpleNamespace(qualified=True, sessionId="axis", observedMonoTime=now, validUntilMonoTime=now+30_000_000, + faultSessionId="axis", faultReason="none", faultEventMonoTime=now, sequence=1, + nativeAcknowledged=True, desiredLateral=True, lateralActive=True, sourceCarStateMonoTime=now, + intentProducerSessionId="producer") + + class Samples(dict): + pass + sm = Samples(aolAxisState=axis, onroadEvents=[]) + sm.valid = sm.alive = sm.seen = {"aolAxisState": True, "onroadEvents": True} + sm.logMonoTime = {"aolAxisState": now, "onroadEvents": now} + host = Car.__new__(Car) + host.CP, host.sm, host.slc_producer_session = cp, sm, "producer" + host.aol_transport_feedback = TransportPauseFeedback(cp) + kwargs = dict(latched=True, panda_ready=True, restricted=False, now_ns=now, pending_since_ns=0) + assert host.aol_honda_native_observation(native, **kwargs) == (0, False, False) + if changed == "cold": + host._honda_aol_acknowledgment = None + elif changed == "session": + native = replace(native, axisSessionId="replacement") + elif changed == "panda": + native = replace(native, pandaSerial="replacement") + elif changed == "cp": + host.CP = final_cp(CAR.HONDA_CIVIC_2022, alpha=False, hybrid=True) + elif changed == "manual_off": + kwargs['latched'] = False + elif changed == "restriction": + kwargs['restricted'] = True + elif changed == "software_pause": + axis.faultReason = "availabilityPause" + host.aol_transport_feedback.retention_qualified = True + elif changed == "host_withdrawal": + axis.desiredLateral = False + elif changed == "invalid_axis": + sm.valid = {"aolAxisState": False} + elif changed == "stale_axis": + kwargs['now_ns'] += 40_000_000 + elif changed == "replaced_producer": + axis.intentProducerSessionId = "replacement" + elif changed == "resuming_pause": + axis.lateralActive = False + elif changed == "pending_pause": + host.aol_transport_feedback.awaiting = True + elif changed == "unpaired_event": + axis.faultEventMonoTime -= 1 + elif changed == "fault_event": + sm["onroadEvents"] = [SimpleNamespace(immediateDisable=True, softDisable=False)] + elif changed == "invalid_event": + sm.valid = {"aolAxisState": True, "onroadEvents": False} + elif changed == "stale_event": + axis.faultEventMonoTime = sm.logMonoTime["onroadEvents"] = now-1_500_000_001 + elif changed == "older_native": + native = replace(native, observedMonoTime=now-1) + withdrawn = replace(native, requestedLateral=False, lateralAllowed=False) + if changed == "requested_denial": + withdrawn = replace(withdrawn, requestedLateral=True) + result = host.aol_honda_native_observation(withdrawn, **kwargs) + if changed == "none": + assert result == (0, False, True) + else: + assert not result[2] # No unexpected-loss cause is inferred from ambiguous zero. + if changed in ("manual_off", "restriction"): + assert result == (0, False, False) + if changed in ("cold", "session", "panda", "cp", "manual_off"): + assert host._honda_aol_acknowledgment is None diff --git a/opendbc_repo/opendbc/safety/modes/honda_stock_aol.h b/opendbc_repo/opendbc/safety/modes/honda_stock_aol.h index 9cd0589dba..4d30596952 100644 --- a/opendbc_repo/opendbc/safety/modes/honda_stock_aol.h +++ b/opendbc_repo/opendbc/safety/modes/honda_stock_aol.h @@ -2,6 +2,8 @@ static bool honda_stock_aol_enabled = false; static bool honda_stock_aol_token = false; +static bool honda_stock_aol_pending = false; +static uint32_t honda_stock_aol_grant_ts = 0U; static bool honda_stock_aol_main_neutral = false; static bool honda_stock_aol_lkas_neutral = false; static bool honda_stock_aol_lkas_previous = false; @@ -18,10 +20,20 @@ static RxCheck honda_rx_workspace[7]; static void honda_stock_aol_clear(void) { honda_stock_aol_token = false; + honda_stock_aol_pending = false; honda_stock_aol_main_neutral = false; honda_stock_aol_lkas_neutral = false; } +// A new physical grant may precede the host engagement heartbeat. It grants no output +// until an engaged heartbeat and a fresh host request acknowledge this gesture. +static void honda_stock_aol_grant(void) { + honda_stock_aol_token = true; + honda_stock_aol_pending = !heartbeat_engaged; + honda_stock_aol_grant_ts = microsecond_timer_get(); + honda_stock_aol_session = heartbeat_engaged; +} + static void honda_stock_aol_reset(void) { honda_stock_aol_clear(); honda_stock_aol_enabled = false; @@ -55,14 +67,25 @@ static bool honda_stock_aol_sources_current(void) { static uint8_t honda_stock_aol_request_mask(void) { uint8_t request = 0U; + const uint32_t now = microsecond_timer_get(); + const uint32_t grant_age = safety_get_ts_elapsed(now, honda_stock_aol_grant_ts); + const bool pending_expired = honda_stock_aol_pending && (grant_age > AOL_HOST_REQUEST_TIMEOUT_US); const bool expired = honda_stock_aol_request_seen && - (safety_get_ts_elapsed(microsecond_timer_get(), aol_host_request_ts) > AOL_HOST_REQUEST_TIMEOUT_US); - if (relay_malfunction || expired || safety_rx_checks_invalid || - (honda_stock_aol_session && (!heartbeat_engaged || !aol_rx_healthy() || !honda_stock_aol_sources_current()))) { + (safety_get_ts_elapsed(now, aol_host_request_ts) > AOL_HOST_REQUEST_TIMEOUT_US); + if (relay_malfunction || expired || pending_expired || safety_rx_checks_invalid || + (honda_stock_aol_session && !heartbeat_engaged) || + ((honda_stock_aol_session || honda_stock_aol_pending) && + (!aol_rx_healthy() || !honda_stock_aol_sources_current()))) { honda_stock_aol_clear(); + honda_stock_aol_session = false; } else if (heartbeat_engaged && aol_rx_healthy() && honda_stock_aol_sources_current()) { - honda_stock_aol_session = true; - request = aol_host_axis_mask; + const bool pending_ack = !honda_stock_aol_pending || ((aol_host_axis_mask != 0U) && + (safety_get_ts_elapsed(now, aol_host_request_ts) <= grant_age)); + if (pending_ack) { + honda_stock_aol_pending = false; + honda_stock_aol_session = true; + request = aol_host_axis_mask; + } } else { } return request; @@ -94,6 +117,13 @@ static bool honda_stock_aol_selected(const CANPacket_t *msg, const RxCheck *chec static void honda_stock_aol_rx(const CANPacket_t *msg, unsigned int pt_bus, bool normal_engagement) { if (honda_stock_aol_enabled) { + // Retire the old session before interpreting a new validated physical edge. + // The following gesture may create a bounded pending grant; a status read + // must not mistake that new grant for the retired session. + if (honda_stock_aol_session && !heartbeat_engaged) { + honda_stock_aol_clear(); + honda_stock_aol_session = false; + } const unsigned int main_addr = honda_stock_aol_alt_main ? 0x1A6U : 0x326U; const unsigned int buttons_addr = honda_stock_aol_alt_main ? 0x1A6U : 0x296U; const unsigned int buttons_len = honda_stock_aol_alt_main ? 8U : 4U; @@ -104,7 +134,7 @@ static void honda_stock_aol_rx(const CANPacket_t *msg, unsigned int pt_bus, bool honda_stock_aol_clear(); honda_stock_aol_main_neutral = true; } else if (honda_stock_aol_main_neutral) { - honda_stock_aol_token = true; + honda_stock_aol_grant(); honda_stock_aol_main_neutral = false; } else { } @@ -115,7 +145,7 @@ static void honda_stock_aol_rx(const CANPacket_t *msg, unsigned int pt_bus, bool if (!lkas) { honda_stock_aol_lkas_neutral = true; } else if (honda_stock_aol_lkas_neutral && !honda_stock_aol_lkas_previous && acc_main_on) { - honda_stock_aol_token = true; + honda_stock_aol_grant(); honda_stock_aol_lkas_neutral = false; } else { } @@ -152,10 +182,9 @@ static void honda_stock_aol_rx(const CANPacket_t *msg, unsigned int pt_bus, bool (honda_stock_aol_eps == 3U) || (honda_stock_aol_eps == 4U) || (honda_stock_aol_eps == 6U); if (normal_engagement && engagement_source && controls_allowed && acc_main_on && honda_stock_aol_drive && eps_known && !brake_pressed && !regen_braking && !steering_disengage && - heartbeat_engaged && !relay_malfunction && !safety_rx_checks_invalid && request_current && + !relay_malfunction && !safety_rx_checks_invalid && request_current && aol_rx_healthy() && honda_stock_aol_sources_current()) { - honda_stock_aol_token = true; - honda_stock_aol_session = true; + honda_stock_aol_grant(); } } } diff --git a/opendbc_repo/opendbc/safety/tests/test_honda_stock_aol.py b/opendbc_repo/opendbc/safety/tests/test_honda_stock_aol.py index 1748347605..967f3e50ac 100644 --- a/opendbc_repo/opendbc/safety/tests/test_honda_stock_aol.py +++ b/opendbc_repo/opendbc/safety/tests/test_honda_stock_aol.py @@ -110,15 +110,13 @@ class TestHondaStockAol(unittest.TestCase): def test_blocked_normal_edge_is_not_replayed_after_sources_recover(self): for case in CASES: - for blocked in ("eps", "reverse", "heartbeat", "brake", "missing", "expired", "request_expired"): + for blocked in ("eps", "reverse", "brake", "missing", "expired", "request_expired"): with self.subTest(case=case[0], blocked=blocked): self.configure(case) self.stream(eps=7 if blocked == "eps" else 0, gear=2 if blocked == "reverse" else None, brake=int(blocked == "brake"), omit="STEER_STATUS" if blocked == "missing" else None) self.assertEqual(self.safety.aol_get_permission_mask(), 0) - if blocked == "heartbeat": - self.safety.set_aol_test_heartbeat(False) - elif blocked in ("expired", "request_expired"): + if blocked in ("expired", "request_expired"): self.safety.set_timer(1_300_001) if blocked == "expired": self.safety.aol_set_host_request(1) @@ -235,3 +233,120 @@ class TestHondaStockAol(unittest.TestCase): self.stream(main=False) self.stream(main=True, lkas=1) self.assertEqual(self.safety.aol_get_permission_mask(), 0) + + def test_cold_valid_normal_grant_precedes_engaged_heartbeat(self): + for case in CASES: + for button in ((3, 4) if case[2] in (10, 11) else (3,)): + with self.subTest(case=case[0], button=button): + self.configure(case) + self.safety.set_aol_test_heartbeat(False) + self.stream(request=False) + self.normal_engage(button=button) + self.assertEqual(self.safety.aol_get_request_mask(), 0) + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + self.safety.set_timer(1_010_000) + self.safety.aol_set_host_request(3) + self.safety.set_aol_test_heartbeat(True) + self.assertEqual(self.safety.aol_get_permission_mask(), 3 if self.scoped else 0) + self.press_brake() + self.assertFalse(self.safety.get_controls_allowed()) + self.assertEqual(self.safety.aol_get_permission_mask(), 1 if self.scoped else 0) + + def test_prior_session_new_lkas_survives_status_before_request_and_heartbeat(self): + for case in CASES: + with self.subTest(case=case[0]): + self.configure(case) + self.stream() + self.normal_engage() + self.safety.aol_set_host_request(3) + self.assertEqual(self.safety.aol_get_permission_mask(), 3 if self.scoped else 0) + self.press_brake() + self.safety.set_aol_test_heartbeat(False) + self.safety.aol_set_host_request(0) + self.safety.set_timer(1_010_000) + self.stream(cruise=1, request=False) # New neutral after old session loss. + self.stream(cruise=1, lkas=1, request=False) + self.assertFalse(self.safety.get_controls_allowed()) + self.assertEqual(self.safety.aol_get_request_mask(), 0) # Actual first status order. + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + self.safety.set_timer(1_020_000) + self.safety.aol_set_host_request(1) + self.safety.set_aol_test_heartbeat(True) + self.assertEqual(self.safety.aol_get_request_mask(), 1 if self.scoped else 0) + self.assertEqual(self.safety.aol_get_permission_mask(), 1 if self.scoped else 0) + self.safety.set_aol_test_heartbeat(False) + self.assertEqual(self.safety.aol_get_permission_mask(), 0) # Active grant still revoked. + self.safety.set_aol_test_heartbeat(True) + self.stream(cruise=1, lkas=1) + self.assertEqual(self.safety.aol_get_permission_mask(), 0) # Held button cannot replay. + + def test_pending_physical_grant_requires_post_gesture_request(self): + for case in CASES: + with self.subTest(case=case[0]): + self.configure(case) + self.safety.set_aol_test_heartbeat(False) + self.stream() + self.safety.set_timer(1_010_000) + self.stream(lkas=1, request=False) + self.safety.set_aol_test_heartbeat(True) + self.assertEqual(self.safety.aol_get_permission_mask(), 0) # Previous request is insufficient. + self.safety.set_timer(1_020_000) + self.safety.aol_set_host_request(1) + self.assertEqual(self.safety.aol_get_permission_mask(), 1 if self.scoped else 0) + + def test_expired_pending_heartbeat_and_sources_require_new_gesture(self): + for case in CASES: + for loss in ("heartbeat_expired", "eps_source", "gear_source", "main_source", "host_expired", "relay", "eps_invalid", "rx_checksum"): + with self.subTest(case=case[0], loss=loss): + self.configure(case) + self.safety.set_aol_test_heartbeat(False) + self.stream(request=False) + self.normal_engage() + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + self.safety.aol_set_host_request(0) + if loss in ("heartbeat_expired", "eps_source", "gear_source", "main_source", "host_expired"): + self.safety.set_timer(1_300_001) + source = {"eps_source": "STEER_STATUS", "gear_source": case[6], + "main_source": "SCM_BUTTONS" if self.alt else "SCM_FEEDBACK"}.get(loss) + self.stream(cruise=1, omit=source, request=False) + if loss != "host_expired": + self.safety.aol_set_host_request(0) + elif loss == "relay": + self.safety.set_relay_malfunction(True) + elif loss == "rx_checksum": + invalid = self.packet("POWERTRAIN_DATA", {"ACC_STATUS": 1}) + invalid.data[7] ^= 1 + self.assertFalse(self.safety.safety_rx_hook(invalid)) + else: + self.stream(cruise=1, eps=7, request=False) + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + self.safety.set_relay_malfunction(False) + self.safety.set_aol_test_heartbeat(True) + self.stream(cruise=1) + self.press_brake() + self.safety.aol_set_host_request(3) + self.assertFalse(self.safety.get_controls_allowed()) + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + self.stream(cruise=0) + self.normal_engage() + self.press_brake() + self.safety.aol_set_host_request(1) + self.assertEqual(self.safety.aol_get_permission_mask(), 1 if self.scoped else 0) + + def test_pending_grant_clock_wrap_and_deadline(self): + for case in CASES: + for delay in (100_000, 300_000, 300_001): + with self.subTest(case=case[0], delay=delay): + self.configure(case) + self.safety.set_timer(0xffff_0000) + self.safety.set_aol_test_heartbeat(False) + self.stream(request=False) + self.stream(lkas=1, request=False) + self.safety.set_timer((0xffff_0000 + delay) & 0xffff_ffff) + self.stream(lkas=1, request=False) # Actual fresh CAN, held level, no new edge. + self.safety.aol_set_host_request(0) + self.assertEqual(self.safety.aol_get_permission_mask(), 0) + self.safety.set_aol_test_heartbeat(True) + self.safety.aol_set_host_request(1) + self.assertEqual(self.safety.aol_get_permission_mask(), + int(self.scoped and delay <= 300_000)) diff --git a/openpilot/selfdrive/car/card.py b/openpilot/selfdrive/car/card.py index 24632a7cd6..4b6cb75c4c 100755 --- a/openpilot/selfdrive/car/card.py +++ b/openpilot/selfdrive/car/card.py @@ -35,7 +35,7 @@ from openpilot.selfdrive.pandad import can_capnp_to_list, can_list_to_can_capnp from openpilot.selfdrive.car.cruise import VCruiseHelper, SlcPendingConfirmation from openpilot.starpilot.speed_limits import physical_actions as slc_physical from openpilot.starpilot.aol.intent import AolProcessFaultContext, independent_axis_requested, read_settings -from openpilot.starpilot.aol.transport_pause import TransportPauseFeedback +from openpilot.starpilot.aol.transport_pause import TransportPauseFeedback, cp_identity from opendbc.car.honda.stock_aol import ( qualified as qualified_honda_stock_aol, native_observation as honda_native_observation, temporary_restriction as honda_temporary_restriction, @@ -55,7 +55,7 @@ from opendbc.car.hyundai.canfd_angle_aol import native_observation, temporary_re from opendbc.car.hyundai.canfd_owner import config_index as hyundai_canfd_config_index from opendbc.car.hyundai.captured_adrv_startup import CapturedADRVStartup from openpilot.starpilot.car.hyundai.aol import aol_safety_config as hyundai_aol_safety_config -from openpilot.starpilot.aol.runtime import current_native +from openpilot.starpilot.aol.runtime import AXIS_MAX_AGE_NS, current_axis, current_native from openpilot.starpilot.aol.vehicle import create_intent as create_aol_intent, native_latch_rejected, policy_for as aol_policy_for from openpilot.starpilot.aol.wire import IntentState, encode_intent from openpilot.starpilot.conditional_mode.manual import Button, ButtonTracker, IoniqMediaMapCache, WheelMapCache, Press, ioniq6_media_eligible @@ -460,7 +460,8 @@ class Car: CS = self.CI.update(can_list) self.timing_mark('interface_update_end') cancel_boot_ns = (time.clock_gettime_ns(time.CLOCK_BOOTTIME) - if getattr(self.CI.CS, 'ev6_aol_source_healthy', False) else 0) + if (getattr(self.CI.CS, 'ev6_aol_source_healthy', False) or + qualified_honda_stock_aol(self.CP, marked_only=True)) else 0) self.observe_ioniq6_long_authority(can_list, CS) self.timing_mark('ioniq_authority_end') media_owner = getattr(self, 'ioniq6_media', None) @@ -532,6 +533,7 @@ class Car: self.aol_transport_feedback = TransportPauseFeedback(self.CP) transport_reset = self.aol_transport_feedback.observe(self.sm, self.CP, CS, now_ns, latched=self.aol_card_intent.allowed_latch, cancel_source_ns=self.aol_cancel_source(CS, now_ns), + brake_source_ns=self.aol_honda_brake_source(CS, now_ns), intent_producer_session=getattr(self, 'slc_producer_session', '')) fault_active = True if transport_reset else None event_ns = int(self.sm.logMonoTime['onroadEvents']) @@ -549,7 +551,7 @@ class Car: ) native_reset = False if permission_owner: - observe = (preap_native_observation if preap_aol else mazda_native_observation if mazda_aol else honda_native_observation if honda_aol else + observe = (preap_native_observation if preap_aol else mazda_native_observation if mazda_aol else self.aol_honda_native_observation if honda_aol else ford_native_observation if ford_aol else native_observation) restrict = (preap_temporary_restriction if preap_aol else mazda_temporary_restriction if mazda_aol else honda_temporary_restriction if honda_aol else ford_temporary_restriction if ford_aol else temporary_restriction) @@ -738,13 +740,35 @@ class Car: return 0 return received_ns + def aol_honda_brake_source(self, CS, now_ns: int) -> int: + observation = getattr(self, '_aol_can_observation', None) + if (not CS.brakePressed or not qualified_honda_stock_aol(self.CP, marked_only=True) or + observation is None or observation[0] is not CS): + return 0 + from opendbc.car import Bus + from opendbc.car.honda.values import HondaFlags + parser = self.CI.can_parsers[Bus.pt] + message = 'BRAKE_MODULE' if self.CP.flags & HondaFlags.BOSCH_ALT_BRAKE else 'POWERTRAIN_DATA' + signals = parser.vl.get(message, {}) + # Match the real parser's applied-brake or two-sample switch decision. + brake = bool(signals.get('BRAKE_PRESSED', 0) or message == 'POWERTRAIN_DATA' and + signals.get('BRAKE_SWITCH', 0) and self.CI.CS.brake_switch_active) + stamp = int(parser.ts_nanos.get(message, {}).get('BRAKE_PRESSED', 0)) + _, received_ns, can_source_ns, boot_ns = observation + # CANParser/CAN batch use BOOTTIME; retain the original MONOTONIC receipt. + if (not brake or not 0 < received_ns <= now_ns or now_ns - received_ns > 30_000_000 or + not 0 < stamp <= can_source_ns <= boot_ns or boot_ns - stamp > 30_000_000 or + can_source_ns - stamp > 30_000_000): + return 0 + return received_ns + def aol_disarming_fault(self, CS, event_ns: int, now_ns: int) -> bool: from openpilot.starpilot.car.gm.aol import intent_disarming_fault if getattr(self, 'aol_transport_feedback', None) is None: self.aol_transport_feedback = TransportPauseFeedback(self.CP) if self.aol_transport_feedback.observe(self.sm, self.CP, CS, now_ns, latched=getattr(getattr(self, 'aol_card_intent', None), 'allowed_latch', True), - cancel_source_ns=self.aol_cancel_source(CS, now_ns), + cancel_source_ns=self.aol_cancel_source(CS, now_ns), brake_source_ns=self.aol_honda_brake_source(CS, now_ns), intent_producer_session=getattr(self, 'slc_producer_session', '')): return True events = self.sm['onroadEvents'] @@ -767,6 +791,44 @@ class Car: cached = self._aol_owner_cache = (self.CP, owners) return cached[1] + def aol_honda_native_observation(self, native, *, latched, panda_ready, restricted, now_ns, pending_since_ns): + acknowledged = getattr(self, '_honda_aol_acknowledgment', None) + key = None + host_requested = False + if native is not None and qualified_honda_stock_aol(self.CP, marked_only=True): + key = (cp_identity(self.CP), str(native.axisSessionId), str(native.pandaSerial), + tuple((p.hardwareSerial, p.safetyModel, p.safetyParam) for p in native.pandaInventory)) + try: + axis = current_axis(self.sm, now_ns=now_ns) + feedback = self.aol_transport_feedback + host_requested = bool(axis is not None and axis.sessionId == native.axisSessionId and + axis.observedMonoTime == self.sm.logMonoTime['aolAxisState'] and + axis.validUntilMonoTime == axis.observedMonoTime + AXIS_MAX_AGE_NS and + axis.faultSessionId == axis.sessionId and str(axis.faultReason) == 'none' and + self.sm.seen['onroadEvents'] and self.sm.valid['onroadEvents'] and self.sm.alive['onroadEvents'] and + 0 < axis.faultEventMonoTime == self.sm.logMonoTime['onroadEvents'] <= axis.observedMonoTime and + now_ns - axis.faultEventMonoTime <= 1_500_000_000 and + not any(event.immediateDisable or event.softDisable for event in self.sm['onroadEvents']) and + axis.sequence > 0 and axis.nativeAcknowledged and axis.desiredLateral and axis.lateralActive and + axis.intentProducerSessionId == self.slc_producer_session and + 0 < axis.sourceCarStateMonoTime <= native.observedMonoTime <= now_ns and + now_ns - axis.sourceCarStateMonoTime <= AXIS_MAX_AGE_NS and + not feedback.qualified and not feedback.retention_qualified and not feedback.paused and + not feedback.awaiting and not feedback.retention_pending) + except (AttributeError, KeyError, TypeError, ValueError): + host_requested = False + previously_acknowledged = bool(acknowledged is not None and acknowledged[0] == key and + 0 < acknowledged[1] <= native.observedMonoTime) if native is not None else False + if not latched or not panda_ready or key is None: + self._honda_aol_acknowledgment = None + elif native.requestedLateral and native.lateralAllowed: + self._honda_aol_acknowledgment = (key, int(native.observedMonoTime)) + elif acknowledged is not None and acknowledged[0] != key: + self._honda_aol_acknowledgment = None + return honda_native_observation(native, latched=latched, panda_ready=panda_ready, restricted=restricted, + now_ns=now_ns, pending_since_ns=pending_since_ns, + previously_acknowledged=previously_acknowledged, host_requested_lateral=host_requested) + def aol_axis_session(self) -> str: # pandad stamps aolSafetyWire with selfdrived's aolAxisState session, not card's producer session. seen = getattr(self.sm, 'seen', {}) diff --git a/openpilot/selfdrive/locationd/models/generated/car.cpp b/openpilot/selfdrive/locationd/models/generated/car.cpp index 9236b2bb22..91be56b64f 100644 --- a/openpilot/selfdrive/locationd/models/generated/car.cpp +++ b/openpilot/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_5032200201446894984) { - out_5032200201446894984[0] = delta_x[0] + nom_x[0]; - out_5032200201446894984[1] = delta_x[1] + nom_x[1]; - out_5032200201446894984[2] = delta_x[2] + nom_x[2]; - out_5032200201446894984[3] = delta_x[3] + nom_x[3]; - out_5032200201446894984[4] = delta_x[4] + nom_x[4]; - out_5032200201446894984[5] = delta_x[5] + nom_x[5]; - out_5032200201446894984[6] = delta_x[6] + nom_x[6]; - out_5032200201446894984[7] = delta_x[7] + nom_x[7]; - out_5032200201446894984[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_1832524583205975398) { + out_1832524583205975398[0] = delta_x[0] + nom_x[0]; + out_1832524583205975398[1] = delta_x[1] + nom_x[1]; + out_1832524583205975398[2] = delta_x[2] + nom_x[2]; + out_1832524583205975398[3] = delta_x[3] + nom_x[3]; + out_1832524583205975398[4] = delta_x[4] + nom_x[4]; + out_1832524583205975398[5] = delta_x[5] + nom_x[5]; + out_1832524583205975398[6] = delta_x[6] + nom_x[6]; + out_1832524583205975398[7] = delta_x[7] + nom_x[7]; + out_1832524583205975398[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_7143447445886649280) { - out_7143447445886649280[0] = -nom_x[0] + true_x[0]; - out_7143447445886649280[1] = -nom_x[1] + true_x[1]; - out_7143447445886649280[2] = -nom_x[2] + true_x[2]; - out_7143447445886649280[3] = -nom_x[3] + true_x[3]; - out_7143447445886649280[4] = -nom_x[4] + true_x[4]; - out_7143447445886649280[5] = -nom_x[5] + true_x[5]; - out_7143447445886649280[6] = -nom_x[6] + true_x[6]; - out_7143447445886649280[7] = -nom_x[7] + true_x[7]; - out_7143447445886649280[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_970452396869078486) { + out_970452396869078486[0] = -nom_x[0] + true_x[0]; + out_970452396869078486[1] = -nom_x[1] + true_x[1]; + out_970452396869078486[2] = -nom_x[2] + true_x[2]; + out_970452396869078486[3] = -nom_x[3] + true_x[3]; + out_970452396869078486[4] = -nom_x[4] + true_x[4]; + out_970452396869078486[5] = -nom_x[5] + true_x[5]; + out_970452396869078486[6] = -nom_x[6] + true_x[6]; + out_970452396869078486[7] = -nom_x[7] + true_x[7]; + out_970452396869078486[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_1685754253580569061) { - out_1685754253580569061[0] = 1.0; - out_1685754253580569061[1] = 0.0; - out_1685754253580569061[2] = 0.0; - out_1685754253580569061[3] = 0.0; - out_1685754253580569061[4] = 0.0; - out_1685754253580569061[5] = 0.0; - out_1685754253580569061[6] = 0.0; - out_1685754253580569061[7] = 0.0; - out_1685754253580569061[8] = 0.0; - out_1685754253580569061[9] = 0.0; - out_1685754253580569061[10] = 1.0; - out_1685754253580569061[11] = 0.0; - out_1685754253580569061[12] = 0.0; - out_1685754253580569061[13] = 0.0; - out_1685754253580569061[14] = 0.0; - out_1685754253580569061[15] = 0.0; - out_1685754253580569061[16] = 0.0; - out_1685754253580569061[17] = 0.0; - out_1685754253580569061[18] = 0.0; - out_1685754253580569061[19] = 0.0; - out_1685754253580569061[20] = 1.0; - out_1685754253580569061[21] = 0.0; - out_1685754253580569061[22] = 0.0; - out_1685754253580569061[23] = 0.0; - out_1685754253580569061[24] = 0.0; - out_1685754253580569061[25] = 0.0; - out_1685754253580569061[26] = 0.0; - out_1685754253580569061[27] = 0.0; - out_1685754253580569061[28] = 0.0; - out_1685754253580569061[29] = 0.0; - out_1685754253580569061[30] = 1.0; - out_1685754253580569061[31] = 0.0; - out_1685754253580569061[32] = 0.0; - out_1685754253580569061[33] = 0.0; - out_1685754253580569061[34] = 0.0; - out_1685754253580569061[35] = 0.0; - out_1685754253580569061[36] = 0.0; - out_1685754253580569061[37] = 0.0; - out_1685754253580569061[38] = 0.0; - out_1685754253580569061[39] = 0.0; - out_1685754253580569061[40] = 1.0; - out_1685754253580569061[41] = 0.0; - out_1685754253580569061[42] = 0.0; - out_1685754253580569061[43] = 0.0; - out_1685754253580569061[44] = 0.0; - out_1685754253580569061[45] = 0.0; - out_1685754253580569061[46] = 0.0; - out_1685754253580569061[47] = 0.0; - out_1685754253580569061[48] = 0.0; - out_1685754253580569061[49] = 0.0; - out_1685754253580569061[50] = 1.0; - out_1685754253580569061[51] = 0.0; - out_1685754253580569061[52] = 0.0; - out_1685754253580569061[53] = 0.0; - out_1685754253580569061[54] = 0.0; - out_1685754253580569061[55] = 0.0; - out_1685754253580569061[56] = 0.0; - out_1685754253580569061[57] = 0.0; - out_1685754253580569061[58] = 0.0; - out_1685754253580569061[59] = 0.0; - out_1685754253580569061[60] = 1.0; - out_1685754253580569061[61] = 0.0; - out_1685754253580569061[62] = 0.0; - out_1685754253580569061[63] = 0.0; - out_1685754253580569061[64] = 0.0; - out_1685754253580569061[65] = 0.0; - out_1685754253580569061[66] = 0.0; - out_1685754253580569061[67] = 0.0; - out_1685754253580569061[68] = 0.0; - out_1685754253580569061[69] = 0.0; - out_1685754253580569061[70] = 1.0; - out_1685754253580569061[71] = 0.0; - out_1685754253580569061[72] = 0.0; - out_1685754253580569061[73] = 0.0; - out_1685754253580569061[74] = 0.0; - out_1685754253580569061[75] = 0.0; - out_1685754253580569061[76] = 0.0; - out_1685754253580569061[77] = 0.0; - out_1685754253580569061[78] = 0.0; - out_1685754253580569061[79] = 0.0; - out_1685754253580569061[80] = 1.0; +void H_mod_fun(double *state, double *out_4593964073461568413) { + out_4593964073461568413[0] = 1.0; + out_4593964073461568413[1] = 0.0; + out_4593964073461568413[2] = 0.0; + out_4593964073461568413[3] = 0.0; + out_4593964073461568413[4] = 0.0; + out_4593964073461568413[5] = 0.0; + out_4593964073461568413[6] = 0.0; + out_4593964073461568413[7] = 0.0; + out_4593964073461568413[8] = 0.0; + out_4593964073461568413[9] = 0.0; + out_4593964073461568413[10] = 1.0; + out_4593964073461568413[11] = 0.0; + out_4593964073461568413[12] = 0.0; + out_4593964073461568413[13] = 0.0; + out_4593964073461568413[14] = 0.0; + out_4593964073461568413[15] = 0.0; + out_4593964073461568413[16] = 0.0; + out_4593964073461568413[17] = 0.0; + out_4593964073461568413[18] = 0.0; + out_4593964073461568413[19] = 0.0; + out_4593964073461568413[20] = 1.0; + out_4593964073461568413[21] = 0.0; + out_4593964073461568413[22] = 0.0; + out_4593964073461568413[23] = 0.0; + out_4593964073461568413[24] = 0.0; + out_4593964073461568413[25] = 0.0; + out_4593964073461568413[26] = 0.0; + out_4593964073461568413[27] = 0.0; + out_4593964073461568413[28] = 0.0; + out_4593964073461568413[29] = 0.0; + out_4593964073461568413[30] = 1.0; + out_4593964073461568413[31] = 0.0; + out_4593964073461568413[32] = 0.0; + out_4593964073461568413[33] = 0.0; + out_4593964073461568413[34] = 0.0; + out_4593964073461568413[35] = 0.0; + out_4593964073461568413[36] = 0.0; + out_4593964073461568413[37] = 0.0; + out_4593964073461568413[38] = 0.0; + out_4593964073461568413[39] = 0.0; + out_4593964073461568413[40] = 1.0; + out_4593964073461568413[41] = 0.0; + out_4593964073461568413[42] = 0.0; + out_4593964073461568413[43] = 0.0; + out_4593964073461568413[44] = 0.0; + out_4593964073461568413[45] = 0.0; + out_4593964073461568413[46] = 0.0; + out_4593964073461568413[47] = 0.0; + out_4593964073461568413[48] = 0.0; + out_4593964073461568413[49] = 0.0; + out_4593964073461568413[50] = 1.0; + out_4593964073461568413[51] = 0.0; + out_4593964073461568413[52] = 0.0; + out_4593964073461568413[53] = 0.0; + out_4593964073461568413[54] = 0.0; + out_4593964073461568413[55] = 0.0; + out_4593964073461568413[56] = 0.0; + out_4593964073461568413[57] = 0.0; + out_4593964073461568413[58] = 0.0; + out_4593964073461568413[59] = 0.0; + out_4593964073461568413[60] = 1.0; + out_4593964073461568413[61] = 0.0; + out_4593964073461568413[62] = 0.0; + out_4593964073461568413[63] = 0.0; + out_4593964073461568413[64] = 0.0; + out_4593964073461568413[65] = 0.0; + out_4593964073461568413[66] = 0.0; + out_4593964073461568413[67] = 0.0; + out_4593964073461568413[68] = 0.0; + out_4593964073461568413[69] = 0.0; + out_4593964073461568413[70] = 1.0; + out_4593964073461568413[71] = 0.0; + out_4593964073461568413[72] = 0.0; + out_4593964073461568413[73] = 0.0; + out_4593964073461568413[74] = 0.0; + out_4593964073461568413[75] = 0.0; + out_4593964073461568413[76] = 0.0; + out_4593964073461568413[77] = 0.0; + out_4593964073461568413[78] = 0.0; + out_4593964073461568413[79] = 0.0; + out_4593964073461568413[80] = 1.0; } -void f_fun(double *state, double dt, double *out_8909179533363476452) { - out_8909179533363476452[0] = state[0]; - out_8909179533363476452[1] = state[1]; - out_8909179533363476452[2] = state[2]; - out_8909179533363476452[3] = state[3]; - out_8909179533363476452[4] = state[4]; - out_8909179533363476452[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_8909179533363476452[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_8909179533363476452[7] = state[7]; - out_8909179533363476452[8] = state[8]; +void f_fun(double *state, double dt, double *out_231308644161984314) { + out_231308644161984314[0] = state[0]; + out_231308644161984314[1] = state[1]; + out_231308644161984314[2] = state[2]; + out_231308644161984314[3] = state[3]; + out_231308644161984314[4] = state[4]; + out_231308644161984314[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_231308644161984314[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_231308644161984314[7] = state[7]; + out_231308644161984314[8] = state[8]; } -void F_fun(double *state, double dt, double *out_3419672683889849262) { - out_3419672683889849262[0] = 1; - out_3419672683889849262[1] = 0; - out_3419672683889849262[2] = 0; - out_3419672683889849262[3] = 0; - out_3419672683889849262[4] = 0; - out_3419672683889849262[5] = 0; - out_3419672683889849262[6] = 0; - out_3419672683889849262[7] = 0; - out_3419672683889849262[8] = 0; - out_3419672683889849262[9] = 0; - out_3419672683889849262[10] = 1; - out_3419672683889849262[11] = 0; - out_3419672683889849262[12] = 0; - out_3419672683889849262[13] = 0; - out_3419672683889849262[14] = 0; - out_3419672683889849262[15] = 0; - out_3419672683889849262[16] = 0; - out_3419672683889849262[17] = 0; - out_3419672683889849262[18] = 0; - out_3419672683889849262[19] = 0; - out_3419672683889849262[20] = 1; - out_3419672683889849262[21] = 0; - out_3419672683889849262[22] = 0; - out_3419672683889849262[23] = 0; - out_3419672683889849262[24] = 0; - out_3419672683889849262[25] = 0; - out_3419672683889849262[26] = 0; - out_3419672683889849262[27] = 0; - out_3419672683889849262[28] = 0; - out_3419672683889849262[29] = 0; - out_3419672683889849262[30] = 1; - out_3419672683889849262[31] = 0; - out_3419672683889849262[32] = 0; - out_3419672683889849262[33] = 0; - out_3419672683889849262[34] = 0; - out_3419672683889849262[35] = 0; - out_3419672683889849262[36] = 0; - out_3419672683889849262[37] = 0; - out_3419672683889849262[38] = 0; - out_3419672683889849262[39] = 0; - out_3419672683889849262[40] = 1; - out_3419672683889849262[41] = 0; - out_3419672683889849262[42] = 0; - out_3419672683889849262[43] = 0; - out_3419672683889849262[44] = 0; - out_3419672683889849262[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_3419672683889849262[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_3419672683889849262[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_3419672683889849262[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_3419672683889849262[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_3419672683889849262[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_3419672683889849262[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_3419672683889849262[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_3419672683889849262[53] = -9.8100000000000005*dt; - out_3419672683889849262[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_3419672683889849262[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_3419672683889849262[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3419672683889849262[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3419672683889849262[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_3419672683889849262[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_3419672683889849262[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_3419672683889849262[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3419672683889849262[62] = 0; - out_3419672683889849262[63] = 0; - out_3419672683889849262[64] = 0; - out_3419672683889849262[65] = 0; - out_3419672683889849262[66] = 0; - out_3419672683889849262[67] = 0; - out_3419672683889849262[68] = 0; - out_3419672683889849262[69] = 0; - out_3419672683889849262[70] = 1; - out_3419672683889849262[71] = 0; - out_3419672683889849262[72] = 0; - out_3419672683889849262[73] = 0; - out_3419672683889849262[74] = 0; - out_3419672683889849262[75] = 0; - out_3419672683889849262[76] = 0; - out_3419672683889849262[77] = 0; - out_3419672683889849262[78] = 0; - out_3419672683889849262[79] = 0; - out_3419672683889849262[80] = 1; +void F_fun(double *state, double dt, double *out_2868289483219690466) { + out_2868289483219690466[0] = 1; + out_2868289483219690466[1] = 0; + out_2868289483219690466[2] = 0; + out_2868289483219690466[3] = 0; + out_2868289483219690466[4] = 0; + out_2868289483219690466[5] = 0; + out_2868289483219690466[6] = 0; + out_2868289483219690466[7] = 0; + out_2868289483219690466[8] = 0; + out_2868289483219690466[9] = 0; + out_2868289483219690466[10] = 1; + out_2868289483219690466[11] = 0; + out_2868289483219690466[12] = 0; + out_2868289483219690466[13] = 0; + out_2868289483219690466[14] = 0; + out_2868289483219690466[15] = 0; + out_2868289483219690466[16] = 0; + out_2868289483219690466[17] = 0; + out_2868289483219690466[18] = 0; + out_2868289483219690466[19] = 0; + out_2868289483219690466[20] = 1; + out_2868289483219690466[21] = 0; + out_2868289483219690466[22] = 0; + out_2868289483219690466[23] = 0; + out_2868289483219690466[24] = 0; + out_2868289483219690466[25] = 0; + out_2868289483219690466[26] = 0; + out_2868289483219690466[27] = 0; + out_2868289483219690466[28] = 0; + out_2868289483219690466[29] = 0; + out_2868289483219690466[30] = 1; + out_2868289483219690466[31] = 0; + out_2868289483219690466[32] = 0; + out_2868289483219690466[33] = 0; + out_2868289483219690466[34] = 0; + out_2868289483219690466[35] = 0; + out_2868289483219690466[36] = 0; + out_2868289483219690466[37] = 0; + out_2868289483219690466[38] = 0; + out_2868289483219690466[39] = 0; + out_2868289483219690466[40] = 1; + out_2868289483219690466[41] = 0; + out_2868289483219690466[42] = 0; + out_2868289483219690466[43] = 0; + out_2868289483219690466[44] = 0; + out_2868289483219690466[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_2868289483219690466[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_2868289483219690466[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2868289483219690466[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2868289483219690466[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_2868289483219690466[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_2868289483219690466[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_2868289483219690466[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_2868289483219690466[53] = -9.8100000000000005*dt; + out_2868289483219690466[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_2868289483219690466[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_2868289483219690466[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2868289483219690466[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2868289483219690466[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_2868289483219690466[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_2868289483219690466[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_2868289483219690466[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2868289483219690466[62] = 0; + out_2868289483219690466[63] = 0; + out_2868289483219690466[64] = 0; + out_2868289483219690466[65] = 0; + out_2868289483219690466[66] = 0; + out_2868289483219690466[67] = 0; + out_2868289483219690466[68] = 0; + out_2868289483219690466[69] = 0; + out_2868289483219690466[70] = 1; + out_2868289483219690466[71] = 0; + out_2868289483219690466[72] = 0; + out_2868289483219690466[73] = 0; + out_2868289483219690466[74] = 0; + out_2868289483219690466[75] = 0; + out_2868289483219690466[76] = 0; + out_2868289483219690466[77] = 0; + out_2868289483219690466[78] = 0; + out_2868289483219690466[79] = 0; + out_2868289483219690466[80] = 1; } -void h_25(double *state, double *unused, double *out_5555793351249648290) { - out_5555793351249648290[0] = state[6]; +void h_25(double *state, double *unused, double *out_7797736608546404621) { + out_7797736608546404621[0] = state[6]; } -void H_25(double *state, double *unused, double *out_4322311178595995255) { - out_4322311178595995255[0] = 0; - out_4322311178595995255[1] = 0; - out_4322311178595995255[2] = 0; - out_4322311178595995255[3] = 0; - out_4322311178595995255[4] = 0; - out_4322311178595995255[5] = 0; - out_4322311178595995255[6] = 1; - out_4322311178595995255[7] = 0; - out_4322311178595995255[8] = 0; +void H_25(double *state, double *unused, double *out_5931441967274123398) { + out_5931441967274123398[0] = 0; + out_5931441967274123398[1] = 0; + out_5931441967274123398[2] = 0; + out_5931441967274123398[3] = 0; + out_5931441967274123398[4] = 0; + out_5931441967274123398[5] = 0; + out_5931441967274123398[6] = 1; + out_5931441967274123398[7] = 0; + out_5931441967274123398[8] = 0; } -void h_24(double *state, double *unused, double *out_8637534363345985562) { - out_8637534363345985562[0] = state[4]; - out_8637534363345985562[1] = state[5]; +void h_24(double *state, double *unused, double *out_9133422513623254856) { + out_9133422513623254856[0] = state[4]; + out_9133422513623254856[1] = state[5]; } -void H_24(double *state, double *unused, double *out_2909282930557586418) { - out_2909282930557586418[0] = 0; - out_2909282930557586418[1] = 0; - out_2909282930557586418[2] = 0; - out_2909282930557586418[3] = 0; - out_2909282930557586418[4] = 1; - out_2909282930557586418[5] = 0; - out_2909282930557586418[6] = 0; - out_2909282930557586418[7] = 0; - out_2909282930557586418[8] = 0; - out_2909282930557586418[9] = 0; - out_2909282930557586418[10] = 0; - out_2909282930557586418[11] = 0; - out_2909282930557586418[12] = 0; - out_2909282930557586418[13] = 0; - out_2909282930557586418[14] = 1; - out_2909282930557586418[15] = 0; - out_2909282930557586418[16] = 0; - out_2909282930557586418[17] = 0; +void H_24(double *state, double *unused, double *out_9101010594292373758) { + out_9101010594292373758[0] = 0; + out_9101010594292373758[1] = 0; + out_9101010594292373758[2] = 0; + out_9101010594292373758[3] = 0; + out_9101010594292373758[4] = 1; + out_9101010594292373758[5] = 0; + out_9101010594292373758[6] = 0; + out_9101010594292373758[7] = 0; + out_9101010594292373758[8] = 0; + out_9101010594292373758[9] = 0; + out_9101010594292373758[10] = 0; + out_9101010594292373758[11] = 0; + out_9101010594292373758[12] = 0; + out_9101010594292373758[13] = 0; + out_9101010594292373758[14] = 1; + out_9101010594292373758[15] = 0; + out_9101010594292373758[16] = 0; + out_9101010594292373758[17] = 0; } -void h_30(double *state, double *unused, double *out_5127685465639230885) { - out_5127685465639230885[0] = state[4]; +void h_30(double *state, double *unused, double *out_2885742208342474554) { + out_2885742208342474554[0] = state[4]; } -void H_30(double *state, double *unused, double *out_8850007508723603453) { - out_8850007508723603453[0] = 0; - out_8850007508723603453[1] = 0; - out_8850007508723603453[2] = 0; - out_8850007508723603453[3] = 0; - out_8850007508723603453[4] = 1; - out_8850007508723603453[5] = 0; - out_8850007508723603453[6] = 0; - out_8850007508723603453[7] = 0; - out_8850007508723603453[8] = 0; +void H_30(double *state, double *unused, double *out_3413109008766874771) { + out_3413109008766874771[0] = 0; + out_3413109008766874771[1] = 0; + out_3413109008766874771[2] = 0; + out_3413109008766874771[3] = 0; + out_3413109008766874771[4] = 1; + out_3413109008766874771[5] = 0; + out_3413109008766874771[6] = 0; + out_3413109008766874771[7] = 0; + out_3413109008766874771[8] = 0; } -void h_26(double *state, double *unused, double *out_4524369580962753368) { - out_4524369580962753368[0] = state[7]; +void h_26(double *state, double *unused, double *out_4918137225453201081) { + out_4918137225453201081[0] = state[7]; } -void H_26(double *state, double *unused, double *out_8063814497470051479) { - out_8063814497470051479[0] = 0; - out_8063814497470051479[1] = 0; - out_8063814497470051479[2] = 0; - out_8063814497470051479[3] = 0; - out_8063814497470051479[4] = 0; - out_8063814497470051479[5] = 0; - out_8063814497470051479[6] = 0; - out_8063814497470051479[7] = 1; - out_8063814497470051479[8] = 0; +void H_26(double *state, double *unused, double *out_8773798787561371994) { + out_8773798787561371994[0] = 0; + out_8773798787561371994[1] = 0; + out_8773798787561371994[2] = 0; + out_8773798787561371994[3] = 0; + out_8773798787561371994[4] = 0; + out_8773798787561371994[5] = 0; + out_8773798787561371994[6] = 0; + out_8773798787561371994[7] = 1; + out_8773798787561371994[8] = 0; } -void h_27(double *state, double *unused, double *out_3908981550453062561) { - out_3908981550453062561[0] = state[3]; +void h_27(double *state, double *unused, double *out_8741246461448067056) { + out_8741246461448067056[0] = state[3]; } -void H_27(double *state, double *unused, double *out_7421973253185523252) { - out_7421973253185523252[0] = 0; - out_7421973253185523252[1] = 0; - out_7421973253185523252[2] = 0; - out_7421973253185523252[3] = 1; - out_7421973253185523252[4] = 0; - out_7421973253185523252[5] = 0; - out_7421973253185523252[6] = 0; - out_7421973253185523252[7] = 0; - out_7421973253185523252[8] = 0; +void H_27(double *state, double *unused, double *out_5587872320567299682) { + out_5587872320567299682[0] = 0; + out_5587872320567299682[1] = 0; + out_5587872320567299682[2] = 0; + out_5587872320567299682[3] = 1; + out_5587872320567299682[4] = 0; + out_5587872320567299682[5] = 0; + out_5587872320567299682[6] = 0; + out_5587872320567299682[7] = 0; + out_5587872320567299682[8] = 0; } -void h_29(double *state, double *unused, double *out_3307563392839699007) { - out_3307563392839699007[0] = state[1]; +void h_29(double *state, double *unused, double *out_3097181630963667199) { + out_3097181630963667199[0] = state[1]; } -void H_29(double *state, double *unused, double *out_8339776164409211269) { - out_8339776164409211269[0] = 0; - out_8339776164409211269[1] = 1; - out_8339776164409211269[2] = 0; - out_8339776164409211269[3] = 0; - out_8339776164409211269[4] = 0; - out_8339776164409211269[5] = 0; - out_8339776164409211269[6] = 0; - out_8339776164409211269[7] = 0; - out_8339776164409211269[8] = 0; +void H_29(double *state, double *unused, double *out_2902877664452482587) { + out_2902877664452482587[0] = 0; + out_2902877664452482587[1] = 1; + out_2902877664452482587[2] = 0; + out_2902877664452482587[3] = 0; + out_2902877664452482587[4] = 0; + out_2902877664452482587[5] = 0; + out_2902877664452482587[6] = 0; + out_2902877664452482587[7] = 0; + out_2902877664452482587[8] = 0; } -void h_28(double *state, double *unused, double *out_8910654818113908206) { - out_8910654818113908206[0] = state[0]; +void h_28(double *state, double *unused, double *out_6668711560817151875) { + out_6668711560817151875[0] = state[0]; } -void H_28(double *state, double *unused, double *out_6376145892843885018) { - out_6376145892843885018[0] = 1; - out_6376145892843885018[1] = 0; - out_6376145892843885018[2] = 0; - out_6376145892843885018[3] = 0; - out_6376145892843885018[4] = 0; - out_6376145892843885018[5] = 0; - out_6376145892843885018[6] = 0; - out_6376145892843885018[7] = 0; - out_6376145892843885018[8] = 0; +void H_28(double *state, double *unused, double *out_7985276681522013161) { + out_7985276681522013161[0] = 1; + out_7985276681522013161[1] = 0; + out_7985276681522013161[2] = 0; + out_7985276681522013161[3] = 0; + out_7985276681522013161[4] = 0; + out_7985276681522013161[5] = 0; + out_7985276681522013161[6] = 0; + out_7985276681522013161[7] = 0; + out_7985276681522013161[8] = 0; } -void h_31(double *state, double *unused, double *out_357939675421100083) { - out_357939675421100083[0] = state[8]; +void h_31(double *state, double *unused, double *out_3787573678896021836) { + out_3787573678896021836[0] = state[8]; } -void H_31(double *state, double *unused, double *out_8690022599703402955) { - out_8690022599703402955[0] = 0; - out_8690022599703402955[1] = 0; - out_8690022599703402955[2] = 0; - out_8690022599703402955[3] = 0; - out_8690022599703402955[4] = 0; - out_8690022599703402955[5] = 0; - out_8690022599703402955[6] = 0; - out_8690022599703402955[7] = 0; - out_8690022599703402955[8] = 1; +void H_31(double *state, double *unused, double *out_8147590685328020518) { + out_8147590685328020518[0] = 0; + out_8147590685328020518[1] = 0; + out_8147590685328020518[2] = 0; + out_8147590685328020518[3] = 0; + out_8147590685328020518[4] = 0; + out_8147590685328020518[5] = 0; + out_8147590685328020518[6] = 0; + out_8147590685328020518[7] = 0; + out_8147590685328020518[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_5032200201446894984) { - err_fun(nom_x, delta_x, out_5032200201446894984); +void car_err_fun(double *nom_x, double *delta_x, double *out_1832524583205975398) { + err_fun(nom_x, delta_x, out_1832524583205975398); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_7143447445886649280) { - inv_err_fun(nom_x, true_x, out_7143447445886649280); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_970452396869078486) { + inv_err_fun(nom_x, true_x, out_970452396869078486); } -void car_H_mod_fun(double *state, double *out_1685754253580569061) { - H_mod_fun(state, out_1685754253580569061); +void car_H_mod_fun(double *state, double *out_4593964073461568413) { + H_mod_fun(state, out_4593964073461568413); } -void car_f_fun(double *state, double dt, double *out_8909179533363476452) { - f_fun(state, dt, out_8909179533363476452); +void car_f_fun(double *state, double dt, double *out_231308644161984314) { + f_fun(state, dt, out_231308644161984314); } -void car_F_fun(double *state, double dt, double *out_3419672683889849262) { - F_fun(state, dt, out_3419672683889849262); +void car_F_fun(double *state, double dt, double *out_2868289483219690466) { + F_fun(state, dt, out_2868289483219690466); } -void car_h_25(double *state, double *unused, double *out_5555793351249648290) { - h_25(state, unused, out_5555793351249648290); +void car_h_25(double *state, double *unused, double *out_7797736608546404621) { + h_25(state, unused, out_7797736608546404621); } -void car_H_25(double *state, double *unused, double *out_4322311178595995255) { - H_25(state, unused, out_4322311178595995255); +void car_H_25(double *state, double *unused, double *out_5931441967274123398) { + H_25(state, unused, out_5931441967274123398); } -void car_h_24(double *state, double *unused, double *out_8637534363345985562) { - h_24(state, unused, out_8637534363345985562); +void car_h_24(double *state, double *unused, double *out_9133422513623254856) { + h_24(state, unused, out_9133422513623254856); } -void car_H_24(double *state, double *unused, double *out_2909282930557586418) { - H_24(state, unused, out_2909282930557586418); +void car_H_24(double *state, double *unused, double *out_9101010594292373758) { + H_24(state, unused, out_9101010594292373758); } -void car_h_30(double *state, double *unused, double *out_5127685465639230885) { - h_30(state, unused, out_5127685465639230885); +void car_h_30(double *state, double *unused, double *out_2885742208342474554) { + h_30(state, unused, out_2885742208342474554); } -void car_H_30(double *state, double *unused, double *out_8850007508723603453) { - H_30(state, unused, out_8850007508723603453); +void car_H_30(double *state, double *unused, double *out_3413109008766874771) { + H_30(state, unused, out_3413109008766874771); } -void car_h_26(double *state, double *unused, double *out_4524369580962753368) { - h_26(state, unused, out_4524369580962753368); +void car_h_26(double *state, double *unused, double *out_4918137225453201081) { + h_26(state, unused, out_4918137225453201081); } -void car_H_26(double *state, double *unused, double *out_8063814497470051479) { - H_26(state, unused, out_8063814497470051479); +void car_H_26(double *state, double *unused, double *out_8773798787561371994) { + H_26(state, unused, out_8773798787561371994); } -void car_h_27(double *state, double *unused, double *out_3908981550453062561) { - h_27(state, unused, out_3908981550453062561); +void car_h_27(double *state, double *unused, double *out_8741246461448067056) { + h_27(state, unused, out_8741246461448067056); } -void car_H_27(double *state, double *unused, double *out_7421973253185523252) { - H_27(state, unused, out_7421973253185523252); +void car_H_27(double *state, double *unused, double *out_5587872320567299682) { + H_27(state, unused, out_5587872320567299682); } -void car_h_29(double *state, double *unused, double *out_3307563392839699007) { - h_29(state, unused, out_3307563392839699007); +void car_h_29(double *state, double *unused, double *out_3097181630963667199) { + h_29(state, unused, out_3097181630963667199); } -void car_H_29(double *state, double *unused, double *out_8339776164409211269) { - H_29(state, unused, out_8339776164409211269); +void car_H_29(double *state, double *unused, double *out_2902877664452482587) { + H_29(state, unused, out_2902877664452482587); } -void car_h_28(double *state, double *unused, double *out_8910654818113908206) { - h_28(state, unused, out_8910654818113908206); +void car_h_28(double *state, double *unused, double *out_6668711560817151875) { + h_28(state, unused, out_6668711560817151875); } -void car_H_28(double *state, double *unused, double *out_6376145892843885018) { - H_28(state, unused, out_6376145892843885018); +void car_H_28(double *state, double *unused, double *out_7985276681522013161) { + H_28(state, unused, out_7985276681522013161); } -void car_h_31(double *state, double *unused, double *out_357939675421100083) { - h_31(state, unused, out_357939675421100083); +void car_h_31(double *state, double *unused, double *out_3787573678896021836) { + h_31(state, unused, out_3787573678896021836); } -void car_H_31(double *state, double *unused, double *out_8690022599703402955) { - H_31(state, unused, out_8690022599703402955); +void car_H_31(double *state, double *unused, double *out_8147590685328020518) { + H_31(state, unused, out_8147590685328020518); } 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/openpilot/selfdrive/locationd/models/generated/car.h b/openpilot/selfdrive/locationd/models/generated/car.h index 99bac5b9d3..c8f7419a6c 100644 --- a/openpilot/selfdrive/locationd/models/generated/car.h +++ b/openpilot/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_5032200201446894984); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_7143447445886649280); -void car_H_mod_fun(double *state, double *out_1685754253580569061); -void car_f_fun(double *state, double dt, double *out_8909179533363476452); -void car_F_fun(double *state, double dt, double *out_3419672683889849262); -void car_h_25(double *state, double *unused, double *out_5555793351249648290); -void car_H_25(double *state, double *unused, double *out_4322311178595995255); -void car_h_24(double *state, double *unused, double *out_8637534363345985562); -void car_H_24(double *state, double *unused, double *out_2909282930557586418); -void car_h_30(double *state, double *unused, double *out_5127685465639230885); -void car_H_30(double *state, double *unused, double *out_8850007508723603453); -void car_h_26(double *state, double *unused, double *out_4524369580962753368); -void car_H_26(double *state, double *unused, double *out_8063814497470051479); -void car_h_27(double *state, double *unused, double *out_3908981550453062561); -void car_H_27(double *state, double *unused, double *out_7421973253185523252); -void car_h_29(double *state, double *unused, double *out_3307563392839699007); -void car_H_29(double *state, double *unused, double *out_8339776164409211269); -void car_h_28(double *state, double *unused, double *out_8910654818113908206); -void car_H_28(double *state, double *unused, double *out_6376145892843885018); -void car_h_31(double *state, double *unused, double *out_357939675421100083); -void car_H_31(double *state, double *unused, double *out_8690022599703402955); +void car_err_fun(double *nom_x, double *delta_x, double *out_1832524583205975398); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_970452396869078486); +void car_H_mod_fun(double *state, double *out_4593964073461568413); +void car_f_fun(double *state, double dt, double *out_231308644161984314); +void car_F_fun(double *state, double dt, double *out_2868289483219690466); +void car_h_25(double *state, double *unused, double *out_7797736608546404621); +void car_H_25(double *state, double *unused, double *out_5931441967274123398); +void car_h_24(double *state, double *unused, double *out_9133422513623254856); +void car_H_24(double *state, double *unused, double *out_9101010594292373758); +void car_h_30(double *state, double *unused, double *out_2885742208342474554); +void car_H_30(double *state, double *unused, double *out_3413109008766874771); +void car_h_26(double *state, double *unused, double *out_4918137225453201081); +void car_H_26(double *state, double *unused, double *out_8773798787561371994); +void car_h_27(double *state, double *unused, double *out_8741246461448067056); +void car_H_27(double *state, double *unused, double *out_5587872320567299682); +void car_h_29(double *state, double *unused, double *out_3097181630963667199); +void car_H_29(double *state, double *unused, double *out_2902877664452482587); +void car_h_28(double *state, double *unused, double *out_6668711560817151875); +void car_H_28(double *state, double *unused, double *out_7985276681522013161); +void car_h_31(double *state, double *unused, double *out_3787573678896021836); +void car_H_31(double *state, double *unused, double *out_8147590685328020518); 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/openpilot/selfdrive/locationd/models/generated/libcar.so b/openpilot/selfdrive/locationd/models/generated/libcar.so index 0d482ecb10..989520c49f 100755 Binary files a/openpilot/selfdrive/locationd/models/generated/libcar.so and b/openpilot/selfdrive/locationd/models/generated/libcar.so differ diff --git a/openpilot/selfdrive/locationd/models/generated/libpose.so b/openpilot/selfdrive/locationd/models/generated/libpose.so index ce5bc58a04..8fd237503d 100755 Binary files a/openpilot/selfdrive/locationd/models/generated/libpose.so and b/openpilot/selfdrive/locationd/models/generated/libpose.so differ diff --git a/openpilot/selfdrive/locationd/models/generated/pose.cpp b/openpilot/selfdrive/locationd/models/generated/pose.cpp index 18886d94e6..4c41ea3b99 100644 --- a/openpilot/selfdrive/locationd/models/generated/pose.cpp +++ b/openpilot/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_5239387107130576177) { - out_5239387107130576177[0] = delta_x[0] + nom_x[0]; - out_5239387107130576177[1] = delta_x[1] + nom_x[1]; - out_5239387107130576177[2] = delta_x[2] + nom_x[2]; - out_5239387107130576177[3] = delta_x[3] + nom_x[3]; - out_5239387107130576177[4] = delta_x[4] + nom_x[4]; - out_5239387107130576177[5] = delta_x[5] + nom_x[5]; - out_5239387107130576177[6] = delta_x[6] + nom_x[6]; - out_5239387107130576177[7] = delta_x[7] + nom_x[7]; - out_5239387107130576177[8] = delta_x[8] + nom_x[8]; - out_5239387107130576177[9] = delta_x[9] + nom_x[9]; - out_5239387107130576177[10] = delta_x[10] + nom_x[10]; - out_5239387107130576177[11] = delta_x[11] + nom_x[11]; - out_5239387107130576177[12] = delta_x[12] + nom_x[12]; - out_5239387107130576177[13] = delta_x[13] + nom_x[13]; - out_5239387107130576177[14] = delta_x[14] + nom_x[14]; - out_5239387107130576177[15] = delta_x[15] + nom_x[15]; - out_5239387107130576177[16] = delta_x[16] + nom_x[16]; - out_5239387107130576177[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_2903893253895971355) { + out_2903893253895971355[0] = delta_x[0] + nom_x[0]; + out_2903893253895971355[1] = delta_x[1] + nom_x[1]; + out_2903893253895971355[2] = delta_x[2] + nom_x[2]; + out_2903893253895971355[3] = delta_x[3] + nom_x[3]; + out_2903893253895971355[4] = delta_x[4] + nom_x[4]; + out_2903893253895971355[5] = delta_x[5] + nom_x[5]; + out_2903893253895971355[6] = delta_x[6] + nom_x[6]; + out_2903893253895971355[7] = delta_x[7] + nom_x[7]; + out_2903893253895971355[8] = delta_x[8] + nom_x[8]; + out_2903893253895971355[9] = delta_x[9] + nom_x[9]; + out_2903893253895971355[10] = delta_x[10] + nom_x[10]; + out_2903893253895971355[11] = delta_x[11] + nom_x[11]; + out_2903893253895971355[12] = delta_x[12] + nom_x[12]; + out_2903893253895971355[13] = delta_x[13] + nom_x[13]; + out_2903893253895971355[14] = delta_x[14] + nom_x[14]; + out_2903893253895971355[15] = delta_x[15] + nom_x[15]; + out_2903893253895971355[16] = delta_x[16] + nom_x[16]; + out_2903893253895971355[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_8017465374679690286) { - out_8017465374679690286[0] = -nom_x[0] + true_x[0]; - out_8017465374679690286[1] = -nom_x[1] + true_x[1]; - out_8017465374679690286[2] = -nom_x[2] + true_x[2]; - out_8017465374679690286[3] = -nom_x[3] + true_x[3]; - out_8017465374679690286[4] = -nom_x[4] + true_x[4]; - out_8017465374679690286[5] = -nom_x[5] + true_x[5]; - out_8017465374679690286[6] = -nom_x[6] + true_x[6]; - out_8017465374679690286[7] = -nom_x[7] + true_x[7]; - out_8017465374679690286[8] = -nom_x[8] + true_x[8]; - out_8017465374679690286[9] = -nom_x[9] + true_x[9]; - out_8017465374679690286[10] = -nom_x[10] + true_x[10]; - out_8017465374679690286[11] = -nom_x[11] + true_x[11]; - out_8017465374679690286[12] = -nom_x[12] + true_x[12]; - out_8017465374679690286[13] = -nom_x[13] + true_x[13]; - out_8017465374679690286[14] = -nom_x[14] + true_x[14]; - out_8017465374679690286[15] = -nom_x[15] + true_x[15]; - out_8017465374679690286[16] = -nom_x[16] + true_x[16]; - out_8017465374679690286[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_3928278332152438487) { + out_3928278332152438487[0] = -nom_x[0] + true_x[0]; + out_3928278332152438487[1] = -nom_x[1] + true_x[1]; + out_3928278332152438487[2] = -nom_x[2] + true_x[2]; + out_3928278332152438487[3] = -nom_x[3] + true_x[3]; + out_3928278332152438487[4] = -nom_x[4] + true_x[4]; + out_3928278332152438487[5] = -nom_x[5] + true_x[5]; + out_3928278332152438487[6] = -nom_x[6] + true_x[6]; + out_3928278332152438487[7] = -nom_x[7] + true_x[7]; + out_3928278332152438487[8] = -nom_x[8] + true_x[8]; + out_3928278332152438487[9] = -nom_x[9] + true_x[9]; + out_3928278332152438487[10] = -nom_x[10] + true_x[10]; + out_3928278332152438487[11] = -nom_x[11] + true_x[11]; + out_3928278332152438487[12] = -nom_x[12] + true_x[12]; + out_3928278332152438487[13] = -nom_x[13] + true_x[13]; + out_3928278332152438487[14] = -nom_x[14] + true_x[14]; + out_3928278332152438487[15] = -nom_x[15] + true_x[15]; + out_3928278332152438487[16] = -nom_x[16] + true_x[16]; + out_3928278332152438487[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_3050146727026118690) { - out_3050146727026118690[0] = 1.0; - out_3050146727026118690[1] = 0.0; - out_3050146727026118690[2] = 0.0; - out_3050146727026118690[3] = 0.0; - out_3050146727026118690[4] = 0.0; - out_3050146727026118690[5] = 0.0; - out_3050146727026118690[6] = 0.0; - out_3050146727026118690[7] = 0.0; - out_3050146727026118690[8] = 0.0; - out_3050146727026118690[9] = 0.0; - out_3050146727026118690[10] = 0.0; - out_3050146727026118690[11] = 0.0; - out_3050146727026118690[12] = 0.0; - out_3050146727026118690[13] = 0.0; - out_3050146727026118690[14] = 0.0; - out_3050146727026118690[15] = 0.0; - out_3050146727026118690[16] = 0.0; - out_3050146727026118690[17] = 0.0; - out_3050146727026118690[18] = 0.0; - out_3050146727026118690[19] = 1.0; - out_3050146727026118690[20] = 0.0; - out_3050146727026118690[21] = 0.0; - out_3050146727026118690[22] = 0.0; - out_3050146727026118690[23] = 0.0; - out_3050146727026118690[24] = 0.0; - out_3050146727026118690[25] = 0.0; - out_3050146727026118690[26] = 0.0; - out_3050146727026118690[27] = 0.0; - out_3050146727026118690[28] = 0.0; - out_3050146727026118690[29] = 0.0; - out_3050146727026118690[30] = 0.0; - out_3050146727026118690[31] = 0.0; - out_3050146727026118690[32] = 0.0; - out_3050146727026118690[33] = 0.0; - out_3050146727026118690[34] = 0.0; - out_3050146727026118690[35] = 0.0; - out_3050146727026118690[36] = 0.0; - out_3050146727026118690[37] = 0.0; - out_3050146727026118690[38] = 1.0; - out_3050146727026118690[39] = 0.0; - out_3050146727026118690[40] = 0.0; - out_3050146727026118690[41] = 0.0; - out_3050146727026118690[42] = 0.0; - out_3050146727026118690[43] = 0.0; - out_3050146727026118690[44] = 0.0; - out_3050146727026118690[45] = 0.0; - out_3050146727026118690[46] = 0.0; - out_3050146727026118690[47] = 0.0; - out_3050146727026118690[48] = 0.0; - out_3050146727026118690[49] = 0.0; - out_3050146727026118690[50] = 0.0; - out_3050146727026118690[51] = 0.0; - out_3050146727026118690[52] = 0.0; - out_3050146727026118690[53] = 0.0; - out_3050146727026118690[54] = 0.0; - out_3050146727026118690[55] = 0.0; - out_3050146727026118690[56] = 0.0; - out_3050146727026118690[57] = 1.0; - out_3050146727026118690[58] = 0.0; - out_3050146727026118690[59] = 0.0; - out_3050146727026118690[60] = 0.0; - out_3050146727026118690[61] = 0.0; - out_3050146727026118690[62] = 0.0; - out_3050146727026118690[63] = 0.0; - out_3050146727026118690[64] = 0.0; - out_3050146727026118690[65] = 0.0; - out_3050146727026118690[66] = 0.0; - out_3050146727026118690[67] = 0.0; - out_3050146727026118690[68] = 0.0; - out_3050146727026118690[69] = 0.0; - out_3050146727026118690[70] = 0.0; - out_3050146727026118690[71] = 0.0; - out_3050146727026118690[72] = 0.0; - out_3050146727026118690[73] = 0.0; - out_3050146727026118690[74] = 0.0; - out_3050146727026118690[75] = 0.0; - out_3050146727026118690[76] = 1.0; - out_3050146727026118690[77] = 0.0; - out_3050146727026118690[78] = 0.0; - out_3050146727026118690[79] = 0.0; - out_3050146727026118690[80] = 0.0; - out_3050146727026118690[81] = 0.0; - out_3050146727026118690[82] = 0.0; - out_3050146727026118690[83] = 0.0; - out_3050146727026118690[84] = 0.0; - out_3050146727026118690[85] = 0.0; - out_3050146727026118690[86] = 0.0; - out_3050146727026118690[87] = 0.0; - out_3050146727026118690[88] = 0.0; - out_3050146727026118690[89] = 0.0; - out_3050146727026118690[90] = 0.0; - out_3050146727026118690[91] = 0.0; - out_3050146727026118690[92] = 0.0; - out_3050146727026118690[93] = 0.0; - out_3050146727026118690[94] = 0.0; - out_3050146727026118690[95] = 1.0; - out_3050146727026118690[96] = 0.0; - out_3050146727026118690[97] = 0.0; - out_3050146727026118690[98] = 0.0; - out_3050146727026118690[99] = 0.0; - out_3050146727026118690[100] = 0.0; - out_3050146727026118690[101] = 0.0; - out_3050146727026118690[102] = 0.0; - out_3050146727026118690[103] = 0.0; - out_3050146727026118690[104] = 0.0; - out_3050146727026118690[105] = 0.0; - out_3050146727026118690[106] = 0.0; - out_3050146727026118690[107] = 0.0; - out_3050146727026118690[108] = 0.0; - out_3050146727026118690[109] = 0.0; - out_3050146727026118690[110] = 0.0; - out_3050146727026118690[111] = 0.0; - out_3050146727026118690[112] = 0.0; - out_3050146727026118690[113] = 0.0; - out_3050146727026118690[114] = 1.0; - out_3050146727026118690[115] = 0.0; - out_3050146727026118690[116] = 0.0; - out_3050146727026118690[117] = 0.0; - out_3050146727026118690[118] = 0.0; - out_3050146727026118690[119] = 0.0; - out_3050146727026118690[120] = 0.0; - out_3050146727026118690[121] = 0.0; - out_3050146727026118690[122] = 0.0; - out_3050146727026118690[123] = 0.0; - out_3050146727026118690[124] = 0.0; - out_3050146727026118690[125] = 0.0; - out_3050146727026118690[126] = 0.0; - out_3050146727026118690[127] = 0.0; - out_3050146727026118690[128] = 0.0; - out_3050146727026118690[129] = 0.0; - out_3050146727026118690[130] = 0.0; - out_3050146727026118690[131] = 0.0; - out_3050146727026118690[132] = 0.0; - out_3050146727026118690[133] = 1.0; - out_3050146727026118690[134] = 0.0; - out_3050146727026118690[135] = 0.0; - out_3050146727026118690[136] = 0.0; - out_3050146727026118690[137] = 0.0; - out_3050146727026118690[138] = 0.0; - out_3050146727026118690[139] = 0.0; - out_3050146727026118690[140] = 0.0; - out_3050146727026118690[141] = 0.0; - out_3050146727026118690[142] = 0.0; - out_3050146727026118690[143] = 0.0; - out_3050146727026118690[144] = 0.0; - out_3050146727026118690[145] = 0.0; - out_3050146727026118690[146] = 0.0; - out_3050146727026118690[147] = 0.0; - out_3050146727026118690[148] = 0.0; - out_3050146727026118690[149] = 0.0; - out_3050146727026118690[150] = 0.0; - out_3050146727026118690[151] = 0.0; - out_3050146727026118690[152] = 1.0; - out_3050146727026118690[153] = 0.0; - out_3050146727026118690[154] = 0.0; - out_3050146727026118690[155] = 0.0; - out_3050146727026118690[156] = 0.0; - out_3050146727026118690[157] = 0.0; - out_3050146727026118690[158] = 0.0; - out_3050146727026118690[159] = 0.0; - out_3050146727026118690[160] = 0.0; - out_3050146727026118690[161] = 0.0; - out_3050146727026118690[162] = 0.0; - out_3050146727026118690[163] = 0.0; - out_3050146727026118690[164] = 0.0; - out_3050146727026118690[165] = 0.0; - out_3050146727026118690[166] = 0.0; - out_3050146727026118690[167] = 0.0; - out_3050146727026118690[168] = 0.0; - out_3050146727026118690[169] = 0.0; - out_3050146727026118690[170] = 0.0; - out_3050146727026118690[171] = 1.0; - out_3050146727026118690[172] = 0.0; - out_3050146727026118690[173] = 0.0; - out_3050146727026118690[174] = 0.0; - out_3050146727026118690[175] = 0.0; - out_3050146727026118690[176] = 0.0; - out_3050146727026118690[177] = 0.0; - out_3050146727026118690[178] = 0.0; - out_3050146727026118690[179] = 0.0; - out_3050146727026118690[180] = 0.0; - out_3050146727026118690[181] = 0.0; - out_3050146727026118690[182] = 0.0; - out_3050146727026118690[183] = 0.0; - out_3050146727026118690[184] = 0.0; - out_3050146727026118690[185] = 0.0; - out_3050146727026118690[186] = 0.0; - out_3050146727026118690[187] = 0.0; - out_3050146727026118690[188] = 0.0; - out_3050146727026118690[189] = 0.0; - out_3050146727026118690[190] = 1.0; - out_3050146727026118690[191] = 0.0; - out_3050146727026118690[192] = 0.0; - out_3050146727026118690[193] = 0.0; - out_3050146727026118690[194] = 0.0; - out_3050146727026118690[195] = 0.0; - out_3050146727026118690[196] = 0.0; - out_3050146727026118690[197] = 0.0; - out_3050146727026118690[198] = 0.0; - out_3050146727026118690[199] = 0.0; - out_3050146727026118690[200] = 0.0; - out_3050146727026118690[201] = 0.0; - out_3050146727026118690[202] = 0.0; - out_3050146727026118690[203] = 0.0; - out_3050146727026118690[204] = 0.0; - out_3050146727026118690[205] = 0.0; - out_3050146727026118690[206] = 0.0; - out_3050146727026118690[207] = 0.0; - out_3050146727026118690[208] = 0.0; - out_3050146727026118690[209] = 1.0; - out_3050146727026118690[210] = 0.0; - out_3050146727026118690[211] = 0.0; - out_3050146727026118690[212] = 0.0; - out_3050146727026118690[213] = 0.0; - out_3050146727026118690[214] = 0.0; - out_3050146727026118690[215] = 0.0; - out_3050146727026118690[216] = 0.0; - out_3050146727026118690[217] = 0.0; - out_3050146727026118690[218] = 0.0; - out_3050146727026118690[219] = 0.0; - out_3050146727026118690[220] = 0.0; - out_3050146727026118690[221] = 0.0; - out_3050146727026118690[222] = 0.0; - out_3050146727026118690[223] = 0.0; - out_3050146727026118690[224] = 0.0; - out_3050146727026118690[225] = 0.0; - out_3050146727026118690[226] = 0.0; - out_3050146727026118690[227] = 0.0; - out_3050146727026118690[228] = 1.0; - out_3050146727026118690[229] = 0.0; - out_3050146727026118690[230] = 0.0; - out_3050146727026118690[231] = 0.0; - out_3050146727026118690[232] = 0.0; - out_3050146727026118690[233] = 0.0; - out_3050146727026118690[234] = 0.0; - out_3050146727026118690[235] = 0.0; - out_3050146727026118690[236] = 0.0; - out_3050146727026118690[237] = 0.0; - out_3050146727026118690[238] = 0.0; - out_3050146727026118690[239] = 0.0; - out_3050146727026118690[240] = 0.0; - out_3050146727026118690[241] = 0.0; - out_3050146727026118690[242] = 0.0; - out_3050146727026118690[243] = 0.0; - out_3050146727026118690[244] = 0.0; - out_3050146727026118690[245] = 0.0; - out_3050146727026118690[246] = 0.0; - out_3050146727026118690[247] = 1.0; - out_3050146727026118690[248] = 0.0; - out_3050146727026118690[249] = 0.0; - out_3050146727026118690[250] = 0.0; - out_3050146727026118690[251] = 0.0; - out_3050146727026118690[252] = 0.0; - out_3050146727026118690[253] = 0.0; - out_3050146727026118690[254] = 0.0; - out_3050146727026118690[255] = 0.0; - out_3050146727026118690[256] = 0.0; - out_3050146727026118690[257] = 0.0; - out_3050146727026118690[258] = 0.0; - out_3050146727026118690[259] = 0.0; - out_3050146727026118690[260] = 0.0; - out_3050146727026118690[261] = 0.0; - out_3050146727026118690[262] = 0.0; - out_3050146727026118690[263] = 0.0; - out_3050146727026118690[264] = 0.0; - out_3050146727026118690[265] = 0.0; - out_3050146727026118690[266] = 1.0; - out_3050146727026118690[267] = 0.0; - out_3050146727026118690[268] = 0.0; - out_3050146727026118690[269] = 0.0; - out_3050146727026118690[270] = 0.0; - out_3050146727026118690[271] = 0.0; - out_3050146727026118690[272] = 0.0; - out_3050146727026118690[273] = 0.0; - out_3050146727026118690[274] = 0.0; - out_3050146727026118690[275] = 0.0; - out_3050146727026118690[276] = 0.0; - out_3050146727026118690[277] = 0.0; - out_3050146727026118690[278] = 0.0; - out_3050146727026118690[279] = 0.0; - out_3050146727026118690[280] = 0.0; - out_3050146727026118690[281] = 0.0; - out_3050146727026118690[282] = 0.0; - out_3050146727026118690[283] = 0.0; - out_3050146727026118690[284] = 0.0; - out_3050146727026118690[285] = 1.0; - out_3050146727026118690[286] = 0.0; - out_3050146727026118690[287] = 0.0; - out_3050146727026118690[288] = 0.0; - out_3050146727026118690[289] = 0.0; - out_3050146727026118690[290] = 0.0; - out_3050146727026118690[291] = 0.0; - out_3050146727026118690[292] = 0.0; - out_3050146727026118690[293] = 0.0; - out_3050146727026118690[294] = 0.0; - out_3050146727026118690[295] = 0.0; - out_3050146727026118690[296] = 0.0; - out_3050146727026118690[297] = 0.0; - out_3050146727026118690[298] = 0.0; - out_3050146727026118690[299] = 0.0; - out_3050146727026118690[300] = 0.0; - out_3050146727026118690[301] = 0.0; - out_3050146727026118690[302] = 0.0; - out_3050146727026118690[303] = 0.0; - out_3050146727026118690[304] = 1.0; - out_3050146727026118690[305] = 0.0; - out_3050146727026118690[306] = 0.0; - out_3050146727026118690[307] = 0.0; - out_3050146727026118690[308] = 0.0; - out_3050146727026118690[309] = 0.0; - out_3050146727026118690[310] = 0.0; - out_3050146727026118690[311] = 0.0; - out_3050146727026118690[312] = 0.0; - out_3050146727026118690[313] = 0.0; - out_3050146727026118690[314] = 0.0; - out_3050146727026118690[315] = 0.0; - out_3050146727026118690[316] = 0.0; - out_3050146727026118690[317] = 0.0; - out_3050146727026118690[318] = 0.0; - out_3050146727026118690[319] = 0.0; - out_3050146727026118690[320] = 0.0; - out_3050146727026118690[321] = 0.0; - out_3050146727026118690[322] = 0.0; - out_3050146727026118690[323] = 1.0; +void H_mod_fun(double *state, double *out_5666984403538513454) { + out_5666984403538513454[0] = 1.0; + out_5666984403538513454[1] = 0.0; + out_5666984403538513454[2] = 0.0; + out_5666984403538513454[3] = 0.0; + out_5666984403538513454[4] = 0.0; + out_5666984403538513454[5] = 0.0; + out_5666984403538513454[6] = 0.0; + out_5666984403538513454[7] = 0.0; + out_5666984403538513454[8] = 0.0; + out_5666984403538513454[9] = 0.0; + out_5666984403538513454[10] = 0.0; + out_5666984403538513454[11] = 0.0; + out_5666984403538513454[12] = 0.0; + out_5666984403538513454[13] = 0.0; + out_5666984403538513454[14] = 0.0; + out_5666984403538513454[15] = 0.0; + out_5666984403538513454[16] = 0.0; + out_5666984403538513454[17] = 0.0; + out_5666984403538513454[18] = 0.0; + out_5666984403538513454[19] = 1.0; + out_5666984403538513454[20] = 0.0; + out_5666984403538513454[21] = 0.0; + out_5666984403538513454[22] = 0.0; + out_5666984403538513454[23] = 0.0; + out_5666984403538513454[24] = 0.0; + out_5666984403538513454[25] = 0.0; + out_5666984403538513454[26] = 0.0; + out_5666984403538513454[27] = 0.0; + out_5666984403538513454[28] = 0.0; + out_5666984403538513454[29] = 0.0; + out_5666984403538513454[30] = 0.0; + out_5666984403538513454[31] = 0.0; + out_5666984403538513454[32] = 0.0; + out_5666984403538513454[33] = 0.0; + out_5666984403538513454[34] = 0.0; + out_5666984403538513454[35] = 0.0; + out_5666984403538513454[36] = 0.0; + out_5666984403538513454[37] = 0.0; + out_5666984403538513454[38] = 1.0; + out_5666984403538513454[39] = 0.0; + out_5666984403538513454[40] = 0.0; + out_5666984403538513454[41] = 0.0; + out_5666984403538513454[42] = 0.0; + out_5666984403538513454[43] = 0.0; + out_5666984403538513454[44] = 0.0; + out_5666984403538513454[45] = 0.0; + out_5666984403538513454[46] = 0.0; + out_5666984403538513454[47] = 0.0; + out_5666984403538513454[48] = 0.0; + out_5666984403538513454[49] = 0.0; + out_5666984403538513454[50] = 0.0; + out_5666984403538513454[51] = 0.0; + out_5666984403538513454[52] = 0.0; + out_5666984403538513454[53] = 0.0; + out_5666984403538513454[54] = 0.0; + out_5666984403538513454[55] = 0.0; + out_5666984403538513454[56] = 0.0; + out_5666984403538513454[57] = 1.0; + out_5666984403538513454[58] = 0.0; + out_5666984403538513454[59] = 0.0; + out_5666984403538513454[60] = 0.0; + out_5666984403538513454[61] = 0.0; + out_5666984403538513454[62] = 0.0; + out_5666984403538513454[63] = 0.0; + out_5666984403538513454[64] = 0.0; + out_5666984403538513454[65] = 0.0; + out_5666984403538513454[66] = 0.0; + out_5666984403538513454[67] = 0.0; + out_5666984403538513454[68] = 0.0; + out_5666984403538513454[69] = 0.0; + out_5666984403538513454[70] = 0.0; + out_5666984403538513454[71] = 0.0; + out_5666984403538513454[72] = 0.0; + out_5666984403538513454[73] = 0.0; + out_5666984403538513454[74] = 0.0; + out_5666984403538513454[75] = 0.0; + out_5666984403538513454[76] = 1.0; + out_5666984403538513454[77] = 0.0; + out_5666984403538513454[78] = 0.0; + out_5666984403538513454[79] = 0.0; + out_5666984403538513454[80] = 0.0; + out_5666984403538513454[81] = 0.0; + out_5666984403538513454[82] = 0.0; + out_5666984403538513454[83] = 0.0; + out_5666984403538513454[84] = 0.0; + out_5666984403538513454[85] = 0.0; + out_5666984403538513454[86] = 0.0; + out_5666984403538513454[87] = 0.0; + out_5666984403538513454[88] = 0.0; + out_5666984403538513454[89] = 0.0; + out_5666984403538513454[90] = 0.0; + out_5666984403538513454[91] = 0.0; + out_5666984403538513454[92] = 0.0; + out_5666984403538513454[93] = 0.0; + out_5666984403538513454[94] = 0.0; + out_5666984403538513454[95] = 1.0; + out_5666984403538513454[96] = 0.0; + out_5666984403538513454[97] = 0.0; + out_5666984403538513454[98] = 0.0; + out_5666984403538513454[99] = 0.0; + out_5666984403538513454[100] = 0.0; + out_5666984403538513454[101] = 0.0; + out_5666984403538513454[102] = 0.0; + out_5666984403538513454[103] = 0.0; + out_5666984403538513454[104] = 0.0; + out_5666984403538513454[105] = 0.0; + out_5666984403538513454[106] = 0.0; + out_5666984403538513454[107] = 0.0; + out_5666984403538513454[108] = 0.0; + out_5666984403538513454[109] = 0.0; + out_5666984403538513454[110] = 0.0; + out_5666984403538513454[111] = 0.0; + out_5666984403538513454[112] = 0.0; + out_5666984403538513454[113] = 0.0; + out_5666984403538513454[114] = 1.0; + out_5666984403538513454[115] = 0.0; + out_5666984403538513454[116] = 0.0; + out_5666984403538513454[117] = 0.0; + out_5666984403538513454[118] = 0.0; + out_5666984403538513454[119] = 0.0; + out_5666984403538513454[120] = 0.0; + out_5666984403538513454[121] = 0.0; + out_5666984403538513454[122] = 0.0; + out_5666984403538513454[123] = 0.0; + out_5666984403538513454[124] = 0.0; + out_5666984403538513454[125] = 0.0; + out_5666984403538513454[126] = 0.0; + out_5666984403538513454[127] = 0.0; + out_5666984403538513454[128] = 0.0; + out_5666984403538513454[129] = 0.0; + out_5666984403538513454[130] = 0.0; + out_5666984403538513454[131] = 0.0; + out_5666984403538513454[132] = 0.0; + out_5666984403538513454[133] = 1.0; + out_5666984403538513454[134] = 0.0; + out_5666984403538513454[135] = 0.0; + out_5666984403538513454[136] = 0.0; + out_5666984403538513454[137] = 0.0; + out_5666984403538513454[138] = 0.0; + out_5666984403538513454[139] = 0.0; + out_5666984403538513454[140] = 0.0; + out_5666984403538513454[141] = 0.0; + out_5666984403538513454[142] = 0.0; + out_5666984403538513454[143] = 0.0; + out_5666984403538513454[144] = 0.0; + out_5666984403538513454[145] = 0.0; + out_5666984403538513454[146] = 0.0; + out_5666984403538513454[147] = 0.0; + out_5666984403538513454[148] = 0.0; + out_5666984403538513454[149] = 0.0; + out_5666984403538513454[150] = 0.0; + out_5666984403538513454[151] = 0.0; + out_5666984403538513454[152] = 1.0; + out_5666984403538513454[153] = 0.0; + out_5666984403538513454[154] = 0.0; + out_5666984403538513454[155] = 0.0; + out_5666984403538513454[156] = 0.0; + out_5666984403538513454[157] = 0.0; + out_5666984403538513454[158] = 0.0; + out_5666984403538513454[159] = 0.0; + out_5666984403538513454[160] = 0.0; + out_5666984403538513454[161] = 0.0; + out_5666984403538513454[162] = 0.0; + out_5666984403538513454[163] = 0.0; + out_5666984403538513454[164] = 0.0; + out_5666984403538513454[165] = 0.0; + out_5666984403538513454[166] = 0.0; + out_5666984403538513454[167] = 0.0; + out_5666984403538513454[168] = 0.0; + out_5666984403538513454[169] = 0.0; + out_5666984403538513454[170] = 0.0; + out_5666984403538513454[171] = 1.0; + out_5666984403538513454[172] = 0.0; + out_5666984403538513454[173] = 0.0; + out_5666984403538513454[174] = 0.0; + out_5666984403538513454[175] = 0.0; + out_5666984403538513454[176] = 0.0; + out_5666984403538513454[177] = 0.0; + out_5666984403538513454[178] = 0.0; + out_5666984403538513454[179] = 0.0; + out_5666984403538513454[180] = 0.0; + out_5666984403538513454[181] = 0.0; + out_5666984403538513454[182] = 0.0; + out_5666984403538513454[183] = 0.0; + out_5666984403538513454[184] = 0.0; + out_5666984403538513454[185] = 0.0; + out_5666984403538513454[186] = 0.0; + out_5666984403538513454[187] = 0.0; + out_5666984403538513454[188] = 0.0; + out_5666984403538513454[189] = 0.0; + out_5666984403538513454[190] = 1.0; + out_5666984403538513454[191] = 0.0; + out_5666984403538513454[192] = 0.0; + out_5666984403538513454[193] = 0.0; + out_5666984403538513454[194] = 0.0; + out_5666984403538513454[195] = 0.0; + out_5666984403538513454[196] = 0.0; + out_5666984403538513454[197] = 0.0; + out_5666984403538513454[198] = 0.0; + out_5666984403538513454[199] = 0.0; + out_5666984403538513454[200] = 0.0; + out_5666984403538513454[201] = 0.0; + out_5666984403538513454[202] = 0.0; + out_5666984403538513454[203] = 0.0; + out_5666984403538513454[204] = 0.0; + out_5666984403538513454[205] = 0.0; + out_5666984403538513454[206] = 0.0; + out_5666984403538513454[207] = 0.0; + out_5666984403538513454[208] = 0.0; + out_5666984403538513454[209] = 1.0; + out_5666984403538513454[210] = 0.0; + out_5666984403538513454[211] = 0.0; + out_5666984403538513454[212] = 0.0; + out_5666984403538513454[213] = 0.0; + out_5666984403538513454[214] = 0.0; + out_5666984403538513454[215] = 0.0; + out_5666984403538513454[216] = 0.0; + out_5666984403538513454[217] = 0.0; + out_5666984403538513454[218] = 0.0; + out_5666984403538513454[219] = 0.0; + out_5666984403538513454[220] = 0.0; + out_5666984403538513454[221] = 0.0; + out_5666984403538513454[222] = 0.0; + out_5666984403538513454[223] = 0.0; + out_5666984403538513454[224] = 0.0; + out_5666984403538513454[225] = 0.0; + out_5666984403538513454[226] = 0.0; + out_5666984403538513454[227] = 0.0; + out_5666984403538513454[228] = 1.0; + out_5666984403538513454[229] = 0.0; + out_5666984403538513454[230] = 0.0; + out_5666984403538513454[231] = 0.0; + out_5666984403538513454[232] = 0.0; + out_5666984403538513454[233] = 0.0; + out_5666984403538513454[234] = 0.0; + out_5666984403538513454[235] = 0.0; + out_5666984403538513454[236] = 0.0; + out_5666984403538513454[237] = 0.0; + out_5666984403538513454[238] = 0.0; + out_5666984403538513454[239] = 0.0; + out_5666984403538513454[240] = 0.0; + out_5666984403538513454[241] = 0.0; + out_5666984403538513454[242] = 0.0; + out_5666984403538513454[243] = 0.0; + out_5666984403538513454[244] = 0.0; + out_5666984403538513454[245] = 0.0; + out_5666984403538513454[246] = 0.0; + out_5666984403538513454[247] = 1.0; + out_5666984403538513454[248] = 0.0; + out_5666984403538513454[249] = 0.0; + out_5666984403538513454[250] = 0.0; + out_5666984403538513454[251] = 0.0; + out_5666984403538513454[252] = 0.0; + out_5666984403538513454[253] = 0.0; + out_5666984403538513454[254] = 0.0; + out_5666984403538513454[255] = 0.0; + out_5666984403538513454[256] = 0.0; + out_5666984403538513454[257] = 0.0; + out_5666984403538513454[258] = 0.0; + out_5666984403538513454[259] = 0.0; + out_5666984403538513454[260] = 0.0; + out_5666984403538513454[261] = 0.0; + out_5666984403538513454[262] = 0.0; + out_5666984403538513454[263] = 0.0; + out_5666984403538513454[264] = 0.0; + out_5666984403538513454[265] = 0.0; + out_5666984403538513454[266] = 1.0; + out_5666984403538513454[267] = 0.0; + out_5666984403538513454[268] = 0.0; + out_5666984403538513454[269] = 0.0; + out_5666984403538513454[270] = 0.0; + out_5666984403538513454[271] = 0.0; + out_5666984403538513454[272] = 0.0; + out_5666984403538513454[273] = 0.0; + out_5666984403538513454[274] = 0.0; + out_5666984403538513454[275] = 0.0; + out_5666984403538513454[276] = 0.0; + out_5666984403538513454[277] = 0.0; + out_5666984403538513454[278] = 0.0; + out_5666984403538513454[279] = 0.0; + out_5666984403538513454[280] = 0.0; + out_5666984403538513454[281] = 0.0; + out_5666984403538513454[282] = 0.0; + out_5666984403538513454[283] = 0.0; + out_5666984403538513454[284] = 0.0; + out_5666984403538513454[285] = 1.0; + out_5666984403538513454[286] = 0.0; + out_5666984403538513454[287] = 0.0; + out_5666984403538513454[288] = 0.0; + out_5666984403538513454[289] = 0.0; + out_5666984403538513454[290] = 0.0; + out_5666984403538513454[291] = 0.0; + out_5666984403538513454[292] = 0.0; + out_5666984403538513454[293] = 0.0; + out_5666984403538513454[294] = 0.0; + out_5666984403538513454[295] = 0.0; + out_5666984403538513454[296] = 0.0; + out_5666984403538513454[297] = 0.0; + out_5666984403538513454[298] = 0.0; + out_5666984403538513454[299] = 0.0; + out_5666984403538513454[300] = 0.0; + out_5666984403538513454[301] = 0.0; + out_5666984403538513454[302] = 0.0; + out_5666984403538513454[303] = 0.0; + out_5666984403538513454[304] = 1.0; + out_5666984403538513454[305] = 0.0; + out_5666984403538513454[306] = 0.0; + out_5666984403538513454[307] = 0.0; + out_5666984403538513454[308] = 0.0; + out_5666984403538513454[309] = 0.0; + out_5666984403538513454[310] = 0.0; + out_5666984403538513454[311] = 0.0; + out_5666984403538513454[312] = 0.0; + out_5666984403538513454[313] = 0.0; + out_5666984403538513454[314] = 0.0; + out_5666984403538513454[315] = 0.0; + out_5666984403538513454[316] = 0.0; + out_5666984403538513454[317] = 0.0; + out_5666984403538513454[318] = 0.0; + out_5666984403538513454[319] = 0.0; + out_5666984403538513454[320] = 0.0; + out_5666984403538513454[321] = 0.0; + out_5666984403538513454[322] = 0.0; + out_5666984403538513454[323] = 1.0; } -void f_fun(double *state, double dt, double *out_7401195424910099109) { - out_7401195424910099109[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_7401195424910099109[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_7401195424910099109[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_7401195424910099109[3] = dt*state[12] + state[3]; - out_7401195424910099109[4] = dt*state[13] + state[4]; - out_7401195424910099109[5] = dt*state[14] + state[5]; - out_7401195424910099109[6] = state[6]; - out_7401195424910099109[7] = state[7]; - out_7401195424910099109[8] = state[8]; - out_7401195424910099109[9] = state[9]; - out_7401195424910099109[10] = state[10]; - out_7401195424910099109[11] = state[11]; - out_7401195424910099109[12] = state[12]; - out_7401195424910099109[13] = state[13]; - out_7401195424910099109[14] = state[14]; - out_7401195424910099109[15] = state[15]; - out_7401195424910099109[16] = state[16]; - out_7401195424910099109[17] = state[17]; +void f_fun(double *state, double dt, double *out_3108653889513613454) { + out_3108653889513613454[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_3108653889513613454[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_3108653889513613454[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_3108653889513613454[3] = dt*state[12] + state[3]; + out_3108653889513613454[4] = dt*state[13] + state[4]; + out_3108653889513613454[5] = dt*state[14] + state[5]; + out_3108653889513613454[6] = state[6]; + out_3108653889513613454[7] = state[7]; + out_3108653889513613454[8] = state[8]; + out_3108653889513613454[9] = state[9]; + out_3108653889513613454[10] = state[10]; + out_3108653889513613454[11] = state[11]; + out_3108653889513613454[12] = state[12]; + out_3108653889513613454[13] = state[13]; + out_3108653889513613454[14] = state[14]; + out_3108653889513613454[15] = state[15]; + out_3108653889513613454[16] = state[16]; + out_3108653889513613454[17] = state[17]; } -void F_fun(double *state, double dt, double *out_7953355824038160444) { - out_7953355824038160444[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_7953355824038160444[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_7953355824038160444[2] = 0; - out_7953355824038160444[3] = 0; - out_7953355824038160444[4] = 0; - out_7953355824038160444[5] = 0; - out_7953355824038160444[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_7953355824038160444[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_7953355824038160444[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_7953355824038160444[9] = 0; - out_7953355824038160444[10] = 0; - out_7953355824038160444[11] = 0; - out_7953355824038160444[12] = 0; - out_7953355824038160444[13] = 0; - out_7953355824038160444[14] = 0; - out_7953355824038160444[15] = 0; - out_7953355824038160444[16] = 0; - out_7953355824038160444[17] = 0; - out_7953355824038160444[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_7953355824038160444[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_7953355824038160444[20] = 0; - out_7953355824038160444[21] = 0; - out_7953355824038160444[22] = 0; - out_7953355824038160444[23] = 0; - out_7953355824038160444[24] = 0; - out_7953355824038160444[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_7953355824038160444[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_7953355824038160444[27] = 0; - out_7953355824038160444[28] = 0; - out_7953355824038160444[29] = 0; - out_7953355824038160444[30] = 0; - out_7953355824038160444[31] = 0; - out_7953355824038160444[32] = 0; - out_7953355824038160444[33] = 0; - out_7953355824038160444[34] = 0; - out_7953355824038160444[35] = 0; - out_7953355824038160444[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_7953355824038160444[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_7953355824038160444[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_7953355824038160444[39] = 0; - out_7953355824038160444[40] = 0; - out_7953355824038160444[41] = 0; - out_7953355824038160444[42] = 0; - out_7953355824038160444[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_7953355824038160444[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_7953355824038160444[45] = 0; - out_7953355824038160444[46] = 0; - out_7953355824038160444[47] = 0; - out_7953355824038160444[48] = 0; - out_7953355824038160444[49] = 0; - out_7953355824038160444[50] = 0; - out_7953355824038160444[51] = 0; - out_7953355824038160444[52] = 0; - out_7953355824038160444[53] = 0; - out_7953355824038160444[54] = 0; - out_7953355824038160444[55] = 0; - out_7953355824038160444[56] = 0; - out_7953355824038160444[57] = 1; - out_7953355824038160444[58] = 0; - out_7953355824038160444[59] = 0; - out_7953355824038160444[60] = 0; - out_7953355824038160444[61] = 0; - out_7953355824038160444[62] = 0; - out_7953355824038160444[63] = 0; - out_7953355824038160444[64] = 0; - out_7953355824038160444[65] = 0; - out_7953355824038160444[66] = dt; - out_7953355824038160444[67] = 0; - out_7953355824038160444[68] = 0; - out_7953355824038160444[69] = 0; - out_7953355824038160444[70] = 0; - out_7953355824038160444[71] = 0; - out_7953355824038160444[72] = 0; - out_7953355824038160444[73] = 0; - out_7953355824038160444[74] = 0; - out_7953355824038160444[75] = 0; - out_7953355824038160444[76] = 1; - out_7953355824038160444[77] = 0; - out_7953355824038160444[78] = 0; - out_7953355824038160444[79] = 0; - out_7953355824038160444[80] = 0; - out_7953355824038160444[81] = 0; - out_7953355824038160444[82] = 0; - out_7953355824038160444[83] = 0; - out_7953355824038160444[84] = 0; - out_7953355824038160444[85] = dt; - out_7953355824038160444[86] = 0; - out_7953355824038160444[87] = 0; - out_7953355824038160444[88] = 0; - out_7953355824038160444[89] = 0; - out_7953355824038160444[90] = 0; - out_7953355824038160444[91] = 0; - out_7953355824038160444[92] = 0; - out_7953355824038160444[93] = 0; - out_7953355824038160444[94] = 0; - out_7953355824038160444[95] = 1; - out_7953355824038160444[96] = 0; - out_7953355824038160444[97] = 0; - out_7953355824038160444[98] = 0; - out_7953355824038160444[99] = 0; - out_7953355824038160444[100] = 0; - out_7953355824038160444[101] = 0; - out_7953355824038160444[102] = 0; - out_7953355824038160444[103] = 0; - out_7953355824038160444[104] = dt; - out_7953355824038160444[105] = 0; - out_7953355824038160444[106] = 0; - out_7953355824038160444[107] = 0; - out_7953355824038160444[108] = 0; - out_7953355824038160444[109] = 0; - out_7953355824038160444[110] = 0; - out_7953355824038160444[111] = 0; - out_7953355824038160444[112] = 0; - out_7953355824038160444[113] = 0; - out_7953355824038160444[114] = 1; - out_7953355824038160444[115] = 0; - out_7953355824038160444[116] = 0; - out_7953355824038160444[117] = 0; - out_7953355824038160444[118] = 0; - out_7953355824038160444[119] = 0; - out_7953355824038160444[120] = 0; - out_7953355824038160444[121] = 0; - out_7953355824038160444[122] = 0; - out_7953355824038160444[123] = 0; - out_7953355824038160444[124] = 0; - out_7953355824038160444[125] = 0; - out_7953355824038160444[126] = 0; - out_7953355824038160444[127] = 0; - out_7953355824038160444[128] = 0; - out_7953355824038160444[129] = 0; - out_7953355824038160444[130] = 0; - out_7953355824038160444[131] = 0; - out_7953355824038160444[132] = 0; - out_7953355824038160444[133] = 1; - out_7953355824038160444[134] = 0; - out_7953355824038160444[135] = 0; - out_7953355824038160444[136] = 0; - out_7953355824038160444[137] = 0; - out_7953355824038160444[138] = 0; - out_7953355824038160444[139] = 0; - out_7953355824038160444[140] = 0; - out_7953355824038160444[141] = 0; - out_7953355824038160444[142] = 0; - out_7953355824038160444[143] = 0; - out_7953355824038160444[144] = 0; - out_7953355824038160444[145] = 0; - out_7953355824038160444[146] = 0; - out_7953355824038160444[147] = 0; - out_7953355824038160444[148] = 0; - out_7953355824038160444[149] = 0; - out_7953355824038160444[150] = 0; - out_7953355824038160444[151] = 0; - out_7953355824038160444[152] = 1; - out_7953355824038160444[153] = 0; - out_7953355824038160444[154] = 0; - out_7953355824038160444[155] = 0; - out_7953355824038160444[156] = 0; - out_7953355824038160444[157] = 0; - out_7953355824038160444[158] = 0; - out_7953355824038160444[159] = 0; - out_7953355824038160444[160] = 0; - out_7953355824038160444[161] = 0; - out_7953355824038160444[162] = 0; - out_7953355824038160444[163] = 0; - out_7953355824038160444[164] = 0; - out_7953355824038160444[165] = 0; - out_7953355824038160444[166] = 0; - out_7953355824038160444[167] = 0; - out_7953355824038160444[168] = 0; - out_7953355824038160444[169] = 0; - out_7953355824038160444[170] = 0; - out_7953355824038160444[171] = 1; - out_7953355824038160444[172] = 0; - out_7953355824038160444[173] = 0; - out_7953355824038160444[174] = 0; - out_7953355824038160444[175] = 0; - out_7953355824038160444[176] = 0; - out_7953355824038160444[177] = 0; - out_7953355824038160444[178] = 0; - out_7953355824038160444[179] = 0; - out_7953355824038160444[180] = 0; - out_7953355824038160444[181] = 0; - out_7953355824038160444[182] = 0; - out_7953355824038160444[183] = 0; - out_7953355824038160444[184] = 0; - out_7953355824038160444[185] = 0; - out_7953355824038160444[186] = 0; - out_7953355824038160444[187] = 0; - out_7953355824038160444[188] = 0; - out_7953355824038160444[189] = 0; - out_7953355824038160444[190] = 1; - out_7953355824038160444[191] = 0; - out_7953355824038160444[192] = 0; - out_7953355824038160444[193] = 0; - out_7953355824038160444[194] = 0; - out_7953355824038160444[195] = 0; - out_7953355824038160444[196] = 0; - out_7953355824038160444[197] = 0; - out_7953355824038160444[198] = 0; - out_7953355824038160444[199] = 0; - out_7953355824038160444[200] = 0; - out_7953355824038160444[201] = 0; - out_7953355824038160444[202] = 0; - out_7953355824038160444[203] = 0; - out_7953355824038160444[204] = 0; - out_7953355824038160444[205] = 0; - out_7953355824038160444[206] = 0; - out_7953355824038160444[207] = 0; - out_7953355824038160444[208] = 0; - out_7953355824038160444[209] = 1; - out_7953355824038160444[210] = 0; - out_7953355824038160444[211] = 0; - out_7953355824038160444[212] = 0; - out_7953355824038160444[213] = 0; - out_7953355824038160444[214] = 0; - out_7953355824038160444[215] = 0; - out_7953355824038160444[216] = 0; - out_7953355824038160444[217] = 0; - out_7953355824038160444[218] = 0; - out_7953355824038160444[219] = 0; - out_7953355824038160444[220] = 0; - out_7953355824038160444[221] = 0; - out_7953355824038160444[222] = 0; - out_7953355824038160444[223] = 0; - out_7953355824038160444[224] = 0; - out_7953355824038160444[225] = 0; - out_7953355824038160444[226] = 0; - out_7953355824038160444[227] = 0; - out_7953355824038160444[228] = 1; - out_7953355824038160444[229] = 0; - out_7953355824038160444[230] = 0; - out_7953355824038160444[231] = 0; - out_7953355824038160444[232] = 0; - out_7953355824038160444[233] = 0; - out_7953355824038160444[234] = 0; - out_7953355824038160444[235] = 0; - out_7953355824038160444[236] = 0; - out_7953355824038160444[237] = 0; - out_7953355824038160444[238] = 0; - out_7953355824038160444[239] = 0; - out_7953355824038160444[240] = 0; - out_7953355824038160444[241] = 0; - out_7953355824038160444[242] = 0; - out_7953355824038160444[243] = 0; - out_7953355824038160444[244] = 0; - out_7953355824038160444[245] = 0; - out_7953355824038160444[246] = 0; - out_7953355824038160444[247] = 1; - out_7953355824038160444[248] = 0; - out_7953355824038160444[249] = 0; - out_7953355824038160444[250] = 0; - out_7953355824038160444[251] = 0; - out_7953355824038160444[252] = 0; - out_7953355824038160444[253] = 0; - out_7953355824038160444[254] = 0; - out_7953355824038160444[255] = 0; - out_7953355824038160444[256] = 0; - out_7953355824038160444[257] = 0; - out_7953355824038160444[258] = 0; - out_7953355824038160444[259] = 0; - out_7953355824038160444[260] = 0; - out_7953355824038160444[261] = 0; - out_7953355824038160444[262] = 0; - out_7953355824038160444[263] = 0; - out_7953355824038160444[264] = 0; - out_7953355824038160444[265] = 0; - out_7953355824038160444[266] = 1; - out_7953355824038160444[267] = 0; - out_7953355824038160444[268] = 0; - out_7953355824038160444[269] = 0; - out_7953355824038160444[270] = 0; - out_7953355824038160444[271] = 0; - out_7953355824038160444[272] = 0; - out_7953355824038160444[273] = 0; - out_7953355824038160444[274] = 0; - out_7953355824038160444[275] = 0; - out_7953355824038160444[276] = 0; - out_7953355824038160444[277] = 0; - out_7953355824038160444[278] = 0; - out_7953355824038160444[279] = 0; - out_7953355824038160444[280] = 0; - out_7953355824038160444[281] = 0; - out_7953355824038160444[282] = 0; - out_7953355824038160444[283] = 0; - out_7953355824038160444[284] = 0; - out_7953355824038160444[285] = 1; - out_7953355824038160444[286] = 0; - out_7953355824038160444[287] = 0; - out_7953355824038160444[288] = 0; - out_7953355824038160444[289] = 0; - out_7953355824038160444[290] = 0; - out_7953355824038160444[291] = 0; - out_7953355824038160444[292] = 0; - out_7953355824038160444[293] = 0; - out_7953355824038160444[294] = 0; - out_7953355824038160444[295] = 0; - out_7953355824038160444[296] = 0; - out_7953355824038160444[297] = 0; - out_7953355824038160444[298] = 0; - out_7953355824038160444[299] = 0; - out_7953355824038160444[300] = 0; - out_7953355824038160444[301] = 0; - out_7953355824038160444[302] = 0; - out_7953355824038160444[303] = 0; - out_7953355824038160444[304] = 1; - out_7953355824038160444[305] = 0; - out_7953355824038160444[306] = 0; - out_7953355824038160444[307] = 0; - out_7953355824038160444[308] = 0; - out_7953355824038160444[309] = 0; - out_7953355824038160444[310] = 0; - out_7953355824038160444[311] = 0; - out_7953355824038160444[312] = 0; - out_7953355824038160444[313] = 0; - out_7953355824038160444[314] = 0; - out_7953355824038160444[315] = 0; - out_7953355824038160444[316] = 0; - out_7953355824038160444[317] = 0; - out_7953355824038160444[318] = 0; - out_7953355824038160444[319] = 0; - out_7953355824038160444[320] = 0; - out_7953355824038160444[321] = 0; - out_7953355824038160444[322] = 0; - out_7953355824038160444[323] = 1; +void F_fun(double *state, double dt, double *out_3156103136607478463) { + out_3156103136607478463[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_3156103136607478463[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_3156103136607478463[2] = 0; + out_3156103136607478463[3] = 0; + out_3156103136607478463[4] = 0; + out_3156103136607478463[5] = 0; + out_3156103136607478463[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_3156103136607478463[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_3156103136607478463[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_3156103136607478463[9] = 0; + out_3156103136607478463[10] = 0; + out_3156103136607478463[11] = 0; + out_3156103136607478463[12] = 0; + out_3156103136607478463[13] = 0; + out_3156103136607478463[14] = 0; + out_3156103136607478463[15] = 0; + out_3156103136607478463[16] = 0; + out_3156103136607478463[17] = 0; + out_3156103136607478463[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_3156103136607478463[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_3156103136607478463[20] = 0; + out_3156103136607478463[21] = 0; + out_3156103136607478463[22] = 0; + out_3156103136607478463[23] = 0; + out_3156103136607478463[24] = 0; + out_3156103136607478463[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_3156103136607478463[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_3156103136607478463[27] = 0; + out_3156103136607478463[28] = 0; + out_3156103136607478463[29] = 0; + out_3156103136607478463[30] = 0; + out_3156103136607478463[31] = 0; + out_3156103136607478463[32] = 0; + out_3156103136607478463[33] = 0; + out_3156103136607478463[34] = 0; + out_3156103136607478463[35] = 0; + out_3156103136607478463[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_3156103136607478463[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_3156103136607478463[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_3156103136607478463[39] = 0; + out_3156103136607478463[40] = 0; + out_3156103136607478463[41] = 0; + out_3156103136607478463[42] = 0; + out_3156103136607478463[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_3156103136607478463[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_3156103136607478463[45] = 0; + out_3156103136607478463[46] = 0; + out_3156103136607478463[47] = 0; + out_3156103136607478463[48] = 0; + out_3156103136607478463[49] = 0; + out_3156103136607478463[50] = 0; + out_3156103136607478463[51] = 0; + out_3156103136607478463[52] = 0; + out_3156103136607478463[53] = 0; + out_3156103136607478463[54] = 0; + out_3156103136607478463[55] = 0; + out_3156103136607478463[56] = 0; + out_3156103136607478463[57] = 1; + out_3156103136607478463[58] = 0; + out_3156103136607478463[59] = 0; + out_3156103136607478463[60] = 0; + out_3156103136607478463[61] = 0; + out_3156103136607478463[62] = 0; + out_3156103136607478463[63] = 0; + out_3156103136607478463[64] = 0; + out_3156103136607478463[65] = 0; + out_3156103136607478463[66] = dt; + out_3156103136607478463[67] = 0; + out_3156103136607478463[68] = 0; + out_3156103136607478463[69] = 0; + out_3156103136607478463[70] = 0; + out_3156103136607478463[71] = 0; + out_3156103136607478463[72] = 0; + out_3156103136607478463[73] = 0; + out_3156103136607478463[74] = 0; + out_3156103136607478463[75] = 0; + out_3156103136607478463[76] = 1; + out_3156103136607478463[77] = 0; + out_3156103136607478463[78] = 0; + out_3156103136607478463[79] = 0; + out_3156103136607478463[80] = 0; + out_3156103136607478463[81] = 0; + out_3156103136607478463[82] = 0; + out_3156103136607478463[83] = 0; + out_3156103136607478463[84] = 0; + out_3156103136607478463[85] = dt; + out_3156103136607478463[86] = 0; + out_3156103136607478463[87] = 0; + out_3156103136607478463[88] = 0; + out_3156103136607478463[89] = 0; + out_3156103136607478463[90] = 0; + out_3156103136607478463[91] = 0; + out_3156103136607478463[92] = 0; + out_3156103136607478463[93] = 0; + out_3156103136607478463[94] = 0; + out_3156103136607478463[95] = 1; + out_3156103136607478463[96] = 0; + out_3156103136607478463[97] = 0; + out_3156103136607478463[98] = 0; + out_3156103136607478463[99] = 0; + out_3156103136607478463[100] = 0; + out_3156103136607478463[101] = 0; + out_3156103136607478463[102] = 0; + out_3156103136607478463[103] = 0; + out_3156103136607478463[104] = dt; + out_3156103136607478463[105] = 0; + out_3156103136607478463[106] = 0; + out_3156103136607478463[107] = 0; + out_3156103136607478463[108] = 0; + out_3156103136607478463[109] = 0; + out_3156103136607478463[110] = 0; + out_3156103136607478463[111] = 0; + out_3156103136607478463[112] = 0; + out_3156103136607478463[113] = 0; + out_3156103136607478463[114] = 1; + out_3156103136607478463[115] = 0; + out_3156103136607478463[116] = 0; + out_3156103136607478463[117] = 0; + out_3156103136607478463[118] = 0; + out_3156103136607478463[119] = 0; + out_3156103136607478463[120] = 0; + out_3156103136607478463[121] = 0; + out_3156103136607478463[122] = 0; + out_3156103136607478463[123] = 0; + out_3156103136607478463[124] = 0; + out_3156103136607478463[125] = 0; + out_3156103136607478463[126] = 0; + out_3156103136607478463[127] = 0; + out_3156103136607478463[128] = 0; + out_3156103136607478463[129] = 0; + out_3156103136607478463[130] = 0; + out_3156103136607478463[131] = 0; + out_3156103136607478463[132] = 0; + out_3156103136607478463[133] = 1; + out_3156103136607478463[134] = 0; + out_3156103136607478463[135] = 0; + out_3156103136607478463[136] = 0; + out_3156103136607478463[137] = 0; + out_3156103136607478463[138] = 0; + out_3156103136607478463[139] = 0; + out_3156103136607478463[140] = 0; + out_3156103136607478463[141] = 0; + out_3156103136607478463[142] = 0; + out_3156103136607478463[143] = 0; + out_3156103136607478463[144] = 0; + out_3156103136607478463[145] = 0; + out_3156103136607478463[146] = 0; + out_3156103136607478463[147] = 0; + out_3156103136607478463[148] = 0; + out_3156103136607478463[149] = 0; + out_3156103136607478463[150] = 0; + out_3156103136607478463[151] = 0; + out_3156103136607478463[152] = 1; + out_3156103136607478463[153] = 0; + out_3156103136607478463[154] = 0; + out_3156103136607478463[155] = 0; + out_3156103136607478463[156] = 0; + out_3156103136607478463[157] = 0; + out_3156103136607478463[158] = 0; + out_3156103136607478463[159] = 0; + out_3156103136607478463[160] = 0; + out_3156103136607478463[161] = 0; + out_3156103136607478463[162] = 0; + out_3156103136607478463[163] = 0; + out_3156103136607478463[164] = 0; + out_3156103136607478463[165] = 0; + out_3156103136607478463[166] = 0; + out_3156103136607478463[167] = 0; + out_3156103136607478463[168] = 0; + out_3156103136607478463[169] = 0; + out_3156103136607478463[170] = 0; + out_3156103136607478463[171] = 1; + out_3156103136607478463[172] = 0; + out_3156103136607478463[173] = 0; + out_3156103136607478463[174] = 0; + out_3156103136607478463[175] = 0; + out_3156103136607478463[176] = 0; + out_3156103136607478463[177] = 0; + out_3156103136607478463[178] = 0; + out_3156103136607478463[179] = 0; + out_3156103136607478463[180] = 0; + out_3156103136607478463[181] = 0; + out_3156103136607478463[182] = 0; + out_3156103136607478463[183] = 0; + out_3156103136607478463[184] = 0; + out_3156103136607478463[185] = 0; + out_3156103136607478463[186] = 0; + out_3156103136607478463[187] = 0; + out_3156103136607478463[188] = 0; + out_3156103136607478463[189] = 0; + out_3156103136607478463[190] = 1; + out_3156103136607478463[191] = 0; + out_3156103136607478463[192] = 0; + out_3156103136607478463[193] = 0; + out_3156103136607478463[194] = 0; + out_3156103136607478463[195] = 0; + out_3156103136607478463[196] = 0; + out_3156103136607478463[197] = 0; + out_3156103136607478463[198] = 0; + out_3156103136607478463[199] = 0; + out_3156103136607478463[200] = 0; + out_3156103136607478463[201] = 0; + out_3156103136607478463[202] = 0; + out_3156103136607478463[203] = 0; + out_3156103136607478463[204] = 0; + out_3156103136607478463[205] = 0; + out_3156103136607478463[206] = 0; + out_3156103136607478463[207] = 0; + out_3156103136607478463[208] = 0; + out_3156103136607478463[209] = 1; + out_3156103136607478463[210] = 0; + out_3156103136607478463[211] = 0; + out_3156103136607478463[212] = 0; + out_3156103136607478463[213] = 0; + out_3156103136607478463[214] = 0; + out_3156103136607478463[215] = 0; + out_3156103136607478463[216] = 0; + out_3156103136607478463[217] = 0; + out_3156103136607478463[218] = 0; + out_3156103136607478463[219] = 0; + out_3156103136607478463[220] = 0; + out_3156103136607478463[221] = 0; + out_3156103136607478463[222] = 0; + out_3156103136607478463[223] = 0; + out_3156103136607478463[224] = 0; + out_3156103136607478463[225] = 0; + out_3156103136607478463[226] = 0; + out_3156103136607478463[227] = 0; + out_3156103136607478463[228] = 1; + out_3156103136607478463[229] = 0; + out_3156103136607478463[230] = 0; + out_3156103136607478463[231] = 0; + out_3156103136607478463[232] = 0; + out_3156103136607478463[233] = 0; + out_3156103136607478463[234] = 0; + out_3156103136607478463[235] = 0; + out_3156103136607478463[236] = 0; + out_3156103136607478463[237] = 0; + out_3156103136607478463[238] = 0; + out_3156103136607478463[239] = 0; + out_3156103136607478463[240] = 0; + out_3156103136607478463[241] = 0; + out_3156103136607478463[242] = 0; + out_3156103136607478463[243] = 0; + out_3156103136607478463[244] = 0; + out_3156103136607478463[245] = 0; + out_3156103136607478463[246] = 0; + out_3156103136607478463[247] = 1; + out_3156103136607478463[248] = 0; + out_3156103136607478463[249] = 0; + out_3156103136607478463[250] = 0; + out_3156103136607478463[251] = 0; + out_3156103136607478463[252] = 0; + out_3156103136607478463[253] = 0; + out_3156103136607478463[254] = 0; + out_3156103136607478463[255] = 0; + out_3156103136607478463[256] = 0; + out_3156103136607478463[257] = 0; + out_3156103136607478463[258] = 0; + out_3156103136607478463[259] = 0; + out_3156103136607478463[260] = 0; + out_3156103136607478463[261] = 0; + out_3156103136607478463[262] = 0; + out_3156103136607478463[263] = 0; + out_3156103136607478463[264] = 0; + out_3156103136607478463[265] = 0; + out_3156103136607478463[266] = 1; + out_3156103136607478463[267] = 0; + out_3156103136607478463[268] = 0; + out_3156103136607478463[269] = 0; + out_3156103136607478463[270] = 0; + out_3156103136607478463[271] = 0; + out_3156103136607478463[272] = 0; + out_3156103136607478463[273] = 0; + out_3156103136607478463[274] = 0; + out_3156103136607478463[275] = 0; + out_3156103136607478463[276] = 0; + out_3156103136607478463[277] = 0; + out_3156103136607478463[278] = 0; + out_3156103136607478463[279] = 0; + out_3156103136607478463[280] = 0; + out_3156103136607478463[281] = 0; + out_3156103136607478463[282] = 0; + out_3156103136607478463[283] = 0; + out_3156103136607478463[284] = 0; + out_3156103136607478463[285] = 1; + out_3156103136607478463[286] = 0; + out_3156103136607478463[287] = 0; + out_3156103136607478463[288] = 0; + out_3156103136607478463[289] = 0; + out_3156103136607478463[290] = 0; + out_3156103136607478463[291] = 0; + out_3156103136607478463[292] = 0; + out_3156103136607478463[293] = 0; + out_3156103136607478463[294] = 0; + out_3156103136607478463[295] = 0; + out_3156103136607478463[296] = 0; + out_3156103136607478463[297] = 0; + out_3156103136607478463[298] = 0; + out_3156103136607478463[299] = 0; + out_3156103136607478463[300] = 0; + out_3156103136607478463[301] = 0; + out_3156103136607478463[302] = 0; + out_3156103136607478463[303] = 0; + out_3156103136607478463[304] = 1; + out_3156103136607478463[305] = 0; + out_3156103136607478463[306] = 0; + out_3156103136607478463[307] = 0; + out_3156103136607478463[308] = 0; + out_3156103136607478463[309] = 0; + out_3156103136607478463[310] = 0; + out_3156103136607478463[311] = 0; + out_3156103136607478463[312] = 0; + out_3156103136607478463[313] = 0; + out_3156103136607478463[314] = 0; + out_3156103136607478463[315] = 0; + out_3156103136607478463[316] = 0; + out_3156103136607478463[317] = 0; + out_3156103136607478463[318] = 0; + out_3156103136607478463[319] = 0; + out_3156103136607478463[320] = 0; + out_3156103136607478463[321] = 0; + out_3156103136607478463[322] = 0; + out_3156103136607478463[323] = 1; } -void h_4(double *state, double *unused, double *out_5685529689104176696) { - out_5685529689104176696[0] = state[6] + state[9]; - out_5685529689104176696[1] = state[7] + state[10]; - out_5685529689104176696[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_4668712772901425910) { + out_4668712772901425910[0] = state[6] + state[9]; + out_4668712772901425910[1] = state[7] + state[10]; + out_4668712772901425910[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_8898228533778077411) { - out_8898228533778077411[0] = 0; - out_8898228533778077411[1] = 0; - out_8898228533778077411[2] = 0; - out_8898228533778077411[3] = 0; - out_8898228533778077411[4] = 0; - out_8898228533778077411[5] = 0; - out_8898228533778077411[6] = 1; - out_8898228533778077411[7] = 0; - out_8898228533778077411[8] = 0; - out_8898228533778077411[9] = 1; - out_8898228533778077411[10] = 0; - out_8898228533778077411[11] = 0; - out_8898228533778077411[12] = 0; - out_8898228533778077411[13] = 0; - out_8898228533778077411[14] = 0; - out_8898228533778077411[15] = 0; - out_8898228533778077411[16] = 0; - out_8898228533778077411[17] = 0; - out_8898228533778077411[18] = 0; - out_8898228533778077411[19] = 0; - out_8898228533778077411[20] = 0; - out_8898228533778077411[21] = 0; - out_8898228533778077411[22] = 0; - out_8898228533778077411[23] = 0; - out_8898228533778077411[24] = 0; - out_8898228533778077411[25] = 1; - out_8898228533778077411[26] = 0; - out_8898228533778077411[27] = 0; - out_8898228533778077411[28] = 1; - out_8898228533778077411[29] = 0; - out_8898228533778077411[30] = 0; - out_8898228533778077411[31] = 0; - out_8898228533778077411[32] = 0; - out_8898228533778077411[33] = 0; - out_8898228533778077411[34] = 0; - out_8898228533778077411[35] = 0; - out_8898228533778077411[36] = 0; - out_8898228533778077411[37] = 0; - out_8898228533778077411[38] = 0; - out_8898228533778077411[39] = 0; - out_8898228533778077411[40] = 0; - out_8898228533778077411[41] = 0; - out_8898228533778077411[42] = 0; - out_8898228533778077411[43] = 0; - out_8898228533778077411[44] = 1; - out_8898228533778077411[45] = 0; - out_8898228533778077411[46] = 0; - out_8898228533778077411[47] = 1; - out_8898228533778077411[48] = 0; - out_8898228533778077411[49] = 0; - out_8898228533778077411[50] = 0; - out_8898228533778077411[51] = 0; - out_8898228533778077411[52] = 0; - out_8898228533778077411[53] = 0; +void H_4(double *state, double *unused, double *out_3277377262519307888) { + out_3277377262519307888[0] = 0; + out_3277377262519307888[1] = 0; + out_3277377262519307888[2] = 0; + out_3277377262519307888[3] = 0; + out_3277377262519307888[4] = 0; + out_3277377262519307888[5] = 0; + out_3277377262519307888[6] = 1; + out_3277377262519307888[7] = 0; + out_3277377262519307888[8] = 0; + out_3277377262519307888[9] = 1; + out_3277377262519307888[10] = 0; + out_3277377262519307888[11] = 0; + out_3277377262519307888[12] = 0; + out_3277377262519307888[13] = 0; + out_3277377262519307888[14] = 0; + out_3277377262519307888[15] = 0; + out_3277377262519307888[16] = 0; + out_3277377262519307888[17] = 0; + out_3277377262519307888[18] = 0; + out_3277377262519307888[19] = 0; + out_3277377262519307888[20] = 0; + out_3277377262519307888[21] = 0; + out_3277377262519307888[22] = 0; + out_3277377262519307888[23] = 0; + out_3277377262519307888[24] = 0; + out_3277377262519307888[25] = 1; + out_3277377262519307888[26] = 0; + out_3277377262519307888[27] = 0; + out_3277377262519307888[28] = 1; + out_3277377262519307888[29] = 0; + out_3277377262519307888[30] = 0; + out_3277377262519307888[31] = 0; + out_3277377262519307888[32] = 0; + out_3277377262519307888[33] = 0; + out_3277377262519307888[34] = 0; + out_3277377262519307888[35] = 0; + out_3277377262519307888[36] = 0; + out_3277377262519307888[37] = 0; + out_3277377262519307888[38] = 0; + out_3277377262519307888[39] = 0; + out_3277377262519307888[40] = 0; + out_3277377262519307888[41] = 0; + out_3277377262519307888[42] = 0; + out_3277377262519307888[43] = 0; + out_3277377262519307888[44] = 1; + out_3277377262519307888[45] = 0; + out_3277377262519307888[46] = 0; + out_3277377262519307888[47] = 1; + out_3277377262519307888[48] = 0; + out_3277377262519307888[49] = 0; + out_3277377262519307888[50] = 0; + out_3277377262519307888[51] = 0; + out_3277377262519307888[52] = 0; + out_3277377262519307888[53] = 0; } -void h_10(double *state, double *unused, double *out_5395236827797613020) { - out_5395236827797613020[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_5395236827797613020[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_5395236827797613020[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_5024807863031837328) { + out_5024807863031837328[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_5024807863031837328[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_5024807863031837328[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_4460820551654126260) { - out_4460820551654126260[0] = 0; - out_4460820551654126260[1] = 9.8100000000000005*cos(state[1]); - out_4460820551654126260[2] = 0; - out_4460820551654126260[3] = 0; - out_4460820551654126260[4] = -state[8]; - out_4460820551654126260[5] = state[7]; - out_4460820551654126260[6] = 0; - out_4460820551654126260[7] = state[5]; - out_4460820551654126260[8] = -state[4]; - out_4460820551654126260[9] = 0; - out_4460820551654126260[10] = 0; - out_4460820551654126260[11] = 0; - out_4460820551654126260[12] = 1; - out_4460820551654126260[13] = 0; - out_4460820551654126260[14] = 0; - out_4460820551654126260[15] = 1; - out_4460820551654126260[16] = 0; - out_4460820551654126260[17] = 0; - out_4460820551654126260[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_4460820551654126260[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_4460820551654126260[20] = 0; - out_4460820551654126260[21] = state[8]; - out_4460820551654126260[22] = 0; - out_4460820551654126260[23] = -state[6]; - out_4460820551654126260[24] = -state[5]; - out_4460820551654126260[25] = 0; - out_4460820551654126260[26] = state[3]; - out_4460820551654126260[27] = 0; - out_4460820551654126260[28] = 0; - out_4460820551654126260[29] = 0; - out_4460820551654126260[30] = 0; - out_4460820551654126260[31] = 1; - out_4460820551654126260[32] = 0; - out_4460820551654126260[33] = 0; - out_4460820551654126260[34] = 1; - out_4460820551654126260[35] = 0; - out_4460820551654126260[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_4460820551654126260[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_4460820551654126260[38] = 0; - out_4460820551654126260[39] = -state[7]; - out_4460820551654126260[40] = state[6]; - out_4460820551654126260[41] = 0; - out_4460820551654126260[42] = state[4]; - out_4460820551654126260[43] = -state[3]; - out_4460820551654126260[44] = 0; - out_4460820551654126260[45] = 0; - out_4460820551654126260[46] = 0; - out_4460820551654126260[47] = 0; - out_4460820551654126260[48] = 0; - out_4460820551654126260[49] = 0; - out_4460820551654126260[50] = 1; - out_4460820551654126260[51] = 0; - out_4460820551654126260[52] = 0; - out_4460820551654126260[53] = 1; +void H_10(double *state, double *unused, double *out_4567419581298515838) { + out_4567419581298515838[0] = 0; + out_4567419581298515838[1] = 9.8100000000000005*cos(state[1]); + out_4567419581298515838[2] = 0; + out_4567419581298515838[3] = 0; + out_4567419581298515838[4] = -state[8]; + out_4567419581298515838[5] = state[7]; + out_4567419581298515838[6] = 0; + out_4567419581298515838[7] = state[5]; + out_4567419581298515838[8] = -state[4]; + out_4567419581298515838[9] = 0; + out_4567419581298515838[10] = 0; + out_4567419581298515838[11] = 0; + out_4567419581298515838[12] = 1; + out_4567419581298515838[13] = 0; + out_4567419581298515838[14] = 0; + out_4567419581298515838[15] = 1; + out_4567419581298515838[16] = 0; + out_4567419581298515838[17] = 0; + out_4567419581298515838[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_4567419581298515838[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_4567419581298515838[20] = 0; + out_4567419581298515838[21] = state[8]; + out_4567419581298515838[22] = 0; + out_4567419581298515838[23] = -state[6]; + out_4567419581298515838[24] = -state[5]; + out_4567419581298515838[25] = 0; + out_4567419581298515838[26] = state[3]; + out_4567419581298515838[27] = 0; + out_4567419581298515838[28] = 0; + out_4567419581298515838[29] = 0; + out_4567419581298515838[30] = 0; + out_4567419581298515838[31] = 1; + out_4567419581298515838[32] = 0; + out_4567419581298515838[33] = 0; + out_4567419581298515838[34] = 1; + out_4567419581298515838[35] = 0; + out_4567419581298515838[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_4567419581298515838[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_4567419581298515838[38] = 0; + out_4567419581298515838[39] = -state[7]; + out_4567419581298515838[40] = state[6]; + out_4567419581298515838[41] = 0; + out_4567419581298515838[42] = state[4]; + out_4567419581298515838[43] = -state[3]; + out_4567419581298515838[44] = 0; + out_4567419581298515838[45] = 0; + out_4567419581298515838[46] = 0; + out_4567419581298515838[47] = 0; + out_4567419581298515838[48] = 0; + out_4567419581298515838[49] = 0; + out_4567419581298515838[50] = 1; + out_4567419581298515838[51] = 0; + out_4567419581298515838[52] = 0; + out_4567419581298515838[53] = 1; } -void h_13(double *state, double *unused, double *out_5553209120626692209) { - out_5553209120626692209[0] = state[3]; - out_5553209120626692209[1] = state[4]; - out_5553209120626692209[2] = state[5]; +void h_13(double *state, double *unused, double *out_711061843632216754) { + out_711061843632216754[0] = state[3]; + out_711061843632216754[1] = state[4]; + out_711061843632216754[2] = state[5]; } -void H_13(double *state, double *unused, double *out_5685954708445744610) { - out_5685954708445744610[0] = 0; - out_5685954708445744610[1] = 0; - out_5685954708445744610[2] = 0; - out_5685954708445744610[3] = 1; - out_5685954708445744610[4] = 0; - out_5685954708445744610[5] = 0; - out_5685954708445744610[6] = 0; - out_5685954708445744610[7] = 0; - out_5685954708445744610[8] = 0; - out_5685954708445744610[9] = 0; - out_5685954708445744610[10] = 0; - out_5685954708445744610[11] = 0; - out_5685954708445744610[12] = 0; - out_5685954708445744610[13] = 0; - out_5685954708445744610[14] = 0; - out_5685954708445744610[15] = 0; - out_5685954708445744610[16] = 0; - out_5685954708445744610[17] = 0; - out_5685954708445744610[18] = 0; - out_5685954708445744610[19] = 0; - out_5685954708445744610[20] = 0; - out_5685954708445744610[21] = 0; - out_5685954708445744610[22] = 1; - out_5685954708445744610[23] = 0; - out_5685954708445744610[24] = 0; - out_5685954708445744610[25] = 0; - out_5685954708445744610[26] = 0; - out_5685954708445744610[27] = 0; - out_5685954708445744610[28] = 0; - out_5685954708445744610[29] = 0; - out_5685954708445744610[30] = 0; - out_5685954708445744610[31] = 0; - out_5685954708445744610[32] = 0; - out_5685954708445744610[33] = 0; - out_5685954708445744610[34] = 0; - out_5685954708445744610[35] = 0; - out_5685954708445744610[36] = 0; - out_5685954708445744610[37] = 0; - out_5685954708445744610[38] = 0; - out_5685954708445744610[39] = 0; - out_5685954708445744610[40] = 0; - out_5685954708445744610[41] = 1; - out_5685954708445744610[42] = 0; - out_5685954708445744610[43] = 0; - out_5685954708445744610[44] = 0; - out_5685954708445744610[45] = 0; - out_5685954708445744610[46] = 0; - out_5685954708445744610[47] = 0; - out_5685954708445744610[48] = 0; - out_5685954708445744610[49] = 0; - out_5685954708445744610[50] = 0; - out_5685954708445744610[51] = 0; - out_5685954708445744610[52] = 0; - out_5685954708445744610[53] = 0; +void H_13(double *state, double *unused, double *out_4333253945797393041) { + out_4333253945797393041[0] = 0; + out_4333253945797393041[1] = 0; + out_4333253945797393041[2] = 0; + out_4333253945797393041[3] = 1; + out_4333253945797393041[4] = 0; + out_4333253945797393041[5] = 0; + out_4333253945797393041[6] = 0; + out_4333253945797393041[7] = 0; + out_4333253945797393041[8] = 0; + out_4333253945797393041[9] = 0; + out_4333253945797393041[10] = 0; + out_4333253945797393041[11] = 0; + out_4333253945797393041[12] = 0; + out_4333253945797393041[13] = 0; + out_4333253945797393041[14] = 0; + out_4333253945797393041[15] = 0; + out_4333253945797393041[16] = 0; + out_4333253945797393041[17] = 0; + out_4333253945797393041[18] = 0; + out_4333253945797393041[19] = 0; + out_4333253945797393041[20] = 0; + out_4333253945797393041[21] = 0; + out_4333253945797393041[22] = 1; + out_4333253945797393041[23] = 0; + out_4333253945797393041[24] = 0; + out_4333253945797393041[25] = 0; + out_4333253945797393041[26] = 0; + out_4333253945797393041[27] = 0; + out_4333253945797393041[28] = 0; + out_4333253945797393041[29] = 0; + out_4333253945797393041[30] = 0; + out_4333253945797393041[31] = 0; + out_4333253945797393041[32] = 0; + out_4333253945797393041[33] = 0; + out_4333253945797393041[34] = 0; + out_4333253945797393041[35] = 0; + out_4333253945797393041[36] = 0; + out_4333253945797393041[37] = 0; + out_4333253945797393041[38] = 0; + out_4333253945797393041[39] = 0; + out_4333253945797393041[40] = 0; + out_4333253945797393041[41] = 1; + out_4333253945797393041[42] = 0; + out_4333253945797393041[43] = 0; + out_4333253945797393041[44] = 0; + out_4333253945797393041[45] = 0; + out_4333253945797393041[46] = 0; + out_4333253945797393041[47] = 0; + out_4333253945797393041[48] = 0; + out_4333253945797393041[49] = 0; + out_4333253945797393041[50] = 0; + out_4333253945797393041[51] = 0; + out_4333253945797393041[52] = 0; + out_4333253945797393041[53] = 0; } -void h_14(double *state, double *unused, double *out_8764675248435452423) { - out_8764675248435452423[0] = state[6]; - out_8764675248435452423[1] = state[7]; - out_8764675248435452423[2] = state[8]; +void h_14(double *state, double *unused, double *out_1546878073048898636) { + out_1546878073048898636[0] = state[6]; + out_1546878073048898636[1] = state[7]; + out_1546878073048898636[2] = state[8]; } -void H_14(double *state, double *unused, double *out_4934987677438592882) { - out_4934987677438592882[0] = 0; - out_4934987677438592882[1] = 0; - out_4934987677438592882[2] = 0; - out_4934987677438592882[3] = 0; - out_4934987677438592882[4] = 0; - out_4934987677438592882[5] = 0; - out_4934987677438592882[6] = 1; - out_4934987677438592882[7] = 0; - out_4934987677438592882[8] = 0; - out_4934987677438592882[9] = 0; - out_4934987677438592882[10] = 0; - out_4934987677438592882[11] = 0; - out_4934987677438592882[12] = 0; - out_4934987677438592882[13] = 0; - out_4934987677438592882[14] = 0; - out_4934987677438592882[15] = 0; - out_4934987677438592882[16] = 0; - out_4934987677438592882[17] = 0; - out_4934987677438592882[18] = 0; - out_4934987677438592882[19] = 0; - out_4934987677438592882[20] = 0; - out_4934987677438592882[21] = 0; - out_4934987677438592882[22] = 0; - out_4934987677438592882[23] = 0; - out_4934987677438592882[24] = 0; - out_4934987677438592882[25] = 1; - out_4934987677438592882[26] = 0; - out_4934987677438592882[27] = 0; - out_4934987677438592882[28] = 0; - out_4934987677438592882[29] = 0; - out_4934987677438592882[30] = 0; - out_4934987677438592882[31] = 0; - out_4934987677438592882[32] = 0; - out_4934987677438592882[33] = 0; - out_4934987677438592882[34] = 0; - out_4934987677438592882[35] = 0; - out_4934987677438592882[36] = 0; - out_4934987677438592882[37] = 0; - out_4934987677438592882[38] = 0; - out_4934987677438592882[39] = 0; - out_4934987677438592882[40] = 0; - out_4934987677438592882[41] = 0; - out_4934987677438592882[42] = 0; - out_4934987677438592882[43] = 0; - out_4934987677438592882[44] = 1; - out_4934987677438592882[45] = 0; - out_4934987677438592882[46] = 0; - out_4934987677438592882[47] = 0; - out_4934987677438592882[48] = 0; - out_4934987677438592882[49] = 0; - out_4934987677438592882[50] = 0; - out_4934987677438592882[51] = 0; - out_4934987677438592882[52] = 0; - out_4934987677438592882[53] = 0; +void H_14(double *state, double *unused, double *out_685863593820176641) { + out_685863593820176641[0] = 0; + out_685863593820176641[1] = 0; + out_685863593820176641[2] = 0; + out_685863593820176641[3] = 0; + out_685863593820176641[4] = 0; + out_685863593820176641[5] = 0; + out_685863593820176641[6] = 1; + out_685863593820176641[7] = 0; + out_685863593820176641[8] = 0; + out_685863593820176641[9] = 0; + out_685863593820176641[10] = 0; + out_685863593820176641[11] = 0; + out_685863593820176641[12] = 0; + out_685863593820176641[13] = 0; + out_685863593820176641[14] = 0; + out_685863593820176641[15] = 0; + out_685863593820176641[16] = 0; + out_685863593820176641[17] = 0; + out_685863593820176641[18] = 0; + out_685863593820176641[19] = 0; + out_685863593820176641[20] = 0; + out_685863593820176641[21] = 0; + out_685863593820176641[22] = 0; + out_685863593820176641[23] = 0; + out_685863593820176641[24] = 0; + out_685863593820176641[25] = 1; + out_685863593820176641[26] = 0; + out_685863593820176641[27] = 0; + out_685863593820176641[28] = 0; + out_685863593820176641[29] = 0; + out_685863593820176641[30] = 0; + out_685863593820176641[31] = 0; + out_685863593820176641[32] = 0; + out_685863593820176641[33] = 0; + out_685863593820176641[34] = 0; + out_685863593820176641[35] = 0; + out_685863593820176641[36] = 0; + out_685863593820176641[37] = 0; + out_685863593820176641[38] = 0; + out_685863593820176641[39] = 0; + out_685863593820176641[40] = 0; + out_685863593820176641[41] = 0; + out_685863593820176641[42] = 0; + out_685863593820176641[43] = 0; + out_685863593820176641[44] = 1; + out_685863593820176641[45] = 0; + out_685863593820176641[46] = 0; + out_685863593820176641[47] = 0; + out_685863593820176641[48] = 0; + out_685863593820176641[49] = 0; + out_685863593820176641[50] = 0; + out_685863593820176641[51] = 0; + out_685863593820176641[52] = 0; + out_685863593820176641[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_5239387107130576177) { - err_fun(nom_x, delta_x, out_5239387107130576177); +void pose_err_fun(double *nom_x, double *delta_x, double *out_2903893253895971355) { + err_fun(nom_x, delta_x, out_2903893253895971355); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8017465374679690286) { - inv_err_fun(nom_x, true_x, out_8017465374679690286); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3928278332152438487) { + inv_err_fun(nom_x, true_x, out_3928278332152438487); } -void pose_H_mod_fun(double *state, double *out_3050146727026118690) { - H_mod_fun(state, out_3050146727026118690); +void pose_H_mod_fun(double *state, double *out_5666984403538513454) { + H_mod_fun(state, out_5666984403538513454); } -void pose_f_fun(double *state, double dt, double *out_7401195424910099109) { - f_fun(state, dt, out_7401195424910099109); +void pose_f_fun(double *state, double dt, double *out_3108653889513613454) { + f_fun(state, dt, out_3108653889513613454); } -void pose_F_fun(double *state, double dt, double *out_7953355824038160444) { - F_fun(state, dt, out_7953355824038160444); +void pose_F_fun(double *state, double dt, double *out_3156103136607478463) { + F_fun(state, dt, out_3156103136607478463); } -void pose_h_4(double *state, double *unused, double *out_5685529689104176696) { - h_4(state, unused, out_5685529689104176696); +void pose_h_4(double *state, double *unused, double *out_4668712772901425910) { + h_4(state, unused, out_4668712772901425910); } -void pose_H_4(double *state, double *unused, double *out_8898228533778077411) { - H_4(state, unused, out_8898228533778077411); +void pose_H_4(double *state, double *unused, double *out_3277377262519307888) { + H_4(state, unused, out_3277377262519307888); } -void pose_h_10(double *state, double *unused, double *out_5395236827797613020) { - h_10(state, unused, out_5395236827797613020); +void pose_h_10(double *state, double *unused, double *out_5024807863031837328) { + h_10(state, unused, out_5024807863031837328); } -void pose_H_10(double *state, double *unused, double *out_4460820551654126260) { - H_10(state, unused, out_4460820551654126260); +void pose_H_10(double *state, double *unused, double *out_4567419581298515838) { + H_10(state, unused, out_4567419581298515838); } -void pose_h_13(double *state, double *unused, double *out_5553209120626692209) { - h_13(state, unused, out_5553209120626692209); +void pose_h_13(double *state, double *unused, double *out_711061843632216754) { + h_13(state, unused, out_711061843632216754); } -void pose_H_13(double *state, double *unused, double *out_5685954708445744610) { - H_13(state, unused, out_5685954708445744610); +void pose_H_13(double *state, double *unused, double *out_4333253945797393041) { + H_13(state, unused, out_4333253945797393041); } -void pose_h_14(double *state, double *unused, double *out_8764675248435452423) { - h_14(state, unused, out_8764675248435452423); +void pose_h_14(double *state, double *unused, double *out_1546878073048898636) { + h_14(state, unused, out_1546878073048898636); } -void pose_H_14(double *state, double *unused, double *out_4934987677438592882) { - H_14(state, unused, out_4934987677438592882); +void pose_H_14(double *state, double *unused, double *out_685863593820176641) { + H_14(state, unused, out_685863593820176641); } 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/openpilot/selfdrive/locationd/models/generated/pose.h b/openpilot/selfdrive/locationd/models/generated/pose.h index 4be5794459..8100399dc3 100644 --- a/openpilot/selfdrive/locationd/models/generated/pose.h +++ b/openpilot/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_5239387107130576177); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8017465374679690286); -void pose_H_mod_fun(double *state, double *out_3050146727026118690); -void pose_f_fun(double *state, double dt, double *out_7401195424910099109); -void pose_F_fun(double *state, double dt, double *out_7953355824038160444); -void pose_h_4(double *state, double *unused, double *out_5685529689104176696); -void pose_H_4(double *state, double *unused, double *out_8898228533778077411); -void pose_h_10(double *state, double *unused, double *out_5395236827797613020); -void pose_H_10(double *state, double *unused, double *out_4460820551654126260); -void pose_h_13(double *state, double *unused, double *out_5553209120626692209); -void pose_H_13(double *state, double *unused, double *out_5685954708445744610); -void pose_h_14(double *state, double *unused, double *out_8764675248435452423); -void pose_H_14(double *state, double *unused, double *out_4934987677438592882); +void pose_err_fun(double *nom_x, double *delta_x, double *out_2903893253895971355); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3928278332152438487); +void pose_H_mod_fun(double *state, double *out_5666984403538513454); +void pose_f_fun(double *state, double dt, double *out_3108653889513613454); +void pose_F_fun(double *state, double dt, double *out_3156103136607478463); +void pose_h_4(double *state, double *unused, double *out_4668712772901425910); +void pose_H_4(double *state, double *unused, double *out_3277377262519307888); +void pose_h_10(double *state, double *unused, double *out_5024807863031837328); +void pose_H_10(double *state, double *unused, double *out_4567419581298515838); +void pose_h_13(double *state, double *unused, double *out_711061843632216754); +void pose_H_13(double *state, double *unused, double *out_4333253945797393041); +void pose_h_14(double *state, double *unused, double *out_1546878073048898636); +void pose_H_14(double *state, double *unused, double *out_685863593820176641); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/openpilot/selfdrive/selfdrived/selfdrived.py b/openpilot/selfdrive/selfdrived/selfdrived.py index 0bf38ea322..4f62166e41 100755 --- a/openpilot/selfdrive/selfdrived/selfdrived.py +++ b/openpilot/selfdrive/selfdrived/selfdrived.py @@ -12,6 +12,7 @@ import openpilot.cereal.messaging as messaging from openpilot.cereal import log from opendbc.car.structs import car from opendbc.car.hyundai.ev6_aol import qualified as ev6_aol_qualified +from opendbc.car.honda.stock_aol import qualified as honda_aol_qualified, temporary_restriction as honda_temporary_restriction from openpilot.cereal.visionipc import VisionStreamType from msgq.visionipc import VisionIpcClient @@ -35,7 +36,7 @@ from openpilot.starpilot.car.gm.steering_companion import SteeringCompanion from opendbc.car.gm.values import GMFlags, is_volt_ascm_longitudinal from openpilot.starpilot.aol.intent import read_settings from openpilot.starpilot.aol.intent_companion import IntentCompanion -from openpilot.starpilot.aol.transport_pause import AvailabilityPause, TransportPause, availability_only +from openpilot.starpilot.aol.transport_pause import AvailabilityPause, TransportPause, availability_only, cp_identity, healthy_transport from openpilot.starpilot.aol.runtime import (INTENT_MAX_AGE_NS, SAFETY_MAX_AGE_NS, AxisDecision, current_intent, current_native, decide_axes, ordinary_lateral_requested, decide_ordinary_axis) from openpilot.starpilot.aol.vehicle import policy_for as axis_policy_for, ordinary_axis_request_allowed @@ -1068,6 +1069,38 @@ class SelfdriveD: sample.vEgo < (self.aol_settings.pause_brake_mps if self.aol_settings is not None else 0.0) for sample, _ in getattr(self, 'car_state_samples', ()))) + def honda_native_lateral_grant_lost(self, CS, native, previous, requested, *, source_current, previous_intent, intent, now_ns): + """An established host LAT request lost its native grant, not a cold wait.""" + pause = getattr(self, 'aol_transport_pause', None) + prior = getattr(pause, 'observed_native', None) + if (not honda_aol_qualified(self.CP, marked_only=True) or not source_current or + not self.initialized or not previous.lateral_active or not requested.desired_lateral or + native is None or previous_intent is None or intent is None or not previous_intent.producerSessionId or + previous_intent.producerSessionId != intent.producerSessionId or + not 0 < previous_intent.carStateLogMonoTime <= native.observedMonoTime <= now_ns or + now_ns - previous_intent.carStateLogMonoTime > INTENT_MAX_AGE_NS or + native.requestedLateral or native.lateralAllowed or + prior is None or not prior.requestedLateral or not prior.lateralAllowed or + getattr(self, 'aol_fault_reason', 'none') != 'none' or + pause.active or getattr(getattr(self, 'aol_availability_pause', None), 'active', False) or + CS.brakePressed or CS.regenBraking or CS.gasPressed or CS.steeringPressed or + honda_temporary_restriction(self.CP, CS) or + any(self.events.contains(event) for event in + (ET.USER_DISABLE, ET.OVERRIDE_LATERAL, ET.OVERRIDE_LONGITUDINAL, ET.IMMEDIATE_DISABLE, ET.SOFT_DISABLE)) or + not self.sm.all_checks(['modelV2', 'extrinsicsCalibration']) or + self.sm['extrinsicsCalibration'].calStatus != log.ExtrinsicsCalibration.Status.calibrated): + return False + def identity(state): + return (state.axisSessionId, state.pandaSerial, state.safetyModel, state.safetyParam, + tuple((p.slotIndex, p.hardwareSerial, p.safetyModel, p.safetyParam, + p.alternativeExperience) for p in state.pandaInventory)) + return bool(pause.cp == cp_identity(self.CP) and pause.session == self.aol_session_id and + identity(prior) == identity(native) and + 0 < prior.observedMonoTime <= native.observedMonoTime <= now_ns <= prior.validUntilMonoTime and + now_ns - prior.observedMonoTime <= SAFETY_MAX_AGE_NS and + current_native(self.sm, self.CP, now_ns=now_ns, axis_session_id=self.aol_session_id) == native and + healthy_transport(self.sm, self.CP, CS, self.aol_session_id, now_ns, native)) + def record_aol_authority_loss(self, *, now_ns, companion, drained, native, source_current, lost_active, lost_companion, lost_permission, upgraded): if now_ns - getattr(self, 'aol_last_authority_report_ns', 0) < 5_000_000_000: @@ -1141,7 +1174,10 @@ class SelfdriveD: self.events.contains(ET.USER_DISABLE)) lost_companion = bool((previous.lateral_active or previous.longitudinal_active) and not source_current and not self.events.contains(ET.USER_DISABLE)) - lost_permission = bool(native is not None and not driver_override and ( + lost_honda_lateral_grant = self.honda_native_lateral_grant_lost( + CS, native, previous, requested, source_current=source_current, + previous_intent=previous_intent, intent=intent, now_ns=now_ns) + lost_permission = bool(lost_honda_lateral_grant or native is not None and not driver_override and ( previous.lateral_active and requested.desired_lateral and native.requestedLateral and not native.lateralAllowed and not CS.steeringPressed and not self.events.contains(ET.OVERRIDE_LATERAL) or previous.longitudinal_active and requested.desired_longitudinal and native.requestedLongitudinal and diff --git a/openpilot/starpilot/aol/tests/test_honda_brake_intent_health.py b/openpilot/starpilot/aol/tests/test_honda_brake_intent_health.py new file mode 100644 index 0000000000..afdf10ec52 --- /dev/null +++ b/openpilot/starpilot/aol/tests/test_honda_brake_intent_health.py @@ -0,0 +1,277 @@ +"""Honda brake only retains existing LAT intent; these are controlled unit peers.""" +from dataclasses import replace +from types import SimpleNamespace +from typing import Any + +import pytest + +from opendbc.car import Bus, gen_empty_fingerprint, structs +from opendbc.car.honda.interface import CarInterface +from opendbc.car.honda.values import CAR, HondaFlags +from openpilot.cereal import log, messaging +from openpilot.selfdrive.car.card import Car +from openpilot.starpilot.aol.runtime import decide_axes +from openpilot.starpilot.aol.transport_pause import TransportPauseFeedback, independent_lateral_health, _healthy_transport_native +from openpilot.starpilot.aol.wire import PandaSlot, SafetyState, encode_safety + +NOW = 10_000_000_000 + + +class Peers: + def __init__(self, messages): + self.messages = {m.which(): m for m in messages} + self.logMonoTime = {n: int(m.logMonoTime) for n, m in self.messages.items()} + self.seen = self.valid = self.alive = dict.fromkeys(self.messages, True) + + def __getitem__(self, name): + return getattr(self.messages[name], name) + + +def fixture(*, coherent=False, long_allowed=True, brake=True): + fp = gen_empty_fingerprint() + fp[0][0x1A3] = 8 + cp = CarInterface.get_params(CAR.HONDA_CIVIC_2022, fp, [], True, False, False) + cp.alternativeExperience = 32 + cp.fingerprintSource = structs.CarParams.FingerprintSource.fw + config = cp.safetyConfigs[0] + assert config.safetyParam == 10 + slots = ((PandaSlot(0, 'unit-honda', config.safetyModel.raw, 10, 32, long_allowed),) if coherent else ()) + native = SafetyState(1, True, NOW - 20_000_000, NOW + 180_000_000, + config.safetyModel.raw, 10, True, long_allowed, True, True, + 'unit-honda', 'unit-session', sourcePandaStatesMonoTime=NOW + 9_000_000_000 if coherent else 0, + pandaInventory=slots) + safety = messaging.new_message('aolSafetyWire', 0, valid=True, logMonoTime=native.observedMonoTime) + safety.aolSafetyWire = encode_safety(native) + pandas = messaging.new_message('pandaStates', 1, valid=True, logMonoTime=NOW) + p = pandas.pandaStates[0] + p.safetyModel, p.safetyParam, p.alternativeExperience = config.safetyModel, 10, 32 + p.controlsAllowed = False + axis = messaging.new_message('aolAxisState', valid=True, logMonoTime=NOW - 1_000_000) + a = axis.aolAxisState + a.sessionId = a.faultSessionId = 'unit-session' + a.qualified, a.sequence, a.faultReason = True, 2, 'none' + a.observedMonoTime, a.validUntilMonoTime = axis.logMonoTime, axis.logMonoTime + 30_000_000 + a.sourceCarStateMonoTime = axis.logMonoTime - 1000 + a.desiredLateral = a.lateralActive = True + a.nativeAcknowledged = True + events = messaging.new_message('onroadEvents', 0, valid=True, logMonoTime=NOW - 1_000_000) + sm = Peers([safety.as_reader(), pandas.as_reader(), axis.as_reader(), events.as_reader()]) + cs = structs.CarState(canValid=True, brakePressed=brake, vEgo=12.5, + gearShifter=structs.CarState.GearShifter.drive) + feedback = TransportPauseFeedback(cp) + # Unit precondition representing an already observed healthy acknowledged LAT baseline. + feedback.session = native.axisSessionId + feedback.healthy_feedback_ns = NOW - 10_000_000 + feedback.healthy_native_ns = native.observedMonoTime + feedback.healthy_inventory = (native.pandaSerial, tuple(p.hardwareSerial for p in slots)) + return cp, cs, sm, native, feedback + + +@pytest.mark.parametrize('coherent', [False, True]) +@pytest.mark.parametrize('long_allowed', [False, True]) +def test_honda_brake_lateral_intent_only_and_long_fail_closed(coherent, long_allowed): + cp, cs, sm, native, feedback = fixture(coherent=coherent, long_allowed=long_allowed) + assert _healthy_transport_native(sm, cp, cs, 'unit-session', NOW) is None + assert independent_lateral_health(sm, cp, cs, 'unit-session', NOW) == native + assert not feedback.observe(sm, cp, cs, NOW, latched=True, brake_source_ns=NOW - 1000) + assert feedback.honda_brake_ns == NOW - 1000 + decision = decide_axes(standard_lateral=False, standard_longitudinal=False, + intent=SimpleNamespace(pauseLateral=False, allowedLatch=True, pauseLongitudinal=False), + native=native, car_state=cs, initialized=True, model_ready=True, no_entry=False, + immediate_disable=False, dm_lockout=False, pause_brake_mps=5.) + assert decision.desired_lateral and decision.lateral_active + assert not decision.desired_longitudinal and not decision.longitudinal_active + assert native.validUntilMonoTime == NOW + 180_000_000 + + +@pytest.mark.parametrize('mutation', ['no_brake', 'no_source', 'stale_source', 'future_source', 'no_latch', + 'wrong_profile', 'wrong_session', 'serial', 'native_expired', 'no_lat', 'lat_denied', + 'rx_fault', 'heartbeat', 'panda_config', 'axis_fault', 'hard_event', 'stale_axis', 'unacknowledged_long']) +def test_honda_brake_cannot_borrow_or_renew_authority(mutation): + cp, cs, sm, native, feedback = fixture(coherent=True, long_allowed=False) + source, now, latched = NOW - 1000, NOW, True + axis = sm.messages['aolAxisState'].as_builder() + pandas = sm.messages['pandaStates'].as_builder() + if mutation == 'no_brake': + cs.brakePressed = False + if mutation == 'no_source': + source = 0 + if mutation == 'stale_source': + source = NOW - 30_000_001 + if mutation == 'future_source': + source = NOW + 1 + if mutation == 'no_latch': + latched = False + if mutation == 'wrong_profile': + cp.alternativeExperience = 0 + if mutation == 'wrong_session': + native = replace(native, axisSessionId='replaced-session') + if mutation == 'serial': + native = replace(native, pandaSerial='replaced-serial') + if mutation == 'native_expired': + native = replace(native, validUntilMonoTime=NOW - 1) + if mutation == 'no_lat': + native = replace(native, requestedLateral=False) + if mutation == 'lat_denied': + native = replace(native, lateralAllowed=False) + if mutation == 'rx_fault': + pandas.pandaStates[0].safetyRxChecksInvalid = True + if mutation == 'heartbeat': + pandas.pandaStates[0].heartbeatLost = True + if mutation == 'panda_config': + pandas.pandaStates[0].safetyParam = 8 + if mutation == 'axis_fault': + axis.aolAxisState.faultReason = 'critical' + if mutation == 'stale_axis': + now = NOW + 30_000_001 + if mutation == 'unacknowledged_long': + axis.aolAxisState.nativeAcknowledged = False + axis.aolAxisState.desiredLongitudinal = True + events = sm.messages['onroadEvents'] + if mutation == 'hard_event': + event = messaging.new_message('onroadEvents', 1, valid=True, logMonoTime=NOW - 1000) + event.onroadEvents[0].name = 'controlsMismatch' + event.onroadEvents[0].immediateDisable = True + events = event.as_reader() + safety = messaging.new_message('aolSafetyWire', 0, valid=True, logMonoTime=native.observedMonoTime) + safety.aolSafetyWire = encode_safety(native) + sm = Peers([safety.as_reader(), pandas.as_reader(), axis.as_reader(), events]) + assert feedback._honda_brake_lateral_current(sm, cp, cs, sm['aolAxisState'], now, + latched=latched, brake_source_ns=source) is None + if mutation != 'no_latch': + assert feedback.observe(sm, cp, cs, now, latched=latched, brake_source_ns=source) + assert feedback.honda_brake_ns == 0 + + +def test_release_and_held_brake_cannot_extend_original_deadline(): + cp, cs, sm, native, feedback = fixture(long_allowed=False) + source = NOW - 1000 + assert not feedback.observe(sm, cp, cs, NOW, latched=True, brake_source_ns=source) + cs.brakePressed = False + assert not feedback.observe(sm, cp, cs, NOW + 1, latched=True) + assert feedback.honda_brake_ns == source + cs.brakePressed = True + assert not feedback.observe(sm, cp, cs, NOW + 2, latched=True, brake_source_ns=NOW + 2) + assert feedback.honda_brake_ns == source + axis = sm['aolAxisState'] + assert feedback._honda_brake_lateral_current(sm, cp, cs, axis, source + 200_000_001, + latched=True, brake_source_ns=source + 200_000_001) is None + assert feedback.honda_brake_ns == source + + +def test_card_brake_source_keeps_boot_and_monotonic_clocks_separate(): + cp, cs, _, _, _ = fixture() + card: Any = Car.__new__(Car) + card.CP = cp + message = 'BRAKE_MODULE' if cp.flags & HondaFlags.BOSCH_ALT_BRAKE else 'POWERTRAIN_DATA' + parser = SimpleNamespace(vl={message: {'BRAKE_PRESSED': 1}}, + ts_nanos={message: {'BRAKE_PRESSED': NOW + 9_000_000_000}}) + card.CI = SimpleNamespace(can_parsers={Bus.pt: parser}, CS=SimpleNamespace(brake_switch_active=False)) + card._aol_can_observation = (cs, NOW - 1000, NOW + 9_000_000_000, NOW + 9_000_001_000) + assert card.aol_honda_brake_source(cs, NOW) == NOW - 1000 + parser.ts_nanos[message]['BRAKE_PRESSED'] -= 30_000_001 + assert card.aol_honda_brake_source(cs, NOW) == 0 + parser.ts_nanos[message]['BRAKE_PRESSED'] += 30_000_001 + assert card.aol_honda_brake_source(structs.CarState(**cs.to_dict()), NOW) == 0 + parser.vl[message]['BRAKE_PRESSED'] = 0 + assert card.aol_honda_brake_source(cs, NOW) == 0 + + +@pytest.mark.parametrize('mutation', ['none', 'cold', 'withdrawn', 'no_source', 'brake', 'steering', 'gear', + 'soft_pause', 'transport_pause', 'availability_pause', 'session', 'serial', 'cp', 'older', 'expired', + 'fault', 'model', 'requested_denial', 'new_session', 'no_prior', 'uninitialized', + 'latest_expired', 'temporary_steer', 'unqualified', 'user_disable', 'rx_fault', + 'receipt_before_source', 'stale_source', 'producer', 'no_prior_source', 'long_override']) +def test_sd_honda_unexpected_zero_grant_loss_is_not_cold_or_commanded_pause(mutation): + from openpilot.selfdrive.selfdrived.events import Events + from openpilot.selfdrive.selfdrived.selfdrived import SelfdriveD + from openpilot.starpilot.aol.runtime import AxisDecision + from openpilot.starpilot.aol.transport_pause import cp_identity + + cp, cs, sm, prior, _ = fixture(coherent=True, brake=False) + prior = replace(prior, observedMonoTime=NOW - 40_000_000, validUntilMonoTime=NOW + 160_000_000) + native = replace(prior, observedMonoTime=NOW - 10_000_000, validUntilMonoTime=NOW + 190_000_000, + requestedLateral=False, lateralAllowed=False, requestedLongitudinal=False, longitudinalAllowed=False) + sd: Any = SelfdriveD.__new__(SelfdriveD) + sd.CP, sd.sm, sd.aol_session_id = cp, sm, 'unit-session' + sd.initialized, sd.events, sd.aol_fault_reason = True, Events(), 'none' + sd.aol_transport_pause = SimpleNamespace(cp=cp_identity(cp), session='unit-session', active=False, observed_native=prior) + sd.aol_availability_pause = SimpleNamespace(active=False) + sm.all_checks = lambda services: True + sm.messages['extrinsicsCalibration'] = SimpleNamespace(extrinsicsCalibration=SimpleNamespace( + calStatus=log.ExtrinsicsCalibration.Status.calibrated)) + previous, requested = AxisDecision(lateral_active=True), AxisDecision(desired_lateral=True) + source_current = True + prior_intent = SimpleNamespace(producerSessionId='unit-producer', carStateLogMonoTime=NOW - 10_000_000) + intent = SimpleNamespace(producerSessionId='unit-producer') + if mutation == 'cold': + previous = AxisDecision() + elif mutation == 'withdrawn': + requested = AxisDecision() + elif mutation == 'no_source': + source_current = False + elif mutation == 'brake': + cs.brakePressed = True + elif mutation == 'steering': + cs.steeringPressed = True + elif mutation == 'gear': + cs.gearShifter = structs.CarState.GearShifter.park + elif mutation == 'soft_pause': + sd.aol_fault_reason = 'availabilityPause' + elif mutation == 'transport_pause': + sd.aol_transport_pause.active = True + elif mutation == 'availability_pause': + sd.aol_availability_pause.active = True + elif mutation == 'session': + native = replace(native, axisSessionId='replacement') + elif mutation == 'serial': + native = replace(native, pandaSerial='replacement') + elif mutation == 'cp': + sd.aol_transport_pause.cp = () + elif mutation == 'older': + native = replace(native, observedMonoTime=prior.observedMonoTime - 1) + elif mutation == 'expired': + prior = replace(prior, validUntilMonoTime=NOW - 1) + elif mutation == 'fault': + sd.events.add(log.OnroadEvent.EventName.controlsMismatch) + elif mutation == 'model': + sm.all_checks = lambda services: False + elif mutation == 'requested_denial': + native = replace(native, requestedLateral=True) + elif mutation == 'new_session': + sd.aol_transport_pause.session = 'replacement' + elif mutation == 'no_prior': + prior = None + elif mutation == 'uninitialized': + sd.initialized = False + elif mutation == 'latest_expired': + native = replace(native, validUntilMonoTime=NOW - 1) + elif mutation == 'temporary_steer': + cs.steerFaultTemporary = True + elif mutation == 'unqualified': + cp.alternativeExperience = 0 + elif mutation == 'user_disable': + sd.events.add(log.OnroadEvent.EventName.buttonCancel) + elif mutation == 'rx_fault': + pandas = sm.messages['pandaStates'].as_builder() + pandas.pandaStates[0].safetyRxChecksInvalid = True + sm.messages['pandaStates'] = pandas.as_reader() + if mutation == 'receipt_before_source': + prior_intent.carStateLogMonoTime = NOW - 9_999_999 + elif mutation == 'stale_source': + prior_intent.carStateLogMonoTime = NOW - 30_000_001 + elif mutation == 'producer': + intent.producerSessionId = 'replacement' + elif mutation == 'no_prior_source': + prior_intent.carStateLogMonoTime = 0 + elif mutation == 'long_override': + sd.events.add(log.OnroadEvent.EventName.gasPressedOverride) + sd.aol_transport_pause.observed_native = prior + event = messaging.new_message('aolSafetyWire', 0, valid=True, logMonoTime=native.observedMonoTime) + event.aolSafetyWire = encode_safety(native) + sm.messages['aolSafetyWire'] = event.as_reader() + sm.logMonoTime['aolSafetyWire'] = native.observedMonoTime + assert sd.honda_native_lateral_grant_lost(cs, native, previous, requested, + source_current=source_current, previous_intent=prior_intent, intent=intent, now_ns=NOW) == (mutation == 'none') + assert native.validUntilMonoTime == (NOW - 1 if mutation == 'latest_expired' else NOW + 190_000_000) + assert not native.lateralAllowed and not native.longitudinalAllowed diff --git a/openpilot/starpilot/aol/transport_pause.py b/openpilot/starpilot/aol/transport_pause.py index 31f69c7952..0f7e092b12 100644 --- a/openpilot/starpilot/aol/transport_pause.py +++ b/openpilot/starpilot/aol/transport_pause.py @@ -11,14 +11,14 @@ def cp_identity(cp): tuple((int(c.safetyModel.raw), int(c.safetyParam)) for c in cp.safetyConfigs)) -def _healthy_transport_native(sm, cp, cs, session, now_ns, native=None): +def _healthy_transport_native(sm, cp, cs, session, now_ns, native=None, *, require_longitudinal=True): """Require the live, exact native configuration, including every Panda.""" try: native = native or current_native(sm, cp, now_ns=now_ns, axis_session_id=session) if (not session or native is None or str(native.axisSessionId) != session or not cs.canValid or cs.canTimeout or cs.steerFaultTemporary or cs.steerFaultPermanent or native.requestedLateral and not native.lateralAllowed or - native.requestedLongitudinal and not native.longitudinalAllowed): + require_longitudinal and native.requestedLongitudinal and not native.longitudinalAllowed): return None coherent = bool(native.pandaInventory) if coherent and (int(sm.logMonoTime['aolSafetyWire']) != native.observedMonoTime or @@ -28,7 +28,7 @@ def _healthy_transport_native(sm, cp, cs, session, now_ns, native=None): return None if coherent and (not native_inventory_matches_cp(native, cp) or any(p.safetyRxChecksInvalid or p.heartbeatLost or p.faults or - (native.requestedLongitudinal and str(config.safetyModel) not in ('silent', 'noOutput') and + (require_longitudinal and native.requestedLongitudinal and str(config.safetyModel) not in ('silent', 'noOutput') and not p.controlsAllowed) for p, config in zip(native.pandaInventory, cp.safetyConfigs, strict=True))): return None @@ -44,12 +44,18 @@ def _healthy_transport_native(sm, cp, cs, session, now_ns, native=None): return native if all(int(p.safetyModel.raw) == int(c.safetyModel.raw) and p.safetyParam == c.safetyParam and p.alternativeExperience == cp.alternativeExperience and not p.safetyRxChecksInvalid and not p.heartbeatLost and not p.faults and - (not native.requestedLongitudinal or str(c.safetyModel) in ('silent', 'noOutput') or p.controlsAllowed) + (not require_longitudinal or not native.requestedLongitudinal or str(c.safetyModel) in ('silent', 'noOutput') or p.controlsAllowed) for p, c in zip(pandas, cp.safetyConfigs, strict=True)) else None except (AttributeError, KeyError, IndexError, TypeError, ValueError): return None +def independent_lateral_health(sm, cp, cs, session, now_ns): + """Structural health for retaining LAT intent; never grant either output axis.""" + native = _healthy_transport_native(sm, cp, cs, session, now_ns, require_longitudinal=False) + return native if native is not None and native.requestedLateral and native.lateralAllowed else None + + def healthy_transport(sm, cp, cs, session, now_ns, native=None): """Require the live, exact native configuration, including every Panda.""" return _healthy_transport_native(sm, cp, cs, session, now_ns, native) is not None @@ -202,6 +208,7 @@ class TransportPauseFeedback: self.lateral_native_ns = 0 self.longitudinal_cancel_ns = 0 self.last_cancel_source_ns = 0 + self.honda_brake_ns = 0 self.retention_qualified = False self.retention_pending = False self.retention_event_ns = 0 @@ -214,6 +221,38 @@ class TransportPauseFeedback: self.healthy_native_ns = 0 self.healthy_inventory = None + def _honda_brake_lateral_current(self, sm, cp, cs, axis, now_ns, *, latched, brake_source_ns): + """Preserve existing LAT intent through brake's LONG withdrawal, never output.""" + from opendbc.car.honda.stock_aol import qualified + from opendbc.car.structs import car + + source_ns = self.honda_brake_ns or (brake_source_ns if cs.brakePressed else 0) + if (not qualified(cp, marked_only=True) or not latched or self.failed or self.paused or self.pending_event or + not self.session or str(axis.sessionId) != self.session or + not self.healthy_feedback_ns or not 0 <= now_ns - self.healthy_feedback_ns <= AXIS_MAX_AGE_NS or + not 0 < source_ns <= now_ns or now_ns - source_ns > SAFETY_MAX_AGE_NS or + (not self.honda_brake_ns and now_ns - source_ns > AXIS_MAX_AGE_NS) or + cs.accFaulted or cs.seatbeltUnlatched or + cs.gearShifter not in (car.CarState.GearShifter.drive, car.CarState.GearShifter.sport, + car.CarState.GearShifter.low, car.CarState.GearShifter.brake) or + str(axis.faultReason) != 'none' or not axis.desiredLateral or not axis.lateralActive or + (not axis.nativeAcknowledged and (axis.desiredLongitudinal or axis.longitudinalActive)) or + not 0 < axis.sourceCarStateMonoTime <= axis.observedMonoTime or + axis.observedMonoTime - axis.sourceCarStateMonoTime > AXIS_MAX_AGE_NS): + return None + event_ns = int(sm.logMonoTime['onroadEvents']) + if (not sm.seen['onroadEvents'] or not sm.valid['onroadEvents'] or not sm.alive['onroadEvents'] or + not 0 < event_ns <= now_ns or now_ns - event_ns > 1_500_000_000 or + any(e.immediateDisable or e.softDisable for e in sm['onroadEvents'])): + return None + native = independent_lateral_health(sm, cp, cs, self.session, now_ns) + if (native is None or not native.requestedLateral or not native.lateralAllowed or + not native.requestedLongitudinal or native.observedMonoTime < self.healthy_native_ns or + (native.pandaSerial, tuple(p.hardwareSerial for p in native.pandaInventory)) != self.healthy_inventory): + return None + self.honda_brake_ns = source_ns + return native + def _longitudinal_cancel_current(self, sm, cp, cs, axis, now_ns, *, latched, cancel_source_ns, withdrawal_required=True) -> SafetyState | None: """Retain existing intent during Cancel's bounded LONG withdrawal handshake.""" from opendbc.car.hyundai.ev6_aol import qualified @@ -399,7 +438,7 @@ class TransportPauseFeedback: self.awaiting = True return True - def observe(self, sm, cp, cs, now_ns, *, latched=True, cancel_source_ns=0, intent_producer_session=''): + def observe(self, sm, cp, cs, now_ns, *, latched=True, cancel_source_ns=0, brake_source_ns=0, intent_producer_session=''): self.qualified = False self.awaiting = False self.retention_qualified = False @@ -420,6 +459,15 @@ class TransportPauseFeedback: cp_identity(cp) == self.cp) continuity = bool(not self.session or axis.sessionId == self.session and (axis.sequence > self.sequence or axis.sequence == self.sequence and signature == self.signature)) + brake_native = (self._honda_brake_lateral_current(sm, cp, cs, axis, now_ns, + latched=latched, brake_source_ns=brake_source_ns) if current and continuity else None) + if brake_native is not None: + # Keep only existing intent. The original native lease and source stamps + # remain unchanged; decide_axes/native TX independently withdraw LONG. + self.healthy_feedback_ns = stamp + self.healthy_native_ns = int(brake_native.observedMonoTime) + self.sequence, self.signature = int(axis.sequence), signature + return False if self._source_availability_retention(sm, cp, cs, axis, stamp, now_ns, latched=latched, continuity=continuity, producer=intent_producer_session, signature=signature): self.sequence, self.signature = int(axis.sequence), signature @@ -468,6 +516,8 @@ class TransportPauseFeedback: self.signature = signature return False if transport_healthy: + if native is not None and not native.requestedLongitudinal: + self.honda_brake_ns = 0 if (not latched or native is not None and not native.requestedLongitudinal or self.longitudinal_cancel_ns and now_ns - self.longitudinal_cancel_ns > SAFETY_MAX_AGE_NS): self.longitudinal_cancel_ns = 0 diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index 8d1e144cd0..48ab1d4fd5 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/bootstub.body_h7.bin b/panda/board/obj/bootstub.body_h7.bin index 8388f16cf0..911e30bec5 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 0cf2dd2f58..67199374c8 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 89effba9e7..569bc7f8ca 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/gitversion.h b/panda/board/obj/gitversion.h index 7b830ff7f0..9430da4e85 100644 --- a/panda/board/obj/gitversion.h +++ b/panda/board/obj/gitversion.h @@ -1,4 +1,4 @@ #pragma once extern const uint8_t gitversion[29]; -const uint8_t gitversion[29] = "DEV-c204f0b1-P92eb5651-DEBUG"; +const uint8_t gitversion[29] = "DEV-7fc969bf-P92eb5651-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 71ba63f631..523c2c5673 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_h7.bin.signed b/panda/board/obj/panda_h7.bin.signed index c1ebf04dc8..3520db43ca 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/version b/panda/board/obj/version index 289e32a420..f674689cd9 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-c204f0b1-P92eb5651-DEBUG \ No newline at end of file +DEV-7fc969bf-P92eb5651-DEBUG \ No newline at end of file diff --git a/panda/tests/libpanda/libpanda.so b/panda/tests/libpanda/libpanda.so index 843dfcfe5b..f55ef94417 100755 Binary files a/panda/tests/libpanda/libpanda.so and b/panda/tests/libpanda/libpanda.so differ diff --git a/prebuilt.json b/prebuilt.json index 9cf5035b64..a6e449059e 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "b54a2d6ca333c30e432a21152166f29f2b5ee1ec880b8bf2d6c28e1ddc8687fa", "schema_version": 1} +{"protected_inventory_sha256": "26215417739b0673ec6dd0e0a744a0323c8f0d8dc846286e4582a2917588caf2", "schema_version": 1}