diff --git a/cereal/libcereal.a b/cereal/libcereal.a index 472668348..2937dd70a 100644 Binary files a/cereal/libcereal.a and b/cereal/libcereal.a differ diff --git a/cereal/log.capnp b/cereal/log.capnp index 17ef36acb..8179fa527 100644 --- a/cereal/log.capnp +++ b/cereal/log.capnp @@ -774,6 +774,7 @@ struct ChestnutState { pcieLtssm @7 :UInt8; supplyVoltage @8 :UInt16; # mV supplyCurrent @9 :Int16; # mA + supplyFault @10 :Bool; } struct RadarState @0x9a185389d6fdd05f { diff --git a/common/libcommon.a b/common/libcommon.a index 356e02b54..fcf773362 100644 Binary files a/common/libcommon.a and b/common/libcommon.a differ diff --git a/common/params_keys.h b/common/params_keys.h index 7d0fa4d6b..79a7b0969 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -113,6 +113,12 @@ inline static std::unordered_map keys = { {"ObdMultiplexingChanged", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}}, {"ObdMultiplexingEnabled", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, BOOL}}, {"Offroad_CarUnrecognized", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}}, + {"Offroad_ChestnutNotDetected", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}}, + {"Offroad_ChestnutOverheated", {CLEAR_ON_MANAGER_START, JSON}}, + {"Offroad_ChestnutPcieUnavailable", {CLEAR_ON_MANAGER_START, JSON}}, + {"Offroad_ChestnutUncompiled", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}}, + {"Offroad_ChestnutUpdateFailed", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}}, + {"Offroad_ChestnutUsbSlow", {CLEAR_ON_MANAGER_START | CLEAR_ON_ONROAD_TRANSITION, JSON}}, {"Offroad_ConnectivityNeeded", {CLEAR_ON_MANAGER_START, JSON}}, {"Offroad_ConnectivityNeededPrompt", {CLEAR_ON_MANAGER_START, JSON}}, {"Offroad_ExcessiveActuation", {PERSISTENT, JSON}}, diff --git a/common/params_pyx.so b/common/params_pyx.so index 19e245202..6370489fa 100755 Binary files a/common/params_pyx.so and b/common/params_pyx.so differ diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index af4fbde31..08dcb92bf 100644 Binary files a/panda/board/obj/body_h7.bin.signed and b/panda/board/obj/body_h7.bin.signed differ diff --git a/panda/board/obj/body_h7/bootstub.elf b/panda/board/obj/body_h7/bootstub.elf index 88224b1a8..87ea300e0 100755 Binary files a/panda/board/obj/body_h7/bootstub.elf and b/panda/board/obj/body_h7/bootstub.elf differ diff --git a/panda/board/obj/body_h7/main.bin b/panda/board/obj/body_h7/main.bin index 2a003e461..706f5e1fa 100755 Binary files a/panda/board/obj/body_h7/main.bin and b/panda/board/obj/body_h7/main.bin differ diff --git a/panda/board/obj/body_h7/main.elf b/panda/board/obj/body_h7/main.elf index 9791450b7..e22d96c3a 100755 Binary files a/panda/board/obj/body_h7/main.elf and b/panda/board/obj/body_h7/main.elf differ diff --git a/panda/board/obj/bootstub.body_h7.bin b/panda/board/obj/bootstub.body_h7.bin index 0f4201aa1..8b0d84ebb 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 6402acd2e..9eb286989 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_can_ignition_only.bin b/panda/board/obj/bootstub.panda_can_ignition_only.bin index 6402acd2e..9eb286989 100755 Binary files a/panda/board/obj/bootstub.panda_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_can_ignition_only.bin differ diff --git a/panda/board/obj/bootstub.panda_h7.bin b/panda/board/obj/bootstub.panda_h7.bin index 6e80611eb..e8cd902e7 100755 Binary files a/panda/board/obj/bootstub.panda_h7.bin and b/panda/board/obj/bootstub.panda_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_can_ignition_only.bin b/panda/board/obj/bootstub.panda_h7_can_ignition_only.bin index 6e80611eb..e8cd902e7 100755 Binary files a/panda/board/obj/bootstub.panda_h7_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_h7_can_ignition_only.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_hkg_remote.bin b/panda/board/obj/bootstub.panda_h7_hkg_remote.bin index 6e80611eb..e8cd902e7 100755 Binary files a/panda/board/obj/bootstub.panda_h7_hkg_remote.bin and b/panda/board/obj/bootstub.panda_h7_hkg_remote.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_hkg_remote_can_ignition_only.bin b/panda/board/obj/bootstub.panda_h7_hkg_remote_can_ignition_only.bin index 6e80611eb..e8cd902e7 100755 Binary files a/panda/board/obj/bootstub.panda_h7_hkg_remote_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_h7_hkg_remote_can_ignition_only.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_remote.bin b/panda/board/obj/bootstub.panda_h7_remote.bin index 6e80611eb..e8cd902e7 100755 Binary files a/panda/board/obj/bootstub.panda_h7_remote.bin and b/panda/board/obj/bootstub.panda_h7_remote.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_remote_can_ignition_only.bin b/panda/board/obj/bootstub.panda_h7_remote_can_ignition_only.bin index 6e80611eb..e8cd902e7 100755 Binary files a/panda/board/obj/bootstub.panda_h7_remote_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_h7_remote_can_ignition_only.bin differ diff --git a/panda/board/obj/bootstub.panda_hkg_remote.bin b/panda/board/obj/bootstub.panda_hkg_remote.bin index 6402acd2e..9eb286989 100755 Binary files a/panda/board/obj/bootstub.panda_hkg_remote.bin and b/panda/board/obj/bootstub.panda_hkg_remote.bin differ diff --git a/panda/board/obj/bootstub.panda_hkg_remote_can_ignition_only.bin b/panda/board/obj/bootstub.panda_hkg_remote_can_ignition_only.bin index 6402acd2e..9eb286989 100755 Binary files a/panda/board/obj/bootstub.panda_hkg_remote_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_hkg_remote_can_ignition_only.bin differ diff --git a/panda/board/obj/bootstub.panda_jungle_h7.bin b/panda/board/obj/bootstub.panda_jungle_h7.bin index 056927e17..76d9e58fa 100755 Binary files a/panda/board/obj/bootstub.panda_jungle_h7.bin and b/panda/board/obj/bootstub.panda_jungle_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_remote.bin b/panda/board/obj/bootstub.panda_remote.bin index 6402acd2e..9eb286989 100755 Binary files a/panda/board/obj/bootstub.panda_remote.bin and b/panda/board/obj/bootstub.panda_remote.bin differ diff --git a/panda/board/obj/bootstub.panda_remote_can_ignition_only.bin b/panda/board/obj/bootstub.panda_remote_can_ignition_only.bin index 6402acd2e..9eb286989 100755 Binary files a/panda/board/obj/bootstub.panda_remote_can_ignition_only.bin and b/panda/board/obj/bootstub.panda_remote_can_ignition_only.bin differ diff --git a/panda/board/obj/gitversion.h b/panda/board/obj/gitversion.h index 17d23e19b..ff8c97d94 100644 --- a/panda/board/obj/gitversion.h +++ b/panda/board/obj/gitversion.h @@ -1,2 +1,2 @@ extern const uint8_t gitversion[19]; -const uint8_t gitversion[19] = "DEV-a5d7db3b-DEBUG"; +const uint8_t gitversion[19] = "DEV-c03d06b4-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index 50475b80a..8afcee462 100644 Binary files a/panda/board/obj/panda.bin.signed and b/panda/board/obj/panda.bin.signed differ diff --git a/panda/board/obj/panda/bootstub.elf b/panda/board/obj/panda/bootstub.elf index bb93b0e2c..3ba17505e 100755 Binary files a/panda/board/obj/panda/bootstub.elf and b/panda/board/obj/panda/bootstub.elf differ diff --git a/panda/board/obj/panda/main.bin b/panda/board/obj/panda/main.bin index e33ad847b..412a7e051 100755 Binary files a/panda/board/obj/panda/main.bin and b/panda/board/obj/panda/main.bin differ diff --git a/panda/board/obj/panda/main.elf b/panda/board/obj/panda/main.elf index 61f16b613..c2d6db922 100755 Binary files a/panda/board/obj/panda/main.elf and b/panda/board/obj/panda/main.elf differ diff --git a/panda/board/obj/panda_can_ignition_only.bin.signed b/panda/board/obj/panda_can_ignition_only.bin.signed index 46a45a123..f09875fd6 100644 Binary files a/panda/board/obj/panda_can_ignition_only.bin.signed and b/panda/board/obj/panda_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_can_ignition_only/bootstub.elf b/panda/board/obj/panda_can_ignition_only/bootstub.elf index cc4f5fb36..9d89684c9 100755 Binary files a/panda/board/obj/panda_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_can_ignition_only/main.bin b/panda/board/obj/panda_can_ignition_only/main.bin index 08fd720b4..2ab8d0e5d 100755 Binary files a/panda/board/obj/panda_can_ignition_only/main.bin and b/panda/board/obj/panda_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_can_ignition_only/main.elf b/panda/board/obj/panda_can_ignition_only/main.elf index 5770fc3ca..e75462e25 100755 Binary files a/panda/board/obj/panda_can_ignition_only/main.elf and b/panda/board/obj/panda_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_h7.bin.signed b/panda/board/obj/panda_h7.bin.signed index 4b22da9c5..75534a288 100644 Binary files a/panda/board/obj/panda_h7.bin.signed and b/panda/board/obj/panda_h7.bin.signed differ diff --git a/panda/board/obj/panda_h7/bootstub.elf b/panda/board/obj/panda_h7/bootstub.elf index 2084ecf11..d3005c2fc 100755 Binary files a/panda/board/obj/panda_h7/bootstub.elf and b/panda/board/obj/panda_h7/bootstub.elf differ diff --git a/panda/board/obj/panda_h7/main.bin b/panda/board/obj/panda_h7/main.bin index 77ad0ff94..afb76811a 100755 Binary files a/panda/board/obj/panda_h7/main.bin and b/panda/board/obj/panda_h7/main.bin differ diff --git a/panda/board/obj/panda_h7/main.elf b/panda/board/obj/panda_h7/main.elf index d328f6ffa..cff5ebe69 100755 Binary files a/panda/board/obj/panda_h7/main.elf and b/panda/board/obj/panda_h7/main.elf differ diff --git a/panda/board/obj/panda_h7_can_ignition_only.bin.signed b/panda/board/obj/panda_h7_can_ignition_only.bin.signed index 1116d20f3..a8052d6d2 100644 Binary files a/panda/board/obj/panda_h7_can_ignition_only.bin.signed and b/panda/board/obj/panda_h7_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_h7_can_ignition_only/bootstub.elf b/panda/board/obj/panda_h7_can_ignition_only/bootstub.elf index 728184539..a68058177 100755 Binary files a/panda/board/obj/panda_h7_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_h7_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_can_ignition_only/main.bin b/panda/board/obj/panda_h7_can_ignition_only/main.bin index c6ed29332..9857ab0c3 100755 Binary files a/panda/board/obj/panda_h7_can_ignition_only/main.bin and b/panda/board/obj/panda_h7_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_h7_can_ignition_only/main.elf b/panda/board/obj/panda_h7_can_ignition_only/main.elf index 396b8d32f..19dd68239 100755 Binary files a/panda/board/obj/panda_h7_can_ignition_only/main.elf and b/panda/board/obj/panda_h7_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_h7_hkg_remote.bin.signed b/panda/board/obj/panda_h7_hkg_remote.bin.signed index b75b4034e..a02118cab 100644 Binary files a/panda/board/obj/panda_h7_hkg_remote.bin.signed and b/panda/board/obj/panda_h7_hkg_remote.bin.signed differ diff --git a/panda/board/obj/panda_h7_hkg_remote/bootstub.elf b/panda/board/obj/panda_h7_hkg_remote/bootstub.elf index 04114ac2d..1bc4531e1 100755 Binary files a/panda/board/obj/panda_h7_hkg_remote/bootstub.elf and b/panda/board/obj/panda_h7_hkg_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_hkg_remote/main.bin b/panda/board/obj/panda_h7_hkg_remote/main.bin index bc62993b3..1128a1bd9 100755 Binary files a/panda/board/obj/panda_h7_hkg_remote/main.bin and b/panda/board/obj/panda_h7_hkg_remote/main.bin differ diff --git a/panda/board/obj/panda_h7_hkg_remote/main.elf b/panda/board/obj/panda_h7_hkg_remote/main.elf index 7e87c64f0..c0cec5b42 100755 Binary files a/panda/board/obj/panda_h7_hkg_remote/main.elf and b/panda/board/obj/panda_h7_hkg_remote/main.elf differ diff --git a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only.bin.signed b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only.bin.signed index 67b19c6b6..668e252c9 100644 Binary files a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only.bin.signed and b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/bootstub.elf b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/bootstub.elf index ec3a3f3ab..2cb77ae3b 100755 Binary files a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.bin b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.bin index b1e8c2e44..3196b0567 100755 Binary files a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.bin and b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.elf b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.elf index d03a9b642..208b20ac8 100755 Binary files a/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.elf and b/panda/board/obj/panda_h7_hkg_remote_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_h7_remote.bin.signed b/panda/board/obj/panda_h7_remote.bin.signed index a694ca497..a04f36978 100644 Binary files a/panda/board/obj/panda_h7_remote.bin.signed and b/panda/board/obj/panda_h7_remote.bin.signed differ diff --git a/panda/board/obj/panda_h7_remote/bootstub.elf b/panda/board/obj/panda_h7_remote/bootstub.elf index 8bc53d5a5..524d260d0 100755 Binary files a/panda/board/obj/panda_h7_remote/bootstub.elf and b/panda/board/obj/panda_h7_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_remote/main.bin b/panda/board/obj/panda_h7_remote/main.bin index d4a43c129..ebacedbcb 100755 Binary files a/panda/board/obj/panda_h7_remote/main.bin and b/panda/board/obj/panda_h7_remote/main.bin differ diff --git a/panda/board/obj/panda_h7_remote/main.elf b/panda/board/obj/panda_h7_remote/main.elf index cc3b10b66..1ef898b35 100755 Binary files a/panda/board/obj/panda_h7_remote/main.elf and b/panda/board/obj/panda_h7_remote/main.elf differ diff --git a/panda/board/obj/panda_h7_remote_can_ignition_only.bin.signed b/panda/board/obj/panda_h7_remote_can_ignition_only.bin.signed index f2391dc39..513998460 100644 Binary files a/panda/board/obj/panda_h7_remote_can_ignition_only.bin.signed and b/panda/board/obj/panda_h7_remote_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_h7_remote_can_ignition_only/bootstub.elf b/panda/board/obj/panda_h7_remote_can_ignition_only/bootstub.elf index 192881dcd..0562a3ed4 100755 Binary files a/panda/board/obj/panda_h7_remote_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_h7_remote_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_remote_can_ignition_only/main.bin b/panda/board/obj/panda_h7_remote_can_ignition_only/main.bin index fef7a041a..2a0fafb4e 100755 Binary files a/panda/board/obj/panda_h7_remote_can_ignition_only/main.bin and b/panda/board/obj/panda_h7_remote_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_h7_remote_can_ignition_only/main.elf b/panda/board/obj/panda_h7_remote_can_ignition_only/main.elf index 274b6597b..b40fb31d0 100755 Binary files a/panda/board/obj/panda_h7_remote_can_ignition_only/main.elf and b/panda/board/obj/panda_h7_remote_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_hkg_remote.bin.signed b/panda/board/obj/panda_hkg_remote.bin.signed index 2ae21069f..0bc801616 100644 Binary files a/panda/board/obj/panda_hkg_remote.bin.signed and b/panda/board/obj/panda_hkg_remote.bin.signed differ diff --git a/panda/board/obj/panda_hkg_remote/bootstub.elf b/panda/board/obj/panda_hkg_remote/bootstub.elf index 180bd5aeb..62144527f 100755 Binary files a/panda/board/obj/panda_hkg_remote/bootstub.elf and b/panda/board/obj/panda_hkg_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_hkg_remote/main.bin b/panda/board/obj/panda_hkg_remote/main.bin index 15faa8633..d96179319 100755 Binary files a/panda/board/obj/panda_hkg_remote/main.bin and b/panda/board/obj/panda_hkg_remote/main.bin differ diff --git a/panda/board/obj/panda_hkg_remote/main.elf b/panda/board/obj/panda_hkg_remote/main.elf index 9c940ee1b..830ce5b85 100755 Binary files a/panda/board/obj/panda_hkg_remote/main.elf and b/panda/board/obj/panda_hkg_remote/main.elf differ diff --git a/panda/board/obj/panda_hkg_remote_can_ignition_only.bin.signed b/panda/board/obj/panda_hkg_remote_can_ignition_only.bin.signed index 37f23e769..51f923e72 100644 Binary files a/panda/board/obj/panda_hkg_remote_can_ignition_only.bin.signed and b/panda/board/obj/panda_hkg_remote_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_hkg_remote_can_ignition_only/bootstub.elf b/panda/board/obj/panda_hkg_remote_can_ignition_only/bootstub.elf index e8c3bb033..21d59c7fd 100755 Binary files a/panda/board/obj/panda_hkg_remote_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_hkg_remote_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_hkg_remote_can_ignition_only/main.bin b/panda/board/obj/panda_hkg_remote_can_ignition_only/main.bin index 3b04e286e..383762f9e 100755 Binary files a/panda/board/obj/panda_hkg_remote_can_ignition_only/main.bin and b/panda/board/obj/panda_hkg_remote_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_hkg_remote_can_ignition_only/main.elf b/panda/board/obj/panda_hkg_remote_can_ignition_only/main.elf index dffcf3320..55f77306a 100755 Binary files a/panda/board/obj/panda_hkg_remote_can_ignition_only/main.elf and b/panda/board/obj/panda_hkg_remote_can_ignition_only/main.elf differ diff --git a/panda/board/obj/panda_jungle_h7.bin.signed b/panda/board/obj/panda_jungle_h7.bin.signed index b5b7e4475..a116351b5 100644 Binary files a/panda/board/obj/panda_jungle_h7.bin.signed and b/panda/board/obj/panda_jungle_h7.bin.signed differ diff --git a/panda/board/obj/panda_jungle_h7/bootstub.elf b/panda/board/obj/panda_jungle_h7/bootstub.elf index 07c65989c..e5cc900aa 100755 Binary files a/panda/board/obj/panda_jungle_h7/bootstub.elf and b/panda/board/obj/panda_jungle_h7/bootstub.elf differ diff --git a/panda/board/obj/panda_jungle_h7/main.bin b/panda/board/obj/panda_jungle_h7/main.bin index 5e34d4a70..26e6b4ca5 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.bin and b/panda/board/obj/panda_jungle_h7/main.bin differ diff --git a/panda/board/obj/panda_jungle_h7/main.elf b/panda/board/obj/panda_jungle_h7/main.elf index 0e191fdf7..2bc5f142d 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.elf and b/panda/board/obj/panda_jungle_h7/main.elf differ diff --git a/panda/board/obj/panda_remote.bin.signed b/panda/board/obj/panda_remote.bin.signed index 14b040fbe..37e69987f 100644 Binary files a/panda/board/obj/panda_remote.bin.signed and b/panda/board/obj/panda_remote.bin.signed differ diff --git a/panda/board/obj/panda_remote/bootstub.elf b/panda/board/obj/panda_remote/bootstub.elf index 903ffcf0f..f69da60de 100755 Binary files a/panda/board/obj/panda_remote/bootstub.elf and b/panda/board/obj/panda_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_remote/main.bin b/panda/board/obj/panda_remote/main.bin index 63be1568c..6300c072f 100755 Binary files a/panda/board/obj/panda_remote/main.bin and b/panda/board/obj/panda_remote/main.bin differ diff --git a/panda/board/obj/panda_remote/main.elf b/panda/board/obj/panda_remote/main.elf index 33341332d..da0192c07 100755 Binary files a/panda/board/obj/panda_remote/main.elf and b/panda/board/obj/panda_remote/main.elf differ diff --git a/panda/board/obj/panda_remote_can_ignition_only.bin.signed b/panda/board/obj/panda_remote_can_ignition_only.bin.signed index f5bd9ac49..394ff1573 100644 Binary files a/panda/board/obj/panda_remote_can_ignition_only.bin.signed and b/panda/board/obj/panda_remote_can_ignition_only.bin.signed differ diff --git a/panda/board/obj/panda_remote_can_ignition_only/bootstub.elf b/panda/board/obj/panda_remote_can_ignition_only/bootstub.elf index 2dda65ef9..324b37252 100755 Binary files a/panda/board/obj/panda_remote_can_ignition_only/bootstub.elf and b/panda/board/obj/panda_remote_can_ignition_only/bootstub.elf differ diff --git a/panda/board/obj/panda_remote_can_ignition_only/main.bin b/panda/board/obj/panda_remote_can_ignition_only/main.bin index 3151e6999..f422dbddc 100755 Binary files a/panda/board/obj/panda_remote_can_ignition_only/main.bin and b/panda/board/obj/panda_remote_can_ignition_only/main.bin differ diff --git a/panda/board/obj/panda_remote_can_ignition_only/main.elf b/panda/board/obj/panda_remote_can_ignition_only/main.elf index 973998c83..6e9ba723c 100755 Binary files a/panda/board/obj/panda_remote_can_ignition_only/main.elf and b/panda/board/obj/panda_remote_can_ignition_only/main.elf differ diff --git a/panda/board/obj/version b/panda/board/obj/version index 3c5994989..456e44c4e 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-a5d7db3b-DEBUG \ No newline at end of file +DEV-c03d06b4-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 9f42d0df4..bdd92f952 100644 --- a/selfdrive/locationd/models/generated/car.cpp +++ b/selfdrive/locationd/models/generated/car.cpp @@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227; * * * This file is part of 'ekf' * ******************************************************************************/ -void err_fun(double *nom_x, double *delta_x, double *out_7751375231448978305) { - out_7751375231448978305[0] = delta_x[0] + nom_x[0]; - out_7751375231448978305[1] = delta_x[1] + nom_x[1]; - out_7751375231448978305[2] = delta_x[2] + nom_x[2]; - out_7751375231448978305[3] = delta_x[3] + nom_x[3]; - out_7751375231448978305[4] = delta_x[4] + nom_x[4]; - out_7751375231448978305[5] = delta_x[5] + nom_x[5]; - out_7751375231448978305[6] = delta_x[6] + nom_x[6]; - out_7751375231448978305[7] = delta_x[7] + nom_x[7]; - out_7751375231448978305[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_789804740016769707) { + out_789804740016769707[0] = delta_x[0] + nom_x[0]; + out_789804740016769707[1] = delta_x[1] + nom_x[1]; + out_789804740016769707[2] = delta_x[2] + nom_x[2]; + out_789804740016769707[3] = delta_x[3] + nom_x[3]; + out_789804740016769707[4] = delta_x[4] + nom_x[4]; + out_789804740016769707[5] = delta_x[5] + nom_x[5]; + out_789804740016769707[6] = delta_x[6] + nom_x[6]; + out_789804740016769707[7] = delta_x[7] + nom_x[7]; + out_789804740016769707[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_1186045100449225538) { - out_1186045100449225538[0] = -nom_x[0] + true_x[0]; - out_1186045100449225538[1] = -nom_x[1] + true_x[1]; - out_1186045100449225538[2] = -nom_x[2] + true_x[2]; - out_1186045100449225538[3] = -nom_x[3] + true_x[3]; - out_1186045100449225538[4] = -nom_x[4] + true_x[4]; - out_1186045100449225538[5] = -nom_x[5] + true_x[5]; - out_1186045100449225538[6] = -nom_x[6] + true_x[6]; - out_1186045100449225538[7] = -nom_x[7] + true_x[7]; - out_1186045100449225538[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_385986033898548509) { + out_385986033898548509[0] = -nom_x[0] + true_x[0]; + out_385986033898548509[1] = -nom_x[1] + true_x[1]; + out_385986033898548509[2] = -nom_x[2] + true_x[2]; + out_385986033898548509[3] = -nom_x[3] + true_x[3]; + out_385986033898548509[4] = -nom_x[4] + true_x[4]; + out_385986033898548509[5] = -nom_x[5] + true_x[5]; + out_385986033898548509[6] = -nom_x[6] + true_x[6]; + out_385986033898548509[7] = -nom_x[7] + true_x[7]; + out_385986033898548509[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_5403210608595880924) { - out_5403210608595880924[0] = 1.0; - out_5403210608595880924[1] = 0.0; - out_5403210608595880924[2] = 0.0; - out_5403210608595880924[3] = 0.0; - out_5403210608595880924[4] = 0.0; - out_5403210608595880924[5] = 0.0; - out_5403210608595880924[6] = 0.0; - out_5403210608595880924[7] = 0.0; - out_5403210608595880924[8] = 0.0; - out_5403210608595880924[9] = 0.0; - out_5403210608595880924[10] = 1.0; - out_5403210608595880924[11] = 0.0; - out_5403210608595880924[12] = 0.0; - out_5403210608595880924[13] = 0.0; - out_5403210608595880924[14] = 0.0; - out_5403210608595880924[15] = 0.0; - out_5403210608595880924[16] = 0.0; - out_5403210608595880924[17] = 0.0; - out_5403210608595880924[18] = 0.0; - out_5403210608595880924[19] = 0.0; - out_5403210608595880924[20] = 1.0; - out_5403210608595880924[21] = 0.0; - out_5403210608595880924[22] = 0.0; - out_5403210608595880924[23] = 0.0; - out_5403210608595880924[24] = 0.0; - out_5403210608595880924[25] = 0.0; - out_5403210608595880924[26] = 0.0; - out_5403210608595880924[27] = 0.0; - out_5403210608595880924[28] = 0.0; - out_5403210608595880924[29] = 0.0; - out_5403210608595880924[30] = 1.0; - out_5403210608595880924[31] = 0.0; - out_5403210608595880924[32] = 0.0; - out_5403210608595880924[33] = 0.0; - out_5403210608595880924[34] = 0.0; - out_5403210608595880924[35] = 0.0; - out_5403210608595880924[36] = 0.0; - out_5403210608595880924[37] = 0.0; - out_5403210608595880924[38] = 0.0; - out_5403210608595880924[39] = 0.0; - out_5403210608595880924[40] = 1.0; - out_5403210608595880924[41] = 0.0; - out_5403210608595880924[42] = 0.0; - out_5403210608595880924[43] = 0.0; - out_5403210608595880924[44] = 0.0; - out_5403210608595880924[45] = 0.0; - out_5403210608595880924[46] = 0.0; - out_5403210608595880924[47] = 0.0; - out_5403210608595880924[48] = 0.0; - out_5403210608595880924[49] = 0.0; - out_5403210608595880924[50] = 1.0; - out_5403210608595880924[51] = 0.0; - out_5403210608595880924[52] = 0.0; - out_5403210608595880924[53] = 0.0; - out_5403210608595880924[54] = 0.0; - out_5403210608595880924[55] = 0.0; - out_5403210608595880924[56] = 0.0; - out_5403210608595880924[57] = 0.0; - out_5403210608595880924[58] = 0.0; - out_5403210608595880924[59] = 0.0; - out_5403210608595880924[60] = 1.0; - out_5403210608595880924[61] = 0.0; - out_5403210608595880924[62] = 0.0; - out_5403210608595880924[63] = 0.0; - out_5403210608595880924[64] = 0.0; - out_5403210608595880924[65] = 0.0; - out_5403210608595880924[66] = 0.0; - out_5403210608595880924[67] = 0.0; - out_5403210608595880924[68] = 0.0; - out_5403210608595880924[69] = 0.0; - out_5403210608595880924[70] = 1.0; - out_5403210608595880924[71] = 0.0; - out_5403210608595880924[72] = 0.0; - out_5403210608595880924[73] = 0.0; - out_5403210608595880924[74] = 0.0; - out_5403210608595880924[75] = 0.0; - out_5403210608595880924[76] = 0.0; - out_5403210608595880924[77] = 0.0; - out_5403210608595880924[78] = 0.0; - out_5403210608595880924[79] = 0.0; - out_5403210608595880924[80] = 1.0; +void H_mod_fun(double *state, double *out_4046004184415297016) { + out_4046004184415297016[0] = 1.0; + out_4046004184415297016[1] = 0.0; + out_4046004184415297016[2] = 0.0; + out_4046004184415297016[3] = 0.0; + out_4046004184415297016[4] = 0.0; + out_4046004184415297016[5] = 0.0; + out_4046004184415297016[6] = 0.0; + out_4046004184415297016[7] = 0.0; + out_4046004184415297016[8] = 0.0; + out_4046004184415297016[9] = 0.0; + out_4046004184415297016[10] = 1.0; + out_4046004184415297016[11] = 0.0; + out_4046004184415297016[12] = 0.0; + out_4046004184415297016[13] = 0.0; + out_4046004184415297016[14] = 0.0; + out_4046004184415297016[15] = 0.0; + out_4046004184415297016[16] = 0.0; + out_4046004184415297016[17] = 0.0; + out_4046004184415297016[18] = 0.0; + out_4046004184415297016[19] = 0.0; + out_4046004184415297016[20] = 1.0; + out_4046004184415297016[21] = 0.0; + out_4046004184415297016[22] = 0.0; + out_4046004184415297016[23] = 0.0; + out_4046004184415297016[24] = 0.0; + out_4046004184415297016[25] = 0.0; + out_4046004184415297016[26] = 0.0; + out_4046004184415297016[27] = 0.0; + out_4046004184415297016[28] = 0.0; + out_4046004184415297016[29] = 0.0; + out_4046004184415297016[30] = 1.0; + out_4046004184415297016[31] = 0.0; + out_4046004184415297016[32] = 0.0; + out_4046004184415297016[33] = 0.0; + out_4046004184415297016[34] = 0.0; + out_4046004184415297016[35] = 0.0; + out_4046004184415297016[36] = 0.0; + out_4046004184415297016[37] = 0.0; + out_4046004184415297016[38] = 0.0; + out_4046004184415297016[39] = 0.0; + out_4046004184415297016[40] = 1.0; + out_4046004184415297016[41] = 0.0; + out_4046004184415297016[42] = 0.0; + out_4046004184415297016[43] = 0.0; + out_4046004184415297016[44] = 0.0; + out_4046004184415297016[45] = 0.0; + out_4046004184415297016[46] = 0.0; + out_4046004184415297016[47] = 0.0; + out_4046004184415297016[48] = 0.0; + out_4046004184415297016[49] = 0.0; + out_4046004184415297016[50] = 1.0; + out_4046004184415297016[51] = 0.0; + out_4046004184415297016[52] = 0.0; + out_4046004184415297016[53] = 0.0; + out_4046004184415297016[54] = 0.0; + out_4046004184415297016[55] = 0.0; + out_4046004184415297016[56] = 0.0; + out_4046004184415297016[57] = 0.0; + out_4046004184415297016[58] = 0.0; + out_4046004184415297016[59] = 0.0; + out_4046004184415297016[60] = 1.0; + out_4046004184415297016[61] = 0.0; + out_4046004184415297016[62] = 0.0; + out_4046004184415297016[63] = 0.0; + out_4046004184415297016[64] = 0.0; + out_4046004184415297016[65] = 0.0; + out_4046004184415297016[66] = 0.0; + out_4046004184415297016[67] = 0.0; + out_4046004184415297016[68] = 0.0; + out_4046004184415297016[69] = 0.0; + out_4046004184415297016[70] = 1.0; + out_4046004184415297016[71] = 0.0; + out_4046004184415297016[72] = 0.0; + out_4046004184415297016[73] = 0.0; + out_4046004184415297016[74] = 0.0; + out_4046004184415297016[75] = 0.0; + out_4046004184415297016[76] = 0.0; + out_4046004184415297016[77] = 0.0; + out_4046004184415297016[78] = 0.0; + out_4046004184415297016[79] = 0.0; + out_4046004184415297016[80] = 1.0; } -void f_fun(double *state, double dt, double *out_3846473641327680019) { - out_3846473641327680019[0] = state[0]; - out_3846473641327680019[1] = state[1]; - out_3846473641327680019[2] = state[2]; - out_3846473641327680019[3] = state[3]; - out_3846473641327680019[4] = state[4]; - out_3846473641327680019[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_3846473641327680019[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_3846473641327680019[7] = state[7]; - out_3846473641327680019[8] = state[8]; +void f_fun(double *state, double dt, double *out_467921565295650481) { + out_467921565295650481[0] = state[0]; + out_467921565295650481[1] = state[1]; + out_467921565295650481[2] = state[2]; + out_467921565295650481[3] = state[3]; + out_467921565295650481[4] = state[4]; + out_467921565295650481[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_467921565295650481[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_467921565295650481[7] = state[7]; + out_467921565295650481[8] = state[8]; } -void F_fun(double *state, double dt, double *out_6505574540477860652) { - out_6505574540477860652[0] = 1; - out_6505574540477860652[1] = 0; - out_6505574540477860652[2] = 0; - out_6505574540477860652[3] = 0; - out_6505574540477860652[4] = 0; - out_6505574540477860652[5] = 0; - out_6505574540477860652[6] = 0; - out_6505574540477860652[7] = 0; - out_6505574540477860652[8] = 0; - out_6505574540477860652[9] = 0; - out_6505574540477860652[10] = 1; - out_6505574540477860652[11] = 0; - out_6505574540477860652[12] = 0; - out_6505574540477860652[13] = 0; - out_6505574540477860652[14] = 0; - out_6505574540477860652[15] = 0; - out_6505574540477860652[16] = 0; - out_6505574540477860652[17] = 0; - out_6505574540477860652[18] = 0; - out_6505574540477860652[19] = 0; - out_6505574540477860652[20] = 1; - out_6505574540477860652[21] = 0; - out_6505574540477860652[22] = 0; - out_6505574540477860652[23] = 0; - out_6505574540477860652[24] = 0; - out_6505574540477860652[25] = 0; - out_6505574540477860652[26] = 0; - out_6505574540477860652[27] = 0; - out_6505574540477860652[28] = 0; - out_6505574540477860652[29] = 0; - out_6505574540477860652[30] = 1; - out_6505574540477860652[31] = 0; - out_6505574540477860652[32] = 0; - out_6505574540477860652[33] = 0; - out_6505574540477860652[34] = 0; - out_6505574540477860652[35] = 0; - out_6505574540477860652[36] = 0; - out_6505574540477860652[37] = 0; - out_6505574540477860652[38] = 0; - out_6505574540477860652[39] = 0; - out_6505574540477860652[40] = 1; - out_6505574540477860652[41] = 0; - out_6505574540477860652[42] = 0; - out_6505574540477860652[43] = 0; - out_6505574540477860652[44] = 0; - out_6505574540477860652[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_6505574540477860652[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_6505574540477860652[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_6505574540477860652[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_6505574540477860652[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_6505574540477860652[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_6505574540477860652[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_6505574540477860652[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_6505574540477860652[53] = -9.8100000000000005*dt; - out_6505574540477860652[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_6505574540477860652[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_6505574540477860652[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6505574540477860652[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6505574540477860652[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_6505574540477860652[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_6505574540477860652[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_6505574540477860652[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6505574540477860652[62] = 0; - out_6505574540477860652[63] = 0; - out_6505574540477860652[64] = 0; - out_6505574540477860652[65] = 0; - out_6505574540477860652[66] = 0; - out_6505574540477860652[67] = 0; - out_6505574540477860652[68] = 0; - out_6505574540477860652[69] = 0; - out_6505574540477860652[70] = 1; - out_6505574540477860652[71] = 0; - out_6505574540477860652[72] = 0; - out_6505574540477860652[73] = 0; - out_6505574540477860652[74] = 0; - out_6505574540477860652[75] = 0; - out_6505574540477860652[76] = 0; - out_6505574540477860652[77] = 0; - out_6505574540477860652[78] = 0; - out_6505574540477860652[79] = 0; - out_6505574540477860652[80] = 1; +void F_fun(double *state, double dt, double *out_2429260194375294140) { + out_2429260194375294140[0] = 1; + out_2429260194375294140[1] = 0; + out_2429260194375294140[2] = 0; + out_2429260194375294140[3] = 0; + out_2429260194375294140[4] = 0; + out_2429260194375294140[5] = 0; + out_2429260194375294140[6] = 0; + out_2429260194375294140[7] = 0; + out_2429260194375294140[8] = 0; + out_2429260194375294140[9] = 0; + out_2429260194375294140[10] = 1; + out_2429260194375294140[11] = 0; + out_2429260194375294140[12] = 0; + out_2429260194375294140[13] = 0; + out_2429260194375294140[14] = 0; + out_2429260194375294140[15] = 0; + out_2429260194375294140[16] = 0; + out_2429260194375294140[17] = 0; + out_2429260194375294140[18] = 0; + out_2429260194375294140[19] = 0; + out_2429260194375294140[20] = 1; + out_2429260194375294140[21] = 0; + out_2429260194375294140[22] = 0; + out_2429260194375294140[23] = 0; + out_2429260194375294140[24] = 0; + out_2429260194375294140[25] = 0; + out_2429260194375294140[26] = 0; + out_2429260194375294140[27] = 0; + out_2429260194375294140[28] = 0; + out_2429260194375294140[29] = 0; + out_2429260194375294140[30] = 1; + out_2429260194375294140[31] = 0; + out_2429260194375294140[32] = 0; + out_2429260194375294140[33] = 0; + out_2429260194375294140[34] = 0; + out_2429260194375294140[35] = 0; + out_2429260194375294140[36] = 0; + out_2429260194375294140[37] = 0; + out_2429260194375294140[38] = 0; + out_2429260194375294140[39] = 0; + out_2429260194375294140[40] = 1; + out_2429260194375294140[41] = 0; + out_2429260194375294140[42] = 0; + out_2429260194375294140[43] = 0; + out_2429260194375294140[44] = 0; + out_2429260194375294140[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_2429260194375294140[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_2429260194375294140[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2429260194375294140[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2429260194375294140[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_2429260194375294140[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_2429260194375294140[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_2429260194375294140[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_2429260194375294140[53] = -9.8100000000000005*dt; + out_2429260194375294140[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_2429260194375294140[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_2429260194375294140[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2429260194375294140[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2429260194375294140[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_2429260194375294140[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_2429260194375294140[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_2429260194375294140[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2429260194375294140[62] = 0; + out_2429260194375294140[63] = 0; + out_2429260194375294140[64] = 0; + out_2429260194375294140[65] = 0; + out_2429260194375294140[66] = 0; + out_2429260194375294140[67] = 0; + out_2429260194375294140[68] = 0; + out_2429260194375294140[69] = 0; + out_2429260194375294140[70] = 1; + out_2429260194375294140[71] = 0; + out_2429260194375294140[72] = 0; + out_2429260194375294140[73] = 0; + out_2429260194375294140[74] = 0; + out_2429260194375294140[75] = 0; + out_2429260194375294140[76] = 0; + out_2429260194375294140[77] = 0; + out_2429260194375294140[78] = 0; + out_2429260194375294140[79] = 0; + out_2429260194375294140[80] = 1; } -void h_25(double *state, double *unused, double *out_1325906349074366668) { - out_1325906349074366668[0] = state[6]; +void h_25(double *state, double *unused, double *out_4530459753918271082) { + out_4530459753918271082[0] = state[6]; } -void H_25(double *state, double *unused, double *out_693697758010493692) { - out_693697758010493692[0] = 0; - out_693697758010493692[1] = 0; - out_693697758010493692[2] = 0; - out_693697758010493692[3] = 0; - out_693697758010493692[4] = 0; - out_693697758010493692[5] = 0; - out_693697758010493692[6] = 1; - out_693697758010493692[7] = 0; - out_693697758010493692[8] = 0; +void H_25(double *state, double *unused, double *out_4551338626615593728) { + out_4551338626615593728[0] = 0; + out_4551338626615593728[1] = 0; + out_4551338626615593728[2] = 0; + out_4551338626615593728[3] = 0; + out_4551338626615593728[4] = 0; + out_4551338626615593728[5] = 0; + out_4551338626615593728[6] = 1; + out_4551338626615593728[7] = 0; + out_4551338626615593728[8] = 0; } -void h_24(double *state, double *unused, double *out_7507130987037302702) { - out_7507130987037302702[0] = state[4]; - out_7507130987037302702[1] = state[5]; +void h_24(double *state, double *unused, double *out_211848015086222289) { + out_211848015086222289[0] = state[4]; + out_211848015086222289[1] = state[5]; } -void H_24(double *state, double *unused, double *out_2864227840791829444) { - out_2864227840791829444[0] = 0; - out_2864227840791829444[1] = 0; - out_2864227840791829444[2] = 0; - out_2864227840791829444[3] = 0; - out_2864227840791829444[4] = 1; - out_2864227840791829444[5] = 0; - out_2864227840791829444[6] = 0; - out_2864227840791829444[7] = 0; - out_2864227840791829444[8] = 0; - out_2864227840791829444[9] = 0; - out_2864227840791829444[10] = 0; - out_2864227840791829444[11] = 0; - out_2864227840791829444[12] = 0; - out_2864227840791829444[13] = 0; - out_2864227840791829444[14] = 1; - out_2864227840791829444[15] = 0; - out_2864227840791829444[16] = 0; - out_2864227840791829444[17] = 0; +void H_24(double *state, double *unused, double *out_7319833640502439787) { + out_7319833640502439787[0] = 0; + out_7319833640502439787[1] = 0; + out_7319833640502439787[2] = 0; + out_7319833640502439787[3] = 0; + out_7319833640502439787[4] = 1; + out_7319833640502439787[5] = 0; + out_7319833640502439787[6] = 0; + out_7319833640502439787[7] = 0; + out_7319833640502439787[8] = 0; + out_7319833640502439787[9] = 0; + out_7319833640502439787[10] = 0; + out_7319833640502439787[11] = 0; + out_7319833640502439787[12] = 0; + out_7319833640502439787[13] = 0; + out_7319833640502439787[14] = 1; + out_7319833640502439787[15] = 0; + out_7319833640502439787[16] = 0; + out_7319833640502439787[17] = 0; } -void h_30(double *state, double *unused, double *out_6134652415174521657) { - out_6134652415174521657[0] = state[4]; +void h_30(double *state, double *unused, double *out_4687646422269400227) { + out_4687646422269400227[0] = state[4]; } -void H_30(double *state, double *unused, double *out_1824635200496754935) { - out_1824635200496754935[0] = 0; - out_1824635200496754935[1] = 0; - out_1824635200496754935[2] = 0; - out_1824635200496754935[3] = 0; - out_1824635200496754935[4] = 1; - out_1824635200496754935[5] = 0; - out_1824635200496754935[6] = 0; - out_1824635200496754935[7] = 0; - out_1824635200496754935[8] = 0; +void H_30(double *state, double *unused, double *out_4421999679472353658) { + out_4421999679472353658[0] = 0; + out_4421999679472353658[1] = 0; + out_4421999679472353658[2] = 0; + out_4421999679472353658[3] = 0; + out_4421999679472353658[4] = 1; + out_4421999679472353658[5] = 0; + out_4421999679472353658[6] = 0; + out_4421999679472353658[7] = 0; + out_4421999679472353658[8] = 0; } -void h_26(double *state, double *unused, double *out_3515782194269032819) { - out_3515782194269032819[0] = state[7]; +void h_26(double *state, double *unused, double *out_1875094318999546844) { + out_1875094318999546844[0] = state[7]; } -void H_26(double *state, double *unused, double *out_4435201076884549916) { - out_4435201076884549916[0] = 0; - out_4435201076884549916[1] = 0; - out_4435201076884549916[2] = 0; - out_4435201076884549916[3] = 0; - out_4435201076884549916[4] = 0; - out_4435201076884549916[5] = 0; - out_4435201076884549916[6] = 0; - out_4435201076884549916[7] = 1; - out_4435201076884549916[8] = 0; +void H_26(double *state, double *unused, double *out_809835307741537504) { + out_809835307741537504[0] = 0; + out_809835307741537504[1] = 0; + out_809835307741537504[2] = 0; + out_809835307741537504[3] = 0; + out_809835307741537504[4] = 0; + out_809835307741537504[5] = 0; + out_809835307741537504[6] = 0; + out_809835307741537504[7] = 1; + out_809835307741537504[8] = 0; } -void h_27(double *state, double *unused, double *out_5073135168339482478) { - out_5073135168339482478[0] = state[3]; +void h_27(double *state, double *unused, double *out_8953904322109987302) { + out_8953904322109987302[0] = state[3]; } -void H_27(double *state, double *unused, double *out_4048229271680698152) { - out_4048229271680698152[0] = 0; - out_4048229271680698152[1] = 0; - out_4048229271680698152[2] = 0; - out_4048229271680698152[3] = 1; - out_4048229271680698152[4] = 0; - out_4048229271680698152[5] = 0; - out_4048229271680698152[6] = 0; - out_4048229271680698152[7] = 0; - out_4048229271680698152[8] = 0; +void H_27(double *state, double *unused, double *out_2247236367671928747) { + out_2247236367671928747[0] = 0; + out_2247236367671928747[1] = 0; + out_2247236367671928747[2] = 0; + out_2247236367671928747[3] = 1; + out_2247236367671928747[4] = 0; + out_2247236367671928747[5] = 0; + out_2247236367671928747[6] = 0; + out_2247236367671928747[7] = 0; + out_2247236367671928747[8] = 0; } -void h_29(double *state, double *unused, double *out_9171438466618854342) { - out_9171438466618854342[0] = state[1]; +void h_29(double *state, double *unused, double *out_5855390223345541964) { + out_5855390223345541964[0] = state[1]; } -void H_29(double *state, double *unused, double *out_2334866544811147119) { - out_2334866544811147119[0] = 0; - out_2334866544811147119[1] = 1; - out_2334866544811147119[2] = 0; - out_2334866544811147119[3] = 0; - out_2334866544811147119[4] = 0; - out_2334866544811147119[5] = 0; - out_2334866544811147119[6] = 0; - out_2334866544811147119[7] = 0; - out_2334866544811147119[8] = 0; +void H_29(double *state, double *unused, double *out_4932231023786745842) { + out_4932231023786745842[0] = 0; + out_4932231023786745842[1] = 1; + out_4932231023786745842[2] = 0; + out_4932231023786745842[3] = 0; + out_4932231023786745842[4] = 0; + out_4932231023786745842[5] = 0; + out_4932231023786745842[6] = 0; + out_4932231023786745842[7] = 0; + out_4932231023786745842[8] = 0; } -void h_28(double *state, double *unused, double *out_6016645021241144913) { - out_6016645021241144913[0] = state[0]; +void h_28(double *state, double *unused, double *out_8510755658264266202) { + out_8510755658264266202[0] = state[0]; } -void H_28(double *state, double *unused, double *out_2747532472258383455) { - out_2747532472258383455[0] = 1; - out_2747532472258383455[1] = 0; - out_2747532472258383455[2] = 0; - out_2747532472258383455[3] = 0; - out_2747532472258383455[4] = 0; - out_2747532472258383455[5] = 0; - out_2747532472258383455[6] = 0; - out_2747532472258383455[7] = 0; - out_2747532472258383455[8] = 0; +void H_28(double *state, double *unused, double *out_6895861295352072093) { + out_6895861295352072093[0] = 1; + out_6895861295352072093[1] = 0; + out_6895861295352072093[2] = 0; + out_6895861295352072093[3] = 0; + out_6895861295352072093[4] = 0; + out_6895861295352072093[5] = 0; + out_6895861295352072093[6] = 0; + out_6895861295352072093[7] = 0; + out_6895861295352072093[8] = 0; } -void h_31(double *state, double *unused, double *out_2311543179179655682) { - out_2311543179179655682[0] = state[8]; +void h_31(double *state, double *unused, double *out_3677036992164635768) { + out_3677036992164635768[0] = state[8]; } -void H_31(double *state, double *unused, double *out_5061409179117901392) { - out_5061409179117901392[0] = 0; - out_5061409179117901392[1] = 0; - out_5061409179117901392[2] = 0; - out_5061409179117901392[3] = 0; - out_5061409179117901392[4] = 0; - out_5061409179117901392[5] = 0; - out_5061409179117901392[6] = 0; - out_5061409179117901392[7] = 0; - out_5061409179117901392[8] = 1; +void H_31(double *state, double *unused, double *out_4581984588492554156) { + out_4581984588492554156[0] = 0; + out_4581984588492554156[1] = 0; + out_4581984588492554156[2] = 0; + out_4581984588492554156[3] = 0; + out_4581984588492554156[4] = 0; + out_4581984588492554156[5] = 0; + out_4581984588492554156[6] = 0; + out_4581984588492554156[7] = 0; + out_4581984588492554156[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_7751375231448978305) { - err_fun(nom_x, delta_x, out_7751375231448978305); +void car_err_fun(double *nom_x, double *delta_x, double *out_789804740016769707) { + err_fun(nom_x, delta_x, out_789804740016769707); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_1186045100449225538) { - inv_err_fun(nom_x, true_x, out_1186045100449225538); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_385986033898548509) { + inv_err_fun(nom_x, true_x, out_385986033898548509); } -void car_H_mod_fun(double *state, double *out_5403210608595880924) { - H_mod_fun(state, out_5403210608595880924); +void car_H_mod_fun(double *state, double *out_4046004184415297016) { + H_mod_fun(state, out_4046004184415297016); } -void car_f_fun(double *state, double dt, double *out_3846473641327680019) { - f_fun(state, dt, out_3846473641327680019); +void car_f_fun(double *state, double dt, double *out_467921565295650481) { + f_fun(state, dt, out_467921565295650481); } -void car_F_fun(double *state, double dt, double *out_6505574540477860652) { - F_fun(state, dt, out_6505574540477860652); +void car_F_fun(double *state, double dt, double *out_2429260194375294140) { + F_fun(state, dt, out_2429260194375294140); } -void car_h_25(double *state, double *unused, double *out_1325906349074366668) { - h_25(state, unused, out_1325906349074366668); +void car_h_25(double *state, double *unused, double *out_4530459753918271082) { + h_25(state, unused, out_4530459753918271082); } -void car_H_25(double *state, double *unused, double *out_693697758010493692) { - H_25(state, unused, out_693697758010493692); +void car_H_25(double *state, double *unused, double *out_4551338626615593728) { + H_25(state, unused, out_4551338626615593728); } -void car_h_24(double *state, double *unused, double *out_7507130987037302702) { - h_24(state, unused, out_7507130987037302702); +void car_h_24(double *state, double *unused, double *out_211848015086222289) { + h_24(state, unused, out_211848015086222289); } -void car_H_24(double *state, double *unused, double *out_2864227840791829444) { - H_24(state, unused, out_2864227840791829444); +void car_H_24(double *state, double *unused, double *out_7319833640502439787) { + H_24(state, unused, out_7319833640502439787); } -void car_h_30(double *state, double *unused, double *out_6134652415174521657) { - h_30(state, unused, out_6134652415174521657); +void car_h_30(double *state, double *unused, double *out_4687646422269400227) { + h_30(state, unused, out_4687646422269400227); } -void car_H_30(double *state, double *unused, double *out_1824635200496754935) { - H_30(state, unused, out_1824635200496754935); +void car_H_30(double *state, double *unused, double *out_4421999679472353658) { + H_30(state, unused, out_4421999679472353658); } -void car_h_26(double *state, double *unused, double *out_3515782194269032819) { - h_26(state, unused, out_3515782194269032819); +void car_h_26(double *state, double *unused, double *out_1875094318999546844) { + h_26(state, unused, out_1875094318999546844); } -void car_H_26(double *state, double *unused, double *out_4435201076884549916) { - H_26(state, unused, out_4435201076884549916); +void car_H_26(double *state, double *unused, double *out_809835307741537504) { + H_26(state, unused, out_809835307741537504); } -void car_h_27(double *state, double *unused, double *out_5073135168339482478) { - h_27(state, unused, out_5073135168339482478); +void car_h_27(double *state, double *unused, double *out_8953904322109987302) { + h_27(state, unused, out_8953904322109987302); } -void car_H_27(double *state, double *unused, double *out_4048229271680698152) { - H_27(state, unused, out_4048229271680698152); +void car_H_27(double *state, double *unused, double *out_2247236367671928747) { + H_27(state, unused, out_2247236367671928747); } -void car_h_29(double *state, double *unused, double *out_9171438466618854342) { - h_29(state, unused, out_9171438466618854342); +void car_h_29(double *state, double *unused, double *out_5855390223345541964) { + h_29(state, unused, out_5855390223345541964); } -void car_H_29(double *state, double *unused, double *out_2334866544811147119) { - H_29(state, unused, out_2334866544811147119); +void car_H_29(double *state, double *unused, double *out_4932231023786745842) { + H_29(state, unused, out_4932231023786745842); } -void car_h_28(double *state, double *unused, double *out_6016645021241144913) { - h_28(state, unused, out_6016645021241144913); +void car_h_28(double *state, double *unused, double *out_8510755658264266202) { + h_28(state, unused, out_8510755658264266202); } -void car_H_28(double *state, double *unused, double *out_2747532472258383455) { - H_28(state, unused, out_2747532472258383455); +void car_H_28(double *state, double *unused, double *out_6895861295352072093) { + H_28(state, unused, out_6895861295352072093); } -void car_h_31(double *state, double *unused, double *out_2311543179179655682) { - h_31(state, unused, out_2311543179179655682); +void car_h_31(double *state, double *unused, double *out_3677036992164635768) { + h_31(state, unused, out_3677036992164635768); } -void car_H_31(double *state, double *unused, double *out_5061409179117901392) { - H_31(state, unused, out_5061409179117901392); +void car_H_31(double *state, double *unused, double *out_4581984588492554156) { + H_31(state, unused, out_4581984588492554156); } void car_predict(double *in_x, double *in_P, double *in_Q, double dt) { predict(in_x, in_P, in_Q, dt); diff --git a/selfdrive/locationd/models/generated/car.h b/selfdrive/locationd/models/generated/car.h index d7d327809..cd791113d 100644 --- a/selfdrive/locationd/models/generated/car.h +++ b/selfdrive/locationd/models/generated/car.h @@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); -void car_err_fun(double *nom_x, double *delta_x, double *out_7751375231448978305); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_1186045100449225538); -void car_H_mod_fun(double *state, double *out_5403210608595880924); -void car_f_fun(double *state, double dt, double *out_3846473641327680019); -void car_F_fun(double *state, double dt, double *out_6505574540477860652); -void car_h_25(double *state, double *unused, double *out_1325906349074366668); -void car_H_25(double *state, double *unused, double *out_693697758010493692); -void car_h_24(double *state, double *unused, double *out_7507130987037302702); -void car_H_24(double *state, double *unused, double *out_2864227840791829444); -void car_h_30(double *state, double *unused, double *out_6134652415174521657); -void car_H_30(double *state, double *unused, double *out_1824635200496754935); -void car_h_26(double *state, double *unused, double *out_3515782194269032819); -void car_H_26(double *state, double *unused, double *out_4435201076884549916); -void car_h_27(double *state, double *unused, double *out_5073135168339482478); -void car_H_27(double *state, double *unused, double *out_4048229271680698152); -void car_h_29(double *state, double *unused, double *out_9171438466618854342); -void car_H_29(double *state, double *unused, double *out_2334866544811147119); -void car_h_28(double *state, double *unused, double *out_6016645021241144913); -void car_H_28(double *state, double *unused, double *out_2747532472258383455); -void car_h_31(double *state, double *unused, double *out_2311543179179655682); -void car_H_31(double *state, double *unused, double *out_5061409179117901392); +void car_err_fun(double *nom_x, double *delta_x, double *out_789804740016769707); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_385986033898548509); +void car_H_mod_fun(double *state, double *out_4046004184415297016); +void car_f_fun(double *state, double dt, double *out_467921565295650481); +void car_F_fun(double *state, double dt, double *out_2429260194375294140); +void car_h_25(double *state, double *unused, double *out_4530459753918271082); +void car_H_25(double *state, double *unused, double *out_4551338626615593728); +void car_h_24(double *state, double *unused, double *out_211848015086222289); +void car_H_24(double *state, double *unused, double *out_7319833640502439787); +void car_h_30(double *state, double *unused, double *out_4687646422269400227); +void car_H_30(double *state, double *unused, double *out_4421999679472353658); +void car_h_26(double *state, double *unused, double *out_1875094318999546844); +void car_H_26(double *state, double *unused, double *out_809835307741537504); +void car_h_27(double *state, double *unused, double *out_8953904322109987302); +void car_H_27(double *state, double *unused, double *out_2247236367671928747); +void car_h_29(double *state, double *unused, double *out_5855390223345541964); +void car_H_29(double *state, double *unused, double *out_4932231023786745842); +void car_h_28(double *state, double *unused, double *out_8510755658264266202); +void car_H_28(double *state, double *unused, double *out_6895861295352072093); +void car_h_31(double *state, double *unused, double *out_3677036992164635768); +void car_H_31(double *state, double *unused, double *out_4581984588492554156); void car_predict(double *in_x, double *in_P, double *in_Q, double dt); void car_set_mass(double x); void car_set_rotational_inertia(double x); diff --git a/selfdrive/locationd/models/generated/pose.cpp b/selfdrive/locationd/models/generated/pose.cpp index c02040c6a..471e20363 100644 --- a/selfdrive/locationd/models/generated/pose.cpp +++ b/selfdrive/locationd/models/generated/pose.cpp @@ -17,961 +17,961 @@ const static double MAHA_THRESH_14 = 7.814727903251177; * * * This file is part of 'ekf' * ******************************************************************************/ -void err_fun(double *nom_x, double *delta_x, double *out_5867685499485526905) { - out_5867685499485526905[0] = delta_x[0] + nom_x[0]; - out_5867685499485526905[1] = delta_x[1] + nom_x[1]; - out_5867685499485526905[2] = delta_x[2] + nom_x[2]; - out_5867685499485526905[3] = delta_x[3] + nom_x[3]; - out_5867685499485526905[4] = delta_x[4] + nom_x[4]; - out_5867685499485526905[5] = delta_x[5] + nom_x[5]; - out_5867685499485526905[6] = delta_x[6] + nom_x[6]; - out_5867685499485526905[7] = delta_x[7] + nom_x[7]; - out_5867685499485526905[8] = delta_x[8] + nom_x[8]; - out_5867685499485526905[9] = delta_x[9] + nom_x[9]; - out_5867685499485526905[10] = delta_x[10] + nom_x[10]; - out_5867685499485526905[11] = delta_x[11] + nom_x[11]; - out_5867685499485526905[12] = delta_x[12] + nom_x[12]; - out_5867685499485526905[13] = delta_x[13] + nom_x[13]; - out_5867685499485526905[14] = delta_x[14] + nom_x[14]; - out_5867685499485526905[15] = delta_x[15] + nom_x[15]; - out_5867685499485526905[16] = delta_x[16] + nom_x[16]; - out_5867685499485526905[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_7754318151462489406) { + out_7754318151462489406[0] = delta_x[0] + nom_x[0]; + out_7754318151462489406[1] = delta_x[1] + nom_x[1]; + out_7754318151462489406[2] = delta_x[2] + nom_x[2]; + out_7754318151462489406[3] = delta_x[3] + nom_x[3]; + out_7754318151462489406[4] = delta_x[4] + nom_x[4]; + out_7754318151462489406[5] = delta_x[5] + nom_x[5]; + out_7754318151462489406[6] = delta_x[6] + nom_x[6]; + out_7754318151462489406[7] = delta_x[7] + nom_x[7]; + out_7754318151462489406[8] = delta_x[8] + nom_x[8]; + out_7754318151462489406[9] = delta_x[9] + nom_x[9]; + out_7754318151462489406[10] = delta_x[10] + nom_x[10]; + out_7754318151462489406[11] = delta_x[11] + nom_x[11]; + out_7754318151462489406[12] = delta_x[12] + nom_x[12]; + out_7754318151462489406[13] = delta_x[13] + nom_x[13]; + out_7754318151462489406[14] = delta_x[14] + nom_x[14]; + out_7754318151462489406[15] = delta_x[15] + nom_x[15]; + out_7754318151462489406[16] = delta_x[16] + nom_x[16]; + out_7754318151462489406[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_7496427572072330735) { - out_7496427572072330735[0] = -nom_x[0] + true_x[0]; - out_7496427572072330735[1] = -nom_x[1] + true_x[1]; - out_7496427572072330735[2] = -nom_x[2] + true_x[2]; - out_7496427572072330735[3] = -nom_x[3] + true_x[3]; - out_7496427572072330735[4] = -nom_x[4] + true_x[4]; - out_7496427572072330735[5] = -nom_x[5] + true_x[5]; - out_7496427572072330735[6] = -nom_x[6] + true_x[6]; - out_7496427572072330735[7] = -nom_x[7] + true_x[7]; - out_7496427572072330735[8] = -nom_x[8] + true_x[8]; - out_7496427572072330735[9] = -nom_x[9] + true_x[9]; - out_7496427572072330735[10] = -nom_x[10] + true_x[10]; - out_7496427572072330735[11] = -nom_x[11] + true_x[11]; - out_7496427572072330735[12] = -nom_x[12] + true_x[12]; - out_7496427572072330735[13] = -nom_x[13] + true_x[13]; - out_7496427572072330735[14] = -nom_x[14] + true_x[14]; - out_7496427572072330735[15] = -nom_x[15] + true_x[15]; - out_7496427572072330735[16] = -nom_x[16] + true_x[16]; - out_7496427572072330735[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_5172808168415891616) { + out_5172808168415891616[0] = -nom_x[0] + true_x[0]; + out_5172808168415891616[1] = -nom_x[1] + true_x[1]; + out_5172808168415891616[2] = -nom_x[2] + true_x[2]; + out_5172808168415891616[3] = -nom_x[3] + true_x[3]; + out_5172808168415891616[4] = -nom_x[4] + true_x[4]; + out_5172808168415891616[5] = -nom_x[5] + true_x[5]; + out_5172808168415891616[6] = -nom_x[6] + true_x[6]; + out_5172808168415891616[7] = -nom_x[7] + true_x[7]; + out_5172808168415891616[8] = -nom_x[8] + true_x[8]; + out_5172808168415891616[9] = -nom_x[9] + true_x[9]; + out_5172808168415891616[10] = -nom_x[10] + true_x[10]; + out_5172808168415891616[11] = -nom_x[11] + true_x[11]; + out_5172808168415891616[12] = -nom_x[12] + true_x[12]; + out_5172808168415891616[13] = -nom_x[13] + true_x[13]; + out_5172808168415891616[14] = -nom_x[14] + true_x[14]; + out_5172808168415891616[15] = -nom_x[15] + true_x[15]; + out_5172808168415891616[16] = -nom_x[16] + true_x[16]; + out_5172808168415891616[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_4599731282943687674) { - out_4599731282943687674[0] = 1.0; - out_4599731282943687674[1] = 0.0; - out_4599731282943687674[2] = 0.0; - out_4599731282943687674[3] = 0.0; - out_4599731282943687674[4] = 0.0; - out_4599731282943687674[5] = 0.0; - out_4599731282943687674[6] = 0.0; - out_4599731282943687674[7] = 0.0; - out_4599731282943687674[8] = 0.0; - out_4599731282943687674[9] = 0.0; - out_4599731282943687674[10] = 0.0; - out_4599731282943687674[11] = 0.0; - out_4599731282943687674[12] = 0.0; - out_4599731282943687674[13] = 0.0; - out_4599731282943687674[14] = 0.0; - out_4599731282943687674[15] = 0.0; - out_4599731282943687674[16] = 0.0; - out_4599731282943687674[17] = 0.0; - out_4599731282943687674[18] = 0.0; - out_4599731282943687674[19] = 1.0; - out_4599731282943687674[20] = 0.0; - out_4599731282943687674[21] = 0.0; - out_4599731282943687674[22] = 0.0; - out_4599731282943687674[23] = 0.0; - out_4599731282943687674[24] = 0.0; - out_4599731282943687674[25] = 0.0; - out_4599731282943687674[26] = 0.0; - out_4599731282943687674[27] = 0.0; - out_4599731282943687674[28] = 0.0; - out_4599731282943687674[29] = 0.0; - out_4599731282943687674[30] = 0.0; - out_4599731282943687674[31] = 0.0; - out_4599731282943687674[32] = 0.0; - out_4599731282943687674[33] = 0.0; - out_4599731282943687674[34] = 0.0; - out_4599731282943687674[35] = 0.0; - out_4599731282943687674[36] = 0.0; - out_4599731282943687674[37] = 0.0; - out_4599731282943687674[38] = 1.0; - out_4599731282943687674[39] = 0.0; - out_4599731282943687674[40] = 0.0; - out_4599731282943687674[41] = 0.0; - out_4599731282943687674[42] = 0.0; - out_4599731282943687674[43] = 0.0; - out_4599731282943687674[44] = 0.0; - out_4599731282943687674[45] = 0.0; - out_4599731282943687674[46] = 0.0; - out_4599731282943687674[47] = 0.0; - out_4599731282943687674[48] = 0.0; - out_4599731282943687674[49] = 0.0; - out_4599731282943687674[50] = 0.0; - out_4599731282943687674[51] = 0.0; - out_4599731282943687674[52] = 0.0; - out_4599731282943687674[53] = 0.0; - out_4599731282943687674[54] = 0.0; - out_4599731282943687674[55] = 0.0; - out_4599731282943687674[56] = 0.0; - out_4599731282943687674[57] = 1.0; - out_4599731282943687674[58] = 0.0; - out_4599731282943687674[59] = 0.0; - out_4599731282943687674[60] = 0.0; - out_4599731282943687674[61] = 0.0; - out_4599731282943687674[62] = 0.0; - out_4599731282943687674[63] = 0.0; - out_4599731282943687674[64] = 0.0; - out_4599731282943687674[65] = 0.0; - out_4599731282943687674[66] = 0.0; - out_4599731282943687674[67] = 0.0; - out_4599731282943687674[68] = 0.0; - out_4599731282943687674[69] = 0.0; - out_4599731282943687674[70] = 0.0; - out_4599731282943687674[71] = 0.0; - out_4599731282943687674[72] = 0.0; - out_4599731282943687674[73] = 0.0; - out_4599731282943687674[74] = 0.0; - out_4599731282943687674[75] = 0.0; - out_4599731282943687674[76] = 1.0; - out_4599731282943687674[77] = 0.0; - out_4599731282943687674[78] = 0.0; - out_4599731282943687674[79] = 0.0; - out_4599731282943687674[80] = 0.0; - out_4599731282943687674[81] = 0.0; - out_4599731282943687674[82] = 0.0; - out_4599731282943687674[83] = 0.0; - out_4599731282943687674[84] = 0.0; - out_4599731282943687674[85] = 0.0; - out_4599731282943687674[86] = 0.0; - out_4599731282943687674[87] = 0.0; - out_4599731282943687674[88] = 0.0; - out_4599731282943687674[89] = 0.0; - out_4599731282943687674[90] = 0.0; - out_4599731282943687674[91] = 0.0; - out_4599731282943687674[92] = 0.0; - out_4599731282943687674[93] = 0.0; - out_4599731282943687674[94] = 0.0; - out_4599731282943687674[95] = 1.0; - out_4599731282943687674[96] = 0.0; - out_4599731282943687674[97] = 0.0; - out_4599731282943687674[98] = 0.0; - out_4599731282943687674[99] = 0.0; - out_4599731282943687674[100] = 0.0; - out_4599731282943687674[101] = 0.0; - out_4599731282943687674[102] = 0.0; - out_4599731282943687674[103] = 0.0; - out_4599731282943687674[104] = 0.0; - out_4599731282943687674[105] = 0.0; - out_4599731282943687674[106] = 0.0; - out_4599731282943687674[107] = 0.0; - out_4599731282943687674[108] = 0.0; - out_4599731282943687674[109] = 0.0; - out_4599731282943687674[110] = 0.0; - out_4599731282943687674[111] = 0.0; - out_4599731282943687674[112] = 0.0; - out_4599731282943687674[113] = 0.0; - out_4599731282943687674[114] = 1.0; - out_4599731282943687674[115] = 0.0; - out_4599731282943687674[116] = 0.0; - out_4599731282943687674[117] = 0.0; - out_4599731282943687674[118] = 0.0; - out_4599731282943687674[119] = 0.0; - out_4599731282943687674[120] = 0.0; - out_4599731282943687674[121] = 0.0; - out_4599731282943687674[122] = 0.0; - out_4599731282943687674[123] = 0.0; - out_4599731282943687674[124] = 0.0; - out_4599731282943687674[125] = 0.0; - out_4599731282943687674[126] = 0.0; - out_4599731282943687674[127] = 0.0; - out_4599731282943687674[128] = 0.0; - out_4599731282943687674[129] = 0.0; - out_4599731282943687674[130] = 0.0; - out_4599731282943687674[131] = 0.0; - out_4599731282943687674[132] = 0.0; - out_4599731282943687674[133] = 1.0; - out_4599731282943687674[134] = 0.0; - out_4599731282943687674[135] = 0.0; - out_4599731282943687674[136] = 0.0; - out_4599731282943687674[137] = 0.0; - out_4599731282943687674[138] = 0.0; - out_4599731282943687674[139] = 0.0; - out_4599731282943687674[140] = 0.0; - out_4599731282943687674[141] = 0.0; - out_4599731282943687674[142] = 0.0; - out_4599731282943687674[143] = 0.0; - out_4599731282943687674[144] = 0.0; - out_4599731282943687674[145] = 0.0; - out_4599731282943687674[146] = 0.0; - out_4599731282943687674[147] = 0.0; - out_4599731282943687674[148] = 0.0; - out_4599731282943687674[149] = 0.0; - out_4599731282943687674[150] = 0.0; - out_4599731282943687674[151] = 0.0; - out_4599731282943687674[152] = 1.0; - out_4599731282943687674[153] = 0.0; - out_4599731282943687674[154] = 0.0; - out_4599731282943687674[155] = 0.0; - out_4599731282943687674[156] = 0.0; - out_4599731282943687674[157] = 0.0; - out_4599731282943687674[158] = 0.0; - out_4599731282943687674[159] = 0.0; - out_4599731282943687674[160] = 0.0; - out_4599731282943687674[161] = 0.0; - out_4599731282943687674[162] = 0.0; - out_4599731282943687674[163] = 0.0; - out_4599731282943687674[164] = 0.0; - out_4599731282943687674[165] = 0.0; - out_4599731282943687674[166] = 0.0; - out_4599731282943687674[167] = 0.0; - out_4599731282943687674[168] = 0.0; - out_4599731282943687674[169] = 0.0; - out_4599731282943687674[170] = 0.0; - out_4599731282943687674[171] = 1.0; - out_4599731282943687674[172] = 0.0; - out_4599731282943687674[173] = 0.0; - out_4599731282943687674[174] = 0.0; - out_4599731282943687674[175] = 0.0; - out_4599731282943687674[176] = 0.0; - out_4599731282943687674[177] = 0.0; - out_4599731282943687674[178] = 0.0; - out_4599731282943687674[179] = 0.0; - out_4599731282943687674[180] = 0.0; - out_4599731282943687674[181] = 0.0; - out_4599731282943687674[182] = 0.0; - out_4599731282943687674[183] = 0.0; - out_4599731282943687674[184] = 0.0; - out_4599731282943687674[185] = 0.0; - out_4599731282943687674[186] = 0.0; - out_4599731282943687674[187] = 0.0; - out_4599731282943687674[188] = 0.0; - out_4599731282943687674[189] = 0.0; - out_4599731282943687674[190] = 1.0; - out_4599731282943687674[191] = 0.0; - out_4599731282943687674[192] = 0.0; - out_4599731282943687674[193] = 0.0; - out_4599731282943687674[194] = 0.0; - out_4599731282943687674[195] = 0.0; - out_4599731282943687674[196] = 0.0; - out_4599731282943687674[197] = 0.0; - out_4599731282943687674[198] = 0.0; - out_4599731282943687674[199] = 0.0; - out_4599731282943687674[200] = 0.0; - out_4599731282943687674[201] = 0.0; - out_4599731282943687674[202] = 0.0; - out_4599731282943687674[203] = 0.0; - out_4599731282943687674[204] = 0.0; - out_4599731282943687674[205] = 0.0; - out_4599731282943687674[206] = 0.0; - out_4599731282943687674[207] = 0.0; - out_4599731282943687674[208] = 0.0; - out_4599731282943687674[209] = 1.0; - out_4599731282943687674[210] = 0.0; - out_4599731282943687674[211] = 0.0; - out_4599731282943687674[212] = 0.0; - out_4599731282943687674[213] = 0.0; - out_4599731282943687674[214] = 0.0; - out_4599731282943687674[215] = 0.0; - out_4599731282943687674[216] = 0.0; - out_4599731282943687674[217] = 0.0; - out_4599731282943687674[218] = 0.0; - out_4599731282943687674[219] = 0.0; - out_4599731282943687674[220] = 0.0; - out_4599731282943687674[221] = 0.0; - out_4599731282943687674[222] = 0.0; - out_4599731282943687674[223] = 0.0; - out_4599731282943687674[224] = 0.0; - out_4599731282943687674[225] = 0.0; - out_4599731282943687674[226] = 0.0; - out_4599731282943687674[227] = 0.0; - out_4599731282943687674[228] = 1.0; - out_4599731282943687674[229] = 0.0; - out_4599731282943687674[230] = 0.0; - out_4599731282943687674[231] = 0.0; - out_4599731282943687674[232] = 0.0; - out_4599731282943687674[233] = 0.0; - out_4599731282943687674[234] = 0.0; - out_4599731282943687674[235] = 0.0; - out_4599731282943687674[236] = 0.0; - out_4599731282943687674[237] = 0.0; - out_4599731282943687674[238] = 0.0; - out_4599731282943687674[239] = 0.0; - out_4599731282943687674[240] = 0.0; - out_4599731282943687674[241] = 0.0; - out_4599731282943687674[242] = 0.0; - out_4599731282943687674[243] = 0.0; - out_4599731282943687674[244] = 0.0; - out_4599731282943687674[245] = 0.0; - out_4599731282943687674[246] = 0.0; - out_4599731282943687674[247] = 1.0; - out_4599731282943687674[248] = 0.0; - out_4599731282943687674[249] = 0.0; - out_4599731282943687674[250] = 0.0; - out_4599731282943687674[251] = 0.0; - out_4599731282943687674[252] = 0.0; - out_4599731282943687674[253] = 0.0; - out_4599731282943687674[254] = 0.0; - out_4599731282943687674[255] = 0.0; - out_4599731282943687674[256] = 0.0; - out_4599731282943687674[257] = 0.0; - out_4599731282943687674[258] = 0.0; - out_4599731282943687674[259] = 0.0; - out_4599731282943687674[260] = 0.0; - out_4599731282943687674[261] = 0.0; - out_4599731282943687674[262] = 0.0; - out_4599731282943687674[263] = 0.0; - out_4599731282943687674[264] = 0.0; - out_4599731282943687674[265] = 0.0; - out_4599731282943687674[266] = 1.0; - out_4599731282943687674[267] = 0.0; - out_4599731282943687674[268] = 0.0; - out_4599731282943687674[269] = 0.0; - out_4599731282943687674[270] = 0.0; - out_4599731282943687674[271] = 0.0; - out_4599731282943687674[272] = 0.0; - out_4599731282943687674[273] = 0.0; - out_4599731282943687674[274] = 0.0; - out_4599731282943687674[275] = 0.0; - out_4599731282943687674[276] = 0.0; - out_4599731282943687674[277] = 0.0; - out_4599731282943687674[278] = 0.0; - out_4599731282943687674[279] = 0.0; - out_4599731282943687674[280] = 0.0; - out_4599731282943687674[281] = 0.0; - out_4599731282943687674[282] = 0.0; - out_4599731282943687674[283] = 0.0; - out_4599731282943687674[284] = 0.0; - out_4599731282943687674[285] = 1.0; - out_4599731282943687674[286] = 0.0; - out_4599731282943687674[287] = 0.0; - out_4599731282943687674[288] = 0.0; - out_4599731282943687674[289] = 0.0; - out_4599731282943687674[290] = 0.0; - out_4599731282943687674[291] = 0.0; - out_4599731282943687674[292] = 0.0; - out_4599731282943687674[293] = 0.0; - out_4599731282943687674[294] = 0.0; - out_4599731282943687674[295] = 0.0; - out_4599731282943687674[296] = 0.0; - out_4599731282943687674[297] = 0.0; - out_4599731282943687674[298] = 0.0; - out_4599731282943687674[299] = 0.0; - out_4599731282943687674[300] = 0.0; - out_4599731282943687674[301] = 0.0; - out_4599731282943687674[302] = 0.0; - out_4599731282943687674[303] = 0.0; - out_4599731282943687674[304] = 1.0; - out_4599731282943687674[305] = 0.0; - out_4599731282943687674[306] = 0.0; - out_4599731282943687674[307] = 0.0; - out_4599731282943687674[308] = 0.0; - out_4599731282943687674[309] = 0.0; - out_4599731282943687674[310] = 0.0; - out_4599731282943687674[311] = 0.0; - out_4599731282943687674[312] = 0.0; - out_4599731282943687674[313] = 0.0; - out_4599731282943687674[314] = 0.0; - out_4599731282943687674[315] = 0.0; - out_4599731282943687674[316] = 0.0; - out_4599731282943687674[317] = 0.0; - out_4599731282943687674[318] = 0.0; - out_4599731282943687674[319] = 0.0; - out_4599731282943687674[320] = 0.0; - out_4599731282943687674[321] = 0.0; - out_4599731282943687674[322] = 0.0; - out_4599731282943687674[323] = 1.0; +void H_mod_fun(double *state, double *out_2254475305049299983) { + out_2254475305049299983[0] = 1.0; + out_2254475305049299983[1] = 0.0; + out_2254475305049299983[2] = 0.0; + out_2254475305049299983[3] = 0.0; + out_2254475305049299983[4] = 0.0; + out_2254475305049299983[5] = 0.0; + out_2254475305049299983[6] = 0.0; + out_2254475305049299983[7] = 0.0; + out_2254475305049299983[8] = 0.0; + out_2254475305049299983[9] = 0.0; + out_2254475305049299983[10] = 0.0; + out_2254475305049299983[11] = 0.0; + out_2254475305049299983[12] = 0.0; + out_2254475305049299983[13] = 0.0; + out_2254475305049299983[14] = 0.0; + out_2254475305049299983[15] = 0.0; + out_2254475305049299983[16] = 0.0; + out_2254475305049299983[17] = 0.0; + out_2254475305049299983[18] = 0.0; + out_2254475305049299983[19] = 1.0; + out_2254475305049299983[20] = 0.0; + out_2254475305049299983[21] = 0.0; + out_2254475305049299983[22] = 0.0; + out_2254475305049299983[23] = 0.0; + out_2254475305049299983[24] = 0.0; + out_2254475305049299983[25] = 0.0; + out_2254475305049299983[26] = 0.0; + out_2254475305049299983[27] = 0.0; + out_2254475305049299983[28] = 0.0; + out_2254475305049299983[29] = 0.0; + out_2254475305049299983[30] = 0.0; + out_2254475305049299983[31] = 0.0; + out_2254475305049299983[32] = 0.0; + out_2254475305049299983[33] = 0.0; + out_2254475305049299983[34] = 0.0; + out_2254475305049299983[35] = 0.0; + out_2254475305049299983[36] = 0.0; + out_2254475305049299983[37] = 0.0; + out_2254475305049299983[38] = 1.0; + out_2254475305049299983[39] = 0.0; + out_2254475305049299983[40] = 0.0; + out_2254475305049299983[41] = 0.0; + out_2254475305049299983[42] = 0.0; + out_2254475305049299983[43] = 0.0; + out_2254475305049299983[44] = 0.0; + out_2254475305049299983[45] = 0.0; + out_2254475305049299983[46] = 0.0; + out_2254475305049299983[47] = 0.0; + out_2254475305049299983[48] = 0.0; + out_2254475305049299983[49] = 0.0; + out_2254475305049299983[50] = 0.0; + out_2254475305049299983[51] = 0.0; + out_2254475305049299983[52] = 0.0; + out_2254475305049299983[53] = 0.0; + out_2254475305049299983[54] = 0.0; + out_2254475305049299983[55] = 0.0; + out_2254475305049299983[56] = 0.0; + out_2254475305049299983[57] = 1.0; + out_2254475305049299983[58] = 0.0; + out_2254475305049299983[59] = 0.0; + out_2254475305049299983[60] = 0.0; + out_2254475305049299983[61] = 0.0; + out_2254475305049299983[62] = 0.0; + out_2254475305049299983[63] = 0.0; + out_2254475305049299983[64] = 0.0; + out_2254475305049299983[65] = 0.0; + out_2254475305049299983[66] = 0.0; + out_2254475305049299983[67] = 0.0; + out_2254475305049299983[68] = 0.0; + out_2254475305049299983[69] = 0.0; + out_2254475305049299983[70] = 0.0; + out_2254475305049299983[71] = 0.0; + out_2254475305049299983[72] = 0.0; + out_2254475305049299983[73] = 0.0; + out_2254475305049299983[74] = 0.0; + out_2254475305049299983[75] = 0.0; + out_2254475305049299983[76] = 1.0; + out_2254475305049299983[77] = 0.0; + out_2254475305049299983[78] = 0.0; + out_2254475305049299983[79] = 0.0; + out_2254475305049299983[80] = 0.0; + out_2254475305049299983[81] = 0.0; + out_2254475305049299983[82] = 0.0; + out_2254475305049299983[83] = 0.0; + out_2254475305049299983[84] = 0.0; + out_2254475305049299983[85] = 0.0; + out_2254475305049299983[86] = 0.0; + out_2254475305049299983[87] = 0.0; + out_2254475305049299983[88] = 0.0; + out_2254475305049299983[89] = 0.0; + out_2254475305049299983[90] = 0.0; + out_2254475305049299983[91] = 0.0; + out_2254475305049299983[92] = 0.0; + out_2254475305049299983[93] = 0.0; + out_2254475305049299983[94] = 0.0; + out_2254475305049299983[95] = 1.0; + out_2254475305049299983[96] = 0.0; + out_2254475305049299983[97] = 0.0; + out_2254475305049299983[98] = 0.0; + out_2254475305049299983[99] = 0.0; + out_2254475305049299983[100] = 0.0; + out_2254475305049299983[101] = 0.0; + out_2254475305049299983[102] = 0.0; + out_2254475305049299983[103] = 0.0; + out_2254475305049299983[104] = 0.0; + out_2254475305049299983[105] = 0.0; + out_2254475305049299983[106] = 0.0; + out_2254475305049299983[107] = 0.0; + out_2254475305049299983[108] = 0.0; + out_2254475305049299983[109] = 0.0; + out_2254475305049299983[110] = 0.0; + out_2254475305049299983[111] = 0.0; + out_2254475305049299983[112] = 0.0; + out_2254475305049299983[113] = 0.0; + out_2254475305049299983[114] = 1.0; + out_2254475305049299983[115] = 0.0; + out_2254475305049299983[116] = 0.0; + out_2254475305049299983[117] = 0.0; + out_2254475305049299983[118] = 0.0; + out_2254475305049299983[119] = 0.0; + out_2254475305049299983[120] = 0.0; + out_2254475305049299983[121] = 0.0; + out_2254475305049299983[122] = 0.0; + out_2254475305049299983[123] = 0.0; + out_2254475305049299983[124] = 0.0; + out_2254475305049299983[125] = 0.0; + out_2254475305049299983[126] = 0.0; + out_2254475305049299983[127] = 0.0; + out_2254475305049299983[128] = 0.0; + out_2254475305049299983[129] = 0.0; + out_2254475305049299983[130] = 0.0; + out_2254475305049299983[131] = 0.0; + out_2254475305049299983[132] = 0.0; + out_2254475305049299983[133] = 1.0; + out_2254475305049299983[134] = 0.0; + out_2254475305049299983[135] = 0.0; + out_2254475305049299983[136] = 0.0; + out_2254475305049299983[137] = 0.0; + out_2254475305049299983[138] = 0.0; + out_2254475305049299983[139] = 0.0; + out_2254475305049299983[140] = 0.0; + out_2254475305049299983[141] = 0.0; + out_2254475305049299983[142] = 0.0; + out_2254475305049299983[143] = 0.0; + out_2254475305049299983[144] = 0.0; + out_2254475305049299983[145] = 0.0; + out_2254475305049299983[146] = 0.0; + out_2254475305049299983[147] = 0.0; + out_2254475305049299983[148] = 0.0; + out_2254475305049299983[149] = 0.0; + out_2254475305049299983[150] = 0.0; + out_2254475305049299983[151] = 0.0; + out_2254475305049299983[152] = 1.0; + out_2254475305049299983[153] = 0.0; + out_2254475305049299983[154] = 0.0; + out_2254475305049299983[155] = 0.0; + out_2254475305049299983[156] = 0.0; + out_2254475305049299983[157] = 0.0; + out_2254475305049299983[158] = 0.0; + out_2254475305049299983[159] = 0.0; + out_2254475305049299983[160] = 0.0; + out_2254475305049299983[161] = 0.0; + out_2254475305049299983[162] = 0.0; + out_2254475305049299983[163] = 0.0; + out_2254475305049299983[164] = 0.0; + out_2254475305049299983[165] = 0.0; + out_2254475305049299983[166] = 0.0; + out_2254475305049299983[167] = 0.0; + out_2254475305049299983[168] = 0.0; + out_2254475305049299983[169] = 0.0; + out_2254475305049299983[170] = 0.0; + out_2254475305049299983[171] = 1.0; + out_2254475305049299983[172] = 0.0; + out_2254475305049299983[173] = 0.0; + out_2254475305049299983[174] = 0.0; + out_2254475305049299983[175] = 0.0; + out_2254475305049299983[176] = 0.0; + out_2254475305049299983[177] = 0.0; + out_2254475305049299983[178] = 0.0; + out_2254475305049299983[179] = 0.0; + out_2254475305049299983[180] = 0.0; + out_2254475305049299983[181] = 0.0; + out_2254475305049299983[182] = 0.0; + out_2254475305049299983[183] = 0.0; + out_2254475305049299983[184] = 0.0; + out_2254475305049299983[185] = 0.0; + out_2254475305049299983[186] = 0.0; + out_2254475305049299983[187] = 0.0; + out_2254475305049299983[188] = 0.0; + out_2254475305049299983[189] = 0.0; + out_2254475305049299983[190] = 1.0; + out_2254475305049299983[191] = 0.0; + out_2254475305049299983[192] = 0.0; + out_2254475305049299983[193] = 0.0; + out_2254475305049299983[194] = 0.0; + out_2254475305049299983[195] = 0.0; + out_2254475305049299983[196] = 0.0; + out_2254475305049299983[197] = 0.0; + out_2254475305049299983[198] = 0.0; + out_2254475305049299983[199] = 0.0; + out_2254475305049299983[200] = 0.0; + out_2254475305049299983[201] = 0.0; + out_2254475305049299983[202] = 0.0; + out_2254475305049299983[203] = 0.0; + out_2254475305049299983[204] = 0.0; + out_2254475305049299983[205] = 0.0; + out_2254475305049299983[206] = 0.0; + out_2254475305049299983[207] = 0.0; + out_2254475305049299983[208] = 0.0; + out_2254475305049299983[209] = 1.0; + out_2254475305049299983[210] = 0.0; + out_2254475305049299983[211] = 0.0; + out_2254475305049299983[212] = 0.0; + out_2254475305049299983[213] = 0.0; + out_2254475305049299983[214] = 0.0; + out_2254475305049299983[215] = 0.0; + out_2254475305049299983[216] = 0.0; + out_2254475305049299983[217] = 0.0; + out_2254475305049299983[218] = 0.0; + out_2254475305049299983[219] = 0.0; + out_2254475305049299983[220] = 0.0; + out_2254475305049299983[221] = 0.0; + out_2254475305049299983[222] = 0.0; + out_2254475305049299983[223] = 0.0; + out_2254475305049299983[224] = 0.0; + out_2254475305049299983[225] = 0.0; + out_2254475305049299983[226] = 0.0; + out_2254475305049299983[227] = 0.0; + out_2254475305049299983[228] = 1.0; + out_2254475305049299983[229] = 0.0; + out_2254475305049299983[230] = 0.0; + out_2254475305049299983[231] = 0.0; + out_2254475305049299983[232] = 0.0; + out_2254475305049299983[233] = 0.0; + out_2254475305049299983[234] = 0.0; + out_2254475305049299983[235] = 0.0; + out_2254475305049299983[236] = 0.0; + out_2254475305049299983[237] = 0.0; + out_2254475305049299983[238] = 0.0; + out_2254475305049299983[239] = 0.0; + out_2254475305049299983[240] = 0.0; + out_2254475305049299983[241] = 0.0; + out_2254475305049299983[242] = 0.0; + out_2254475305049299983[243] = 0.0; + out_2254475305049299983[244] = 0.0; + out_2254475305049299983[245] = 0.0; + out_2254475305049299983[246] = 0.0; + out_2254475305049299983[247] = 1.0; + out_2254475305049299983[248] = 0.0; + out_2254475305049299983[249] = 0.0; + out_2254475305049299983[250] = 0.0; + out_2254475305049299983[251] = 0.0; + out_2254475305049299983[252] = 0.0; + out_2254475305049299983[253] = 0.0; + out_2254475305049299983[254] = 0.0; + out_2254475305049299983[255] = 0.0; + out_2254475305049299983[256] = 0.0; + out_2254475305049299983[257] = 0.0; + out_2254475305049299983[258] = 0.0; + out_2254475305049299983[259] = 0.0; + out_2254475305049299983[260] = 0.0; + out_2254475305049299983[261] = 0.0; + out_2254475305049299983[262] = 0.0; + out_2254475305049299983[263] = 0.0; + out_2254475305049299983[264] = 0.0; + out_2254475305049299983[265] = 0.0; + out_2254475305049299983[266] = 1.0; + out_2254475305049299983[267] = 0.0; + out_2254475305049299983[268] = 0.0; + out_2254475305049299983[269] = 0.0; + out_2254475305049299983[270] = 0.0; + out_2254475305049299983[271] = 0.0; + out_2254475305049299983[272] = 0.0; + out_2254475305049299983[273] = 0.0; + out_2254475305049299983[274] = 0.0; + out_2254475305049299983[275] = 0.0; + out_2254475305049299983[276] = 0.0; + out_2254475305049299983[277] = 0.0; + out_2254475305049299983[278] = 0.0; + out_2254475305049299983[279] = 0.0; + out_2254475305049299983[280] = 0.0; + out_2254475305049299983[281] = 0.0; + out_2254475305049299983[282] = 0.0; + out_2254475305049299983[283] = 0.0; + out_2254475305049299983[284] = 0.0; + out_2254475305049299983[285] = 1.0; + out_2254475305049299983[286] = 0.0; + out_2254475305049299983[287] = 0.0; + out_2254475305049299983[288] = 0.0; + out_2254475305049299983[289] = 0.0; + out_2254475305049299983[290] = 0.0; + out_2254475305049299983[291] = 0.0; + out_2254475305049299983[292] = 0.0; + out_2254475305049299983[293] = 0.0; + out_2254475305049299983[294] = 0.0; + out_2254475305049299983[295] = 0.0; + out_2254475305049299983[296] = 0.0; + out_2254475305049299983[297] = 0.0; + out_2254475305049299983[298] = 0.0; + out_2254475305049299983[299] = 0.0; + out_2254475305049299983[300] = 0.0; + out_2254475305049299983[301] = 0.0; + out_2254475305049299983[302] = 0.0; + out_2254475305049299983[303] = 0.0; + out_2254475305049299983[304] = 1.0; + out_2254475305049299983[305] = 0.0; + out_2254475305049299983[306] = 0.0; + out_2254475305049299983[307] = 0.0; + out_2254475305049299983[308] = 0.0; + out_2254475305049299983[309] = 0.0; + out_2254475305049299983[310] = 0.0; + out_2254475305049299983[311] = 0.0; + out_2254475305049299983[312] = 0.0; + out_2254475305049299983[313] = 0.0; + out_2254475305049299983[314] = 0.0; + out_2254475305049299983[315] = 0.0; + out_2254475305049299983[316] = 0.0; + out_2254475305049299983[317] = 0.0; + out_2254475305049299983[318] = 0.0; + out_2254475305049299983[319] = 0.0; + out_2254475305049299983[320] = 0.0; + out_2254475305049299983[321] = 0.0; + out_2254475305049299983[322] = 0.0; + out_2254475305049299983[323] = 1.0; } -void f_fun(double *state, double dt, double *out_8448766243959956852) { - out_8448766243959956852[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_8448766243959956852[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_8448766243959956852[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_8448766243959956852[3] = dt*state[12] + state[3]; - out_8448766243959956852[4] = dt*state[13] + state[4]; - out_8448766243959956852[5] = dt*state[14] + state[5]; - out_8448766243959956852[6] = state[6]; - out_8448766243959956852[7] = state[7]; - out_8448766243959956852[8] = state[8]; - out_8448766243959956852[9] = state[9]; - out_8448766243959956852[10] = state[10]; - out_8448766243959956852[11] = state[11]; - out_8448766243959956852[12] = state[12]; - out_8448766243959956852[13] = state[13]; - out_8448766243959956852[14] = state[14]; - out_8448766243959956852[15] = state[15]; - out_8448766243959956852[16] = state[16]; - out_8448766243959956852[17] = state[17]; +void f_fun(double *state, double dt, double *out_9187930556967356165) { + out_9187930556967356165[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_9187930556967356165[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_9187930556967356165[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_9187930556967356165[3] = dt*state[12] + state[3]; + out_9187930556967356165[4] = dt*state[13] + state[4]; + out_9187930556967356165[5] = dt*state[14] + state[5]; + out_9187930556967356165[6] = state[6]; + out_9187930556967356165[7] = state[7]; + out_9187930556967356165[8] = state[8]; + out_9187930556967356165[9] = state[9]; + out_9187930556967356165[10] = state[10]; + out_9187930556967356165[11] = state[11]; + out_9187930556967356165[12] = state[12]; + out_9187930556967356165[13] = state[13]; + out_9187930556967356165[14] = state[14]; + out_9187930556967356165[15] = state[15]; + out_9187930556967356165[16] = state[16]; + out_9187930556967356165[17] = state[17]; } -void F_fun(double *state, double dt, double *out_3524034210859456461) { - out_3524034210859456461[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_3524034210859456461[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_3524034210859456461[2] = 0; - out_3524034210859456461[3] = 0; - out_3524034210859456461[4] = 0; - out_3524034210859456461[5] = 0; - out_3524034210859456461[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_3524034210859456461[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_3524034210859456461[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_3524034210859456461[9] = 0; - out_3524034210859456461[10] = 0; - out_3524034210859456461[11] = 0; - out_3524034210859456461[12] = 0; - out_3524034210859456461[13] = 0; - out_3524034210859456461[14] = 0; - out_3524034210859456461[15] = 0; - out_3524034210859456461[16] = 0; - out_3524034210859456461[17] = 0; - out_3524034210859456461[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_3524034210859456461[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_3524034210859456461[20] = 0; - out_3524034210859456461[21] = 0; - out_3524034210859456461[22] = 0; - out_3524034210859456461[23] = 0; - out_3524034210859456461[24] = 0; - out_3524034210859456461[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_3524034210859456461[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_3524034210859456461[27] = 0; - out_3524034210859456461[28] = 0; - out_3524034210859456461[29] = 0; - out_3524034210859456461[30] = 0; - out_3524034210859456461[31] = 0; - out_3524034210859456461[32] = 0; - out_3524034210859456461[33] = 0; - out_3524034210859456461[34] = 0; - out_3524034210859456461[35] = 0; - out_3524034210859456461[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_3524034210859456461[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_3524034210859456461[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_3524034210859456461[39] = 0; - out_3524034210859456461[40] = 0; - out_3524034210859456461[41] = 0; - out_3524034210859456461[42] = 0; - out_3524034210859456461[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_3524034210859456461[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_3524034210859456461[45] = 0; - out_3524034210859456461[46] = 0; - out_3524034210859456461[47] = 0; - out_3524034210859456461[48] = 0; - out_3524034210859456461[49] = 0; - out_3524034210859456461[50] = 0; - out_3524034210859456461[51] = 0; - out_3524034210859456461[52] = 0; - out_3524034210859456461[53] = 0; - out_3524034210859456461[54] = 0; - out_3524034210859456461[55] = 0; - out_3524034210859456461[56] = 0; - out_3524034210859456461[57] = 1; - out_3524034210859456461[58] = 0; - out_3524034210859456461[59] = 0; - out_3524034210859456461[60] = 0; - out_3524034210859456461[61] = 0; - out_3524034210859456461[62] = 0; - out_3524034210859456461[63] = 0; - out_3524034210859456461[64] = 0; - out_3524034210859456461[65] = 0; - out_3524034210859456461[66] = dt; - out_3524034210859456461[67] = 0; - out_3524034210859456461[68] = 0; - out_3524034210859456461[69] = 0; - out_3524034210859456461[70] = 0; - out_3524034210859456461[71] = 0; - out_3524034210859456461[72] = 0; - out_3524034210859456461[73] = 0; - out_3524034210859456461[74] = 0; - out_3524034210859456461[75] = 0; - out_3524034210859456461[76] = 1; - out_3524034210859456461[77] = 0; - out_3524034210859456461[78] = 0; - out_3524034210859456461[79] = 0; - out_3524034210859456461[80] = 0; - out_3524034210859456461[81] = 0; - out_3524034210859456461[82] = 0; - out_3524034210859456461[83] = 0; - out_3524034210859456461[84] = 0; - out_3524034210859456461[85] = dt; - out_3524034210859456461[86] = 0; - out_3524034210859456461[87] = 0; - out_3524034210859456461[88] = 0; - out_3524034210859456461[89] = 0; - out_3524034210859456461[90] = 0; - out_3524034210859456461[91] = 0; - out_3524034210859456461[92] = 0; - out_3524034210859456461[93] = 0; - out_3524034210859456461[94] = 0; - out_3524034210859456461[95] = 1; - out_3524034210859456461[96] = 0; - out_3524034210859456461[97] = 0; - out_3524034210859456461[98] = 0; - out_3524034210859456461[99] = 0; - out_3524034210859456461[100] = 0; - out_3524034210859456461[101] = 0; - out_3524034210859456461[102] = 0; - out_3524034210859456461[103] = 0; - out_3524034210859456461[104] = dt; - out_3524034210859456461[105] = 0; - out_3524034210859456461[106] = 0; - out_3524034210859456461[107] = 0; - out_3524034210859456461[108] = 0; - out_3524034210859456461[109] = 0; - out_3524034210859456461[110] = 0; - out_3524034210859456461[111] = 0; - out_3524034210859456461[112] = 0; - out_3524034210859456461[113] = 0; - out_3524034210859456461[114] = 1; - out_3524034210859456461[115] = 0; - out_3524034210859456461[116] = 0; - out_3524034210859456461[117] = 0; - out_3524034210859456461[118] = 0; - out_3524034210859456461[119] = 0; - out_3524034210859456461[120] = 0; - out_3524034210859456461[121] = 0; - out_3524034210859456461[122] = 0; - out_3524034210859456461[123] = 0; - out_3524034210859456461[124] = 0; - out_3524034210859456461[125] = 0; - out_3524034210859456461[126] = 0; - out_3524034210859456461[127] = 0; - out_3524034210859456461[128] = 0; - out_3524034210859456461[129] = 0; - out_3524034210859456461[130] = 0; - out_3524034210859456461[131] = 0; - out_3524034210859456461[132] = 0; - out_3524034210859456461[133] = 1; - out_3524034210859456461[134] = 0; - out_3524034210859456461[135] = 0; - out_3524034210859456461[136] = 0; - out_3524034210859456461[137] = 0; - out_3524034210859456461[138] = 0; - out_3524034210859456461[139] = 0; - out_3524034210859456461[140] = 0; - out_3524034210859456461[141] = 0; - out_3524034210859456461[142] = 0; - out_3524034210859456461[143] = 0; - out_3524034210859456461[144] = 0; - out_3524034210859456461[145] = 0; - out_3524034210859456461[146] = 0; - out_3524034210859456461[147] = 0; - out_3524034210859456461[148] = 0; - out_3524034210859456461[149] = 0; - out_3524034210859456461[150] = 0; - out_3524034210859456461[151] = 0; - out_3524034210859456461[152] = 1; - out_3524034210859456461[153] = 0; - out_3524034210859456461[154] = 0; - out_3524034210859456461[155] = 0; - out_3524034210859456461[156] = 0; - out_3524034210859456461[157] = 0; - out_3524034210859456461[158] = 0; - out_3524034210859456461[159] = 0; - out_3524034210859456461[160] = 0; - out_3524034210859456461[161] = 0; - out_3524034210859456461[162] = 0; - out_3524034210859456461[163] = 0; - out_3524034210859456461[164] = 0; - out_3524034210859456461[165] = 0; - out_3524034210859456461[166] = 0; - out_3524034210859456461[167] = 0; - out_3524034210859456461[168] = 0; - out_3524034210859456461[169] = 0; - out_3524034210859456461[170] = 0; - out_3524034210859456461[171] = 1; - out_3524034210859456461[172] = 0; - out_3524034210859456461[173] = 0; - out_3524034210859456461[174] = 0; - out_3524034210859456461[175] = 0; - out_3524034210859456461[176] = 0; - out_3524034210859456461[177] = 0; - out_3524034210859456461[178] = 0; - out_3524034210859456461[179] = 0; - out_3524034210859456461[180] = 0; - out_3524034210859456461[181] = 0; - out_3524034210859456461[182] = 0; - out_3524034210859456461[183] = 0; - out_3524034210859456461[184] = 0; - out_3524034210859456461[185] = 0; - out_3524034210859456461[186] = 0; - out_3524034210859456461[187] = 0; - out_3524034210859456461[188] = 0; - out_3524034210859456461[189] = 0; - out_3524034210859456461[190] = 1; - out_3524034210859456461[191] = 0; - out_3524034210859456461[192] = 0; - out_3524034210859456461[193] = 0; - out_3524034210859456461[194] = 0; - out_3524034210859456461[195] = 0; - out_3524034210859456461[196] = 0; - out_3524034210859456461[197] = 0; - out_3524034210859456461[198] = 0; - out_3524034210859456461[199] = 0; - out_3524034210859456461[200] = 0; - out_3524034210859456461[201] = 0; - out_3524034210859456461[202] = 0; - out_3524034210859456461[203] = 0; - out_3524034210859456461[204] = 0; - out_3524034210859456461[205] = 0; - out_3524034210859456461[206] = 0; - out_3524034210859456461[207] = 0; - out_3524034210859456461[208] = 0; - out_3524034210859456461[209] = 1; - out_3524034210859456461[210] = 0; - out_3524034210859456461[211] = 0; - out_3524034210859456461[212] = 0; - out_3524034210859456461[213] = 0; - out_3524034210859456461[214] = 0; - out_3524034210859456461[215] = 0; - out_3524034210859456461[216] = 0; - out_3524034210859456461[217] = 0; - out_3524034210859456461[218] = 0; - out_3524034210859456461[219] = 0; - out_3524034210859456461[220] = 0; - out_3524034210859456461[221] = 0; - out_3524034210859456461[222] = 0; - out_3524034210859456461[223] = 0; - out_3524034210859456461[224] = 0; - out_3524034210859456461[225] = 0; - out_3524034210859456461[226] = 0; - out_3524034210859456461[227] = 0; - out_3524034210859456461[228] = 1; - out_3524034210859456461[229] = 0; - out_3524034210859456461[230] = 0; - out_3524034210859456461[231] = 0; - out_3524034210859456461[232] = 0; - out_3524034210859456461[233] = 0; - out_3524034210859456461[234] = 0; - out_3524034210859456461[235] = 0; - out_3524034210859456461[236] = 0; - out_3524034210859456461[237] = 0; - out_3524034210859456461[238] = 0; - out_3524034210859456461[239] = 0; - out_3524034210859456461[240] = 0; - out_3524034210859456461[241] = 0; - out_3524034210859456461[242] = 0; - out_3524034210859456461[243] = 0; - out_3524034210859456461[244] = 0; - out_3524034210859456461[245] = 0; - out_3524034210859456461[246] = 0; - out_3524034210859456461[247] = 1; - out_3524034210859456461[248] = 0; - out_3524034210859456461[249] = 0; - out_3524034210859456461[250] = 0; - out_3524034210859456461[251] = 0; - out_3524034210859456461[252] = 0; - out_3524034210859456461[253] = 0; - out_3524034210859456461[254] = 0; - out_3524034210859456461[255] = 0; - out_3524034210859456461[256] = 0; - out_3524034210859456461[257] = 0; - out_3524034210859456461[258] = 0; - out_3524034210859456461[259] = 0; - out_3524034210859456461[260] = 0; - out_3524034210859456461[261] = 0; - out_3524034210859456461[262] = 0; - out_3524034210859456461[263] = 0; - out_3524034210859456461[264] = 0; - out_3524034210859456461[265] = 0; - out_3524034210859456461[266] = 1; - out_3524034210859456461[267] = 0; - out_3524034210859456461[268] = 0; - out_3524034210859456461[269] = 0; - out_3524034210859456461[270] = 0; - out_3524034210859456461[271] = 0; - out_3524034210859456461[272] = 0; - out_3524034210859456461[273] = 0; - out_3524034210859456461[274] = 0; - out_3524034210859456461[275] = 0; - out_3524034210859456461[276] = 0; - out_3524034210859456461[277] = 0; - out_3524034210859456461[278] = 0; - out_3524034210859456461[279] = 0; - out_3524034210859456461[280] = 0; - out_3524034210859456461[281] = 0; - out_3524034210859456461[282] = 0; - out_3524034210859456461[283] = 0; - out_3524034210859456461[284] = 0; - out_3524034210859456461[285] = 1; - out_3524034210859456461[286] = 0; - out_3524034210859456461[287] = 0; - out_3524034210859456461[288] = 0; - out_3524034210859456461[289] = 0; - out_3524034210859456461[290] = 0; - out_3524034210859456461[291] = 0; - out_3524034210859456461[292] = 0; - out_3524034210859456461[293] = 0; - out_3524034210859456461[294] = 0; - out_3524034210859456461[295] = 0; - out_3524034210859456461[296] = 0; - out_3524034210859456461[297] = 0; - out_3524034210859456461[298] = 0; - out_3524034210859456461[299] = 0; - out_3524034210859456461[300] = 0; - out_3524034210859456461[301] = 0; - out_3524034210859456461[302] = 0; - out_3524034210859456461[303] = 0; - out_3524034210859456461[304] = 1; - out_3524034210859456461[305] = 0; - out_3524034210859456461[306] = 0; - out_3524034210859456461[307] = 0; - out_3524034210859456461[308] = 0; - out_3524034210859456461[309] = 0; - out_3524034210859456461[310] = 0; - out_3524034210859456461[311] = 0; - out_3524034210859456461[312] = 0; - out_3524034210859456461[313] = 0; - out_3524034210859456461[314] = 0; - out_3524034210859456461[315] = 0; - out_3524034210859456461[316] = 0; - out_3524034210859456461[317] = 0; - out_3524034210859456461[318] = 0; - out_3524034210859456461[319] = 0; - out_3524034210859456461[320] = 0; - out_3524034210859456461[321] = 0; - out_3524034210859456461[322] = 0; - out_3524034210859456461[323] = 1; +void F_fun(double *state, double dt, double *out_1250378920670806697) { + out_1250378920670806697[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_1250378920670806697[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_1250378920670806697[2] = 0; + out_1250378920670806697[3] = 0; + out_1250378920670806697[4] = 0; + out_1250378920670806697[5] = 0; + out_1250378920670806697[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_1250378920670806697[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_1250378920670806697[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_1250378920670806697[9] = 0; + out_1250378920670806697[10] = 0; + out_1250378920670806697[11] = 0; + out_1250378920670806697[12] = 0; + out_1250378920670806697[13] = 0; + out_1250378920670806697[14] = 0; + out_1250378920670806697[15] = 0; + out_1250378920670806697[16] = 0; + out_1250378920670806697[17] = 0; + out_1250378920670806697[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_1250378920670806697[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_1250378920670806697[20] = 0; + out_1250378920670806697[21] = 0; + out_1250378920670806697[22] = 0; + out_1250378920670806697[23] = 0; + out_1250378920670806697[24] = 0; + out_1250378920670806697[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_1250378920670806697[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_1250378920670806697[27] = 0; + out_1250378920670806697[28] = 0; + out_1250378920670806697[29] = 0; + out_1250378920670806697[30] = 0; + out_1250378920670806697[31] = 0; + out_1250378920670806697[32] = 0; + out_1250378920670806697[33] = 0; + out_1250378920670806697[34] = 0; + out_1250378920670806697[35] = 0; + out_1250378920670806697[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_1250378920670806697[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_1250378920670806697[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_1250378920670806697[39] = 0; + out_1250378920670806697[40] = 0; + out_1250378920670806697[41] = 0; + out_1250378920670806697[42] = 0; + out_1250378920670806697[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_1250378920670806697[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_1250378920670806697[45] = 0; + out_1250378920670806697[46] = 0; + out_1250378920670806697[47] = 0; + out_1250378920670806697[48] = 0; + out_1250378920670806697[49] = 0; + out_1250378920670806697[50] = 0; + out_1250378920670806697[51] = 0; + out_1250378920670806697[52] = 0; + out_1250378920670806697[53] = 0; + out_1250378920670806697[54] = 0; + out_1250378920670806697[55] = 0; + out_1250378920670806697[56] = 0; + out_1250378920670806697[57] = 1; + out_1250378920670806697[58] = 0; + out_1250378920670806697[59] = 0; + out_1250378920670806697[60] = 0; + out_1250378920670806697[61] = 0; + out_1250378920670806697[62] = 0; + out_1250378920670806697[63] = 0; + out_1250378920670806697[64] = 0; + out_1250378920670806697[65] = 0; + out_1250378920670806697[66] = dt; + out_1250378920670806697[67] = 0; + out_1250378920670806697[68] = 0; + out_1250378920670806697[69] = 0; + out_1250378920670806697[70] = 0; + out_1250378920670806697[71] = 0; + out_1250378920670806697[72] = 0; + out_1250378920670806697[73] = 0; + out_1250378920670806697[74] = 0; + out_1250378920670806697[75] = 0; + out_1250378920670806697[76] = 1; + out_1250378920670806697[77] = 0; + out_1250378920670806697[78] = 0; + out_1250378920670806697[79] = 0; + out_1250378920670806697[80] = 0; + out_1250378920670806697[81] = 0; + out_1250378920670806697[82] = 0; + out_1250378920670806697[83] = 0; + out_1250378920670806697[84] = 0; + out_1250378920670806697[85] = dt; + out_1250378920670806697[86] = 0; + out_1250378920670806697[87] = 0; + out_1250378920670806697[88] = 0; + out_1250378920670806697[89] = 0; + out_1250378920670806697[90] = 0; + out_1250378920670806697[91] = 0; + out_1250378920670806697[92] = 0; + out_1250378920670806697[93] = 0; + out_1250378920670806697[94] = 0; + out_1250378920670806697[95] = 1; + out_1250378920670806697[96] = 0; + out_1250378920670806697[97] = 0; + out_1250378920670806697[98] = 0; + out_1250378920670806697[99] = 0; + out_1250378920670806697[100] = 0; + out_1250378920670806697[101] = 0; + out_1250378920670806697[102] = 0; + out_1250378920670806697[103] = 0; + out_1250378920670806697[104] = dt; + out_1250378920670806697[105] = 0; + out_1250378920670806697[106] = 0; + out_1250378920670806697[107] = 0; + out_1250378920670806697[108] = 0; + out_1250378920670806697[109] = 0; + out_1250378920670806697[110] = 0; + out_1250378920670806697[111] = 0; + out_1250378920670806697[112] = 0; + out_1250378920670806697[113] = 0; + out_1250378920670806697[114] = 1; + out_1250378920670806697[115] = 0; + out_1250378920670806697[116] = 0; + out_1250378920670806697[117] = 0; + out_1250378920670806697[118] = 0; + out_1250378920670806697[119] = 0; + out_1250378920670806697[120] = 0; + out_1250378920670806697[121] = 0; + out_1250378920670806697[122] = 0; + out_1250378920670806697[123] = 0; + out_1250378920670806697[124] = 0; + out_1250378920670806697[125] = 0; + out_1250378920670806697[126] = 0; + out_1250378920670806697[127] = 0; + out_1250378920670806697[128] = 0; + out_1250378920670806697[129] = 0; + out_1250378920670806697[130] = 0; + out_1250378920670806697[131] = 0; + out_1250378920670806697[132] = 0; + out_1250378920670806697[133] = 1; + out_1250378920670806697[134] = 0; + out_1250378920670806697[135] = 0; + out_1250378920670806697[136] = 0; + out_1250378920670806697[137] = 0; + out_1250378920670806697[138] = 0; + out_1250378920670806697[139] = 0; + out_1250378920670806697[140] = 0; + out_1250378920670806697[141] = 0; + out_1250378920670806697[142] = 0; + out_1250378920670806697[143] = 0; + out_1250378920670806697[144] = 0; + out_1250378920670806697[145] = 0; + out_1250378920670806697[146] = 0; + out_1250378920670806697[147] = 0; + out_1250378920670806697[148] = 0; + out_1250378920670806697[149] = 0; + out_1250378920670806697[150] = 0; + out_1250378920670806697[151] = 0; + out_1250378920670806697[152] = 1; + out_1250378920670806697[153] = 0; + out_1250378920670806697[154] = 0; + out_1250378920670806697[155] = 0; + out_1250378920670806697[156] = 0; + out_1250378920670806697[157] = 0; + out_1250378920670806697[158] = 0; + out_1250378920670806697[159] = 0; + out_1250378920670806697[160] = 0; + out_1250378920670806697[161] = 0; + out_1250378920670806697[162] = 0; + out_1250378920670806697[163] = 0; + out_1250378920670806697[164] = 0; + out_1250378920670806697[165] = 0; + out_1250378920670806697[166] = 0; + out_1250378920670806697[167] = 0; + out_1250378920670806697[168] = 0; + out_1250378920670806697[169] = 0; + out_1250378920670806697[170] = 0; + out_1250378920670806697[171] = 1; + out_1250378920670806697[172] = 0; + out_1250378920670806697[173] = 0; + out_1250378920670806697[174] = 0; + out_1250378920670806697[175] = 0; + out_1250378920670806697[176] = 0; + out_1250378920670806697[177] = 0; + out_1250378920670806697[178] = 0; + out_1250378920670806697[179] = 0; + out_1250378920670806697[180] = 0; + out_1250378920670806697[181] = 0; + out_1250378920670806697[182] = 0; + out_1250378920670806697[183] = 0; + out_1250378920670806697[184] = 0; + out_1250378920670806697[185] = 0; + out_1250378920670806697[186] = 0; + out_1250378920670806697[187] = 0; + out_1250378920670806697[188] = 0; + out_1250378920670806697[189] = 0; + out_1250378920670806697[190] = 1; + out_1250378920670806697[191] = 0; + out_1250378920670806697[192] = 0; + out_1250378920670806697[193] = 0; + out_1250378920670806697[194] = 0; + out_1250378920670806697[195] = 0; + out_1250378920670806697[196] = 0; + out_1250378920670806697[197] = 0; + out_1250378920670806697[198] = 0; + out_1250378920670806697[199] = 0; + out_1250378920670806697[200] = 0; + out_1250378920670806697[201] = 0; + out_1250378920670806697[202] = 0; + out_1250378920670806697[203] = 0; + out_1250378920670806697[204] = 0; + out_1250378920670806697[205] = 0; + out_1250378920670806697[206] = 0; + out_1250378920670806697[207] = 0; + out_1250378920670806697[208] = 0; + out_1250378920670806697[209] = 1; + out_1250378920670806697[210] = 0; + out_1250378920670806697[211] = 0; + out_1250378920670806697[212] = 0; + out_1250378920670806697[213] = 0; + out_1250378920670806697[214] = 0; + out_1250378920670806697[215] = 0; + out_1250378920670806697[216] = 0; + out_1250378920670806697[217] = 0; + out_1250378920670806697[218] = 0; + out_1250378920670806697[219] = 0; + out_1250378920670806697[220] = 0; + out_1250378920670806697[221] = 0; + out_1250378920670806697[222] = 0; + out_1250378920670806697[223] = 0; + out_1250378920670806697[224] = 0; + out_1250378920670806697[225] = 0; + out_1250378920670806697[226] = 0; + out_1250378920670806697[227] = 0; + out_1250378920670806697[228] = 1; + out_1250378920670806697[229] = 0; + out_1250378920670806697[230] = 0; + out_1250378920670806697[231] = 0; + out_1250378920670806697[232] = 0; + out_1250378920670806697[233] = 0; + out_1250378920670806697[234] = 0; + out_1250378920670806697[235] = 0; + out_1250378920670806697[236] = 0; + out_1250378920670806697[237] = 0; + out_1250378920670806697[238] = 0; + out_1250378920670806697[239] = 0; + out_1250378920670806697[240] = 0; + out_1250378920670806697[241] = 0; + out_1250378920670806697[242] = 0; + out_1250378920670806697[243] = 0; + out_1250378920670806697[244] = 0; + out_1250378920670806697[245] = 0; + out_1250378920670806697[246] = 0; + out_1250378920670806697[247] = 1; + out_1250378920670806697[248] = 0; + out_1250378920670806697[249] = 0; + out_1250378920670806697[250] = 0; + out_1250378920670806697[251] = 0; + out_1250378920670806697[252] = 0; + out_1250378920670806697[253] = 0; + out_1250378920670806697[254] = 0; + out_1250378920670806697[255] = 0; + out_1250378920670806697[256] = 0; + out_1250378920670806697[257] = 0; + out_1250378920670806697[258] = 0; + out_1250378920670806697[259] = 0; + out_1250378920670806697[260] = 0; + out_1250378920670806697[261] = 0; + out_1250378920670806697[262] = 0; + out_1250378920670806697[263] = 0; + out_1250378920670806697[264] = 0; + out_1250378920670806697[265] = 0; + out_1250378920670806697[266] = 1; + out_1250378920670806697[267] = 0; + out_1250378920670806697[268] = 0; + out_1250378920670806697[269] = 0; + out_1250378920670806697[270] = 0; + out_1250378920670806697[271] = 0; + out_1250378920670806697[272] = 0; + out_1250378920670806697[273] = 0; + out_1250378920670806697[274] = 0; + out_1250378920670806697[275] = 0; + out_1250378920670806697[276] = 0; + out_1250378920670806697[277] = 0; + out_1250378920670806697[278] = 0; + out_1250378920670806697[279] = 0; + out_1250378920670806697[280] = 0; + out_1250378920670806697[281] = 0; + out_1250378920670806697[282] = 0; + out_1250378920670806697[283] = 0; + out_1250378920670806697[284] = 0; + out_1250378920670806697[285] = 1; + out_1250378920670806697[286] = 0; + out_1250378920670806697[287] = 0; + out_1250378920670806697[288] = 0; + out_1250378920670806697[289] = 0; + out_1250378920670806697[290] = 0; + out_1250378920670806697[291] = 0; + out_1250378920670806697[292] = 0; + out_1250378920670806697[293] = 0; + out_1250378920670806697[294] = 0; + out_1250378920670806697[295] = 0; + out_1250378920670806697[296] = 0; + out_1250378920670806697[297] = 0; + out_1250378920670806697[298] = 0; + out_1250378920670806697[299] = 0; + out_1250378920670806697[300] = 0; + out_1250378920670806697[301] = 0; + out_1250378920670806697[302] = 0; + out_1250378920670806697[303] = 0; + out_1250378920670806697[304] = 1; + out_1250378920670806697[305] = 0; + out_1250378920670806697[306] = 0; + out_1250378920670806697[307] = 0; + out_1250378920670806697[308] = 0; + out_1250378920670806697[309] = 0; + out_1250378920670806697[310] = 0; + out_1250378920670806697[311] = 0; + out_1250378920670806697[312] = 0; + out_1250378920670806697[313] = 0; + out_1250378920670806697[314] = 0; + out_1250378920670806697[315] = 0; + out_1250378920670806697[316] = 0; + out_1250378920670806697[317] = 0; + out_1250378920670806697[318] = 0; + out_1250378920670806697[319] = 0; + out_1250378920670806697[320] = 0; + out_1250378920670806697[321] = 0; + out_1250378920670806697[322] = 0; + out_1250378920670806697[323] = 1; } -void h_4(double *state, double *unused, double *out_1468084297579901706) { - out_1468084297579901706[0] = state[6] + state[9]; - out_1468084297579901706[1] = state[7] + state[10]; - out_1468084297579901706[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_3431796064752050994) { + out_3431796064752050994[0] = state[6] + state[9]; + out_3431796064752050994[1] = state[7] + state[10]; + out_3431796064752050994[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_552787102095393061) { - out_552787102095393061[0] = 0; - out_552787102095393061[1] = 0; - out_552787102095393061[2] = 0; - out_552787102095393061[3] = 0; - out_552787102095393061[4] = 0; - out_552787102095393061[5] = 0; - out_552787102095393061[6] = 1; - out_552787102095393061[7] = 0; - out_552787102095393061[8] = 0; - out_552787102095393061[9] = 1; - out_552787102095393061[10] = 0; - out_552787102095393061[11] = 0; - out_552787102095393061[12] = 0; - out_552787102095393061[13] = 0; - out_552787102095393061[14] = 0; - out_552787102095393061[15] = 0; - out_552787102095393061[16] = 0; - out_552787102095393061[17] = 0; - out_552787102095393061[18] = 0; - out_552787102095393061[19] = 0; - out_552787102095393061[20] = 0; - out_552787102095393061[21] = 0; - out_552787102095393061[22] = 0; - out_552787102095393061[23] = 0; - out_552787102095393061[24] = 0; - out_552787102095393061[25] = 1; - out_552787102095393061[26] = 0; - out_552787102095393061[27] = 0; - out_552787102095393061[28] = 1; - out_552787102095393061[29] = 0; - out_552787102095393061[30] = 0; - out_552787102095393061[31] = 0; - out_552787102095393061[32] = 0; - out_552787102095393061[33] = 0; - out_552787102095393061[34] = 0; - out_552787102095393061[35] = 0; - out_552787102095393061[36] = 0; - out_552787102095393061[37] = 0; - out_552787102095393061[38] = 0; - out_552787102095393061[39] = 0; - out_552787102095393061[40] = 0; - out_552787102095393061[41] = 0; - out_552787102095393061[42] = 0; - out_552787102095393061[43] = 0; - out_552787102095393061[44] = 1; - out_552787102095393061[45] = 0; - out_552787102095393061[46] = 0; - out_552787102095393061[47] = 1; - out_552787102095393061[48] = 0; - out_552787102095393061[49] = 0; - out_552787102095393061[50] = 0; - out_552787102095393061[51] = 0; - out_552787102095393061[52] = 0; - out_552787102095393061[53] = 0; +void H_4(double *state, double *unused, double *out_1019746773741649270) { + out_1019746773741649270[0] = 0; + out_1019746773741649270[1] = 0; + out_1019746773741649270[2] = 0; + out_1019746773741649270[3] = 0; + out_1019746773741649270[4] = 0; + out_1019746773741649270[5] = 0; + out_1019746773741649270[6] = 1; + out_1019746773741649270[7] = 0; + out_1019746773741649270[8] = 0; + out_1019746773741649270[9] = 1; + out_1019746773741649270[10] = 0; + out_1019746773741649270[11] = 0; + out_1019746773741649270[12] = 0; + out_1019746773741649270[13] = 0; + out_1019746773741649270[14] = 0; + out_1019746773741649270[15] = 0; + out_1019746773741649270[16] = 0; + out_1019746773741649270[17] = 0; + out_1019746773741649270[18] = 0; + out_1019746773741649270[19] = 0; + out_1019746773741649270[20] = 0; + out_1019746773741649270[21] = 0; + out_1019746773741649270[22] = 0; + out_1019746773741649270[23] = 0; + out_1019746773741649270[24] = 0; + out_1019746773741649270[25] = 1; + out_1019746773741649270[26] = 0; + out_1019746773741649270[27] = 0; + out_1019746773741649270[28] = 1; + out_1019746773741649270[29] = 0; + out_1019746773741649270[30] = 0; + out_1019746773741649270[31] = 0; + out_1019746773741649270[32] = 0; + out_1019746773741649270[33] = 0; + out_1019746773741649270[34] = 0; + out_1019746773741649270[35] = 0; + out_1019746773741649270[36] = 0; + out_1019746773741649270[37] = 0; + out_1019746773741649270[38] = 0; + out_1019746773741649270[39] = 0; + out_1019746773741649270[40] = 0; + out_1019746773741649270[41] = 0; + out_1019746773741649270[42] = 0; + out_1019746773741649270[43] = 0; + out_1019746773741649270[44] = 1; + out_1019746773741649270[45] = 0; + out_1019746773741649270[46] = 0; + out_1019746773741649270[47] = 1; + out_1019746773741649270[48] = 0; + out_1019746773741649270[49] = 0; + out_1019746773741649270[50] = 0; + out_1019746773741649270[51] = 0; + out_1019746773741649270[52] = 0; + out_1019746773741649270[53] = 0; } -void h_10(double *state, double *unused, double *out_1051779536617211070) { - out_1051779536617211070[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_1051779536617211070[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_1051779536617211070[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_4742817048768310237) { + out_4742817048768310237[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_4742817048768310237[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_4742817048768310237[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_7440841037325716699) { - out_7440841037325716699[0] = 0; - out_7440841037325716699[1] = 9.8100000000000005*cos(state[1]); - out_7440841037325716699[2] = 0; - out_7440841037325716699[3] = 0; - out_7440841037325716699[4] = -state[8]; - out_7440841037325716699[5] = state[7]; - out_7440841037325716699[6] = 0; - out_7440841037325716699[7] = state[5]; - out_7440841037325716699[8] = -state[4]; - out_7440841037325716699[9] = 0; - out_7440841037325716699[10] = 0; - out_7440841037325716699[11] = 0; - out_7440841037325716699[12] = 1; - out_7440841037325716699[13] = 0; - out_7440841037325716699[14] = 0; - out_7440841037325716699[15] = 1; - out_7440841037325716699[16] = 0; - out_7440841037325716699[17] = 0; - out_7440841037325716699[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_7440841037325716699[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_7440841037325716699[20] = 0; - out_7440841037325716699[21] = state[8]; - out_7440841037325716699[22] = 0; - out_7440841037325716699[23] = -state[6]; - out_7440841037325716699[24] = -state[5]; - out_7440841037325716699[25] = 0; - out_7440841037325716699[26] = state[3]; - out_7440841037325716699[27] = 0; - out_7440841037325716699[28] = 0; - out_7440841037325716699[29] = 0; - out_7440841037325716699[30] = 0; - out_7440841037325716699[31] = 1; - out_7440841037325716699[32] = 0; - out_7440841037325716699[33] = 0; - out_7440841037325716699[34] = 1; - out_7440841037325716699[35] = 0; - out_7440841037325716699[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_7440841037325716699[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_7440841037325716699[38] = 0; - out_7440841037325716699[39] = -state[7]; - out_7440841037325716699[40] = state[6]; - out_7440841037325716699[41] = 0; - out_7440841037325716699[42] = state[4]; - out_7440841037325716699[43] = -state[3]; - out_7440841037325716699[44] = 0; - out_7440841037325716699[45] = 0; - out_7440841037325716699[46] = 0; - out_7440841037325716699[47] = 0; - out_7440841037325716699[48] = 0; - out_7440841037325716699[49] = 0; - out_7440841037325716699[50] = 1; - out_7440841037325716699[51] = 0; - out_7440841037325716699[52] = 0; - out_7440841037325716699[53] = 1; +void H_10(double *state, double *unused, double *out_7482829619336622044) { + out_7482829619336622044[0] = 0; + out_7482829619336622044[1] = 9.8100000000000005*cos(state[1]); + out_7482829619336622044[2] = 0; + out_7482829619336622044[3] = 0; + out_7482829619336622044[4] = -state[8]; + out_7482829619336622044[5] = state[7]; + out_7482829619336622044[6] = 0; + out_7482829619336622044[7] = state[5]; + out_7482829619336622044[8] = -state[4]; + out_7482829619336622044[9] = 0; + out_7482829619336622044[10] = 0; + out_7482829619336622044[11] = 0; + out_7482829619336622044[12] = 1; + out_7482829619336622044[13] = 0; + out_7482829619336622044[14] = 0; + out_7482829619336622044[15] = 1; + out_7482829619336622044[16] = 0; + out_7482829619336622044[17] = 0; + out_7482829619336622044[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_7482829619336622044[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_7482829619336622044[20] = 0; + out_7482829619336622044[21] = state[8]; + out_7482829619336622044[22] = 0; + out_7482829619336622044[23] = -state[6]; + out_7482829619336622044[24] = -state[5]; + out_7482829619336622044[25] = 0; + out_7482829619336622044[26] = state[3]; + out_7482829619336622044[27] = 0; + out_7482829619336622044[28] = 0; + out_7482829619336622044[29] = 0; + out_7482829619336622044[30] = 0; + out_7482829619336622044[31] = 1; + out_7482829619336622044[32] = 0; + out_7482829619336622044[33] = 0; + out_7482829619336622044[34] = 1; + out_7482829619336622044[35] = 0; + out_7482829619336622044[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_7482829619336622044[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_7482829619336622044[38] = 0; + out_7482829619336622044[39] = -state[7]; + out_7482829619336622044[40] = state[6]; + out_7482829619336622044[41] = 0; + out_7482829619336622044[42] = state[4]; + out_7482829619336622044[43] = -state[3]; + out_7482829619336622044[44] = 0; + out_7482829619336622044[45] = 0; + out_7482829619336622044[46] = 0; + out_7482829619336622044[47] = 0; + out_7482829619336622044[48] = 0; + out_7482829619336622044[49] = 0; + out_7482829619336622044[50] = 1; + out_7482829619336622044[51] = 0; + out_7482829619336622044[52] = 0; + out_7482829619336622044[53] = 1; } -void h_13(double *state, double *unused, double *out_4266757043601180240) { - out_4266757043601180240[0] = state[3]; - out_4266757043601180240[1] = state[4]; - out_4266757043601180240[2] = state[5]; +void h_13(double *state, double *unused, double *out_3124060736866225144) { + out_3124060736866225144[0] = state[3]; + out_3124060736866225144[1] = state[4]; + out_3124060736866225144[2] = state[5]; } -void H_13(double *state, double *unused, double *out_3765060927427725862) { - out_3765060927427725862[0] = 0; - out_3765060927427725862[1] = 0; - out_3765060927427725862[2] = 0; - out_3765060927427725862[3] = 1; - out_3765060927427725862[4] = 0; - out_3765060927427725862[5] = 0; - out_3765060927427725862[6] = 0; - out_3765060927427725862[7] = 0; - out_3765060927427725862[8] = 0; - out_3765060927427725862[9] = 0; - out_3765060927427725862[10] = 0; - out_3765060927427725862[11] = 0; - out_3765060927427725862[12] = 0; - out_3765060927427725862[13] = 0; - out_3765060927427725862[14] = 0; - out_3765060927427725862[15] = 0; - out_3765060927427725862[16] = 0; - out_3765060927427725862[17] = 0; - out_3765060927427725862[18] = 0; - out_3765060927427725862[19] = 0; - out_3765060927427725862[20] = 0; - out_3765060927427725862[21] = 0; - out_3765060927427725862[22] = 1; - out_3765060927427725862[23] = 0; - out_3765060927427725862[24] = 0; - out_3765060927427725862[25] = 0; - out_3765060927427725862[26] = 0; - out_3765060927427725862[27] = 0; - out_3765060927427725862[28] = 0; - out_3765060927427725862[29] = 0; - out_3765060927427725862[30] = 0; - out_3765060927427725862[31] = 0; - out_3765060927427725862[32] = 0; - out_3765060927427725862[33] = 0; - out_3765060927427725862[34] = 0; - out_3765060927427725862[35] = 0; - out_3765060927427725862[36] = 0; - out_3765060927427725862[37] = 0; - out_3765060927427725862[38] = 0; - out_3765060927427725862[39] = 0; - out_3765060927427725862[40] = 0; - out_3765060927427725862[41] = 1; - out_3765060927427725862[42] = 0; - out_3765060927427725862[43] = 0; - out_3765060927427725862[44] = 0; - out_3765060927427725862[45] = 0; - out_3765060927427725862[46] = 0; - out_3765060927427725862[47] = 0; - out_3765060927427725862[48] = 0; - out_3765060927427725862[49] = 0; - out_3765060927427725862[50] = 0; - out_3765060927427725862[51] = 0; - out_3765060927427725862[52] = 0; - out_3765060927427725862[53] = 0; +void H_13(double *state, double *unused, double *out_2192527051590683531) { + out_2192527051590683531[0] = 0; + out_2192527051590683531[1] = 0; + out_2192527051590683531[2] = 0; + out_2192527051590683531[3] = 1; + out_2192527051590683531[4] = 0; + out_2192527051590683531[5] = 0; + out_2192527051590683531[6] = 0; + out_2192527051590683531[7] = 0; + out_2192527051590683531[8] = 0; + out_2192527051590683531[9] = 0; + out_2192527051590683531[10] = 0; + out_2192527051590683531[11] = 0; + out_2192527051590683531[12] = 0; + out_2192527051590683531[13] = 0; + out_2192527051590683531[14] = 0; + out_2192527051590683531[15] = 0; + out_2192527051590683531[16] = 0; + out_2192527051590683531[17] = 0; + out_2192527051590683531[18] = 0; + out_2192527051590683531[19] = 0; + out_2192527051590683531[20] = 0; + out_2192527051590683531[21] = 0; + out_2192527051590683531[22] = 1; + out_2192527051590683531[23] = 0; + out_2192527051590683531[24] = 0; + out_2192527051590683531[25] = 0; + out_2192527051590683531[26] = 0; + out_2192527051590683531[27] = 0; + out_2192527051590683531[28] = 0; + out_2192527051590683531[29] = 0; + out_2192527051590683531[30] = 0; + out_2192527051590683531[31] = 0; + out_2192527051590683531[32] = 0; + out_2192527051590683531[33] = 0; + out_2192527051590683531[34] = 0; + out_2192527051590683531[35] = 0; + out_2192527051590683531[36] = 0; + out_2192527051590683531[37] = 0; + out_2192527051590683531[38] = 0; + out_2192527051590683531[39] = 0; + out_2192527051590683531[40] = 0; + out_2192527051590683531[41] = 1; + out_2192527051590683531[42] = 0; + out_2192527051590683531[43] = 0; + out_2192527051590683531[44] = 0; + out_2192527051590683531[45] = 0; + out_2192527051590683531[46] = 0; + out_2192527051590683531[47] = 0; + out_2192527051590683531[48] = 0; + out_2192527051590683531[49] = 0; + out_2192527051590683531[50] = 0; + out_2192527051590683531[51] = 0; + out_2192527051590683531[52] = 0; + out_2192527051590683531[53] = 0; } -void h_14(double *state, double *unused, double *out_1847032621455587499) { - out_1847032621455587499[0] = state[6]; - out_1847032621455587499[1] = state[7]; - out_1847032621455587499[2] = state[8]; +void h_14(double *state, double *unused, double *out_3873502266244196496) { + out_3873502266244196496[0] = state[6]; + out_3873502266244196496[1] = state[7]; + out_3873502266244196496[2] = state[8]; } -void H_14(double *state, double *unused, double *out_117670575450509462) { - out_117670575450509462[0] = 0; - out_117670575450509462[1] = 0; - out_117670575450509462[2] = 0; - out_117670575450509462[3] = 0; - out_117670575450509462[4] = 0; - out_117670575450509462[5] = 0; - out_117670575450509462[6] = 1; - out_117670575450509462[7] = 0; - out_117670575450509462[8] = 0; - out_117670575450509462[9] = 0; - out_117670575450509462[10] = 0; - out_117670575450509462[11] = 0; - out_117670575450509462[12] = 0; - out_117670575450509462[13] = 0; - out_117670575450509462[14] = 0; - out_117670575450509462[15] = 0; - out_117670575450509462[16] = 0; - out_117670575450509462[17] = 0; - out_117670575450509462[18] = 0; - out_117670575450509462[19] = 0; - out_117670575450509462[20] = 0; - out_117670575450509462[21] = 0; - out_117670575450509462[22] = 0; - out_117670575450509462[23] = 0; - out_117670575450509462[24] = 0; - out_117670575450509462[25] = 1; - out_117670575450509462[26] = 0; - out_117670575450509462[27] = 0; - out_117670575450509462[28] = 0; - out_117670575450509462[29] = 0; - out_117670575450509462[30] = 0; - out_117670575450509462[31] = 0; - out_117670575450509462[32] = 0; - out_117670575450509462[33] = 0; - out_117670575450509462[34] = 0; - out_117670575450509462[35] = 0; - out_117670575450509462[36] = 0; - out_117670575450509462[37] = 0; - out_117670575450509462[38] = 0; - out_117670575450509462[39] = 0; - out_117670575450509462[40] = 0; - out_117670575450509462[41] = 0; - out_117670575450509462[42] = 0; - out_117670575450509462[43] = 0; - out_117670575450509462[44] = 1; - out_117670575450509462[45] = 0; - out_117670575450509462[46] = 0; - out_117670575450509462[47] = 0; - out_117670575450509462[48] = 0; - out_117670575450509462[49] = 0; - out_117670575450509462[50] = 0; - out_117670575450509462[51] = 0; - out_117670575450509462[52] = 0; - out_117670575450509462[53] = 0; +void H_14(double *state, double *unused, double *out_2943494082597835259) { + out_2943494082597835259[0] = 0; + out_2943494082597835259[1] = 0; + out_2943494082597835259[2] = 0; + out_2943494082597835259[3] = 0; + out_2943494082597835259[4] = 0; + out_2943494082597835259[5] = 0; + out_2943494082597835259[6] = 1; + out_2943494082597835259[7] = 0; + out_2943494082597835259[8] = 0; + out_2943494082597835259[9] = 0; + out_2943494082597835259[10] = 0; + out_2943494082597835259[11] = 0; + out_2943494082597835259[12] = 0; + out_2943494082597835259[13] = 0; + out_2943494082597835259[14] = 0; + out_2943494082597835259[15] = 0; + out_2943494082597835259[16] = 0; + out_2943494082597835259[17] = 0; + out_2943494082597835259[18] = 0; + out_2943494082597835259[19] = 0; + out_2943494082597835259[20] = 0; + out_2943494082597835259[21] = 0; + out_2943494082597835259[22] = 0; + out_2943494082597835259[23] = 0; + out_2943494082597835259[24] = 0; + out_2943494082597835259[25] = 1; + out_2943494082597835259[26] = 0; + out_2943494082597835259[27] = 0; + out_2943494082597835259[28] = 0; + out_2943494082597835259[29] = 0; + out_2943494082597835259[30] = 0; + out_2943494082597835259[31] = 0; + out_2943494082597835259[32] = 0; + out_2943494082597835259[33] = 0; + out_2943494082597835259[34] = 0; + out_2943494082597835259[35] = 0; + out_2943494082597835259[36] = 0; + out_2943494082597835259[37] = 0; + out_2943494082597835259[38] = 0; + out_2943494082597835259[39] = 0; + out_2943494082597835259[40] = 0; + out_2943494082597835259[41] = 0; + out_2943494082597835259[42] = 0; + out_2943494082597835259[43] = 0; + out_2943494082597835259[44] = 1; + out_2943494082597835259[45] = 0; + out_2943494082597835259[46] = 0; + out_2943494082597835259[47] = 0; + out_2943494082597835259[48] = 0; + out_2943494082597835259[49] = 0; + out_2943494082597835259[50] = 0; + out_2943494082597835259[51] = 0; + out_2943494082597835259[52] = 0; + out_2943494082597835259[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_5867685499485526905) { - err_fun(nom_x, delta_x, out_5867685499485526905); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7754318151462489406) { + err_fun(nom_x, delta_x, out_7754318151462489406); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7496427572072330735) { - inv_err_fun(nom_x, true_x, out_7496427572072330735); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5172808168415891616) { + inv_err_fun(nom_x, true_x, out_5172808168415891616); } -void pose_H_mod_fun(double *state, double *out_4599731282943687674) { - H_mod_fun(state, out_4599731282943687674); +void pose_H_mod_fun(double *state, double *out_2254475305049299983) { + H_mod_fun(state, out_2254475305049299983); } -void pose_f_fun(double *state, double dt, double *out_8448766243959956852) { - f_fun(state, dt, out_8448766243959956852); +void pose_f_fun(double *state, double dt, double *out_9187930556967356165) { + f_fun(state, dt, out_9187930556967356165); } -void pose_F_fun(double *state, double dt, double *out_3524034210859456461) { - F_fun(state, dt, out_3524034210859456461); +void pose_F_fun(double *state, double dt, double *out_1250378920670806697) { + F_fun(state, dt, out_1250378920670806697); } -void pose_h_4(double *state, double *unused, double *out_1468084297579901706) { - h_4(state, unused, out_1468084297579901706); +void pose_h_4(double *state, double *unused, double *out_3431796064752050994) { + h_4(state, unused, out_3431796064752050994); } -void pose_H_4(double *state, double *unused, double *out_552787102095393061) { - H_4(state, unused, out_552787102095393061); +void pose_H_4(double *state, double *unused, double *out_1019746773741649270) { + H_4(state, unused, out_1019746773741649270); } -void pose_h_10(double *state, double *unused, double *out_1051779536617211070) { - h_10(state, unused, out_1051779536617211070); +void pose_h_10(double *state, double *unused, double *out_4742817048768310237) { + h_10(state, unused, out_4742817048768310237); } -void pose_H_10(double *state, double *unused, double *out_7440841037325716699) { - H_10(state, unused, out_7440841037325716699); +void pose_H_10(double *state, double *unused, double *out_7482829619336622044) { + H_10(state, unused, out_7482829619336622044); } -void pose_h_13(double *state, double *unused, double *out_4266757043601180240) { - h_13(state, unused, out_4266757043601180240); +void pose_h_13(double *state, double *unused, double *out_3124060736866225144) { + h_13(state, unused, out_3124060736866225144); } -void pose_H_13(double *state, double *unused, double *out_3765060927427725862) { - H_13(state, unused, out_3765060927427725862); +void pose_H_13(double *state, double *unused, double *out_2192527051590683531) { + H_13(state, unused, out_2192527051590683531); } -void pose_h_14(double *state, double *unused, double *out_1847032621455587499) { - h_14(state, unused, out_1847032621455587499); +void pose_h_14(double *state, double *unused, double *out_3873502266244196496) { + h_14(state, unused, out_3873502266244196496); } -void pose_H_14(double *state, double *unused, double *out_117670575450509462) { - H_14(state, unused, out_117670575450509462); +void pose_H_14(double *state, double *unused, double *out_2943494082597835259) { + H_14(state, unused, out_2943494082597835259); } void pose_predict(double *in_x, double *in_P, double *in_Q, double dt) { predict(in_x, in_P, in_Q, dt); diff --git a/selfdrive/locationd/models/generated/pose.h b/selfdrive/locationd/models/generated/pose.h index e962a9486..a9ed46fb8 100644 --- a/selfdrive/locationd/models/generated/pose.h +++ b/selfdrive/locationd/models/generated/pose.h @@ -5,18 +5,18 @@ void pose_update_4(double *in_x, double *in_P, double *in_z, double *in_R, doubl void pose_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); -void pose_err_fun(double *nom_x, double *delta_x, double *out_5867685499485526905); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7496427572072330735); -void pose_H_mod_fun(double *state, double *out_4599731282943687674); -void pose_f_fun(double *state, double dt, double *out_8448766243959956852); -void pose_F_fun(double *state, double dt, double *out_3524034210859456461); -void pose_h_4(double *state, double *unused, double *out_1468084297579901706); -void pose_H_4(double *state, double *unused, double *out_552787102095393061); -void pose_h_10(double *state, double *unused, double *out_1051779536617211070); -void pose_H_10(double *state, double *unused, double *out_7440841037325716699); -void pose_h_13(double *state, double *unused, double *out_4266757043601180240); -void pose_H_13(double *state, double *unused, double *out_3765060927427725862); -void pose_h_14(double *state, double *unused, double *out_1847032621455587499); -void pose_H_14(double *state, double *unused, double *out_117670575450509462); +void pose_err_fun(double *nom_x, double *delta_x, double *out_7754318151462489406); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5172808168415891616); +void pose_H_mod_fun(double *state, double *out_2254475305049299983); +void pose_f_fun(double *state, double dt, double *out_9187930556967356165); +void pose_F_fun(double *state, double dt, double *out_1250378920670806697); +void pose_h_4(double *state, double *unused, double *out_3431796064752050994); +void pose_H_4(double *state, double *unused, double *out_1019746773741649270); +void pose_h_10(double *state, double *unused, double *out_4742817048768310237); +void pose_H_10(double *state, double *unused, double *out_7482829619336622044); +void pose_h_13(double *state, double *unused, double *out_3124060736866225144); +void pose_H_13(double *state, double *unused, double *out_2192527051590683531); +void pose_h_14(double *state, double *unused, double *out_3873502266244196496); +void pose_H_14(double *state, double *unused, double *out_2943494082597835259); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 688a40d82..63f3592f3 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -5,6 +5,7 @@ from functools import cached_property import json import os import struct +import usb1 from openpilot.system.hardware import HARDWARE, TICI os.environ['GMMU'] = '0' os.environ['DEV'] = 'QCOM' if TICI else 'LLVM' @@ -50,6 +51,7 @@ from openpilot.selfdrive.modeld.compile_modeld import ( ) from openpilot.selfdrive.modeld.helpers import get_tg_input_devices, load_oob, tinygrad_dev_config, usbgpu_present from openpilot.selfdrive.modeld.usbgpu_link import wait_usbgpu_link +from openpilot.system.hardware.usb import CHESTNUT_USB_IDS from openpilot.starpilot.assets.model_manager import ( ModelManager, get_model_profile, @@ -212,6 +214,38 @@ class ChestnutState: self.valid = True self.sends = 0 self.metrics = {} + self._asm_usb = None + + def _close_asm_usb(self) -> None: + if self._asm_usb is not None: + self._asm_usb.close() + self._asm_usb = None + + def _open_asm_usb(self): + context = usb1.USBContext() + for vendor_id, product_id in CHESTNUT_USB_IDS: + handle = context.openByVendorIDAndProductID(vendor_id, product_id, skip_on_error=True) + if handle is not None: + return handle + context.close() + + def _read_ina(self) -> tuple[int, int, bool]: + if "AMD" in Device._opened_devices and self._asm_usb is None: + try: + raw = Device["AMD"].iface.pci_dev.usb.usb.control_read(0xC0, 5) + return struct.unpack(" int: @@ -253,12 +287,14 @@ class ChestnutState: setattr(state, key, value) asm_valid = False + try: + state.supplyVoltage, state.supplyCurrent, state.supplyFault = self._read_ina() + asm_valid = True + except Exception: + pass if "AMD" in Device._opened_devices: try: - asm = Device["AMD"].iface.pci_dev.usb - state.pcieLtssm = asm.read(0xB450, 1)[0] - state.supplyVoltage, state.supplyCurrent = struct.unpack(" None: + detected = [d for d in usb_state if is_chestnut_usb_id(d["vendorId"], d["productId"], include_bootloader=True)] + devices = [d for d in detected if is_chestnut_usb_id(d["vendorId"], d["productId"])] + firmware_ok = len(devices) == 1 and devices[0]["product"] == CHESTNUT_USB_PRODUCT + + if self.offroad and not offroad: + self.pcie_failed = False + self.power_seen = False + self.power_unavailable = False + self.power_lost = False + self.power_restored = False + self.link_failures = 0 + self.model_loading_seen = False + self.model_attempted = False + self.usb_seen = firmware_ok + self.usb_failed = False + + self.model_loading_seen |= model_loading + self.model_attempted |= self.model_loading_seen and not model_loading and model_active is not None + + if expected and not offroad and self.usb_seen and not firmware_ok: + self.usb_failed = True + + power_lost = False + if expected and not offroad and state is not None: + powered = state.supplyVoltage >= CHESTNUT_POWERED_VOLTAGE + power_lost = state.supplyFault or not powered + if self.model_attempted and power_lost and not self.power_lost: + self.power_unavailable = not self.power_seen + self.power_seen |= powered + + if expected and not offroad and self.model_attempted and state is not None: + self.link_failures = self.link_failures + 1 if state.pcieLtssm != 0x78 else 0 + self.pcie_failed |= self.link_failures >= 2 or power_lost + self.power_lost |= power_lost + + if self.pcie_failed and self.power_lost and state is not None: + self.power_restored |= not state.supplyFault and state.supplyVoltage >= CHESTNUT_POWERED_VOLTAGE + if self.usb_failed: + self.pcie_failed = False + self.power_seen = False + self.power_unavailable = False + self.power_lost = False + self.power_restored = False + + if state is not None: + gpu_limit = GPU_TEMP_LIMIT - (TEMP_HYSTERESIS if self.overheated else 0.0) + memory_limit = MEMORY_TEMP_LIMIT - (TEMP_HYSTERESIS if self.overheated else 0.0) + self.overheated = state.tempC >= gpu_limit or state.memoryTempC >= memory_limit + + missing = expected and (self.usb_failed or (offroad and time.monotonic() - self.started > 10.0 and len(detected) != 1)) + slow_usb = offroad and len(devices) == 1 and devices[0]["speedMbps"] < 5000 + set_alert("Offroad_ChestnutNotDetected", missing) + set_alert("Offroad_ChestnutOverheated", self.overheated, f"{state.tempC:.0f} °C" if state is not None else None) + set_alert("Offroad_ChestnutUsbSlow", slow_usb, f"{devices[0]['speedMbps']} Mbps" if slow_usb else None) + if self.power_lost: + pcie_alert = ("Chestnut power restored. 12V is stable again, cycle ignition." if self.power_restored else + "Chestnut power disconnected. Check 12V connection, then cycle ignition." if self.power_unavailable else + "Chestnut power lost. Possibly caused by an engine-crank voltage drop. Check 12V connection, then cycle ignition.") + else: + pcie_alert = "Chestnut GPU unavailable. PCIe link is not up. Check the GPU is securely seated." + set_alert("Offroad_ChestnutPcieUnavailable", expected and self.pcie_failed, pcie_alert) + set_alert("Offroad_ChestnutUncompiled", offroad and expected and firmware_ok and not compiled) + set_alert("Offroad_ChestnutUpdateFailed", offroad and firmware_failed) + self.offroad = offroad diff --git a/system/hardware/hardwared.py b/system/hardware/hardwared.py index 9fb9be0f6..9463df11d 100644 --- a/system/hardware/hardwared.py +++ b/system/hardware/hardwared.py @@ -29,12 +29,13 @@ from openpilot.system.hardware.power_monitoring import PowerMonitoring from openpilot.system.hardware.fan_controller import TiciFanController from openpilot.system.hardware.usb import ( CHESTNUT_FW_VERSION, - CHESTNUT_ROM_USB_IDS, - CHESTNUT_USB_IDS, + CHESTNUT_USB_PRODUCT, get_usb_state, get_usb_topology, + is_chestnut_usb_id, set_usb_state, ) +from openpilot.system.hardware.chestnut.status import ChestnutStatus from openpilot.system.version import terms_version, training_version from openpilot.system.athena.registration import UNREGISTERED_DONGLE_ID @@ -118,11 +119,15 @@ class Chestnut: self.attempts = 0 self.last_attempt = 0.0 self.flashed = False + self.mismatch = False + + @property + def failed(self) -> bool: + return self.mismatch and self.attempts >= self.MAX_ATTEMPTS and self.thread is not None and not self.thread.is_alive() and not self.flashed def _firmware_mismatch(self, usb_state: list[dict]) -> bool: - expected = f"custom {CHESTNUT_FW_VERSION}-CLEAN" - ids = CHESTNUT_USB_IDS + CHESTNUT_ROM_USB_IDS - return any((device["vendorId"], device["productId"]) in ids and device["product"] != expected for device in usb_state) + return any(is_chestnut_usb_id(device["vendorId"], device["productId"], include_bootloader=True) and + device["product"] != CHESTNUT_USB_PRODUCT for device in usb_state) def _flash(self) -> None: script = os.path.join(os.path.dirname(__file__), "chestnut", "flash.py") @@ -137,7 +142,8 @@ class Chestnut: self.flashed = result.returncode == 0 def update(self, offroad: bool, usb_state: list[dict]) -> None: - if not self._firmware_mismatch(usb_state): + self.mismatch = self._firmware_mismatch(usb_state) + if not self.mismatch: self.flashed = False return if not offroad or self.flashed or self.attempts >= self.MAX_ATTEMPTS: @@ -295,7 +301,7 @@ def hw_state_thread(end_event, hw_queue): def hardware_thread(end_event, hw_queue) -> None: pm = messaging.PubMaster(['deviceState']) - sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "selfdriveState", "pandaStates"], poll="pandaStates") + sm = messaging.SubMaster(["peripheralState", "gpsLocationExternal", "selfdriveState", "pandaStates", "chestnutState"], poll="pandaStates") count = 0 @@ -337,6 +343,7 @@ def hardware_thread(end_event, hw_queue) -> None: params = Params() power_monitor = PowerMonitoring() chestnut = Chestnut() if AGNOS else None + chestnut_status = ChestnutStatus() if AGNOS else None uptime_offroad: float = params.get("UptimeOffroad", return_default=True) uptime_onroad: float = params.get("UptimeOnroad", return_default=True) @@ -420,6 +427,23 @@ def hardware_thread(end_event, hw_queue) -> None: set_usb_state(msg.deviceState, last_hw_state.usb_state) if chestnut is not None: chestnut.update(started_ts is None, last_hw_state.usb_state) + model_lab_config = params.get("ModelLabConfig") + chestnut_expected = bool(params.get("ActiveBigModel")) or ( + isinstance(model_lab_config, dict) and bool(model_lab_config.get("enabled")) + ) + chestnut_state = sm["chestnutState"] + chestnut_valid = sm.alive["chestnutState"] and sm.valid["chestnutState"] + chestnut_status.update( + started_ts is None, + chestnut_expected, + last_hw_state.usb_state, + chestnut.failed, + params.get_bool("UsbGpuLoading"), + params.get("UsbGpuActive"), + params.get_bool("UsbGpuCompiled"), + chestnut_state if chestnut_valid else None, + set_offroad_alert_if_changed, + ) # this subset is only used for offroad temp_sources = [ diff --git a/system/hardware/tests/test_chestnut_status.py b/system/hardware/tests/test_chestnut_status.py new file mode 100644 index 000000000..ba1eccf12 --- /dev/null +++ b/system/hardware/tests/test_chestnut_status.py @@ -0,0 +1,118 @@ +from types import SimpleNamespace + +from openpilot.system.hardware.chestnut import status as chestnut_status +from openpilot.system.hardware.chestnut.status import ChestnutStatus +from openpilot.system.hardware.usb import CHESTNUT_USB_PRODUCT + + +DEVICE = { + "vendorId": 0x3801, + "productId": 0x0001, + "product": CHESTNUT_USB_PRODUCT, + "speedMbps": 5000, +} +STATE = SimpleNamespace( + supplyVoltage=12000, + supplyFault=False, + pcieLtssm=0x78, + tempC=50.0, + memoryTempC=60.0, +) + + +class Alerts: + def __init__(self): + self.values = {} + + def set(self, name, enabled, extra=None): + self.values[name] = (enabled, extra) + + @property + def active(self): + return {name for name, (enabled, _) in self.values.items() if enabled} + + +def update(status, alerts=None, *, offroad=True, expected=True, devices=None, firmware_failed=False, + loading=False, active=False, compiled=True, state=STATE): + alerts = alerts or Alerts() + status.update( + offroad, + expected, + [DEVICE] if devices is None else devices, + firmware_failed, + loading, + active, + compiled, + state, + alerts.set, + ) + return alerts + + +def test_missing_chestnut_only_alerts_when_expected(monkeypatch): + status = ChestnutStatus() + monkeypatch.setattr(chestnut_status.time, "monotonic", lambda: status.started + 11.0) + assert not update(status, expected=False, devices=[]).active + alerts = update(status, expected=True, devices=[]) + assert alerts.active == {"Offroad_ChestnutNotDetected"} + + +def test_slow_usb_and_uncompiled_alerts(): + status = ChestnutStatus() + alerts = update(status, devices=[DEVICE | {"speedMbps": 480}]) + assert alerts.active == {"Offroad_ChestnutUsbSlow"} + assert alerts.values["Offroad_ChestnutUsbSlow"][1] == "480 Mbps" + + alerts = update(status, compiled=False) + assert alerts.active == {"Offroad_ChestnutUncompiled"} + + +def test_firmware_failure_alert(): + alerts = update(ChestnutStatus(), firmware_failed=True) + assert alerts.active == {"Offroad_ChestnutUpdateFailed"} + + +def test_overheat_hysteresis(): + status = ChestnutStatus() + hot = SimpleNamespace(**(vars(STATE) | {"tempC": 101.0})) + alerts = update(status, state=hot) + assert alerts.active == {"Offroad_ChestnutOverheated"} + assert alerts.values["Offroad_ChestnutOverheated"][1] == "101 °C" + + warm = SimpleNamespace(**(vars(STATE) | {"tempC": 97.0})) + assert "Offroad_ChestnutOverheated" in update(status, state=warm).active + assert "Offroad_ChestnutOverheated" not in update(status).active + + +def test_pcie_failure_after_model_load(): + status, alerts = ChestnutStatus(), Alerts() + update(status, alerts, offroad=False, loading=True) + update(status, alerts, offroad=False, loading=False, active=False) + link_down = SimpleNamespace(**(vars(STATE) | {"pcieLtssm": 0x00})) + update(status, alerts, offroad=False, state=link_down) + assert "Offroad_ChestnutPcieUnavailable" not in alerts.active + update(status, alerts, offroad=False, state=link_down) + assert alerts.active == {"Offroad_ChestnutPcieUnavailable"} + assert "PCIe link is not up" in alerts.values["Offroad_ChestnutPcieUnavailable"][1] + + +def test_power_loss_reports_cause_and_recovery(): + status, alerts = ChestnutStatus(), Alerts() + update(status, alerts, offroad=False, loading=True) + update(status, alerts, offroad=False, loading=False, active=True) + low = SimpleNamespace(**(vars(STATE) | {"supplyVoltage": 3000, "supplyFault": True, "pcieLtssm": 0x00})) + update(status, alerts, offroad=False, state=low) + assert alerts.active == {"Offroad_ChestnutPcieUnavailable"} + assert "power lost" in alerts.values["Offroad_ChestnutPcieUnavailable"][1] + + update(status, alerts, offroad=False, state=STATE) + assert "power restored" in alerts.values["Offroad_ChestnutPcieUnavailable"][1] + + +def test_onroad_usb_disconnect_is_latched(): + status, alerts = ChestnutStatus(), Alerts() + update(status, alerts, offroad=False) + update(status, alerts, offroad=False, devices=[], state=None) + assert alerts.active == {"Offroad_ChestnutNotDetected"} + update(status, alerts, offroad=False) + assert "Offroad_ChestnutNotDetected" in alerts.active diff --git a/system/hardware/tests/test_usb.py b/system/hardware/tests/test_usb.py index f03da4b73..f776482cc 100644 --- a/system/hardware/tests/test_usb.py +++ b/system/hardware/tests/test_usb.py @@ -24,6 +24,13 @@ def test_chestnut_present_with_comma_vendor_id(tmp_path, monkeypatch): assert usb.chestnut_present() +def test_chestnut_usb_ids_include_runtime_and_bootloader(): + assert usb.is_chestnut_usb_id(0x3801, 0x0001) + assert usb.is_chestnut_usb_id(0xADD1, 0x0001) + assert not usb.is_chestnut_usb_id(0x174C, 0x2464) + assert usb.is_chestnut_usb_id(0x174C, 0x2464, include_bootloader=True) + + def test_chestnut_absent_for_other_usb_device(tmp_path, monkeypatch): monkeypatch.setattr(usb, "USB_DEVICES_PATH", tmp_path) device = tmp_path / "1-1" diff --git a/system/hardware/usb.py b/system/hardware/usb.py index a2ca7cb6f..2704a596e 100644 --- a/system/hardware/usb.py +++ b/system/hardware/usb.py @@ -7,11 +7,17 @@ CHESTNUT_PRODUCT_ID = 0x0001 CHESTNUT_USB_IDS = tuple((vendor_id, CHESTNUT_PRODUCT_ID) for vendor_id in CHESTNUT_VENDOR_IDS) CHESTNUT_FW_VERSION = "ed4e39b7" CHESTNUT_ROM_USB_IDS = ((0x174C, 0x2464), (0x174C, 0x2463)) +CHESTNUT_USB_PRODUCT = f"custom {CHESTNUT_FW_VERSION}-CLEAN" USB_DEVICES_PATH = Path("/sys/bus/usb/devices") TYPEC_CC_ORIENTATION_PATH = Path("/sys/class/power_supply/usb/typec_cc_orientation") PRIMARY_USB_CONTROLLER = "a600000.ssusb" +def is_chestnut_usb_id(vendor_id: int, product_id: int, include_bootloader: bool = False) -> bool: + ids = CHESTNUT_USB_IDS + CHESTNUT_ROM_USB_IDS if include_bootloader else CHESTNUT_USB_IDS + return (vendor_id, product_id) in ids + + def get_usb_topology() -> set[str]: try: return set(os.listdir(USB_DEVICES_PATH)) @@ -47,16 +53,15 @@ def usb_devices() -> list[Path]: def chestnut_present() -> bool: return any( - (read_int(device / "idVendor", 16), read_int(device / "idProduct", 16)) in CHESTNUT_USB_IDS + is_chestnut_usb_id(read_int(device / "idVendor", 16), read_int(device / "idProduct", 16)) for device in usb_devices() ) def chestnut_firmware_ready() -> bool: - expected = f"custom {CHESTNUT_FW_VERSION}-CLEAN" return any( - (read_int(device / "idVendor", 16), read_int(device / "idProduct", 16)) in CHESTNUT_USB_IDS and - read_text(device / "product") == expected + is_chestnut_usb_id(read_int(device / "idVendor", 16), read_int(device / "idProduct", 16)) and + read_text(device / "product") == CHESTNUT_USB_PRODUCT for device in usb_devices() ) @@ -103,7 +108,7 @@ def set_usb_state(device_state, devices: list[dict]) -> None: entry.linkErrorCount = device["linkErrorCount"] entry.usb3Lane = device.get("usb3Lane", "unknown") - if (entry.vendorId, entry.productId) in CHESTNUT_USB_IDS: + if is_chestnut_usb_id(entry.vendorId, entry.productId): chestnut_found = True device_state.chestnutPresent = chestnut_found diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index f0960e3c2..d6664f19d 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 6a0d45d40..2ad651f1d 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index 658ed74e5..7e83c9347 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ