diff --git a/common/libcommon.a b/common/libcommon.a index 41e07fe21..6aaf18497 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 5e74c5acb..3bdbf23a2 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -70,6 +70,7 @@ inline static std::unordered_map keys = { {"IsDriverViewEnabled", {CLEAR_ON_MANAGER_START, BOOL}}, {"IsEngaged", {PERSISTENT, BOOL}}, {"IsLdwEnabled", {PERSISTENT, BOOL}}, + {"IsLiveStreaming", {CLEAR_ON_MANAGER_START, BOOL}}, {"IsMetric", {PERSISTENT, BOOL}}, {"IsOffroad", {CLEAR_ON_MANAGER_START, BOOL}}, {"IsOnroad", {PERSISTENT, BOOL}}, @@ -94,6 +95,8 @@ inline static std::unordered_map keys = { {"LiveDelay", {PERSISTENT, BYTES}}, {"LiveParameters", {PERSISTENT, JSON}}, {"LiveParametersV2", {PERSISTENT, BYTES}}, + {"LivestreamEncoderBitrate", {CLEAR_ON_MANAGER_START | DONT_LOG, INT}}, + {"LivestreamRequestKeyframe", {CLEAR_ON_MANAGER_START | DONT_LOG, BOOL}}, {"LiveTorqueParameters", {PERSISTENT | DONT_LOG, BYTES}}, {"LateralManeuverMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}}, {"LocationFilterInitialState", {PERSISTENT, BYTES}}, diff --git a/common/params_pyx.so b/common/params_pyx.so index 823f21218..1ca3ef653 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 e23516cd6..805ec472e 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 7a6575db1..3294b045a 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 2345362bf..0ccc5fc70 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 fb3012453..3b6bd05ac 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 9bb6c3601..352e1aa49 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 ef1fabfea..afb36499e 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 ef1fabfea..afb36499e 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 47813d11f..24b057c71 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 47813d11f..24b057c71 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 47813d11f..24b057c71 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 47813d11f..24b057c71 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 47813d11f..24b057c71 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 47813d11f..24b057c71 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 ef1fabfea..afb36499e 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 ef1fabfea..afb36499e 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 8c2bfbcea..bb1b09b02 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 ef1fabfea..afb36499e 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 ef1fabfea..afb36499e 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 ca365b7c9..a4c211355 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-6d4089fe-DEBUG"; +const uint8_t gitversion[19] = "DEV-d89371c5-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index ae4d0548a..8ba14e650 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 d276b4762..f722c8898 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 4a88b92c9..5cc6e51c5 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 5ccff3755..95c45d420 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 9f1fb097c..8df764cdd 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 56befedae..ae6bb2a44 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 9b469f353..dd08ebd9d 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 e8c8368cb..690f4be02 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 89ee0233d..37479059f 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 56e5ffc59..e14883e6c 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 4b222907b..f01c5cb8a 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 c8ac129fe..778c69737 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 67cd09a2e..e3068e393 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 1872062aa..9d4bc2750 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 c2b6e905a..7b6e96e14 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 6b3a09ce6..3eae693f4 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 952c5feb7..f4892d95e 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 4701ead34..e3500577b 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 1c023d326..c2aa7d40f 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 9acef5f2e..4f0e09c42 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 e98dca39e..5929b688c 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 4fa101059..6f613db34 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 a1296c4b8..9c528d7bf 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 ec18aa0fd..d30c66d73 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 83b197e6c..ebc672641 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 c36758255..9a184bd66 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 6d33ed7a6..5b76ce2f6 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 3abe95bba..c79c73734 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 a159cb704..3142cd546 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 0867643af..6813c1621 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 fb2b706ff..41f830d5a 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 146121cd0..071236268 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 4fdf8a975..cc85cf82a 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 95460cf04..14825ce7b 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 4b49ab113..29fd1135d 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 db77206d8..4858bdc91 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 f22063a49..270de13b7 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 3027d3331..7ffe17941 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 528f33699..f3129db8d 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 c1d35c788..7c7c06558 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 d622aa417..c9fcf068f 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 f007518f0..49ed199af 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 872feaa19..35e2ad787 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 269c1ea0d..bf4c83ddd 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 6900dad63..7a0f192d8 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 4a4ead2df..f58156f41 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 f5aa08275..7a3061e21 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 dd0b10381..db305c7a2 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 0d7778419..6d2adfe0f 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 bdee4e107..b8592086e 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 23d5c7f46..f3fe3c460 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 b13c68580..a2de9cda3 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 662e193f3..65a707d8d 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-6d4089fe-DEBUG \ No newline at end of file +DEV-d89371c5-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 2b5419ced..375c18bb8 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_3645428350016324042) { - out_3645428350016324042[0] = delta_x[0] + nom_x[0]; - out_3645428350016324042[1] = delta_x[1] + nom_x[1]; - out_3645428350016324042[2] = delta_x[2] + nom_x[2]; - out_3645428350016324042[3] = delta_x[3] + nom_x[3]; - out_3645428350016324042[4] = delta_x[4] + nom_x[4]; - out_3645428350016324042[5] = delta_x[5] + nom_x[5]; - out_3645428350016324042[6] = delta_x[6] + nom_x[6]; - out_3645428350016324042[7] = delta_x[7] + nom_x[7]; - out_3645428350016324042[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_3928386937016580969) { + out_3928386937016580969[0] = delta_x[0] + nom_x[0]; + out_3928386937016580969[1] = delta_x[1] + nom_x[1]; + out_3928386937016580969[2] = delta_x[2] + nom_x[2]; + out_3928386937016580969[3] = delta_x[3] + nom_x[3]; + out_3928386937016580969[4] = delta_x[4] + nom_x[4]; + out_3928386937016580969[5] = delta_x[5] + nom_x[5]; + out_3928386937016580969[6] = delta_x[6] + nom_x[6]; + out_3928386937016580969[7] = delta_x[7] + nom_x[7]; + out_3928386937016580969[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_6760420506725333180) { - out_6760420506725333180[0] = -nom_x[0] + true_x[0]; - out_6760420506725333180[1] = -nom_x[1] + true_x[1]; - out_6760420506725333180[2] = -nom_x[2] + true_x[2]; - out_6760420506725333180[3] = -nom_x[3] + true_x[3]; - out_6760420506725333180[4] = -nom_x[4] + true_x[4]; - out_6760420506725333180[5] = -nom_x[5] + true_x[5]; - out_6760420506725333180[6] = -nom_x[6] + true_x[6]; - out_6760420506725333180[7] = -nom_x[7] + true_x[7]; - out_6760420506725333180[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_5072261536488927858) { + out_5072261536488927858[0] = -nom_x[0] + true_x[0]; + out_5072261536488927858[1] = -nom_x[1] + true_x[1]; + out_5072261536488927858[2] = -nom_x[2] + true_x[2]; + out_5072261536488927858[3] = -nom_x[3] + true_x[3]; + out_5072261536488927858[4] = -nom_x[4] + true_x[4]; + out_5072261536488927858[5] = -nom_x[5] + true_x[5]; + out_5072261536488927858[6] = -nom_x[6] + true_x[6]; + out_5072261536488927858[7] = -nom_x[7] + true_x[7]; + out_5072261536488927858[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_2494414039273921066) { - out_2494414039273921066[0] = 1.0; - out_2494414039273921066[1] = 0.0; - out_2494414039273921066[2] = 0.0; - out_2494414039273921066[3] = 0.0; - out_2494414039273921066[4] = 0.0; - out_2494414039273921066[5] = 0.0; - out_2494414039273921066[6] = 0.0; - out_2494414039273921066[7] = 0.0; - out_2494414039273921066[8] = 0.0; - out_2494414039273921066[9] = 0.0; - out_2494414039273921066[10] = 1.0; - out_2494414039273921066[11] = 0.0; - out_2494414039273921066[12] = 0.0; - out_2494414039273921066[13] = 0.0; - out_2494414039273921066[14] = 0.0; - out_2494414039273921066[15] = 0.0; - out_2494414039273921066[16] = 0.0; - out_2494414039273921066[17] = 0.0; - out_2494414039273921066[18] = 0.0; - out_2494414039273921066[19] = 0.0; - out_2494414039273921066[20] = 1.0; - out_2494414039273921066[21] = 0.0; - out_2494414039273921066[22] = 0.0; - out_2494414039273921066[23] = 0.0; - out_2494414039273921066[24] = 0.0; - out_2494414039273921066[25] = 0.0; - out_2494414039273921066[26] = 0.0; - out_2494414039273921066[27] = 0.0; - out_2494414039273921066[28] = 0.0; - out_2494414039273921066[29] = 0.0; - out_2494414039273921066[30] = 1.0; - out_2494414039273921066[31] = 0.0; - out_2494414039273921066[32] = 0.0; - out_2494414039273921066[33] = 0.0; - out_2494414039273921066[34] = 0.0; - out_2494414039273921066[35] = 0.0; - out_2494414039273921066[36] = 0.0; - out_2494414039273921066[37] = 0.0; - out_2494414039273921066[38] = 0.0; - out_2494414039273921066[39] = 0.0; - out_2494414039273921066[40] = 1.0; - out_2494414039273921066[41] = 0.0; - out_2494414039273921066[42] = 0.0; - out_2494414039273921066[43] = 0.0; - out_2494414039273921066[44] = 0.0; - out_2494414039273921066[45] = 0.0; - out_2494414039273921066[46] = 0.0; - out_2494414039273921066[47] = 0.0; - out_2494414039273921066[48] = 0.0; - out_2494414039273921066[49] = 0.0; - out_2494414039273921066[50] = 1.0; - out_2494414039273921066[51] = 0.0; - out_2494414039273921066[52] = 0.0; - out_2494414039273921066[53] = 0.0; - out_2494414039273921066[54] = 0.0; - out_2494414039273921066[55] = 0.0; - out_2494414039273921066[56] = 0.0; - out_2494414039273921066[57] = 0.0; - out_2494414039273921066[58] = 0.0; - out_2494414039273921066[59] = 0.0; - out_2494414039273921066[60] = 1.0; - out_2494414039273921066[61] = 0.0; - out_2494414039273921066[62] = 0.0; - out_2494414039273921066[63] = 0.0; - out_2494414039273921066[64] = 0.0; - out_2494414039273921066[65] = 0.0; - out_2494414039273921066[66] = 0.0; - out_2494414039273921066[67] = 0.0; - out_2494414039273921066[68] = 0.0; - out_2494414039273921066[69] = 0.0; - out_2494414039273921066[70] = 1.0; - out_2494414039273921066[71] = 0.0; - out_2494414039273921066[72] = 0.0; - out_2494414039273921066[73] = 0.0; - out_2494414039273921066[74] = 0.0; - out_2494414039273921066[75] = 0.0; - out_2494414039273921066[76] = 0.0; - out_2494414039273921066[77] = 0.0; - out_2494414039273921066[78] = 0.0; - out_2494414039273921066[79] = 0.0; - out_2494414039273921066[80] = 1.0; +void H_mod_fun(double *state, double *out_1805157416881491113) { + out_1805157416881491113[0] = 1.0; + out_1805157416881491113[1] = 0.0; + out_1805157416881491113[2] = 0.0; + out_1805157416881491113[3] = 0.0; + out_1805157416881491113[4] = 0.0; + out_1805157416881491113[5] = 0.0; + out_1805157416881491113[6] = 0.0; + out_1805157416881491113[7] = 0.0; + out_1805157416881491113[8] = 0.0; + out_1805157416881491113[9] = 0.0; + out_1805157416881491113[10] = 1.0; + out_1805157416881491113[11] = 0.0; + out_1805157416881491113[12] = 0.0; + out_1805157416881491113[13] = 0.0; + out_1805157416881491113[14] = 0.0; + out_1805157416881491113[15] = 0.0; + out_1805157416881491113[16] = 0.0; + out_1805157416881491113[17] = 0.0; + out_1805157416881491113[18] = 0.0; + out_1805157416881491113[19] = 0.0; + out_1805157416881491113[20] = 1.0; + out_1805157416881491113[21] = 0.0; + out_1805157416881491113[22] = 0.0; + out_1805157416881491113[23] = 0.0; + out_1805157416881491113[24] = 0.0; + out_1805157416881491113[25] = 0.0; + out_1805157416881491113[26] = 0.0; + out_1805157416881491113[27] = 0.0; + out_1805157416881491113[28] = 0.0; + out_1805157416881491113[29] = 0.0; + out_1805157416881491113[30] = 1.0; + out_1805157416881491113[31] = 0.0; + out_1805157416881491113[32] = 0.0; + out_1805157416881491113[33] = 0.0; + out_1805157416881491113[34] = 0.0; + out_1805157416881491113[35] = 0.0; + out_1805157416881491113[36] = 0.0; + out_1805157416881491113[37] = 0.0; + out_1805157416881491113[38] = 0.0; + out_1805157416881491113[39] = 0.0; + out_1805157416881491113[40] = 1.0; + out_1805157416881491113[41] = 0.0; + out_1805157416881491113[42] = 0.0; + out_1805157416881491113[43] = 0.0; + out_1805157416881491113[44] = 0.0; + out_1805157416881491113[45] = 0.0; + out_1805157416881491113[46] = 0.0; + out_1805157416881491113[47] = 0.0; + out_1805157416881491113[48] = 0.0; + out_1805157416881491113[49] = 0.0; + out_1805157416881491113[50] = 1.0; + out_1805157416881491113[51] = 0.0; + out_1805157416881491113[52] = 0.0; + out_1805157416881491113[53] = 0.0; + out_1805157416881491113[54] = 0.0; + out_1805157416881491113[55] = 0.0; + out_1805157416881491113[56] = 0.0; + out_1805157416881491113[57] = 0.0; + out_1805157416881491113[58] = 0.0; + out_1805157416881491113[59] = 0.0; + out_1805157416881491113[60] = 1.0; + out_1805157416881491113[61] = 0.0; + out_1805157416881491113[62] = 0.0; + out_1805157416881491113[63] = 0.0; + out_1805157416881491113[64] = 0.0; + out_1805157416881491113[65] = 0.0; + out_1805157416881491113[66] = 0.0; + out_1805157416881491113[67] = 0.0; + out_1805157416881491113[68] = 0.0; + out_1805157416881491113[69] = 0.0; + out_1805157416881491113[70] = 1.0; + out_1805157416881491113[71] = 0.0; + out_1805157416881491113[72] = 0.0; + out_1805157416881491113[73] = 0.0; + out_1805157416881491113[74] = 0.0; + out_1805157416881491113[75] = 0.0; + out_1805157416881491113[76] = 0.0; + out_1805157416881491113[77] = 0.0; + out_1805157416881491113[78] = 0.0; + out_1805157416881491113[79] = 0.0; + out_1805157416881491113[80] = 1.0; } -void f_fun(double *state, double dt, double *out_7248228104803536253) { - out_7248228104803536253[0] = state[0]; - out_7248228104803536253[1] = state[1]; - out_7248228104803536253[2] = state[2]; - out_7248228104803536253[3] = state[3]; - out_7248228104803536253[4] = state[4]; - out_7248228104803536253[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_7248228104803536253[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_7248228104803536253[7] = state[7]; - out_7248228104803536253[8] = state[8]; +void f_fun(double *state, double dt, double *out_779920196187082914) { + out_779920196187082914[0] = state[0]; + out_779920196187082914[1] = state[1]; + out_779920196187082914[2] = state[2]; + out_779920196187082914[3] = state[3]; + out_779920196187082914[4] = state[4]; + out_779920196187082914[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_779920196187082914[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_779920196187082914[7] = state[7]; + out_779920196187082914[8] = state[8]; } -void F_fun(double *state, double dt, double *out_8524982651096840533) { - out_8524982651096840533[0] = 1; - out_8524982651096840533[1] = 0; - out_8524982651096840533[2] = 0; - out_8524982651096840533[3] = 0; - out_8524982651096840533[4] = 0; - out_8524982651096840533[5] = 0; - out_8524982651096840533[6] = 0; - out_8524982651096840533[7] = 0; - out_8524982651096840533[8] = 0; - out_8524982651096840533[9] = 0; - out_8524982651096840533[10] = 1; - out_8524982651096840533[11] = 0; - out_8524982651096840533[12] = 0; - out_8524982651096840533[13] = 0; - out_8524982651096840533[14] = 0; - out_8524982651096840533[15] = 0; - out_8524982651096840533[16] = 0; - out_8524982651096840533[17] = 0; - out_8524982651096840533[18] = 0; - out_8524982651096840533[19] = 0; - out_8524982651096840533[20] = 1; - out_8524982651096840533[21] = 0; - out_8524982651096840533[22] = 0; - out_8524982651096840533[23] = 0; - out_8524982651096840533[24] = 0; - out_8524982651096840533[25] = 0; - out_8524982651096840533[26] = 0; - out_8524982651096840533[27] = 0; - out_8524982651096840533[28] = 0; - out_8524982651096840533[29] = 0; - out_8524982651096840533[30] = 1; - out_8524982651096840533[31] = 0; - out_8524982651096840533[32] = 0; - out_8524982651096840533[33] = 0; - out_8524982651096840533[34] = 0; - out_8524982651096840533[35] = 0; - out_8524982651096840533[36] = 0; - out_8524982651096840533[37] = 0; - out_8524982651096840533[38] = 0; - out_8524982651096840533[39] = 0; - out_8524982651096840533[40] = 1; - out_8524982651096840533[41] = 0; - out_8524982651096840533[42] = 0; - out_8524982651096840533[43] = 0; - out_8524982651096840533[44] = 0; - out_8524982651096840533[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_8524982651096840533[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_8524982651096840533[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_8524982651096840533[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_8524982651096840533[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_8524982651096840533[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_8524982651096840533[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_8524982651096840533[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_8524982651096840533[53] = -9.8100000000000005*dt; - out_8524982651096840533[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_8524982651096840533[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_8524982651096840533[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_8524982651096840533[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_8524982651096840533[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_8524982651096840533[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_8524982651096840533[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_8524982651096840533[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_8524982651096840533[62] = 0; - out_8524982651096840533[63] = 0; - out_8524982651096840533[64] = 0; - out_8524982651096840533[65] = 0; - out_8524982651096840533[66] = 0; - out_8524982651096840533[67] = 0; - out_8524982651096840533[68] = 0; - out_8524982651096840533[69] = 0; - out_8524982651096840533[70] = 1; - out_8524982651096840533[71] = 0; - out_8524982651096840533[72] = 0; - out_8524982651096840533[73] = 0; - out_8524982651096840533[74] = 0; - out_8524982651096840533[75] = 0; - out_8524982651096840533[76] = 0; - out_8524982651096840533[77] = 0; - out_8524982651096840533[78] = 0; - out_8524982651096840533[79] = 0; - out_8524982651096840533[80] = 1; +void F_fun(double *state, double dt, double *out_3629707451146971735) { + out_3629707451146971735[0] = 1; + out_3629707451146971735[1] = 0; + out_3629707451146971735[2] = 0; + out_3629707451146971735[3] = 0; + out_3629707451146971735[4] = 0; + out_3629707451146971735[5] = 0; + out_3629707451146971735[6] = 0; + out_3629707451146971735[7] = 0; + out_3629707451146971735[8] = 0; + out_3629707451146971735[9] = 0; + out_3629707451146971735[10] = 1; + out_3629707451146971735[11] = 0; + out_3629707451146971735[12] = 0; + out_3629707451146971735[13] = 0; + out_3629707451146971735[14] = 0; + out_3629707451146971735[15] = 0; + out_3629707451146971735[16] = 0; + out_3629707451146971735[17] = 0; + out_3629707451146971735[18] = 0; + out_3629707451146971735[19] = 0; + out_3629707451146971735[20] = 1; + out_3629707451146971735[21] = 0; + out_3629707451146971735[22] = 0; + out_3629707451146971735[23] = 0; + out_3629707451146971735[24] = 0; + out_3629707451146971735[25] = 0; + out_3629707451146971735[26] = 0; + out_3629707451146971735[27] = 0; + out_3629707451146971735[28] = 0; + out_3629707451146971735[29] = 0; + out_3629707451146971735[30] = 1; + out_3629707451146971735[31] = 0; + out_3629707451146971735[32] = 0; + out_3629707451146971735[33] = 0; + out_3629707451146971735[34] = 0; + out_3629707451146971735[35] = 0; + out_3629707451146971735[36] = 0; + out_3629707451146971735[37] = 0; + out_3629707451146971735[38] = 0; + out_3629707451146971735[39] = 0; + out_3629707451146971735[40] = 1; + out_3629707451146971735[41] = 0; + out_3629707451146971735[42] = 0; + out_3629707451146971735[43] = 0; + out_3629707451146971735[44] = 0; + out_3629707451146971735[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_3629707451146971735[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_3629707451146971735[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_3629707451146971735[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_3629707451146971735[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_3629707451146971735[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_3629707451146971735[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_3629707451146971735[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_3629707451146971735[53] = -9.8100000000000005*dt; + out_3629707451146971735[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_3629707451146971735[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_3629707451146971735[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_3629707451146971735[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_3629707451146971735[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_3629707451146971735[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_3629707451146971735[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_3629707451146971735[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_3629707451146971735[62] = 0; + out_3629707451146971735[63] = 0; + out_3629707451146971735[64] = 0; + out_3629707451146971735[65] = 0; + out_3629707451146971735[66] = 0; + out_3629707451146971735[67] = 0; + out_3629707451146971735[68] = 0; + out_3629707451146971735[69] = 0; + out_3629707451146971735[70] = 1; + out_3629707451146971735[71] = 0; + out_3629707451146971735[72] = 0; + out_3629707451146971735[73] = 0; + out_3629707451146971735[74] = 0; + out_3629707451146971735[75] = 0; + out_3629707451146971735[76] = 0; + out_3629707451146971735[77] = 0; + out_3629707451146971735[78] = 0; + out_3629707451146971735[79] = 0; + out_3629707451146971735[80] = 1; } -void h_25(double *state, double *unused, double *out_8901373037917505693) { - out_8901373037917505693[0] = state[6]; +void h_25(double *state, double *unused, double *out_3062657207957604668) { + out_3062657207957604668[0] = state[6]; } -void H_25(double *state, double *unused, double *out_6698774186638316171) { - out_6698774186638316171[0] = 0; - out_6698774186638316171[1] = 0; - out_6698774186638316171[2] = 0; - out_6698774186638316171[3] = 0; - out_6698774186638316171[4] = 0; - out_6698774186638316171[5] = 0; - out_6698774186638316171[6] = 1; - out_6698774186638316171[7] = 0; - out_6698774186638316171[8] = 0; +void H_25(double *state, double *unused, double *out_6170957893956391582) { + out_6170957893956391582[0] = 0; + out_6170957893956391582[1] = 0; + out_6170957893956391582[2] = 0; + out_6170957893956391582[3] = 0; + out_6170957893956391582[4] = 0; + out_6170957893956391582[5] = 0; + out_6170957893956391582[6] = 1; + out_6170957893956391582[7] = 0; + out_6170957893956391582[8] = 0; } -void h_24(double *state, double *unused, double *out_4440266777255545355) { - out_4440266777255545355[0] = state[4]; - out_4440266777255545355[1] = state[5]; +void h_24(double *state, double *unused, double *out_4363661235373079824) { + out_4363661235373079824[0] = state[4]; + out_4363661235373079824[1] = state[5]; } -void H_24(double *state, double *unused, double *out_2529290999430879054) { - out_2529290999430879054[0] = 0; - out_2529290999430879054[1] = 0; - out_2529290999430879054[2] = 0; - out_2529290999430879054[3] = 0; - out_2529290999430879054[4] = 1; - out_2529290999430879054[5] = 0; - out_2529290999430879054[6] = 0; - out_2529290999430879054[7] = 0; - out_2529290999430879054[8] = 0; - out_2529290999430879054[9] = 0; - out_2529290999430879054[10] = 0; - out_2529290999430879054[11] = 0; - out_2529290999430879054[12] = 0; - out_2529290999430879054[13] = 0; - out_2529290999430879054[14] = 1; - out_2529290999430879054[15] = 0; - out_2529290999430879054[16] = 0; - out_2529290999430879054[17] = 0; +void H_24(double *state, double *unused, double *out_6464400548730383069) { + out_6464400548730383069[0] = 0; + out_6464400548730383069[1] = 0; + out_6464400548730383069[2] = 0; + out_6464400548730383069[3] = 0; + out_6464400548730383069[4] = 1; + out_6464400548730383069[5] = 0; + out_6464400548730383069[6] = 0; + out_6464400548730383069[7] = 0; + out_6464400548730383069[8] = 0; + out_6464400548730383069[9] = 0; + out_6464400548730383069[10] = 0; + out_6464400548730383069[11] = 0; + out_6464400548730383069[12] = 0; + out_6464400548730383069[13] = 0; + out_6464400548730383069[14] = 1; + out_6464400548730383069[15] = 0; + out_6464400548730383069[16] = 0; + out_6464400548730383069[17] = 0; } -void h_30(double *state, double *unused, double *out_261495561198963809) { - out_261495561198963809[0] = state[4]; +void h_30(double *state, double *unused, double *out_8192537752896435152) { + out_8192537752896435152[0] = state[4]; } -void H_30(double *state, double *unused, double *out_7220273556943627247) { - out_7220273556943627247[0] = 0; - out_7220273556943627247[1] = 0; - out_7220273556943627247[2] = 0; - out_7220273556943627247[3] = 0; - out_7220273556943627247[4] = 1; - out_7220273556943627247[5] = 0; - out_7220273556943627247[6] = 0; - out_7220273556943627247[7] = 0; - out_7220273556943627247[8] = 0; +void H_30(double *state, double *unused, double *out_5359095838261543279) { + out_5359095838261543279[0] = 0; + out_5359095838261543279[1] = 0; + out_5359095838261543279[2] = 0; + out_5359095838261543279[3] = 0; + out_5359095838261543279[4] = 1; + out_5359095838261543279[5] = 0; + out_5359095838261543279[6] = 0; + out_5359095838261543279[7] = 0; + out_5359095838261543279[8] = 0; } -void h_26(double *state, double *unused, double *out_4740177706461881836) { - out_4740177706461881836[0] = state[7]; +void h_26(double *state, double *unused, double *out_1793496235482586006) { + out_1793496235482586006[0] = state[7]; } -void H_26(double *state, double *unused, double *out_8006466568197179221) { - out_8006466568197179221[0] = 0; - out_8006466568197179221[1] = 0; - out_8006466568197179221[2] = 0; - out_8006466568197179221[3] = 0; - out_8006466568197179221[4] = 0; - out_8006466568197179221[5] = 0; - out_8006466568197179221[6] = 0; - out_8006466568197179221[7] = 1; - out_8006466568197179221[8] = 0; +void H_26(double *state, double *unused, double *out_8971260209992359433) { + out_8971260209992359433[0] = 0; + out_8971260209992359433[1] = 0; + out_8971260209992359433[2] = 0; + out_8971260209992359433[3] = 0; + out_8971260209992359433[4] = 0; + out_8971260209992359433[5] = 0; + out_8971260209992359433[6] = 0; + out_8971260209992359433[7] = 1; + out_8971260209992359433[8] = 0; } -void h_27(double *state, double *unused, double *out_6088820934647652310) { - out_6088820934647652310[0] = state[3]; +void h_27(double *state, double *unused, double *out_8064330475618450313) { + out_8064330475618450313[0] = state[3]; } -void H_27(double *state, double *unused, double *out_9002876445581981152) { - out_9002876445581981152[0] = 0; - out_9002876445581981152[1] = 0; - out_9002876445581981152[2] = 0; - out_9002876445581981152[3] = 1; - out_9002876445581981152[4] = 0; - out_9002876445581981152[5] = 0; - out_9002876445581981152[6] = 0; - out_9002876445581981152[7] = 0; - out_9002876445581981152[8] = 0; +void H_27(double *state, double *unused, double *out_7533859150061968190) { + out_7533859150061968190[0] = 0; + out_7533859150061968190[1] = 0; + out_7533859150061968190[2] = 0; + out_7533859150061968190[3] = 1; + out_7533859150061968190[4] = 0; + out_7533859150061968190[5] = 0; + out_7533859150061968190[6] = 0; + out_7533859150061968190[7] = 0; + out_7533859150061968190[8] = 0; } -void h_29(double *state, double *unused, double *out_906248239877177928) { - out_906248239877177928[0] = state[1]; +void h_29(double *state, double *unused, double *out_6613699082989567146) { + out_6613699082989567146[0] = state[1]; } -void H_29(double *state, double *unused, double *out_7730504901258019431) { - out_7730504901258019431[0] = 0; - out_7730504901258019431[1] = 1; - out_7730504901258019431[2] = 0; - out_7730504901258019431[3] = 0; - out_7730504901258019431[4] = 0; - out_7730504901258019431[5] = 0; - out_7730504901258019431[6] = 0; - out_7730504901258019431[7] = 0; - out_7730504901258019431[8] = 0; +void H_29(double *state, double *unused, double *out_4848864493947151095) { + out_4848864493947151095[0] = 0; + out_4848864493947151095[1] = 1; + out_4848864493947151095[2] = 0; + out_4848864493947151095[3] = 0; + out_4848864493947151095[4] = 0; + out_4848864493947151095[5] = 0; + out_4848864493947151095[6] = 0; + out_4848864493947151095[7] = 0; + out_4848864493947151095[8] = 0; } -void h_28(double *state, double *unused, double *out_3561613674795902166) { - out_3561613674795902166[0] = state[0]; +void h_28(double *state, double *unused, double *out_7120820622716787878) { + out_7120820622716787878[0] = state[0]; } -void H_28(double *state, double *unused, double *out_2648105884188488857) { - out_2648105884188488857[0] = 1; - out_2648105884188488857[1] = 0; - out_2648105884188488857[2] = 0; - out_2648105884188488857[3] = 0; - out_2648105884188488857[4] = 0; - out_2648105884188488857[5] = 0; - out_2648105884188488857[6] = 0; - out_2648105884188488857[7] = 0; - out_2648105884188488857[8] = 0; +void H_28(double *state, double *unused, double *out_4117123179708501819) { + out_4117123179708501819[0] = 1; + out_4117123179708501819[1] = 0; + out_4117123179708501819[2] = 0; + out_4117123179708501819[3] = 0; + out_4117123179708501819[4] = 0; + out_4117123179708501819[5] = 0; + out_4117123179708501819[6] = 0; + out_4117123179708501819[7] = 0; + out_4117123179708501819[8] = 0; } -void h_31(double *state, double *unused, double *out_8626178975632999804) { - out_8626178975632999804[0] = state[8]; +void h_31(double *state, double *unused, double *out_574792320296417682) { + out_574792320296417682[0] = state[8]; } -void H_31(double *state, double *unused, double *out_6668128224761355743) { - out_6668128224761355743[0] = 0; - out_6668128224761355743[1] = 0; - out_6668128224761355743[2] = 0; - out_6668128224761355743[3] = 0; - out_6668128224761355743[4] = 0; - out_6668128224761355743[5] = 0; - out_6668128224761355743[6] = 0; - out_6668128224761355743[7] = 0; - out_6668128224761355743[8] = 1; +void H_31(double *state, double *unused, double *out_5199110929241342781) { + out_5199110929241342781[0] = 0; + out_5199110929241342781[1] = 0; + out_5199110929241342781[2] = 0; + out_5199110929241342781[3] = 0; + out_5199110929241342781[4] = 0; + out_5199110929241342781[5] = 0; + out_5199110929241342781[6] = 0; + out_5199110929241342781[7] = 0; + out_5199110929241342781[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_3645428350016324042) { - err_fun(nom_x, delta_x, out_3645428350016324042); +void car_err_fun(double *nom_x, double *delta_x, double *out_3928386937016580969) { + err_fun(nom_x, delta_x, out_3928386937016580969); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_6760420506725333180) { - inv_err_fun(nom_x, true_x, out_6760420506725333180); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_5072261536488927858) { + inv_err_fun(nom_x, true_x, out_5072261536488927858); } -void car_H_mod_fun(double *state, double *out_2494414039273921066) { - H_mod_fun(state, out_2494414039273921066); +void car_H_mod_fun(double *state, double *out_1805157416881491113) { + H_mod_fun(state, out_1805157416881491113); } -void car_f_fun(double *state, double dt, double *out_7248228104803536253) { - f_fun(state, dt, out_7248228104803536253); +void car_f_fun(double *state, double dt, double *out_779920196187082914) { + f_fun(state, dt, out_779920196187082914); } -void car_F_fun(double *state, double dt, double *out_8524982651096840533) { - F_fun(state, dt, out_8524982651096840533); +void car_F_fun(double *state, double dt, double *out_3629707451146971735) { + F_fun(state, dt, out_3629707451146971735); } -void car_h_25(double *state, double *unused, double *out_8901373037917505693) { - h_25(state, unused, out_8901373037917505693); +void car_h_25(double *state, double *unused, double *out_3062657207957604668) { + h_25(state, unused, out_3062657207957604668); } -void car_H_25(double *state, double *unused, double *out_6698774186638316171) { - H_25(state, unused, out_6698774186638316171); +void car_H_25(double *state, double *unused, double *out_6170957893956391582) { + H_25(state, unused, out_6170957893956391582); } -void car_h_24(double *state, double *unused, double *out_4440266777255545355) { - h_24(state, unused, out_4440266777255545355); +void car_h_24(double *state, double *unused, double *out_4363661235373079824) { + h_24(state, unused, out_4363661235373079824); } -void car_H_24(double *state, double *unused, double *out_2529290999430879054) { - H_24(state, unused, out_2529290999430879054); +void car_H_24(double *state, double *unused, double *out_6464400548730383069) { + H_24(state, unused, out_6464400548730383069); } -void car_h_30(double *state, double *unused, double *out_261495561198963809) { - h_30(state, unused, out_261495561198963809); +void car_h_30(double *state, double *unused, double *out_8192537752896435152) { + h_30(state, unused, out_8192537752896435152); } -void car_H_30(double *state, double *unused, double *out_7220273556943627247) { - H_30(state, unused, out_7220273556943627247); +void car_H_30(double *state, double *unused, double *out_5359095838261543279) { + H_30(state, unused, out_5359095838261543279); } -void car_h_26(double *state, double *unused, double *out_4740177706461881836) { - h_26(state, unused, out_4740177706461881836); +void car_h_26(double *state, double *unused, double *out_1793496235482586006) { + h_26(state, unused, out_1793496235482586006); } -void car_H_26(double *state, double *unused, double *out_8006466568197179221) { - H_26(state, unused, out_8006466568197179221); +void car_H_26(double *state, double *unused, double *out_8971260209992359433) { + H_26(state, unused, out_8971260209992359433); } -void car_h_27(double *state, double *unused, double *out_6088820934647652310) { - h_27(state, unused, out_6088820934647652310); +void car_h_27(double *state, double *unused, double *out_8064330475618450313) { + h_27(state, unused, out_8064330475618450313); } -void car_H_27(double *state, double *unused, double *out_9002876445581981152) { - H_27(state, unused, out_9002876445581981152); +void car_H_27(double *state, double *unused, double *out_7533859150061968190) { + H_27(state, unused, out_7533859150061968190); } -void car_h_29(double *state, double *unused, double *out_906248239877177928) { - h_29(state, unused, out_906248239877177928); +void car_h_29(double *state, double *unused, double *out_6613699082989567146) { + h_29(state, unused, out_6613699082989567146); } -void car_H_29(double *state, double *unused, double *out_7730504901258019431) { - H_29(state, unused, out_7730504901258019431); +void car_H_29(double *state, double *unused, double *out_4848864493947151095) { + H_29(state, unused, out_4848864493947151095); } -void car_h_28(double *state, double *unused, double *out_3561613674795902166) { - h_28(state, unused, out_3561613674795902166); +void car_h_28(double *state, double *unused, double *out_7120820622716787878) { + h_28(state, unused, out_7120820622716787878); } -void car_H_28(double *state, double *unused, double *out_2648105884188488857) { - H_28(state, unused, out_2648105884188488857); +void car_H_28(double *state, double *unused, double *out_4117123179708501819) { + H_28(state, unused, out_4117123179708501819); } -void car_h_31(double *state, double *unused, double *out_8626178975632999804) { - h_31(state, unused, out_8626178975632999804); +void car_h_31(double *state, double *unused, double *out_574792320296417682) { + h_31(state, unused, out_574792320296417682); } -void car_H_31(double *state, double *unused, double *out_6668128224761355743) { - H_31(state, unused, out_6668128224761355743); +void car_H_31(double *state, double *unused, double *out_5199110929241342781) { + H_31(state, unused, out_5199110929241342781); } 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 f58a6108c..83ddcfab1 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_3645428350016324042); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_6760420506725333180); -void car_H_mod_fun(double *state, double *out_2494414039273921066); -void car_f_fun(double *state, double dt, double *out_7248228104803536253); -void car_F_fun(double *state, double dt, double *out_8524982651096840533); -void car_h_25(double *state, double *unused, double *out_8901373037917505693); -void car_H_25(double *state, double *unused, double *out_6698774186638316171); -void car_h_24(double *state, double *unused, double *out_4440266777255545355); -void car_H_24(double *state, double *unused, double *out_2529290999430879054); -void car_h_30(double *state, double *unused, double *out_261495561198963809); -void car_H_30(double *state, double *unused, double *out_7220273556943627247); -void car_h_26(double *state, double *unused, double *out_4740177706461881836); -void car_H_26(double *state, double *unused, double *out_8006466568197179221); -void car_h_27(double *state, double *unused, double *out_6088820934647652310); -void car_H_27(double *state, double *unused, double *out_9002876445581981152); -void car_h_29(double *state, double *unused, double *out_906248239877177928); -void car_H_29(double *state, double *unused, double *out_7730504901258019431); -void car_h_28(double *state, double *unused, double *out_3561613674795902166); -void car_H_28(double *state, double *unused, double *out_2648105884188488857); -void car_h_31(double *state, double *unused, double *out_8626178975632999804); -void car_H_31(double *state, double *unused, double *out_6668128224761355743); +void car_err_fun(double *nom_x, double *delta_x, double *out_3928386937016580969); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_5072261536488927858); +void car_H_mod_fun(double *state, double *out_1805157416881491113); +void car_f_fun(double *state, double dt, double *out_779920196187082914); +void car_F_fun(double *state, double dt, double *out_3629707451146971735); +void car_h_25(double *state, double *unused, double *out_3062657207957604668); +void car_H_25(double *state, double *unused, double *out_6170957893956391582); +void car_h_24(double *state, double *unused, double *out_4363661235373079824); +void car_H_24(double *state, double *unused, double *out_6464400548730383069); +void car_h_30(double *state, double *unused, double *out_8192537752896435152); +void car_H_30(double *state, double *unused, double *out_5359095838261543279); +void car_h_26(double *state, double *unused, double *out_1793496235482586006); +void car_H_26(double *state, double *unused, double *out_8971260209992359433); +void car_h_27(double *state, double *unused, double *out_8064330475618450313); +void car_H_27(double *state, double *unused, double *out_7533859150061968190); +void car_h_29(double *state, double *unused, double *out_6613699082989567146); +void car_H_29(double *state, double *unused, double *out_4848864493947151095); +void car_h_28(double *state, double *unused, double *out_7120820622716787878); +void car_H_28(double *state, double *unused, double *out_4117123179708501819); +void car_h_31(double *state, double *unused, double *out_574792320296417682); +void car_H_31(double *state, double *unused, double *out_5199110929241342781); 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 2d76d407c..45c5c36d5 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_2394949464519988932) { - out_2394949464519988932[0] = delta_x[0] + nom_x[0]; - out_2394949464519988932[1] = delta_x[1] + nom_x[1]; - out_2394949464519988932[2] = delta_x[2] + nom_x[2]; - out_2394949464519988932[3] = delta_x[3] + nom_x[3]; - out_2394949464519988932[4] = delta_x[4] + nom_x[4]; - out_2394949464519988932[5] = delta_x[5] + nom_x[5]; - out_2394949464519988932[6] = delta_x[6] + nom_x[6]; - out_2394949464519988932[7] = delta_x[7] + nom_x[7]; - out_2394949464519988932[8] = delta_x[8] + nom_x[8]; - out_2394949464519988932[9] = delta_x[9] + nom_x[9]; - out_2394949464519988932[10] = delta_x[10] + nom_x[10]; - out_2394949464519988932[11] = delta_x[11] + nom_x[11]; - out_2394949464519988932[12] = delta_x[12] + nom_x[12]; - out_2394949464519988932[13] = delta_x[13] + nom_x[13]; - out_2394949464519988932[14] = delta_x[14] + nom_x[14]; - out_2394949464519988932[15] = delta_x[15] + nom_x[15]; - out_2394949464519988932[16] = delta_x[16] + nom_x[16]; - out_2394949464519988932[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_9137514370372247790) { + out_9137514370372247790[0] = delta_x[0] + nom_x[0]; + out_9137514370372247790[1] = delta_x[1] + nom_x[1]; + out_9137514370372247790[2] = delta_x[2] + nom_x[2]; + out_9137514370372247790[3] = delta_x[3] + nom_x[3]; + out_9137514370372247790[4] = delta_x[4] + nom_x[4]; + out_9137514370372247790[5] = delta_x[5] + nom_x[5]; + out_9137514370372247790[6] = delta_x[6] + nom_x[6]; + out_9137514370372247790[7] = delta_x[7] + nom_x[7]; + out_9137514370372247790[8] = delta_x[8] + nom_x[8]; + out_9137514370372247790[9] = delta_x[9] + nom_x[9]; + out_9137514370372247790[10] = delta_x[10] + nom_x[10]; + out_9137514370372247790[11] = delta_x[11] + nom_x[11]; + out_9137514370372247790[12] = delta_x[12] + nom_x[12]; + out_9137514370372247790[13] = delta_x[13] + nom_x[13]; + out_9137514370372247790[14] = delta_x[14] + nom_x[14]; + out_9137514370372247790[15] = delta_x[15] + nom_x[15]; + out_9137514370372247790[16] = delta_x[16] + nom_x[16]; + out_9137514370372247790[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_6906452154256624155) { - out_6906452154256624155[0] = -nom_x[0] + true_x[0]; - out_6906452154256624155[1] = -nom_x[1] + true_x[1]; - out_6906452154256624155[2] = -nom_x[2] + true_x[2]; - out_6906452154256624155[3] = -nom_x[3] + true_x[3]; - out_6906452154256624155[4] = -nom_x[4] + true_x[4]; - out_6906452154256624155[5] = -nom_x[5] + true_x[5]; - out_6906452154256624155[6] = -nom_x[6] + true_x[6]; - out_6906452154256624155[7] = -nom_x[7] + true_x[7]; - out_6906452154256624155[8] = -nom_x[8] + true_x[8]; - out_6906452154256624155[9] = -nom_x[9] + true_x[9]; - out_6906452154256624155[10] = -nom_x[10] + true_x[10]; - out_6906452154256624155[11] = -nom_x[11] + true_x[11]; - out_6906452154256624155[12] = -nom_x[12] + true_x[12]; - out_6906452154256624155[13] = -nom_x[13] + true_x[13]; - out_6906452154256624155[14] = -nom_x[14] + true_x[14]; - out_6906452154256624155[15] = -nom_x[15] + true_x[15]; - out_6906452154256624155[16] = -nom_x[16] + true_x[16]; - out_6906452154256624155[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_3476070474607186610) { + out_3476070474607186610[0] = -nom_x[0] + true_x[0]; + out_3476070474607186610[1] = -nom_x[1] + true_x[1]; + out_3476070474607186610[2] = -nom_x[2] + true_x[2]; + out_3476070474607186610[3] = -nom_x[3] + true_x[3]; + out_3476070474607186610[4] = -nom_x[4] + true_x[4]; + out_3476070474607186610[5] = -nom_x[5] + true_x[5]; + out_3476070474607186610[6] = -nom_x[6] + true_x[6]; + out_3476070474607186610[7] = -nom_x[7] + true_x[7]; + out_3476070474607186610[8] = -nom_x[8] + true_x[8]; + out_3476070474607186610[9] = -nom_x[9] + true_x[9]; + out_3476070474607186610[10] = -nom_x[10] + true_x[10]; + out_3476070474607186610[11] = -nom_x[11] + true_x[11]; + out_3476070474607186610[12] = -nom_x[12] + true_x[12]; + out_3476070474607186610[13] = -nom_x[13] + true_x[13]; + out_3476070474607186610[14] = -nom_x[14] + true_x[14]; + out_3476070474607186610[15] = -nom_x[15] + true_x[15]; + out_3476070474607186610[16] = -nom_x[16] + true_x[16]; + out_3476070474607186610[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_4838410850925167920) { - out_4838410850925167920[0] = 1.0; - out_4838410850925167920[1] = 0.0; - out_4838410850925167920[2] = 0.0; - out_4838410850925167920[3] = 0.0; - out_4838410850925167920[4] = 0.0; - out_4838410850925167920[5] = 0.0; - out_4838410850925167920[6] = 0.0; - out_4838410850925167920[7] = 0.0; - out_4838410850925167920[8] = 0.0; - out_4838410850925167920[9] = 0.0; - out_4838410850925167920[10] = 0.0; - out_4838410850925167920[11] = 0.0; - out_4838410850925167920[12] = 0.0; - out_4838410850925167920[13] = 0.0; - out_4838410850925167920[14] = 0.0; - out_4838410850925167920[15] = 0.0; - out_4838410850925167920[16] = 0.0; - out_4838410850925167920[17] = 0.0; - out_4838410850925167920[18] = 0.0; - out_4838410850925167920[19] = 1.0; - out_4838410850925167920[20] = 0.0; - out_4838410850925167920[21] = 0.0; - out_4838410850925167920[22] = 0.0; - out_4838410850925167920[23] = 0.0; - out_4838410850925167920[24] = 0.0; - out_4838410850925167920[25] = 0.0; - out_4838410850925167920[26] = 0.0; - out_4838410850925167920[27] = 0.0; - out_4838410850925167920[28] = 0.0; - out_4838410850925167920[29] = 0.0; - out_4838410850925167920[30] = 0.0; - out_4838410850925167920[31] = 0.0; - out_4838410850925167920[32] = 0.0; - out_4838410850925167920[33] = 0.0; - out_4838410850925167920[34] = 0.0; - out_4838410850925167920[35] = 0.0; - out_4838410850925167920[36] = 0.0; - out_4838410850925167920[37] = 0.0; - out_4838410850925167920[38] = 1.0; - out_4838410850925167920[39] = 0.0; - out_4838410850925167920[40] = 0.0; - out_4838410850925167920[41] = 0.0; - out_4838410850925167920[42] = 0.0; - out_4838410850925167920[43] = 0.0; - out_4838410850925167920[44] = 0.0; - out_4838410850925167920[45] = 0.0; - out_4838410850925167920[46] = 0.0; - out_4838410850925167920[47] = 0.0; - out_4838410850925167920[48] = 0.0; - out_4838410850925167920[49] = 0.0; - out_4838410850925167920[50] = 0.0; - out_4838410850925167920[51] = 0.0; - out_4838410850925167920[52] = 0.0; - out_4838410850925167920[53] = 0.0; - out_4838410850925167920[54] = 0.0; - out_4838410850925167920[55] = 0.0; - out_4838410850925167920[56] = 0.0; - out_4838410850925167920[57] = 1.0; - out_4838410850925167920[58] = 0.0; - out_4838410850925167920[59] = 0.0; - out_4838410850925167920[60] = 0.0; - out_4838410850925167920[61] = 0.0; - out_4838410850925167920[62] = 0.0; - out_4838410850925167920[63] = 0.0; - out_4838410850925167920[64] = 0.0; - out_4838410850925167920[65] = 0.0; - out_4838410850925167920[66] = 0.0; - out_4838410850925167920[67] = 0.0; - out_4838410850925167920[68] = 0.0; - out_4838410850925167920[69] = 0.0; - out_4838410850925167920[70] = 0.0; - out_4838410850925167920[71] = 0.0; - out_4838410850925167920[72] = 0.0; - out_4838410850925167920[73] = 0.0; - out_4838410850925167920[74] = 0.0; - out_4838410850925167920[75] = 0.0; - out_4838410850925167920[76] = 1.0; - out_4838410850925167920[77] = 0.0; - out_4838410850925167920[78] = 0.0; - out_4838410850925167920[79] = 0.0; - out_4838410850925167920[80] = 0.0; - out_4838410850925167920[81] = 0.0; - out_4838410850925167920[82] = 0.0; - out_4838410850925167920[83] = 0.0; - out_4838410850925167920[84] = 0.0; - out_4838410850925167920[85] = 0.0; - out_4838410850925167920[86] = 0.0; - out_4838410850925167920[87] = 0.0; - out_4838410850925167920[88] = 0.0; - out_4838410850925167920[89] = 0.0; - out_4838410850925167920[90] = 0.0; - out_4838410850925167920[91] = 0.0; - out_4838410850925167920[92] = 0.0; - out_4838410850925167920[93] = 0.0; - out_4838410850925167920[94] = 0.0; - out_4838410850925167920[95] = 1.0; - out_4838410850925167920[96] = 0.0; - out_4838410850925167920[97] = 0.0; - out_4838410850925167920[98] = 0.0; - out_4838410850925167920[99] = 0.0; - out_4838410850925167920[100] = 0.0; - out_4838410850925167920[101] = 0.0; - out_4838410850925167920[102] = 0.0; - out_4838410850925167920[103] = 0.0; - out_4838410850925167920[104] = 0.0; - out_4838410850925167920[105] = 0.0; - out_4838410850925167920[106] = 0.0; - out_4838410850925167920[107] = 0.0; - out_4838410850925167920[108] = 0.0; - out_4838410850925167920[109] = 0.0; - out_4838410850925167920[110] = 0.0; - out_4838410850925167920[111] = 0.0; - out_4838410850925167920[112] = 0.0; - out_4838410850925167920[113] = 0.0; - out_4838410850925167920[114] = 1.0; - out_4838410850925167920[115] = 0.0; - out_4838410850925167920[116] = 0.0; - out_4838410850925167920[117] = 0.0; - out_4838410850925167920[118] = 0.0; - out_4838410850925167920[119] = 0.0; - out_4838410850925167920[120] = 0.0; - out_4838410850925167920[121] = 0.0; - out_4838410850925167920[122] = 0.0; - out_4838410850925167920[123] = 0.0; - out_4838410850925167920[124] = 0.0; - out_4838410850925167920[125] = 0.0; - out_4838410850925167920[126] = 0.0; - out_4838410850925167920[127] = 0.0; - out_4838410850925167920[128] = 0.0; - out_4838410850925167920[129] = 0.0; - out_4838410850925167920[130] = 0.0; - out_4838410850925167920[131] = 0.0; - out_4838410850925167920[132] = 0.0; - out_4838410850925167920[133] = 1.0; - out_4838410850925167920[134] = 0.0; - out_4838410850925167920[135] = 0.0; - out_4838410850925167920[136] = 0.0; - out_4838410850925167920[137] = 0.0; - out_4838410850925167920[138] = 0.0; - out_4838410850925167920[139] = 0.0; - out_4838410850925167920[140] = 0.0; - out_4838410850925167920[141] = 0.0; - out_4838410850925167920[142] = 0.0; - out_4838410850925167920[143] = 0.0; - out_4838410850925167920[144] = 0.0; - out_4838410850925167920[145] = 0.0; - out_4838410850925167920[146] = 0.0; - out_4838410850925167920[147] = 0.0; - out_4838410850925167920[148] = 0.0; - out_4838410850925167920[149] = 0.0; - out_4838410850925167920[150] = 0.0; - out_4838410850925167920[151] = 0.0; - out_4838410850925167920[152] = 1.0; - out_4838410850925167920[153] = 0.0; - out_4838410850925167920[154] = 0.0; - out_4838410850925167920[155] = 0.0; - out_4838410850925167920[156] = 0.0; - out_4838410850925167920[157] = 0.0; - out_4838410850925167920[158] = 0.0; - out_4838410850925167920[159] = 0.0; - out_4838410850925167920[160] = 0.0; - out_4838410850925167920[161] = 0.0; - out_4838410850925167920[162] = 0.0; - out_4838410850925167920[163] = 0.0; - out_4838410850925167920[164] = 0.0; - out_4838410850925167920[165] = 0.0; - out_4838410850925167920[166] = 0.0; - out_4838410850925167920[167] = 0.0; - out_4838410850925167920[168] = 0.0; - out_4838410850925167920[169] = 0.0; - out_4838410850925167920[170] = 0.0; - out_4838410850925167920[171] = 1.0; - out_4838410850925167920[172] = 0.0; - out_4838410850925167920[173] = 0.0; - out_4838410850925167920[174] = 0.0; - out_4838410850925167920[175] = 0.0; - out_4838410850925167920[176] = 0.0; - out_4838410850925167920[177] = 0.0; - out_4838410850925167920[178] = 0.0; - out_4838410850925167920[179] = 0.0; - out_4838410850925167920[180] = 0.0; - out_4838410850925167920[181] = 0.0; - out_4838410850925167920[182] = 0.0; - out_4838410850925167920[183] = 0.0; - out_4838410850925167920[184] = 0.0; - out_4838410850925167920[185] = 0.0; - out_4838410850925167920[186] = 0.0; - out_4838410850925167920[187] = 0.0; - out_4838410850925167920[188] = 0.0; - out_4838410850925167920[189] = 0.0; - out_4838410850925167920[190] = 1.0; - out_4838410850925167920[191] = 0.0; - out_4838410850925167920[192] = 0.0; - out_4838410850925167920[193] = 0.0; - out_4838410850925167920[194] = 0.0; - out_4838410850925167920[195] = 0.0; - out_4838410850925167920[196] = 0.0; - out_4838410850925167920[197] = 0.0; - out_4838410850925167920[198] = 0.0; - out_4838410850925167920[199] = 0.0; - out_4838410850925167920[200] = 0.0; - out_4838410850925167920[201] = 0.0; - out_4838410850925167920[202] = 0.0; - out_4838410850925167920[203] = 0.0; - out_4838410850925167920[204] = 0.0; - out_4838410850925167920[205] = 0.0; - out_4838410850925167920[206] = 0.0; - out_4838410850925167920[207] = 0.0; - out_4838410850925167920[208] = 0.0; - out_4838410850925167920[209] = 1.0; - out_4838410850925167920[210] = 0.0; - out_4838410850925167920[211] = 0.0; - out_4838410850925167920[212] = 0.0; - out_4838410850925167920[213] = 0.0; - out_4838410850925167920[214] = 0.0; - out_4838410850925167920[215] = 0.0; - out_4838410850925167920[216] = 0.0; - out_4838410850925167920[217] = 0.0; - out_4838410850925167920[218] = 0.0; - out_4838410850925167920[219] = 0.0; - out_4838410850925167920[220] = 0.0; - out_4838410850925167920[221] = 0.0; - out_4838410850925167920[222] = 0.0; - out_4838410850925167920[223] = 0.0; - out_4838410850925167920[224] = 0.0; - out_4838410850925167920[225] = 0.0; - out_4838410850925167920[226] = 0.0; - out_4838410850925167920[227] = 0.0; - out_4838410850925167920[228] = 1.0; - out_4838410850925167920[229] = 0.0; - out_4838410850925167920[230] = 0.0; - out_4838410850925167920[231] = 0.0; - out_4838410850925167920[232] = 0.0; - out_4838410850925167920[233] = 0.0; - out_4838410850925167920[234] = 0.0; - out_4838410850925167920[235] = 0.0; - out_4838410850925167920[236] = 0.0; - out_4838410850925167920[237] = 0.0; - out_4838410850925167920[238] = 0.0; - out_4838410850925167920[239] = 0.0; - out_4838410850925167920[240] = 0.0; - out_4838410850925167920[241] = 0.0; - out_4838410850925167920[242] = 0.0; - out_4838410850925167920[243] = 0.0; - out_4838410850925167920[244] = 0.0; - out_4838410850925167920[245] = 0.0; - out_4838410850925167920[246] = 0.0; - out_4838410850925167920[247] = 1.0; - out_4838410850925167920[248] = 0.0; - out_4838410850925167920[249] = 0.0; - out_4838410850925167920[250] = 0.0; - out_4838410850925167920[251] = 0.0; - out_4838410850925167920[252] = 0.0; - out_4838410850925167920[253] = 0.0; - out_4838410850925167920[254] = 0.0; - out_4838410850925167920[255] = 0.0; - out_4838410850925167920[256] = 0.0; - out_4838410850925167920[257] = 0.0; - out_4838410850925167920[258] = 0.0; - out_4838410850925167920[259] = 0.0; - out_4838410850925167920[260] = 0.0; - out_4838410850925167920[261] = 0.0; - out_4838410850925167920[262] = 0.0; - out_4838410850925167920[263] = 0.0; - out_4838410850925167920[264] = 0.0; - out_4838410850925167920[265] = 0.0; - out_4838410850925167920[266] = 1.0; - out_4838410850925167920[267] = 0.0; - out_4838410850925167920[268] = 0.0; - out_4838410850925167920[269] = 0.0; - out_4838410850925167920[270] = 0.0; - out_4838410850925167920[271] = 0.0; - out_4838410850925167920[272] = 0.0; - out_4838410850925167920[273] = 0.0; - out_4838410850925167920[274] = 0.0; - out_4838410850925167920[275] = 0.0; - out_4838410850925167920[276] = 0.0; - out_4838410850925167920[277] = 0.0; - out_4838410850925167920[278] = 0.0; - out_4838410850925167920[279] = 0.0; - out_4838410850925167920[280] = 0.0; - out_4838410850925167920[281] = 0.0; - out_4838410850925167920[282] = 0.0; - out_4838410850925167920[283] = 0.0; - out_4838410850925167920[284] = 0.0; - out_4838410850925167920[285] = 1.0; - out_4838410850925167920[286] = 0.0; - out_4838410850925167920[287] = 0.0; - out_4838410850925167920[288] = 0.0; - out_4838410850925167920[289] = 0.0; - out_4838410850925167920[290] = 0.0; - out_4838410850925167920[291] = 0.0; - out_4838410850925167920[292] = 0.0; - out_4838410850925167920[293] = 0.0; - out_4838410850925167920[294] = 0.0; - out_4838410850925167920[295] = 0.0; - out_4838410850925167920[296] = 0.0; - out_4838410850925167920[297] = 0.0; - out_4838410850925167920[298] = 0.0; - out_4838410850925167920[299] = 0.0; - out_4838410850925167920[300] = 0.0; - out_4838410850925167920[301] = 0.0; - out_4838410850925167920[302] = 0.0; - out_4838410850925167920[303] = 0.0; - out_4838410850925167920[304] = 1.0; - out_4838410850925167920[305] = 0.0; - out_4838410850925167920[306] = 0.0; - out_4838410850925167920[307] = 0.0; - out_4838410850925167920[308] = 0.0; - out_4838410850925167920[309] = 0.0; - out_4838410850925167920[310] = 0.0; - out_4838410850925167920[311] = 0.0; - out_4838410850925167920[312] = 0.0; - out_4838410850925167920[313] = 0.0; - out_4838410850925167920[314] = 0.0; - out_4838410850925167920[315] = 0.0; - out_4838410850925167920[316] = 0.0; - out_4838410850925167920[317] = 0.0; - out_4838410850925167920[318] = 0.0; - out_4838410850925167920[319] = 0.0; - out_4838410850925167920[320] = 0.0; - out_4838410850925167920[321] = 0.0; - out_4838410850925167920[322] = 0.0; - out_4838410850925167920[323] = 1.0; +void H_mod_fun(double *state, double *out_2374218852650606410) { + out_2374218852650606410[0] = 1.0; + out_2374218852650606410[1] = 0.0; + out_2374218852650606410[2] = 0.0; + out_2374218852650606410[3] = 0.0; + out_2374218852650606410[4] = 0.0; + out_2374218852650606410[5] = 0.0; + out_2374218852650606410[6] = 0.0; + out_2374218852650606410[7] = 0.0; + out_2374218852650606410[8] = 0.0; + out_2374218852650606410[9] = 0.0; + out_2374218852650606410[10] = 0.0; + out_2374218852650606410[11] = 0.0; + out_2374218852650606410[12] = 0.0; + out_2374218852650606410[13] = 0.0; + out_2374218852650606410[14] = 0.0; + out_2374218852650606410[15] = 0.0; + out_2374218852650606410[16] = 0.0; + out_2374218852650606410[17] = 0.0; + out_2374218852650606410[18] = 0.0; + out_2374218852650606410[19] = 1.0; + out_2374218852650606410[20] = 0.0; + out_2374218852650606410[21] = 0.0; + out_2374218852650606410[22] = 0.0; + out_2374218852650606410[23] = 0.0; + out_2374218852650606410[24] = 0.0; + out_2374218852650606410[25] = 0.0; + out_2374218852650606410[26] = 0.0; + out_2374218852650606410[27] = 0.0; + out_2374218852650606410[28] = 0.0; + out_2374218852650606410[29] = 0.0; + out_2374218852650606410[30] = 0.0; + out_2374218852650606410[31] = 0.0; + out_2374218852650606410[32] = 0.0; + out_2374218852650606410[33] = 0.0; + out_2374218852650606410[34] = 0.0; + out_2374218852650606410[35] = 0.0; + out_2374218852650606410[36] = 0.0; + out_2374218852650606410[37] = 0.0; + out_2374218852650606410[38] = 1.0; + out_2374218852650606410[39] = 0.0; + out_2374218852650606410[40] = 0.0; + out_2374218852650606410[41] = 0.0; + out_2374218852650606410[42] = 0.0; + out_2374218852650606410[43] = 0.0; + out_2374218852650606410[44] = 0.0; + out_2374218852650606410[45] = 0.0; + out_2374218852650606410[46] = 0.0; + out_2374218852650606410[47] = 0.0; + out_2374218852650606410[48] = 0.0; + out_2374218852650606410[49] = 0.0; + out_2374218852650606410[50] = 0.0; + out_2374218852650606410[51] = 0.0; + out_2374218852650606410[52] = 0.0; + out_2374218852650606410[53] = 0.0; + out_2374218852650606410[54] = 0.0; + out_2374218852650606410[55] = 0.0; + out_2374218852650606410[56] = 0.0; + out_2374218852650606410[57] = 1.0; + out_2374218852650606410[58] = 0.0; + out_2374218852650606410[59] = 0.0; + out_2374218852650606410[60] = 0.0; + out_2374218852650606410[61] = 0.0; + out_2374218852650606410[62] = 0.0; + out_2374218852650606410[63] = 0.0; + out_2374218852650606410[64] = 0.0; + out_2374218852650606410[65] = 0.0; + out_2374218852650606410[66] = 0.0; + out_2374218852650606410[67] = 0.0; + out_2374218852650606410[68] = 0.0; + out_2374218852650606410[69] = 0.0; + out_2374218852650606410[70] = 0.0; + out_2374218852650606410[71] = 0.0; + out_2374218852650606410[72] = 0.0; + out_2374218852650606410[73] = 0.0; + out_2374218852650606410[74] = 0.0; + out_2374218852650606410[75] = 0.0; + out_2374218852650606410[76] = 1.0; + out_2374218852650606410[77] = 0.0; + out_2374218852650606410[78] = 0.0; + out_2374218852650606410[79] = 0.0; + out_2374218852650606410[80] = 0.0; + out_2374218852650606410[81] = 0.0; + out_2374218852650606410[82] = 0.0; + out_2374218852650606410[83] = 0.0; + out_2374218852650606410[84] = 0.0; + out_2374218852650606410[85] = 0.0; + out_2374218852650606410[86] = 0.0; + out_2374218852650606410[87] = 0.0; + out_2374218852650606410[88] = 0.0; + out_2374218852650606410[89] = 0.0; + out_2374218852650606410[90] = 0.0; + out_2374218852650606410[91] = 0.0; + out_2374218852650606410[92] = 0.0; + out_2374218852650606410[93] = 0.0; + out_2374218852650606410[94] = 0.0; + out_2374218852650606410[95] = 1.0; + out_2374218852650606410[96] = 0.0; + out_2374218852650606410[97] = 0.0; + out_2374218852650606410[98] = 0.0; + out_2374218852650606410[99] = 0.0; + out_2374218852650606410[100] = 0.0; + out_2374218852650606410[101] = 0.0; + out_2374218852650606410[102] = 0.0; + out_2374218852650606410[103] = 0.0; + out_2374218852650606410[104] = 0.0; + out_2374218852650606410[105] = 0.0; + out_2374218852650606410[106] = 0.0; + out_2374218852650606410[107] = 0.0; + out_2374218852650606410[108] = 0.0; + out_2374218852650606410[109] = 0.0; + out_2374218852650606410[110] = 0.0; + out_2374218852650606410[111] = 0.0; + out_2374218852650606410[112] = 0.0; + out_2374218852650606410[113] = 0.0; + out_2374218852650606410[114] = 1.0; + out_2374218852650606410[115] = 0.0; + out_2374218852650606410[116] = 0.0; + out_2374218852650606410[117] = 0.0; + out_2374218852650606410[118] = 0.0; + out_2374218852650606410[119] = 0.0; + out_2374218852650606410[120] = 0.0; + out_2374218852650606410[121] = 0.0; + out_2374218852650606410[122] = 0.0; + out_2374218852650606410[123] = 0.0; + out_2374218852650606410[124] = 0.0; + out_2374218852650606410[125] = 0.0; + out_2374218852650606410[126] = 0.0; + out_2374218852650606410[127] = 0.0; + out_2374218852650606410[128] = 0.0; + out_2374218852650606410[129] = 0.0; + out_2374218852650606410[130] = 0.0; + out_2374218852650606410[131] = 0.0; + out_2374218852650606410[132] = 0.0; + out_2374218852650606410[133] = 1.0; + out_2374218852650606410[134] = 0.0; + out_2374218852650606410[135] = 0.0; + out_2374218852650606410[136] = 0.0; + out_2374218852650606410[137] = 0.0; + out_2374218852650606410[138] = 0.0; + out_2374218852650606410[139] = 0.0; + out_2374218852650606410[140] = 0.0; + out_2374218852650606410[141] = 0.0; + out_2374218852650606410[142] = 0.0; + out_2374218852650606410[143] = 0.0; + out_2374218852650606410[144] = 0.0; + out_2374218852650606410[145] = 0.0; + out_2374218852650606410[146] = 0.0; + out_2374218852650606410[147] = 0.0; + out_2374218852650606410[148] = 0.0; + out_2374218852650606410[149] = 0.0; + out_2374218852650606410[150] = 0.0; + out_2374218852650606410[151] = 0.0; + out_2374218852650606410[152] = 1.0; + out_2374218852650606410[153] = 0.0; + out_2374218852650606410[154] = 0.0; + out_2374218852650606410[155] = 0.0; + out_2374218852650606410[156] = 0.0; + out_2374218852650606410[157] = 0.0; + out_2374218852650606410[158] = 0.0; + out_2374218852650606410[159] = 0.0; + out_2374218852650606410[160] = 0.0; + out_2374218852650606410[161] = 0.0; + out_2374218852650606410[162] = 0.0; + out_2374218852650606410[163] = 0.0; + out_2374218852650606410[164] = 0.0; + out_2374218852650606410[165] = 0.0; + out_2374218852650606410[166] = 0.0; + out_2374218852650606410[167] = 0.0; + out_2374218852650606410[168] = 0.0; + out_2374218852650606410[169] = 0.0; + out_2374218852650606410[170] = 0.0; + out_2374218852650606410[171] = 1.0; + out_2374218852650606410[172] = 0.0; + out_2374218852650606410[173] = 0.0; + out_2374218852650606410[174] = 0.0; + out_2374218852650606410[175] = 0.0; + out_2374218852650606410[176] = 0.0; + out_2374218852650606410[177] = 0.0; + out_2374218852650606410[178] = 0.0; + out_2374218852650606410[179] = 0.0; + out_2374218852650606410[180] = 0.0; + out_2374218852650606410[181] = 0.0; + out_2374218852650606410[182] = 0.0; + out_2374218852650606410[183] = 0.0; + out_2374218852650606410[184] = 0.0; + out_2374218852650606410[185] = 0.0; + out_2374218852650606410[186] = 0.0; + out_2374218852650606410[187] = 0.0; + out_2374218852650606410[188] = 0.0; + out_2374218852650606410[189] = 0.0; + out_2374218852650606410[190] = 1.0; + out_2374218852650606410[191] = 0.0; + out_2374218852650606410[192] = 0.0; + out_2374218852650606410[193] = 0.0; + out_2374218852650606410[194] = 0.0; + out_2374218852650606410[195] = 0.0; + out_2374218852650606410[196] = 0.0; + out_2374218852650606410[197] = 0.0; + out_2374218852650606410[198] = 0.0; + out_2374218852650606410[199] = 0.0; + out_2374218852650606410[200] = 0.0; + out_2374218852650606410[201] = 0.0; + out_2374218852650606410[202] = 0.0; + out_2374218852650606410[203] = 0.0; + out_2374218852650606410[204] = 0.0; + out_2374218852650606410[205] = 0.0; + out_2374218852650606410[206] = 0.0; + out_2374218852650606410[207] = 0.0; + out_2374218852650606410[208] = 0.0; + out_2374218852650606410[209] = 1.0; + out_2374218852650606410[210] = 0.0; + out_2374218852650606410[211] = 0.0; + out_2374218852650606410[212] = 0.0; + out_2374218852650606410[213] = 0.0; + out_2374218852650606410[214] = 0.0; + out_2374218852650606410[215] = 0.0; + out_2374218852650606410[216] = 0.0; + out_2374218852650606410[217] = 0.0; + out_2374218852650606410[218] = 0.0; + out_2374218852650606410[219] = 0.0; + out_2374218852650606410[220] = 0.0; + out_2374218852650606410[221] = 0.0; + out_2374218852650606410[222] = 0.0; + out_2374218852650606410[223] = 0.0; + out_2374218852650606410[224] = 0.0; + out_2374218852650606410[225] = 0.0; + out_2374218852650606410[226] = 0.0; + out_2374218852650606410[227] = 0.0; + out_2374218852650606410[228] = 1.0; + out_2374218852650606410[229] = 0.0; + out_2374218852650606410[230] = 0.0; + out_2374218852650606410[231] = 0.0; + out_2374218852650606410[232] = 0.0; + out_2374218852650606410[233] = 0.0; + out_2374218852650606410[234] = 0.0; + out_2374218852650606410[235] = 0.0; + out_2374218852650606410[236] = 0.0; + out_2374218852650606410[237] = 0.0; + out_2374218852650606410[238] = 0.0; + out_2374218852650606410[239] = 0.0; + out_2374218852650606410[240] = 0.0; + out_2374218852650606410[241] = 0.0; + out_2374218852650606410[242] = 0.0; + out_2374218852650606410[243] = 0.0; + out_2374218852650606410[244] = 0.0; + out_2374218852650606410[245] = 0.0; + out_2374218852650606410[246] = 0.0; + out_2374218852650606410[247] = 1.0; + out_2374218852650606410[248] = 0.0; + out_2374218852650606410[249] = 0.0; + out_2374218852650606410[250] = 0.0; + out_2374218852650606410[251] = 0.0; + out_2374218852650606410[252] = 0.0; + out_2374218852650606410[253] = 0.0; + out_2374218852650606410[254] = 0.0; + out_2374218852650606410[255] = 0.0; + out_2374218852650606410[256] = 0.0; + out_2374218852650606410[257] = 0.0; + out_2374218852650606410[258] = 0.0; + out_2374218852650606410[259] = 0.0; + out_2374218852650606410[260] = 0.0; + out_2374218852650606410[261] = 0.0; + out_2374218852650606410[262] = 0.0; + out_2374218852650606410[263] = 0.0; + out_2374218852650606410[264] = 0.0; + out_2374218852650606410[265] = 0.0; + out_2374218852650606410[266] = 1.0; + out_2374218852650606410[267] = 0.0; + out_2374218852650606410[268] = 0.0; + out_2374218852650606410[269] = 0.0; + out_2374218852650606410[270] = 0.0; + out_2374218852650606410[271] = 0.0; + out_2374218852650606410[272] = 0.0; + out_2374218852650606410[273] = 0.0; + out_2374218852650606410[274] = 0.0; + out_2374218852650606410[275] = 0.0; + out_2374218852650606410[276] = 0.0; + out_2374218852650606410[277] = 0.0; + out_2374218852650606410[278] = 0.0; + out_2374218852650606410[279] = 0.0; + out_2374218852650606410[280] = 0.0; + out_2374218852650606410[281] = 0.0; + out_2374218852650606410[282] = 0.0; + out_2374218852650606410[283] = 0.0; + out_2374218852650606410[284] = 0.0; + out_2374218852650606410[285] = 1.0; + out_2374218852650606410[286] = 0.0; + out_2374218852650606410[287] = 0.0; + out_2374218852650606410[288] = 0.0; + out_2374218852650606410[289] = 0.0; + out_2374218852650606410[290] = 0.0; + out_2374218852650606410[291] = 0.0; + out_2374218852650606410[292] = 0.0; + out_2374218852650606410[293] = 0.0; + out_2374218852650606410[294] = 0.0; + out_2374218852650606410[295] = 0.0; + out_2374218852650606410[296] = 0.0; + out_2374218852650606410[297] = 0.0; + out_2374218852650606410[298] = 0.0; + out_2374218852650606410[299] = 0.0; + out_2374218852650606410[300] = 0.0; + out_2374218852650606410[301] = 0.0; + out_2374218852650606410[302] = 0.0; + out_2374218852650606410[303] = 0.0; + out_2374218852650606410[304] = 1.0; + out_2374218852650606410[305] = 0.0; + out_2374218852650606410[306] = 0.0; + out_2374218852650606410[307] = 0.0; + out_2374218852650606410[308] = 0.0; + out_2374218852650606410[309] = 0.0; + out_2374218852650606410[310] = 0.0; + out_2374218852650606410[311] = 0.0; + out_2374218852650606410[312] = 0.0; + out_2374218852650606410[313] = 0.0; + out_2374218852650606410[314] = 0.0; + out_2374218852650606410[315] = 0.0; + out_2374218852650606410[316] = 0.0; + out_2374218852650606410[317] = 0.0; + out_2374218852650606410[318] = 0.0; + out_2374218852650606410[319] = 0.0; + out_2374218852650606410[320] = 0.0; + out_2374218852650606410[321] = 0.0; + out_2374218852650606410[322] = 0.0; + out_2374218852650606410[323] = 1.0; } -void f_fun(double *state, double dt, double *out_8751525120696163789) { - out_8751525120696163789[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_8751525120696163789[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_8751525120696163789[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_8751525120696163789[3] = dt*state[12] + state[3]; - out_8751525120696163789[4] = dt*state[13] + state[4]; - out_8751525120696163789[5] = dt*state[14] + state[5]; - out_8751525120696163789[6] = state[6]; - out_8751525120696163789[7] = state[7]; - out_8751525120696163789[8] = state[8]; - out_8751525120696163789[9] = state[9]; - out_8751525120696163789[10] = state[10]; - out_8751525120696163789[11] = state[11]; - out_8751525120696163789[12] = state[12]; - out_8751525120696163789[13] = state[13]; - out_8751525120696163789[14] = state[14]; - out_8751525120696163789[15] = state[15]; - out_8751525120696163789[16] = state[16]; - out_8751525120696163789[17] = state[17]; +void f_fun(double *state, double dt, double *out_4392889107205501252) { + out_4392889107205501252[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_4392889107205501252[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_4392889107205501252[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_4392889107205501252[3] = dt*state[12] + state[3]; + out_4392889107205501252[4] = dt*state[13] + state[4]; + out_4392889107205501252[5] = dt*state[14] + state[5]; + out_4392889107205501252[6] = state[6]; + out_4392889107205501252[7] = state[7]; + out_4392889107205501252[8] = state[8]; + out_4392889107205501252[9] = state[9]; + out_4392889107205501252[10] = state[10]; + out_4392889107205501252[11] = state[11]; + out_4392889107205501252[12] = state[12]; + out_4392889107205501252[13] = state[13]; + out_4392889107205501252[14] = state[14]; + out_4392889107205501252[15] = state[15]; + out_4392889107205501252[16] = state[16]; + out_4392889107205501252[17] = state[17]; } -void F_fun(double *state, double dt, double *out_7175128552495691270) { - out_7175128552495691270[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_7175128552495691270[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_7175128552495691270[2] = 0; - out_7175128552495691270[3] = 0; - out_7175128552495691270[4] = 0; - out_7175128552495691270[5] = 0; - out_7175128552495691270[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_7175128552495691270[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_7175128552495691270[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_7175128552495691270[9] = 0; - out_7175128552495691270[10] = 0; - out_7175128552495691270[11] = 0; - out_7175128552495691270[12] = 0; - out_7175128552495691270[13] = 0; - out_7175128552495691270[14] = 0; - out_7175128552495691270[15] = 0; - out_7175128552495691270[16] = 0; - out_7175128552495691270[17] = 0; - out_7175128552495691270[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_7175128552495691270[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_7175128552495691270[20] = 0; - out_7175128552495691270[21] = 0; - out_7175128552495691270[22] = 0; - out_7175128552495691270[23] = 0; - out_7175128552495691270[24] = 0; - out_7175128552495691270[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_7175128552495691270[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_7175128552495691270[27] = 0; - out_7175128552495691270[28] = 0; - out_7175128552495691270[29] = 0; - out_7175128552495691270[30] = 0; - out_7175128552495691270[31] = 0; - out_7175128552495691270[32] = 0; - out_7175128552495691270[33] = 0; - out_7175128552495691270[34] = 0; - out_7175128552495691270[35] = 0; - out_7175128552495691270[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_7175128552495691270[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_7175128552495691270[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_7175128552495691270[39] = 0; - out_7175128552495691270[40] = 0; - out_7175128552495691270[41] = 0; - out_7175128552495691270[42] = 0; - out_7175128552495691270[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_7175128552495691270[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_7175128552495691270[45] = 0; - out_7175128552495691270[46] = 0; - out_7175128552495691270[47] = 0; - out_7175128552495691270[48] = 0; - out_7175128552495691270[49] = 0; - out_7175128552495691270[50] = 0; - out_7175128552495691270[51] = 0; - out_7175128552495691270[52] = 0; - out_7175128552495691270[53] = 0; - out_7175128552495691270[54] = 0; - out_7175128552495691270[55] = 0; - out_7175128552495691270[56] = 0; - out_7175128552495691270[57] = 1; - out_7175128552495691270[58] = 0; - out_7175128552495691270[59] = 0; - out_7175128552495691270[60] = 0; - out_7175128552495691270[61] = 0; - out_7175128552495691270[62] = 0; - out_7175128552495691270[63] = 0; - out_7175128552495691270[64] = 0; - out_7175128552495691270[65] = 0; - out_7175128552495691270[66] = dt; - out_7175128552495691270[67] = 0; - out_7175128552495691270[68] = 0; - out_7175128552495691270[69] = 0; - out_7175128552495691270[70] = 0; - out_7175128552495691270[71] = 0; - out_7175128552495691270[72] = 0; - out_7175128552495691270[73] = 0; - out_7175128552495691270[74] = 0; - out_7175128552495691270[75] = 0; - out_7175128552495691270[76] = 1; - out_7175128552495691270[77] = 0; - out_7175128552495691270[78] = 0; - out_7175128552495691270[79] = 0; - out_7175128552495691270[80] = 0; - out_7175128552495691270[81] = 0; - out_7175128552495691270[82] = 0; - out_7175128552495691270[83] = 0; - out_7175128552495691270[84] = 0; - out_7175128552495691270[85] = dt; - out_7175128552495691270[86] = 0; - out_7175128552495691270[87] = 0; - out_7175128552495691270[88] = 0; - out_7175128552495691270[89] = 0; - out_7175128552495691270[90] = 0; - out_7175128552495691270[91] = 0; - out_7175128552495691270[92] = 0; - out_7175128552495691270[93] = 0; - out_7175128552495691270[94] = 0; - out_7175128552495691270[95] = 1; - out_7175128552495691270[96] = 0; - out_7175128552495691270[97] = 0; - out_7175128552495691270[98] = 0; - out_7175128552495691270[99] = 0; - out_7175128552495691270[100] = 0; - out_7175128552495691270[101] = 0; - out_7175128552495691270[102] = 0; - out_7175128552495691270[103] = 0; - out_7175128552495691270[104] = dt; - out_7175128552495691270[105] = 0; - out_7175128552495691270[106] = 0; - out_7175128552495691270[107] = 0; - out_7175128552495691270[108] = 0; - out_7175128552495691270[109] = 0; - out_7175128552495691270[110] = 0; - out_7175128552495691270[111] = 0; - out_7175128552495691270[112] = 0; - out_7175128552495691270[113] = 0; - out_7175128552495691270[114] = 1; - out_7175128552495691270[115] = 0; - out_7175128552495691270[116] = 0; - out_7175128552495691270[117] = 0; - out_7175128552495691270[118] = 0; - out_7175128552495691270[119] = 0; - out_7175128552495691270[120] = 0; - out_7175128552495691270[121] = 0; - out_7175128552495691270[122] = 0; - out_7175128552495691270[123] = 0; - out_7175128552495691270[124] = 0; - out_7175128552495691270[125] = 0; - out_7175128552495691270[126] = 0; - out_7175128552495691270[127] = 0; - out_7175128552495691270[128] = 0; - out_7175128552495691270[129] = 0; - out_7175128552495691270[130] = 0; - out_7175128552495691270[131] = 0; - out_7175128552495691270[132] = 0; - out_7175128552495691270[133] = 1; - out_7175128552495691270[134] = 0; - out_7175128552495691270[135] = 0; - out_7175128552495691270[136] = 0; - out_7175128552495691270[137] = 0; - out_7175128552495691270[138] = 0; - out_7175128552495691270[139] = 0; - out_7175128552495691270[140] = 0; - out_7175128552495691270[141] = 0; - out_7175128552495691270[142] = 0; - out_7175128552495691270[143] = 0; - out_7175128552495691270[144] = 0; - out_7175128552495691270[145] = 0; - out_7175128552495691270[146] = 0; - out_7175128552495691270[147] = 0; - out_7175128552495691270[148] = 0; - out_7175128552495691270[149] = 0; - out_7175128552495691270[150] = 0; - out_7175128552495691270[151] = 0; - out_7175128552495691270[152] = 1; - out_7175128552495691270[153] = 0; - out_7175128552495691270[154] = 0; - out_7175128552495691270[155] = 0; - out_7175128552495691270[156] = 0; - out_7175128552495691270[157] = 0; - out_7175128552495691270[158] = 0; - out_7175128552495691270[159] = 0; - out_7175128552495691270[160] = 0; - out_7175128552495691270[161] = 0; - out_7175128552495691270[162] = 0; - out_7175128552495691270[163] = 0; - out_7175128552495691270[164] = 0; - out_7175128552495691270[165] = 0; - out_7175128552495691270[166] = 0; - out_7175128552495691270[167] = 0; - out_7175128552495691270[168] = 0; - out_7175128552495691270[169] = 0; - out_7175128552495691270[170] = 0; - out_7175128552495691270[171] = 1; - out_7175128552495691270[172] = 0; - out_7175128552495691270[173] = 0; - out_7175128552495691270[174] = 0; - out_7175128552495691270[175] = 0; - out_7175128552495691270[176] = 0; - out_7175128552495691270[177] = 0; - out_7175128552495691270[178] = 0; - out_7175128552495691270[179] = 0; - out_7175128552495691270[180] = 0; - out_7175128552495691270[181] = 0; - out_7175128552495691270[182] = 0; - out_7175128552495691270[183] = 0; - out_7175128552495691270[184] = 0; - out_7175128552495691270[185] = 0; - out_7175128552495691270[186] = 0; - out_7175128552495691270[187] = 0; - out_7175128552495691270[188] = 0; - out_7175128552495691270[189] = 0; - out_7175128552495691270[190] = 1; - out_7175128552495691270[191] = 0; - out_7175128552495691270[192] = 0; - out_7175128552495691270[193] = 0; - out_7175128552495691270[194] = 0; - out_7175128552495691270[195] = 0; - out_7175128552495691270[196] = 0; - out_7175128552495691270[197] = 0; - out_7175128552495691270[198] = 0; - out_7175128552495691270[199] = 0; - out_7175128552495691270[200] = 0; - out_7175128552495691270[201] = 0; - out_7175128552495691270[202] = 0; - out_7175128552495691270[203] = 0; - out_7175128552495691270[204] = 0; - out_7175128552495691270[205] = 0; - out_7175128552495691270[206] = 0; - out_7175128552495691270[207] = 0; - out_7175128552495691270[208] = 0; - out_7175128552495691270[209] = 1; - out_7175128552495691270[210] = 0; - out_7175128552495691270[211] = 0; - out_7175128552495691270[212] = 0; - out_7175128552495691270[213] = 0; - out_7175128552495691270[214] = 0; - out_7175128552495691270[215] = 0; - out_7175128552495691270[216] = 0; - out_7175128552495691270[217] = 0; - out_7175128552495691270[218] = 0; - out_7175128552495691270[219] = 0; - out_7175128552495691270[220] = 0; - out_7175128552495691270[221] = 0; - out_7175128552495691270[222] = 0; - out_7175128552495691270[223] = 0; - out_7175128552495691270[224] = 0; - out_7175128552495691270[225] = 0; - out_7175128552495691270[226] = 0; - out_7175128552495691270[227] = 0; - out_7175128552495691270[228] = 1; - out_7175128552495691270[229] = 0; - out_7175128552495691270[230] = 0; - out_7175128552495691270[231] = 0; - out_7175128552495691270[232] = 0; - out_7175128552495691270[233] = 0; - out_7175128552495691270[234] = 0; - out_7175128552495691270[235] = 0; - out_7175128552495691270[236] = 0; - out_7175128552495691270[237] = 0; - out_7175128552495691270[238] = 0; - out_7175128552495691270[239] = 0; - out_7175128552495691270[240] = 0; - out_7175128552495691270[241] = 0; - out_7175128552495691270[242] = 0; - out_7175128552495691270[243] = 0; - out_7175128552495691270[244] = 0; - out_7175128552495691270[245] = 0; - out_7175128552495691270[246] = 0; - out_7175128552495691270[247] = 1; - out_7175128552495691270[248] = 0; - out_7175128552495691270[249] = 0; - out_7175128552495691270[250] = 0; - out_7175128552495691270[251] = 0; - out_7175128552495691270[252] = 0; - out_7175128552495691270[253] = 0; - out_7175128552495691270[254] = 0; - out_7175128552495691270[255] = 0; - out_7175128552495691270[256] = 0; - out_7175128552495691270[257] = 0; - out_7175128552495691270[258] = 0; - out_7175128552495691270[259] = 0; - out_7175128552495691270[260] = 0; - out_7175128552495691270[261] = 0; - out_7175128552495691270[262] = 0; - out_7175128552495691270[263] = 0; - out_7175128552495691270[264] = 0; - out_7175128552495691270[265] = 0; - out_7175128552495691270[266] = 1; - out_7175128552495691270[267] = 0; - out_7175128552495691270[268] = 0; - out_7175128552495691270[269] = 0; - out_7175128552495691270[270] = 0; - out_7175128552495691270[271] = 0; - out_7175128552495691270[272] = 0; - out_7175128552495691270[273] = 0; - out_7175128552495691270[274] = 0; - out_7175128552495691270[275] = 0; - out_7175128552495691270[276] = 0; - out_7175128552495691270[277] = 0; - out_7175128552495691270[278] = 0; - out_7175128552495691270[279] = 0; - out_7175128552495691270[280] = 0; - out_7175128552495691270[281] = 0; - out_7175128552495691270[282] = 0; - out_7175128552495691270[283] = 0; - out_7175128552495691270[284] = 0; - out_7175128552495691270[285] = 1; - out_7175128552495691270[286] = 0; - out_7175128552495691270[287] = 0; - out_7175128552495691270[288] = 0; - out_7175128552495691270[289] = 0; - out_7175128552495691270[290] = 0; - out_7175128552495691270[291] = 0; - out_7175128552495691270[292] = 0; - out_7175128552495691270[293] = 0; - out_7175128552495691270[294] = 0; - out_7175128552495691270[295] = 0; - out_7175128552495691270[296] = 0; - out_7175128552495691270[297] = 0; - out_7175128552495691270[298] = 0; - out_7175128552495691270[299] = 0; - out_7175128552495691270[300] = 0; - out_7175128552495691270[301] = 0; - out_7175128552495691270[302] = 0; - out_7175128552495691270[303] = 0; - out_7175128552495691270[304] = 1; - out_7175128552495691270[305] = 0; - out_7175128552495691270[306] = 0; - out_7175128552495691270[307] = 0; - out_7175128552495691270[308] = 0; - out_7175128552495691270[309] = 0; - out_7175128552495691270[310] = 0; - out_7175128552495691270[311] = 0; - out_7175128552495691270[312] = 0; - out_7175128552495691270[313] = 0; - out_7175128552495691270[314] = 0; - out_7175128552495691270[315] = 0; - out_7175128552495691270[316] = 0; - out_7175128552495691270[317] = 0; - out_7175128552495691270[318] = 0; - out_7175128552495691270[319] = 0; - out_7175128552495691270[320] = 0; - out_7175128552495691270[321] = 0; - out_7175128552495691270[322] = 0; - out_7175128552495691270[323] = 1; +void F_fun(double *state, double dt, double *out_5161498735714983758) { + out_5161498735714983758[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_5161498735714983758[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_5161498735714983758[2] = 0; + out_5161498735714983758[3] = 0; + out_5161498735714983758[4] = 0; + out_5161498735714983758[5] = 0; + out_5161498735714983758[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_5161498735714983758[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_5161498735714983758[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_5161498735714983758[9] = 0; + out_5161498735714983758[10] = 0; + out_5161498735714983758[11] = 0; + out_5161498735714983758[12] = 0; + out_5161498735714983758[13] = 0; + out_5161498735714983758[14] = 0; + out_5161498735714983758[15] = 0; + out_5161498735714983758[16] = 0; + out_5161498735714983758[17] = 0; + out_5161498735714983758[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_5161498735714983758[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_5161498735714983758[20] = 0; + out_5161498735714983758[21] = 0; + out_5161498735714983758[22] = 0; + out_5161498735714983758[23] = 0; + out_5161498735714983758[24] = 0; + out_5161498735714983758[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_5161498735714983758[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_5161498735714983758[27] = 0; + out_5161498735714983758[28] = 0; + out_5161498735714983758[29] = 0; + out_5161498735714983758[30] = 0; + out_5161498735714983758[31] = 0; + out_5161498735714983758[32] = 0; + out_5161498735714983758[33] = 0; + out_5161498735714983758[34] = 0; + out_5161498735714983758[35] = 0; + out_5161498735714983758[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_5161498735714983758[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_5161498735714983758[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_5161498735714983758[39] = 0; + out_5161498735714983758[40] = 0; + out_5161498735714983758[41] = 0; + out_5161498735714983758[42] = 0; + out_5161498735714983758[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_5161498735714983758[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_5161498735714983758[45] = 0; + out_5161498735714983758[46] = 0; + out_5161498735714983758[47] = 0; + out_5161498735714983758[48] = 0; + out_5161498735714983758[49] = 0; + out_5161498735714983758[50] = 0; + out_5161498735714983758[51] = 0; + out_5161498735714983758[52] = 0; + out_5161498735714983758[53] = 0; + out_5161498735714983758[54] = 0; + out_5161498735714983758[55] = 0; + out_5161498735714983758[56] = 0; + out_5161498735714983758[57] = 1; + out_5161498735714983758[58] = 0; + out_5161498735714983758[59] = 0; + out_5161498735714983758[60] = 0; + out_5161498735714983758[61] = 0; + out_5161498735714983758[62] = 0; + out_5161498735714983758[63] = 0; + out_5161498735714983758[64] = 0; + out_5161498735714983758[65] = 0; + out_5161498735714983758[66] = dt; + out_5161498735714983758[67] = 0; + out_5161498735714983758[68] = 0; + out_5161498735714983758[69] = 0; + out_5161498735714983758[70] = 0; + out_5161498735714983758[71] = 0; + out_5161498735714983758[72] = 0; + out_5161498735714983758[73] = 0; + out_5161498735714983758[74] = 0; + out_5161498735714983758[75] = 0; + out_5161498735714983758[76] = 1; + out_5161498735714983758[77] = 0; + out_5161498735714983758[78] = 0; + out_5161498735714983758[79] = 0; + out_5161498735714983758[80] = 0; + out_5161498735714983758[81] = 0; + out_5161498735714983758[82] = 0; + out_5161498735714983758[83] = 0; + out_5161498735714983758[84] = 0; + out_5161498735714983758[85] = dt; + out_5161498735714983758[86] = 0; + out_5161498735714983758[87] = 0; + out_5161498735714983758[88] = 0; + out_5161498735714983758[89] = 0; + out_5161498735714983758[90] = 0; + out_5161498735714983758[91] = 0; + out_5161498735714983758[92] = 0; + out_5161498735714983758[93] = 0; + out_5161498735714983758[94] = 0; + out_5161498735714983758[95] = 1; + out_5161498735714983758[96] = 0; + out_5161498735714983758[97] = 0; + out_5161498735714983758[98] = 0; + out_5161498735714983758[99] = 0; + out_5161498735714983758[100] = 0; + out_5161498735714983758[101] = 0; + out_5161498735714983758[102] = 0; + out_5161498735714983758[103] = 0; + out_5161498735714983758[104] = dt; + out_5161498735714983758[105] = 0; + out_5161498735714983758[106] = 0; + out_5161498735714983758[107] = 0; + out_5161498735714983758[108] = 0; + out_5161498735714983758[109] = 0; + out_5161498735714983758[110] = 0; + out_5161498735714983758[111] = 0; + out_5161498735714983758[112] = 0; + out_5161498735714983758[113] = 0; + out_5161498735714983758[114] = 1; + out_5161498735714983758[115] = 0; + out_5161498735714983758[116] = 0; + out_5161498735714983758[117] = 0; + out_5161498735714983758[118] = 0; + out_5161498735714983758[119] = 0; + out_5161498735714983758[120] = 0; + out_5161498735714983758[121] = 0; + out_5161498735714983758[122] = 0; + out_5161498735714983758[123] = 0; + out_5161498735714983758[124] = 0; + out_5161498735714983758[125] = 0; + out_5161498735714983758[126] = 0; + out_5161498735714983758[127] = 0; + out_5161498735714983758[128] = 0; + out_5161498735714983758[129] = 0; + out_5161498735714983758[130] = 0; + out_5161498735714983758[131] = 0; + out_5161498735714983758[132] = 0; + out_5161498735714983758[133] = 1; + out_5161498735714983758[134] = 0; + out_5161498735714983758[135] = 0; + out_5161498735714983758[136] = 0; + out_5161498735714983758[137] = 0; + out_5161498735714983758[138] = 0; + out_5161498735714983758[139] = 0; + out_5161498735714983758[140] = 0; + out_5161498735714983758[141] = 0; + out_5161498735714983758[142] = 0; + out_5161498735714983758[143] = 0; + out_5161498735714983758[144] = 0; + out_5161498735714983758[145] = 0; + out_5161498735714983758[146] = 0; + out_5161498735714983758[147] = 0; + out_5161498735714983758[148] = 0; + out_5161498735714983758[149] = 0; + out_5161498735714983758[150] = 0; + out_5161498735714983758[151] = 0; + out_5161498735714983758[152] = 1; + out_5161498735714983758[153] = 0; + out_5161498735714983758[154] = 0; + out_5161498735714983758[155] = 0; + out_5161498735714983758[156] = 0; + out_5161498735714983758[157] = 0; + out_5161498735714983758[158] = 0; + out_5161498735714983758[159] = 0; + out_5161498735714983758[160] = 0; + out_5161498735714983758[161] = 0; + out_5161498735714983758[162] = 0; + out_5161498735714983758[163] = 0; + out_5161498735714983758[164] = 0; + out_5161498735714983758[165] = 0; + out_5161498735714983758[166] = 0; + out_5161498735714983758[167] = 0; + out_5161498735714983758[168] = 0; + out_5161498735714983758[169] = 0; + out_5161498735714983758[170] = 0; + out_5161498735714983758[171] = 1; + out_5161498735714983758[172] = 0; + out_5161498735714983758[173] = 0; + out_5161498735714983758[174] = 0; + out_5161498735714983758[175] = 0; + out_5161498735714983758[176] = 0; + out_5161498735714983758[177] = 0; + out_5161498735714983758[178] = 0; + out_5161498735714983758[179] = 0; + out_5161498735714983758[180] = 0; + out_5161498735714983758[181] = 0; + out_5161498735714983758[182] = 0; + out_5161498735714983758[183] = 0; + out_5161498735714983758[184] = 0; + out_5161498735714983758[185] = 0; + out_5161498735714983758[186] = 0; + out_5161498735714983758[187] = 0; + out_5161498735714983758[188] = 0; + out_5161498735714983758[189] = 0; + out_5161498735714983758[190] = 1; + out_5161498735714983758[191] = 0; + out_5161498735714983758[192] = 0; + out_5161498735714983758[193] = 0; + out_5161498735714983758[194] = 0; + out_5161498735714983758[195] = 0; + out_5161498735714983758[196] = 0; + out_5161498735714983758[197] = 0; + out_5161498735714983758[198] = 0; + out_5161498735714983758[199] = 0; + out_5161498735714983758[200] = 0; + out_5161498735714983758[201] = 0; + out_5161498735714983758[202] = 0; + out_5161498735714983758[203] = 0; + out_5161498735714983758[204] = 0; + out_5161498735714983758[205] = 0; + out_5161498735714983758[206] = 0; + out_5161498735714983758[207] = 0; + out_5161498735714983758[208] = 0; + out_5161498735714983758[209] = 1; + out_5161498735714983758[210] = 0; + out_5161498735714983758[211] = 0; + out_5161498735714983758[212] = 0; + out_5161498735714983758[213] = 0; + out_5161498735714983758[214] = 0; + out_5161498735714983758[215] = 0; + out_5161498735714983758[216] = 0; + out_5161498735714983758[217] = 0; + out_5161498735714983758[218] = 0; + out_5161498735714983758[219] = 0; + out_5161498735714983758[220] = 0; + out_5161498735714983758[221] = 0; + out_5161498735714983758[222] = 0; + out_5161498735714983758[223] = 0; + out_5161498735714983758[224] = 0; + out_5161498735714983758[225] = 0; + out_5161498735714983758[226] = 0; + out_5161498735714983758[227] = 0; + out_5161498735714983758[228] = 1; + out_5161498735714983758[229] = 0; + out_5161498735714983758[230] = 0; + out_5161498735714983758[231] = 0; + out_5161498735714983758[232] = 0; + out_5161498735714983758[233] = 0; + out_5161498735714983758[234] = 0; + out_5161498735714983758[235] = 0; + out_5161498735714983758[236] = 0; + out_5161498735714983758[237] = 0; + out_5161498735714983758[238] = 0; + out_5161498735714983758[239] = 0; + out_5161498735714983758[240] = 0; + out_5161498735714983758[241] = 0; + out_5161498735714983758[242] = 0; + out_5161498735714983758[243] = 0; + out_5161498735714983758[244] = 0; + out_5161498735714983758[245] = 0; + out_5161498735714983758[246] = 0; + out_5161498735714983758[247] = 1; + out_5161498735714983758[248] = 0; + out_5161498735714983758[249] = 0; + out_5161498735714983758[250] = 0; + out_5161498735714983758[251] = 0; + out_5161498735714983758[252] = 0; + out_5161498735714983758[253] = 0; + out_5161498735714983758[254] = 0; + out_5161498735714983758[255] = 0; + out_5161498735714983758[256] = 0; + out_5161498735714983758[257] = 0; + out_5161498735714983758[258] = 0; + out_5161498735714983758[259] = 0; + out_5161498735714983758[260] = 0; + out_5161498735714983758[261] = 0; + out_5161498735714983758[262] = 0; + out_5161498735714983758[263] = 0; + out_5161498735714983758[264] = 0; + out_5161498735714983758[265] = 0; + out_5161498735714983758[266] = 1; + out_5161498735714983758[267] = 0; + out_5161498735714983758[268] = 0; + out_5161498735714983758[269] = 0; + out_5161498735714983758[270] = 0; + out_5161498735714983758[271] = 0; + out_5161498735714983758[272] = 0; + out_5161498735714983758[273] = 0; + out_5161498735714983758[274] = 0; + out_5161498735714983758[275] = 0; + out_5161498735714983758[276] = 0; + out_5161498735714983758[277] = 0; + out_5161498735714983758[278] = 0; + out_5161498735714983758[279] = 0; + out_5161498735714983758[280] = 0; + out_5161498735714983758[281] = 0; + out_5161498735714983758[282] = 0; + out_5161498735714983758[283] = 0; + out_5161498735714983758[284] = 0; + out_5161498735714983758[285] = 1; + out_5161498735714983758[286] = 0; + out_5161498735714983758[287] = 0; + out_5161498735714983758[288] = 0; + out_5161498735714983758[289] = 0; + out_5161498735714983758[290] = 0; + out_5161498735714983758[291] = 0; + out_5161498735714983758[292] = 0; + out_5161498735714983758[293] = 0; + out_5161498735714983758[294] = 0; + out_5161498735714983758[295] = 0; + out_5161498735714983758[296] = 0; + out_5161498735714983758[297] = 0; + out_5161498735714983758[298] = 0; + out_5161498735714983758[299] = 0; + out_5161498735714983758[300] = 0; + out_5161498735714983758[301] = 0; + out_5161498735714983758[302] = 0; + out_5161498735714983758[303] = 0; + out_5161498735714983758[304] = 1; + out_5161498735714983758[305] = 0; + out_5161498735714983758[306] = 0; + out_5161498735714983758[307] = 0; + out_5161498735714983758[308] = 0; + out_5161498735714983758[309] = 0; + out_5161498735714983758[310] = 0; + out_5161498735714983758[311] = 0; + out_5161498735714983758[312] = 0; + out_5161498735714983758[313] = 0; + out_5161498735714983758[314] = 0; + out_5161498735714983758[315] = 0; + out_5161498735714983758[316] = 0; + out_5161498735714983758[317] = 0; + out_5161498735714983758[318] = 0; + out_5161498735714983758[319] = 0; + out_5161498735714983758[320] = 0; + out_5161498735714983758[321] = 0; + out_5161498735714983758[322] = 0; + out_5161498735714983758[323] = 1; } -void h_4(double *state, double *unused, double *out_1724585592717515492) { - out_1724585592717515492[0] = state[6] + state[9]; - out_1724585592717515492[1] = state[7] + state[10]; - out_1724585592717515492[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_6700070712992729001) { + out_6700070712992729001[0] = state[6] + state[9]; + out_6700070712992729001[1] = state[7] + state[10]; + out_6700070712992729001[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_3018712125018871059) { - out_3018712125018871059[0] = 0; - out_3018712125018871059[1] = 0; - out_3018712125018871059[2] = 0; - out_3018712125018871059[3] = 0; - out_3018712125018871059[4] = 0; - out_3018712125018871059[5] = 0; - out_3018712125018871059[6] = 1; - out_3018712125018871059[7] = 0; - out_3018712125018871059[8] = 0; - out_3018712125018871059[9] = 1; - out_3018712125018871059[10] = 0; - out_3018712125018871059[11] = 0; - out_3018712125018871059[12] = 0; - out_3018712125018871059[13] = 0; - out_3018712125018871059[14] = 0; - out_3018712125018871059[15] = 0; - out_3018712125018871059[16] = 0; - out_3018712125018871059[17] = 0; - out_3018712125018871059[18] = 0; - out_3018712125018871059[19] = 0; - out_3018712125018871059[20] = 0; - out_3018712125018871059[21] = 0; - out_3018712125018871059[22] = 0; - out_3018712125018871059[23] = 0; - out_3018712125018871059[24] = 0; - out_3018712125018871059[25] = 1; - out_3018712125018871059[26] = 0; - out_3018712125018871059[27] = 0; - out_3018712125018871059[28] = 1; - out_3018712125018871059[29] = 0; - out_3018712125018871059[30] = 0; - out_3018712125018871059[31] = 0; - out_3018712125018871059[32] = 0; - out_3018712125018871059[33] = 0; - out_3018712125018871059[34] = 0; - out_3018712125018871059[35] = 0; - out_3018712125018871059[36] = 0; - out_3018712125018871059[37] = 0; - out_3018712125018871059[38] = 0; - out_3018712125018871059[39] = 0; - out_3018712125018871059[40] = 0; - out_3018712125018871059[41] = 0; - out_3018712125018871059[42] = 0; - out_3018712125018871059[43] = 0; - out_3018712125018871059[44] = 1; - out_3018712125018871059[45] = 0; - out_3018712125018871059[46] = 0; - out_3018712125018871059[47] = 1; - out_3018712125018871059[48] = 0; - out_3018712125018871059[49] = 0; - out_3018712125018871059[50] = 0; - out_3018712125018871059[51] = 0; - out_3018712125018871059[52] = 0; - out_3018712125018871059[53] = 0; +void H_4(double *state, double *unused, double *out_554520126744309549) { + out_554520126744309549[0] = 0; + out_554520126744309549[1] = 0; + out_554520126744309549[2] = 0; + out_554520126744309549[3] = 0; + out_554520126744309549[4] = 0; + out_554520126744309549[5] = 0; + out_554520126744309549[6] = 1; + out_554520126744309549[7] = 0; + out_554520126744309549[8] = 0; + out_554520126744309549[9] = 1; + out_554520126744309549[10] = 0; + out_554520126744309549[11] = 0; + out_554520126744309549[12] = 0; + out_554520126744309549[13] = 0; + out_554520126744309549[14] = 0; + out_554520126744309549[15] = 0; + out_554520126744309549[16] = 0; + out_554520126744309549[17] = 0; + out_554520126744309549[18] = 0; + out_554520126744309549[19] = 0; + out_554520126744309549[20] = 0; + out_554520126744309549[21] = 0; + out_554520126744309549[22] = 0; + out_554520126744309549[23] = 0; + out_554520126744309549[24] = 0; + out_554520126744309549[25] = 1; + out_554520126744309549[26] = 0; + out_554520126744309549[27] = 0; + out_554520126744309549[28] = 1; + out_554520126744309549[29] = 0; + out_554520126744309549[30] = 0; + out_554520126744309549[31] = 0; + out_554520126744309549[32] = 0; + out_554520126744309549[33] = 0; + out_554520126744309549[34] = 0; + out_554520126744309549[35] = 0; + out_554520126744309549[36] = 0; + out_554520126744309549[37] = 0; + out_554520126744309549[38] = 0; + out_554520126744309549[39] = 0; + out_554520126744309549[40] = 0; + out_554520126744309549[41] = 0; + out_554520126744309549[42] = 0; + out_554520126744309549[43] = 0; + out_554520126744309549[44] = 1; + out_554520126744309549[45] = 0; + out_554520126744309549[46] = 0; + out_554520126744309549[47] = 1; + out_554520126744309549[48] = 0; + out_554520126744309549[49] = 0; + out_554520126744309549[50] = 0; + out_554520126744309549[51] = 0; + out_554520126744309549[52] = 0; + out_554520126744309549[53] = 0; } -void h_10(double *state, double *unused, double *out_1452906161636095115) { - out_1452906161636095115[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_1452906161636095115[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_1452906161636095115[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_326755883218172788) { + out_326755883218172788[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_326755883218172788[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_326755883218172788[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_3099904708442444122) { - out_3099904708442444122[0] = 0; - out_3099904708442444122[1] = 9.8100000000000005*cos(state[1]); - out_3099904708442444122[2] = 0; - out_3099904708442444122[3] = 0; - out_3099904708442444122[4] = -state[8]; - out_3099904708442444122[5] = state[7]; - out_3099904708442444122[6] = 0; - out_3099904708442444122[7] = state[5]; - out_3099904708442444122[8] = -state[4]; - out_3099904708442444122[9] = 0; - out_3099904708442444122[10] = 0; - out_3099904708442444122[11] = 0; - out_3099904708442444122[12] = 1; - out_3099904708442444122[13] = 0; - out_3099904708442444122[14] = 0; - out_3099904708442444122[15] = 1; - out_3099904708442444122[16] = 0; - out_3099904708442444122[17] = 0; - out_3099904708442444122[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_3099904708442444122[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_3099904708442444122[20] = 0; - out_3099904708442444122[21] = state[8]; - out_3099904708442444122[22] = 0; - out_3099904708442444122[23] = -state[6]; - out_3099904708442444122[24] = -state[5]; - out_3099904708442444122[25] = 0; - out_3099904708442444122[26] = state[3]; - out_3099904708442444122[27] = 0; - out_3099904708442444122[28] = 0; - out_3099904708442444122[29] = 0; - out_3099904708442444122[30] = 0; - out_3099904708442444122[31] = 1; - out_3099904708442444122[32] = 0; - out_3099904708442444122[33] = 0; - out_3099904708442444122[34] = 1; - out_3099904708442444122[35] = 0; - out_3099904708442444122[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_3099904708442444122[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_3099904708442444122[38] = 0; - out_3099904708442444122[39] = -state[7]; - out_3099904708442444122[40] = state[6]; - out_3099904708442444122[41] = 0; - out_3099904708442444122[42] = state[4]; - out_3099904708442444122[43] = -state[3]; - out_3099904708442444122[44] = 0; - out_3099904708442444122[45] = 0; - out_3099904708442444122[46] = 0; - out_3099904708442444122[47] = 0; - out_3099904708442444122[48] = 0; - out_3099904708442444122[49] = 0; - out_3099904708442444122[50] = 1; - out_3099904708442444122[51] = 0; - out_3099904708442444122[52] = 0; - out_3099904708442444122[53] = 1; +void H_10(double *state, double *unused, double *out_3618077715207225770) { + out_3618077715207225770[0] = 0; + out_3618077715207225770[1] = 9.8100000000000005*cos(state[1]); + out_3618077715207225770[2] = 0; + out_3618077715207225770[3] = 0; + out_3618077715207225770[4] = -state[8]; + out_3618077715207225770[5] = state[7]; + out_3618077715207225770[6] = 0; + out_3618077715207225770[7] = state[5]; + out_3618077715207225770[8] = -state[4]; + out_3618077715207225770[9] = 0; + out_3618077715207225770[10] = 0; + out_3618077715207225770[11] = 0; + out_3618077715207225770[12] = 1; + out_3618077715207225770[13] = 0; + out_3618077715207225770[14] = 0; + out_3618077715207225770[15] = 1; + out_3618077715207225770[16] = 0; + out_3618077715207225770[17] = 0; + out_3618077715207225770[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_3618077715207225770[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_3618077715207225770[20] = 0; + out_3618077715207225770[21] = state[8]; + out_3618077715207225770[22] = 0; + out_3618077715207225770[23] = -state[6]; + out_3618077715207225770[24] = -state[5]; + out_3618077715207225770[25] = 0; + out_3618077715207225770[26] = state[3]; + out_3618077715207225770[27] = 0; + out_3618077715207225770[28] = 0; + out_3618077715207225770[29] = 0; + out_3618077715207225770[30] = 0; + out_3618077715207225770[31] = 1; + out_3618077715207225770[32] = 0; + out_3618077715207225770[33] = 0; + out_3618077715207225770[34] = 1; + out_3618077715207225770[35] = 0; + out_3618077715207225770[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_3618077715207225770[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_3618077715207225770[38] = 0; + out_3618077715207225770[39] = -state[7]; + out_3618077715207225770[40] = state[6]; + out_3618077715207225770[41] = 0; + out_3618077715207225770[42] = state[4]; + out_3618077715207225770[43] = -state[3]; + out_3618077715207225770[44] = 0; + out_3618077715207225770[45] = 0; + out_3618077715207225770[46] = 0; + out_3618077715207225770[47] = 0; + out_3618077715207225770[48] = 0; + out_3618077715207225770[49] = 0; + out_3618077715207225770[50] = 1; + out_3618077715207225770[51] = 0; + out_3618077715207225770[52] = 0; + out_3618077715207225770[53] = 1; } -void h_13(double *state, double *unused, double *out_7528264878139249656) { - out_7528264878139249656[0] = state[3]; - out_7528264878139249656[1] = state[4]; - out_7528264878139249656[2] = state[5]; +void h_13(double *state, double *unused, double *out_8852328989829848020) { + out_8852328989829848020[0] = state[3]; + out_8852328989829848020[1] = state[4]; + out_8852328989829848020[2] = state[5]; } -void H_13(double *state, double *unused, double *out_3583314044700715163) { - out_3583314044700715163[0] = 0; - out_3583314044700715163[1] = 0; - out_3583314044700715163[2] = 0; - out_3583314044700715163[3] = 1; - out_3583314044700715163[4] = 0; - out_3583314044700715163[5] = 0; - out_3583314044700715163[6] = 0; - out_3583314044700715163[7] = 0; - out_3583314044700715163[8] = 0; - out_3583314044700715163[9] = 0; - out_3583314044700715163[10] = 0; - out_3583314044700715163[11] = 0; - out_3583314044700715163[12] = 0; - out_3583314044700715163[13] = 0; - out_3583314044700715163[14] = 0; - out_3583314044700715163[15] = 0; - out_3583314044700715163[16] = 0; - out_3583314044700715163[17] = 0; - out_3583314044700715163[18] = 0; - out_3583314044700715163[19] = 0; - out_3583314044700715163[20] = 0; - out_3583314044700715163[21] = 0; - out_3583314044700715163[22] = 1; - out_3583314044700715163[23] = 0; - out_3583314044700715163[24] = 0; - out_3583314044700715163[25] = 0; - out_3583314044700715163[26] = 0; - out_3583314044700715163[27] = 0; - out_3583314044700715163[28] = 0; - out_3583314044700715163[29] = 0; - out_3583314044700715163[30] = 0; - out_3583314044700715163[31] = 0; - out_3583314044700715163[32] = 0; - out_3583314044700715163[33] = 0; - out_3583314044700715163[34] = 0; - out_3583314044700715163[35] = 0; - out_3583314044700715163[36] = 0; - out_3583314044700715163[37] = 0; - out_3583314044700715163[38] = 0; - out_3583314044700715163[39] = 0; - out_3583314044700715163[40] = 0; - out_3583314044700715163[41] = 1; - out_3583314044700715163[42] = 0; - out_3583314044700715163[43] = 0; - out_3583314044700715163[44] = 0; - out_3583314044700715163[45] = 0; - out_3583314044700715163[46] = 0; - out_3583314044700715163[47] = 0; - out_3583314044700715163[48] = 0; - out_3583314044700715163[49] = 0; - out_3583314044700715163[50] = 0; - out_3583314044700715163[51] = 0; - out_3583314044700715163[52] = 0; - out_3583314044700715163[53] = 0; +void H_13(double *state, double *unused, double *out_3279235336558214475) { + out_3279235336558214475[0] = 0; + out_3279235336558214475[1] = 0; + out_3279235336558214475[2] = 0; + out_3279235336558214475[3] = 1; + out_3279235336558214475[4] = 0; + out_3279235336558214475[5] = 0; + out_3279235336558214475[6] = 0; + out_3279235336558214475[7] = 0; + out_3279235336558214475[8] = 0; + out_3279235336558214475[9] = 0; + out_3279235336558214475[10] = 0; + out_3279235336558214475[11] = 0; + out_3279235336558214475[12] = 0; + out_3279235336558214475[13] = 0; + out_3279235336558214475[14] = 0; + out_3279235336558214475[15] = 0; + out_3279235336558214475[16] = 0; + out_3279235336558214475[17] = 0; + out_3279235336558214475[18] = 0; + out_3279235336558214475[19] = 0; + out_3279235336558214475[20] = 0; + out_3279235336558214475[21] = 0; + out_3279235336558214475[22] = 1; + out_3279235336558214475[23] = 0; + out_3279235336558214475[24] = 0; + out_3279235336558214475[25] = 0; + out_3279235336558214475[26] = 0; + out_3279235336558214475[27] = 0; + out_3279235336558214475[28] = 0; + out_3279235336558214475[29] = 0; + out_3279235336558214475[30] = 0; + out_3279235336558214475[31] = 0; + out_3279235336558214475[32] = 0; + out_3279235336558214475[33] = 0; + out_3279235336558214475[34] = 0; + out_3279235336558214475[35] = 0; + out_3279235336558214475[36] = 0; + out_3279235336558214475[37] = 0; + out_3279235336558214475[38] = 0; + out_3279235336558214475[39] = 0; + out_3279235336558214475[40] = 0; + out_3279235336558214475[41] = 1; + out_3279235336558214475[42] = 0; + out_3279235336558214475[43] = 0; + out_3279235336558214475[44] = 0; + out_3279235336558214475[45] = 0; + out_3279235336558214475[46] = 0; + out_3279235336558214475[47] = 0; + out_3279235336558214475[48] = 0; + out_3279235336558214475[49] = 0; + out_3279235336558214475[50] = 0; + out_3279235336558214475[51] = 0; + out_3279235336558214475[52] = 0; + out_3279235336558214475[53] = 0; } -void h_14(double *state, double *unused, double *out_4413124467477602634) { - out_4413124467477602634[0] = state[6]; - out_4413124467477602634[1] = state[7]; - out_4413124467477602634[2] = state[8]; +void h_14(double *state, double *unused, double *out_2099262646432106248) { + out_2099262646432106248[0] = state[6]; + out_2099262646432106248[1] = state[7]; + out_2099262646432106248[2] = state[8]; } -void H_14(double *state, double *unused, double *out_64076307276501237) { - out_64076307276501237[0] = 0; - out_64076307276501237[1] = 0; - out_64076307276501237[2] = 0; - out_64076307276501237[3] = 0; - out_64076307276501237[4] = 0; - out_64076307276501237[5] = 0; - out_64076307276501237[6] = 1; - out_64076307276501237[7] = 0; - out_64076307276501237[8] = 0; - out_64076307276501237[9] = 0; - out_64076307276501237[10] = 0; - out_64076307276501237[11] = 0; - out_64076307276501237[12] = 0; - out_64076307276501237[13] = 0; - out_64076307276501237[14] = 0; - out_64076307276501237[15] = 0; - out_64076307276501237[16] = 0; - out_64076307276501237[17] = 0; - out_64076307276501237[18] = 0; - out_64076307276501237[19] = 0; - out_64076307276501237[20] = 0; - out_64076307276501237[21] = 0; - out_64076307276501237[22] = 0; - out_64076307276501237[23] = 0; - out_64076307276501237[24] = 0; - out_64076307276501237[25] = 1; - out_64076307276501237[26] = 0; - out_64076307276501237[27] = 0; - out_64076307276501237[28] = 0; - out_64076307276501237[29] = 0; - out_64076307276501237[30] = 0; - out_64076307276501237[31] = 0; - out_64076307276501237[32] = 0; - out_64076307276501237[33] = 0; - out_64076307276501237[34] = 0; - out_64076307276501237[35] = 0; - out_64076307276501237[36] = 0; - out_64076307276501237[37] = 0; - out_64076307276501237[38] = 0; - out_64076307276501237[39] = 0; - out_64076307276501237[40] = 0; - out_64076307276501237[41] = 0; - out_64076307276501237[42] = 0; - out_64076307276501237[43] = 0; - out_64076307276501237[44] = 1; - out_64076307276501237[45] = 0; - out_64076307276501237[46] = 0; - out_64076307276501237[47] = 0; - out_64076307276501237[48] = 0; - out_64076307276501237[49] = 0; - out_64076307276501237[50] = 0; - out_64076307276501237[51] = 0; - out_64076307276501237[52] = 0; - out_64076307276501237[53] = 0; +void H_14(double *state, double *unused, double *out_2528268305551062747) { + out_2528268305551062747[0] = 0; + out_2528268305551062747[1] = 0; + out_2528268305551062747[2] = 0; + out_2528268305551062747[3] = 0; + out_2528268305551062747[4] = 0; + out_2528268305551062747[5] = 0; + out_2528268305551062747[6] = 1; + out_2528268305551062747[7] = 0; + out_2528268305551062747[8] = 0; + out_2528268305551062747[9] = 0; + out_2528268305551062747[10] = 0; + out_2528268305551062747[11] = 0; + out_2528268305551062747[12] = 0; + out_2528268305551062747[13] = 0; + out_2528268305551062747[14] = 0; + out_2528268305551062747[15] = 0; + out_2528268305551062747[16] = 0; + out_2528268305551062747[17] = 0; + out_2528268305551062747[18] = 0; + out_2528268305551062747[19] = 0; + out_2528268305551062747[20] = 0; + out_2528268305551062747[21] = 0; + out_2528268305551062747[22] = 0; + out_2528268305551062747[23] = 0; + out_2528268305551062747[24] = 0; + out_2528268305551062747[25] = 1; + out_2528268305551062747[26] = 0; + out_2528268305551062747[27] = 0; + out_2528268305551062747[28] = 0; + out_2528268305551062747[29] = 0; + out_2528268305551062747[30] = 0; + out_2528268305551062747[31] = 0; + out_2528268305551062747[32] = 0; + out_2528268305551062747[33] = 0; + out_2528268305551062747[34] = 0; + out_2528268305551062747[35] = 0; + out_2528268305551062747[36] = 0; + out_2528268305551062747[37] = 0; + out_2528268305551062747[38] = 0; + out_2528268305551062747[39] = 0; + out_2528268305551062747[40] = 0; + out_2528268305551062747[41] = 0; + out_2528268305551062747[42] = 0; + out_2528268305551062747[43] = 0; + out_2528268305551062747[44] = 1; + out_2528268305551062747[45] = 0; + out_2528268305551062747[46] = 0; + out_2528268305551062747[47] = 0; + out_2528268305551062747[48] = 0; + out_2528268305551062747[49] = 0; + out_2528268305551062747[50] = 0; + out_2528268305551062747[51] = 0; + out_2528268305551062747[52] = 0; + out_2528268305551062747[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_2394949464519988932) { - err_fun(nom_x, delta_x, out_2394949464519988932); +void pose_err_fun(double *nom_x, double *delta_x, double *out_9137514370372247790) { + err_fun(nom_x, delta_x, out_9137514370372247790); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6906452154256624155) { - inv_err_fun(nom_x, true_x, out_6906452154256624155); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3476070474607186610) { + inv_err_fun(nom_x, true_x, out_3476070474607186610); } -void pose_H_mod_fun(double *state, double *out_4838410850925167920) { - H_mod_fun(state, out_4838410850925167920); +void pose_H_mod_fun(double *state, double *out_2374218852650606410) { + H_mod_fun(state, out_2374218852650606410); } -void pose_f_fun(double *state, double dt, double *out_8751525120696163789) { - f_fun(state, dt, out_8751525120696163789); +void pose_f_fun(double *state, double dt, double *out_4392889107205501252) { + f_fun(state, dt, out_4392889107205501252); } -void pose_F_fun(double *state, double dt, double *out_7175128552495691270) { - F_fun(state, dt, out_7175128552495691270); +void pose_F_fun(double *state, double dt, double *out_5161498735714983758) { + F_fun(state, dt, out_5161498735714983758); } -void pose_h_4(double *state, double *unused, double *out_1724585592717515492) { - h_4(state, unused, out_1724585592717515492); +void pose_h_4(double *state, double *unused, double *out_6700070712992729001) { + h_4(state, unused, out_6700070712992729001); } -void pose_H_4(double *state, double *unused, double *out_3018712125018871059) { - H_4(state, unused, out_3018712125018871059); +void pose_H_4(double *state, double *unused, double *out_554520126744309549) { + H_4(state, unused, out_554520126744309549); } -void pose_h_10(double *state, double *unused, double *out_1452906161636095115) { - h_10(state, unused, out_1452906161636095115); +void pose_h_10(double *state, double *unused, double *out_326755883218172788) { + h_10(state, unused, out_326755883218172788); } -void pose_H_10(double *state, double *unused, double *out_3099904708442444122) { - H_10(state, unused, out_3099904708442444122); +void pose_H_10(double *state, double *unused, double *out_3618077715207225770) { + H_10(state, unused, out_3618077715207225770); } -void pose_h_13(double *state, double *unused, double *out_7528264878139249656) { - h_13(state, unused, out_7528264878139249656); +void pose_h_13(double *state, double *unused, double *out_8852328989829848020) { + h_13(state, unused, out_8852328989829848020); } -void pose_H_13(double *state, double *unused, double *out_3583314044700715163) { - H_13(state, unused, out_3583314044700715163); +void pose_H_13(double *state, double *unused, double *out_3279235336558214475) { + H_13(state, unused, out_3279235336558214475); } -void pose_h_14(double *state, double *unused, double *out_4413124467477602634) { - h_14(state, unused, out_4413124467477602634); +void pose_h_14(double *state, double *unused, double *out_2099262646432106248) { + h_14(state, unused, out_2099262646432106248); } -void pose_H_14(double *state, double *unused, double *out_64076307276501237) { - H_14(state, unused, out_64076307276501237); +void pose_H_14(double *state, double *unused, double *out_2528268305551062747) { + H_14(state, unused, out_2528268305551062747); } 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 a3d7ff09b..9e3c551b6 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_2394949464519988932); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6906452154256624155); -void pose_H_mod_fun(double *state, double *out_4838410850925167920); -void pose_f_fun(double *state, double dt, double *out_8751525120696163789); -void pose_F_fun(double *state, double dt, double *out_7175128552495691270); -void pose_h_4(double *state, double *unused, double *out_1724585592717515492); -void pose_H_4(double *state, double *unused, double *out_3018712125018871059); -void pose_h_10(double *state, double *unused, double *out_1452906161636095115); -void pose_H_10(double *state, double *unused, double *out_3099904708442444122); -void pose_h_13(double *state, double *unused, double *out_7528264878139249656); -void pose_H_13(double *state, double *unused, double *out_3583314044700715163); -void pose_h_14(double *state, double *unused, double *out_4413124467477602634); -void pose_H_14(double *state, double *unused, double *out_64076307276501237); +void pose_err_fun(double *nom_x, double *delta_x, double *out_9137514370372247790); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_3476070474607186610); +void pose_H_mod_fun(double *state, double *out_2374218852650606410); +void pose_f_fun(double *state, double dt, double *out_4392889107205501252); +void pose_F_fun(double *state, double dt, double *out_5161498735714983758); +void pose_h_4(double *state, double *unused, double *out_6700070712992729001); +void pose_H_4(double *state, double *unused, double *out_554520126744309549); +void pose_h_10(double *state, double *unused, double *out_326755883218172788); +void pose_H_10(double *state, double *unused, double *out_3618077715207225770); +void pose_h_13(double *state, double *unused, double *out_8852328989829848020); +void pose_H_13(double *state, double *unused, double *out_3279235336558214475); +void pose_h_14(double *state, double *unused, double *out_2099262646432106248); +void pose_H_14(double *state, double *unused, double *out_2528268305551062747); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/pandad/pandad b/selfdrive/pandad/pandad index eeac5660d..5ca7c342b 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 07cb38e01..64092b411 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/athena/athenad.py b/system/athena/athenad.py index ed167c417..3043a62a1 100755 --- a/system/athena/athenad.py +++ b/system/athena/athenad.py @@ -28,7 +28,7 @@ from websocket import (ABNF, WebSocket, WebSocketException, WebSocketTimeoutExce create_connection) import cereal.messaging as messaging -from cereal import log +from cereal import car, log from cereal.services import SERVICE_LIST from openpilot.common.api import Api, get_key_pair from openpilot.common.utils import CallbackReader, get_upload_stream @@ -565,6 +565,34 @@ def getNetworks(): return HARDWARE.get_networks() +@dispatcher.add_method +def startStream(sdp: str, enabled: bool) -> dict: + from openpilot.system.webrtc.helpers import StreamRequestBody, post_stream_request, wait_for_webrtcd + + params = Params() + bridge_services_in = [] + + # The manager shuts webrtcd down on ignition transitions, so persistent car + # params are safe here and remain available while the device is offroad. + cp_bytes = params.get("CarParamsPersistent") + if cp_bytes is None: + raise Exception("failed to get CarParamsPersistent") + + with car.CarParams.from_bytes(cp_bytes) as CP: + if CP.notCar: + bridge_services_in.append("testJoystick") + + if params.get_bool("IsOffroad"): + # Wake camerad, stream_encoderd, and webrtcd. webrtcd clears this after the + # last session ends, allowing manager to stop the extra processes again. + params.put_bool("IsLiveStreaming", True) + wait_for_webrtcd() + + return post_stream_request(StreamRequestBody( + sdp, "wideRoad", enabled, bridge_services_in, ["carState", "deviceState"], + )) + + @dispatcher.add_method def takeSnapshot() -> str | dict[str, str] | None: from openpilot.system.camerad.snapshot import jpeg_write, snapshot diff --git a/system/camerad/camerad b/system/camerad/camerad index 6f415a716..5bce524b0 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 965d1133d..58b5a8010 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoder/encoder.h b/system/loggerd/encoder/encoder.h index 57146fafc..cf3883444 100644 --- a/system/loggerd/encoder/encoder.h +++ b/system/loggerd/encoder/encoder.h @@ -26,6 +26,8 @@ public: virtual int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) = 0; virtual void encoder_open() = 0; virtual void encoder_close() = 0; + virtual void set_bitrate(int bitrate) = 0; + virtual void request_keyframe() = 0; void publisher_publish(int segment_num, uint32_t idx, VisionIpcBufExtra &extra, unsigned int flags, kj::ArrayPtr header, kj::ArrayPtr dat); diff --git a/system/loggerd/encoder/ffmpeg_encoder.cc b/system/loggerd/encoder/ffmpeg_encoder.cc index 4d6be4718..bf38aad35 100644 --- a/system/loggerd/encoder/ffmpeg_encoder.cc +++ b/system/loggerd/encoder/ffmpeg_encoder.cc @@ -72,6 +72,14 @@ void FfmpegEncoder::encoder_close() { is_open = false; } +void FfmpegEncoder::set_bitrate(int bitrate) { + LOGE("adaptive bitrate is not supported for ffmpeg encoder %s", encoder_info.publish_name); +} + +void FfmpegEncoder::request_keyframe() { + LOGE("keyframe request is not supported for ffmpeg encoder %s", encoder_info.publish_name); +} + int FfmpegEncoder::encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra) { assert(buf->width == this->in_width); assert(buf->height == this->in_height); diff --git a/system/loggerd/encoder/ffmpeg_encoder.h b/system/loggerd/encoder/ffmpeg_encoder.h index cd5ac1e13..3f7c674db 100644 --- a/system/loggerd/encoder/ffmpeg_encoder.h +++ b/system/loggerd/encoder/ffmpeg_encoder.h @@ -21,6 +21,8 @@ public: int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra); void encoder_open(); void encoder_close(); + void set_bitrate(int bitrate); + void request_keyframe(); private: int segment_num = -1; diff --git a/system/loggerd/encoder/v4l_encoder.cc b/system/loggerd/encoder/v4l_encoder.cc index 6ee3af13b..99666391c 100644 --- a/system/loggerd/encoder/v4l_encoder.cc +++ b/system/loggerd/encoder/v4l_encoder.cc @@ -156,6 +156,7 @@ V4LEncoder::V4LEncoder(const EncoderInfo &encoder_info, int in_width, int in_hei assert(strcmp((const char *)cap.card, "msm_vidc_venc") == 0); EncoderSettings encoder_settings = encoder_info.get_settings(in_width); + current_bitrate = encoder_settings.bitrate; bool is_h265 = encoder_settings.encode_type == cereal::EncodeIndex::Type::FULL_H_E_V_C; struct v4l2_format fmt_out = { @@ -305,6 +306,36 @@ void V4LEncoder::encoder_close() { this->is_open = false; } +void V4LEncoder::set_bitrate(int bitrate) { + if (bitrate == current_bitrate) return; + if (bitrate <= 0) { + LOGE("invalid livestream encoder bitrate %d", bitrate); + return; + } + + struct v4l2_control ctrl = { + .id = V4L2_CID_MPEG_VIDEO_BITRATE, + .value = bitrate, + }; + + if (util::safe_ioctl(fd, VIDIOC_S_CTRL, &ctrl) == -1) { + LOGE("failed to update %s bitrate to %d", encoder_info.publish_name, bitrate); + return; + } + current_bitrate = bitrate; +} + +void V4LEncoder::request_keyframe() { + struct v4l2_control ctrl = { + .id = V4L2_CID_MPEG_VIDC_VIDEO_REQUEST_IFRAME, + .value = 1, + }; + + if (util::safe_ioctl(fd, VIDIOC_S_CTRL, &ctrl) == -1) { + LOGE("failed to request keyframe for %s", encoder_info.publish_name); + } +} + V4LEncoder::~V4LEncoder() { encoder_close(); v4l2_buf_type buf_type = V4L2_BUF_TYPE_VIDEO_OUTPUT_MPLANE; diff --git a/system/loggerd/encoder/v4l_encoder.h b/system/loggerd/encoder/v4l_encoder.h index 58011d60e..b1a84e654 100644 --- a/system/loggerd/encoder/v4l_encoder.h +++ b/system/loggerd/encoder/v4l_encoder.h @@ -13,6 +13,8 @@ public: int encode_frame(VisionBuf* buf, VisionIpcBufExtra *extra); void encoder_open(); void encoder_close(); + void set_bitrate(int bitrate); + void request_keyframe(); private: int fd; @@ -20,6 +22,7 @@ private: bool is_open = false; int segment_num = -1; int counter = 0; + int current_bitrate = -1; SafeQueue extras; diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 7d7b593ab..35115baf9 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/encoderd.cc b/system/loggerd/encoderd.cc index 9d4b81a3f..5ca794dd5 100644 --- a/system/loggerd/encoderd.cc +++ b/system/loggerd/encoderd.cc @@ -44,6 +44,19 @@ bool sync_encoders(EncoderdState *s, VisionStreamType cam_type, uint32_t frame_i } } +void encoder_set_bitrate(std::unique_ptr &e) { + static Params params; + std::string val = params.get("LivestreamEncoderBitrate"); + if (val.empty()) return; + e->set_bitrate(std::stoi(val)); +} + +void encoder_request_keyframe(std::unique_ptr &e) { + static Params params; + if (!params.getBool("LivestreamRequestKeyframe")) return; + e->request_keyframe(); +} + void encoder_thread(EncoderdState *s, const LogCameraInfo &cam_info) { util::set_thread_name(cam_info.thread_name); @@ -110,6 +123,11 @@ void encoder_thread(EncoderdState *s, const LogCameraInfo &cam_info) { // encode a frame for (int i = 0; i < encoders.size(); ++i) { + if (cam_info.encoder_infos[i].is_live) { + encoder_set_bitrate(encoders[i]); + encoder_request_keyframe(encoders[i]); + } + int out_id = encoders[i]->encode_frame(buf, &extra); if (out_id == -1) { diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index af8480db0..59bad932e 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ diff --git a/system/loggerd/loggerd.h b/system/loggerd/loggerd.h index 0949aae68..431d0b558 100644 --- a/system/loggerd/loggerd.h +++ b/system/loggerd/loggerd.h @@ -55,8 +55,8 @@ struct EncoderSettings { } static EncoderSettings StreamEncoderSettings() { - int _stream_bitrate = getenv("STREAM_BITRATE") ? atoi(getenv("STREAM_BITRATE")) : 1'000'000; - return EncoderSettings{.encode_type = cereal::EncodeIndex::Type::QCAMERA_H264, .bitrate = _stream_bitrate , .gop_size = 15}; + int _stream_bitrate = getenv("STREAM_BITRATE") ? atoi(getenv("STREAM_BITRATE")) : 5'000'000; + return EncoderSettings{.encode_type = cereal::EncodeIndex::Type::QCAMERA_H264, .bitrate = _stream_bitrate , .gop_size = 5}; } }; @@ -67,6 +67,7 @@ public: const char *filename = NULL; bool record = true; bool include_audio = false; + bool is_live = false; int frame_width = -1; int frame_height = -1; int fps = MAIN_FPS; @@ -112,6 +113,7 @@ const EncoderInfo stream_road_encoder_info = { .publish_name = "livestreamRoadEncodeData", //.thumbnail_name = "thumbnail", .record = false, + .is_live = true, .get_settings = [](int){return EncoderSettings::StreamEncoderSettings();}, INIT_ENCODE_FUNCTIONS(LivestreamRoadEncode), }; @@ -119,6 +121,7 @@ const EncoderInfo stream_road_encoder_info = { const EncoderInfo stream_wide_road_encoder_info = { .publish_name = "livestreamWideRoadEncodeData", .record = false, + .is_live = true, .get_settings = [](int){return EncoderSettings::StreamEncoderSettings();}, INIT_ENCODE_FUNCTIONS(LivestreamWideRoadEncode), }; @@ -126,6 +129,7 @@ const EncoderInfo stream_wide_road_encoder_info = { const EncoderInfo stream_driver_encoder_info = { .publish_name = "livestreamDriverEncodeData", .record = false, + .is_live = true, .get_settings = [](int){return EncoderSettings::StreamEncoderSettings();}, INIT_ENCODE_FUNCTIONS(LivestreamDriverEncode), }; diff --git a/system/manager/process_config.py b/system/manager/process_config.py index 9c4162346..fe59d24e1 100644 --- a/system/manager/process_config.py +++ b/system/manager/process_config.py @@ -62,12 +62,18 @@ def only_onroad(started: bool, params: Params, CP: car.CarParams, starpilot_togg def only_offroad(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool: return not started +def livestream(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool: + return params.get_bool("IsLiveStreaming") + def or_(*fns): return lambda *args: operator.or_(*(fn(*args) for fn in fns)) def and_(*fns): return lambda *args: operator.and_(*(fn(*args) for fn in fns)) +def not_(*fns): + return lambda *args: operator.not_(*(fn(*args) for fn in fns)) + # StarPilot variables def allow_logging(started: bool, params: Params, CP: car.CarParams, starpilot_toggles: SimpleNamespace) -> bool: return not starpilot_toggles.no_logging @@ -173,10 +179,10 @@ procs = [ NativeProcess("loggerd", "system/loggerd", ["./loggerd"], and_(allow_logging, logging)), NativeProcess("encoderd", "system/loggerd", ["./encoderd"], and_(allow_logging, only_onroad)), - NativeProcess("stream_encoderd", "system/loggerd", ["./encoderd", "--stream"], notcar), + NativeProcess("stream_encoderd", "system/loggerd", ["./encoderd", "--stream"], or_(and_(livestream, not_(iscar)), notcar)), PythonProcess("logmessaged", "system.logmessaged", always_run), - NativeProcess("camerad", "system/camerad", ["./camerad"], driverview, enabled=not WEBCAM), + NativeProcess("camerad", "system/camerad", ["./camerad"], or_(driverview, livestream), enabled=not WEBCAM), PythonProcess("webcamerad", "tools.webcam.camerad", driverview, enabled=WEBCAM), PythonProcess("proclogd", "system.proclogd", and_(allow_logging, only_onroad), enabled=platform.system() != "Darwin"), PythonProcess("journald", "system.journald", and_(allow_logging, only_onroad), platform.system() != "Darwin"), @@ -217,7 +223,7 @@ procs = [ # debug procs NativeProcess("bridge", "cereal/messaging", ["./bridge"], notcar), - PythonProcess("webrtcd", "system.webrtc.webrtcd", notcar), + PythonProcess("webrtcd", "system.webrtc.webrtcd", or_(and_(livestream, not_(iscar)), notcar)), PythonProcess("webjoystick", "tools.bodyteleop.web", notcar), PythonProcess("joystick", "tools.joystick.joystick_control", and_(joystick, iscar)), ] diff --git a/system/webrtc/device/video.py b/system/webrtc/device/video.py index 50feab4f4..fbe19c3dc 100644 --- a/system/webrtc/device/video.py +++ b/system/webrtc/device/video.py @@ -1,11 +1,22 @@ import asyncio +import struct import time import av from teleoprtc.tracks import TiciVideoStreamTrack +from aiortc.mediastreams import MediaStreamError from cereal import messaging -from openpilot.common.realtime import DT_MDL, DT_DMON +from openpilot.common.params import Params +from openpilot.common.realtime import DT_MDL + + +V4L2_BUF_FLAG_KEYFRAME = 0x8 +TIMING_SEI_UUID = bytes([ + 0xa5, 0xe0, 0xc4, 0xa4, 0x5b, 0x6e, 0x4e, 0x1e, + 0x9c, 0x7e, 0x12, 0x34, 0x56, 0x78, 0x9a, 0xbc, +]) +_SEI_PREFIX = b'\x00\x00\x00\x01\x06\x05\x30' + TIMING_SEI_UUID class LiveStreamVideoStreamTrack(TiciVideoStreamTrack): @@ -15,24 +26,64 @@ class LiveStreamVideoStreamTrack(TiciVideoStreamTrack): "road": "livestreamRoadEncodeData", } - def __init__(self, camera_type: str): - dt = DT_DMON if camera_type == "driver" else DT_MDL - super().__init__(camera_type, dt) + def __init__(self, camera_type: str, video_enabled: bool = True): + super().__init__(camera_type, DT_MDL) - self._sock = messaging.sub_sock(self.camera_to_sock_mapping[camera_type], conflate=True) + self._sock = self._make_sock(camera_type) self._pts = 0 self._t0_ns = time.monotonic_ns() + self.timing_sei_enabled = False + self.params = Params() + self._seen_keyframe = False + self.video_enabled = video_enabled + + def stop(self) -> None: + super().stop() + self._sock = None + + def _make_sock(self, camera_type: str) -> messaging.SubSocket: + return messaging.sub_sock(self.camera_to_sock_mapping[camera_type], conflate=True) + + def switch_camera(self, camera_type: str) -> None: + self._sock = self._make_sock(camera_type) + + def enable(self, enabled: bool): + self.video_enabled = enabled + if not enabled: + self._seen_keyframe = False + + def _build_frame_data(self, msg) -> bytes: + encode_data = getattr(msg, msg.which()) + if not self.timing_sei_enabled: + return encode_data.header + encode_data.data + + idx = encode_data.idx + sei_nal = _SEI_PREFIX + struct.pack('>4d', + (idx.timestampEof - idx.timestampSof) / 1e6, + (msg.logMonoTime - idx.timestampEof) / 1e6, + (time.monotonic_ns() - msg.logMonoTime) / 1e6, + time.time() * 1000, # noqa: TID251 + ) + b'\x80' + return encode_data.header + sei_nal + encode_data.data async def recv(self): while True: + if self.readyState != "live": + raise MediaStreamError + + if not self.video_enabled: + await asyncio.sleep(0.005) + continue + msg = messaging.recv_one_or_none(self._sock) if msg is not None: + if not self._seen_keyframe and (getattr(msg, msg.which()).idx.flags & V4L2_BUF_FLAG_KEYFRAME): + self._seen_keyframe = True + self.params.put("LivestreamRequestKeyframe", False) break await asyncio.sleep(0.005) - evta = getattr(msg, msg.which()) - - packet = av.Packet(evta.header + evta.data) + packet = av.Packet(self._build_frame_data(msg)) packet.time_base = self._time_base self._pts = ((time.monotonic_ns() - self._t0_ns) * self._clock_rate) // 1_000_000_000 diff --git a/system/webrtc/helpers.py b/system/webrtc/helpers.py new file mode 100644 index 000000000..1cc341a0e --- /dev/null +++ b/system/webrtc/helpers.py @@ -0,0 +1,41 @@ +import time +import requests +from dataclasses import asdict, dataclass, field + + +WEBRTCD_PORT = 5001 + + +@dataclass +class StreamRequestBody: + sdp: str + init_camera: str + enabled: bool + bridge_services_in: list[str] = field(default_factory=list) + bridge_services_out: list[str] = field(default_factory=list) + + +def post_stream_request(body: StreamRequestBody) -> dict: + t_start = time.monotonic() + try: + resp = requests.post(f"http://localhost:{WEBRTCD_PORT}/stream", json=asdict(body), timeout=10) + t_end = time.monotonic() + ret = resp.json() + ret["time"] = (t_end - t_start) * 1000 + return ret + except requests.ConnectTimeout as e: + raise Exception("webrtc took too long to respond.") from e + except requests.ConnectionError as e: + raise Exception("webrtc server on device is not running.") from e + + +def wait_for_webrtcd(max_retries: float = 10) -> None: + attempts = 0 + while attempts < max_retries: + try: + if requests.get(f"http://localhost:{WEBRTCD_PORT}/schema", timeout=1).ok: + return + except requests.ConnectionError: + attempts += 1 + time.sleep(0.5) + raise TimeoutError("webrtcd did not initialize in time.") diff --git a/system/webrtc/tests/test_stream_session.py b/system/webrtc/tests/test_stream_session.py index e31fda372..f8316d203 100644 --- a/system/webrtc/tests/test_stream_session.py +++ b/system/webrtc/tests/test_stream_session.py @@ -39,7 +39,8 @@ class TestStreamSession: mocked_submaster.update_msgs(0, [test_msg]) mocker.patch.object(messaging.SubMaster, "update", side_effect=mocked_update) - proxy = CerealOutgoingMessageProxy(mocked_submaster) + proxy = CerealOutgoingMessageProxy(["customReservedRawData0"]) + proxy.sm = mocked_submaster proxy.add_channel(channel) proxy.update() @@ -70,6 +71,7 @@ class TestStreamSession: def test_livestream_track(self, mocker): fake_msg = messaging.new_message("livestreamDriverEncodeData") + fake_msg.livestreamDriverEncodeData.idx.flags = 8 config = {"receive.return_value": fake_msg.to_bytes()} mocker.patch("msgq.SubSocket", spec=True, **config) diff --git a/system/webrtc/tests/test_webrtcd.py b/system/webrtc/tests/test_webrtcd.py index 4fa6d8953..23a7f6ddc 100644 --- a/system/webrtc/tests/test_webrtcd.py +++ b/system/webrtc/tests/test_webrtcd.py @@ -31,7 +31,8 @@ class TestWebrtcdProc: async def test_webrtcd(self, mocker): mock_request = mocker.MagicMock() async def connect(offer): - body = {'sdp': offer.sdp, 'cameras': offer.video, 'bridge_services_in': self.in_services, 'bridge_services_out': self.out_services} + body = {'sdp': offer.sdp, 'init_camera': offer.video[0], 'enabled': True, + 'bridge_services_in': self.in_services, 'bridge_services_out': self.out_services} mock_request.json.side_effect = mocker.AsyncMock(return_value=body) response = await get_stream(mock_request) response_json = json.loads(response.text) @@ -62,4 +63,3 @@ class TestWebrtcdProc: assert mock_request.app["streams"].__setitem__.called, "Implementation changed, please update this test" _, session = mock_request.app["streams"].__setitem__.call_args.args await self.assertCompletesWithTimeout(session.post_run_cleanup()) - diff --git a/system/webrtc/webrtcd.py b/system/webrtc/webrtcd.py old mode 100755 new mode 100644 index c19f1bf9d..837eda817 --- a/system/webrtc/webrtcd.py +++ b/system/webrtc/webrtcd.py @@ -1,11 +1,15 @@ #!/usr/bin/env python3 +from abc import abstractmethod +import os +import socket +import time import argparse import asyncio +import contextlib import json import uuid import logging -from dataclasses import dataclass, field from typing import Any, TYPE_CHECKING # aiortc and its dependencies have lots of internal warnings :( @@ -17,19 +21,77 @@ import capnp from aiohttp import web if TYPE_CHECKING: from aiortc.rtcdatachannel import RTCDataChannel +import aioice.ice +from openpilot.system.webrtc.helpers import StreamRequestBody from openpilot.system.webrtc.schema import generate_field +from openpilot.common.params import Params from cereal import messaging, log -class CerealOutgoingMessageProxy: - def __init__(self, sm: messaging.SubMaster): - self.sm = sm +# socket trick: route lookup for 8.8.8.8 (nothing is sent or actually connected to) +# return the source interfaces IP which is the default interface of the device +def _default_route_ip() -> str | None: + s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + try: + s.connect(("8.8.8.8", 53)) # selects a route, sends nothing + return s.getsockname()[0] + except OSError: + return None + finally: + s.close() + +# aioice patch: gather ICE candidates only on the default-route interface +_get_host_addresses = aioice.ice.get_host_addresses +def _primary_host_addresses(use_ipv4: bool, use_ipv6: bool) -> list[str]: + addresses = _get_host_addresses(use_ipv4, use_ipv6) + primary = _default_route_ip() + if primary not in addresses: + return addresses + return [primary, ] +aioice.ice.get_host_addresses = _primary_host_addresses + + +class AsyncTaskRunner: + def __init__(self): + self.is_running = False + self.task = None + self.logger = logging.getLogger("webrtcd") + + def start(self): + assert self.task is None + self.task = asyncio.create_task(self.run()) + + async def stop(self): + if self.task is None: + return + task = self.task + self.task = None + if task.done(): + return + task.cancel() + with contextlib.suppress(asyncio.CancelledError): + await task + + @abstractmethod + async def run(self): + pass + + +class CerealOutgoingMessageProxy(AsyncTaskRunner): + def __init__(self, services: list[str], enabled: bool = True): + super().__init__() + self.services = list(services) + self.sm = messaging.SubMaster(self.services) self.channels: list[RTCDataChannel] = [] + self._enabled = enabled def add_channel(self, channel: 'RTCDataChannel'): self.channels.append(channel) + def enable(self, enable: bool): + self._enabled = enable + def to_json(self, msg_content: Any): if isinstance(msg_content, capnp._DynamicStructReader): msg_dict = msg_content.to_dict() @@ -55,6 +117,22 @@ class CerealOutgoingMessageProxy: for channel in self.channels: channel.send(encoded_msg) + async def run(self): + from aiortc.exceptions import InvalidStateError + + while True: + if not self._enabled: + await asyncio.sleep(0.01) + continue + try: + self.update() + except InvalidStateError: + self.logger.warning("Cereal outgoing proxy invalid state (connection closed)") + break + except Exception: + self.logger.exception("Cereal outgoing proxy failure") + await asyncio.sleep(0.01) + class CerealIncomingMessageProxy: def __init__(self, pm: messaging.PubMaster): @@ -72,37 +150,6 @@ class CerealIncomingMessageProxy: self.pm.send(msg_type, msg) -class CerealProxyRunner: - def __init__(self, proxy: CerealOutgoingMessageProxy): - self.proxy = proxy - self.is_running = False - self.task = None - self.logger = logging.getLogger("webrtcd") - - def start(self): - assert self.task is None - self.task = asyncio.create_task(self.run()) - - def stop(self): - if self.task is None or self.task.done(): - return - self.task.cancel() - self.task = None - - async def run(self): - from aiortc.exceptions import InvalidStateError - - while True: - try: - self.proxy.update() - except InvalidStateError: - self.logger.warning("Cereal outgoing proxy invalid state (connection closed)") - break - except Exception: - self.logger.exception("Cereal outgoing proxy failure") - await asyncio.sleep(0.01) - - class DynamicPubMaster(messaging.PubMaster): def __init__(self, *args, **kwargs): super().__init__(*args, **kwargs) @@ -115,107 +162,235 @@ class DynamicPubMaster(messaging.PubMaster): self.sock[service] = messaging.pub_sock(service) +class LivestreamBitrateController(AsyncTaskRunner): + bitrates = [500_000, 1_500_000, int(os.environ.get("STREAM_BITRATE", 5_000_000))] + label_to_bitrate = { "high": bitrates[2], "med": bitrates[1], "low": bitrates[0]} + sample_interval = 0.2 + high_level = 0.1 # drop immediately + med_level = 0.05 # drop after # of samples + low_level = 0 # raise after # of samples + down_samples = 5 # 1s + param_name = "LivestreamEncoderBitrate" + + def __init__(self, peer_connection: Any, params: Params, enabled: bool = True): + super().__init__() + self.pc = peer_connection + self.params = params + + self.level = 2 + self._publish(self.bitrates[self.level]) + self.prev_lost, self.prev_sent = None, None + self.counter = 0 + self.up_samples = 5 # 1s + self._auto = True + self._enabled = enabled + + def enable(self, enable: bool): + self._enabled = enable + + async def run(self): + while True: + await asyncio.sleep(self.sample_interval) + if not self._enabled: + continue + if not self._auto: + continue + + loss_rate = await self._sample() + if loss_rate is None: + continue + if loss_rate >= self.med_level and self.level > 0: + self.counter += 1 + if self.counter >= self.down_samples or loss_rate >= self.high_level: + self.level -= 1 + self.up_samples *= 2 # exponential backoff before raising again + self.counter = 0 + self._publish(self.bitrates[self.level]) + elif loss_rate <= self.low_level and self.level < len(self.bitrates) - 1: + self.counter -= 1 + if -self.counter >= self.up_samples: + self.level += 1 + self.counter = 0 + self._publish(self.bitrates[self.level]) + + async def _sample(self) -> float | None: + report = await self.pc.getStats() + packets_lost = packets_sent = 0 + for s in report.values(): + if s.type == "remote-inbound-rtp": + packets_lost += s.packetsLost + elif s.type == "outbound-rtp": + packets_sent += s.packetsSent + + if self.prev_lost is None: + self.prev_lost, self.prev_sent = packets_lost, packets_sent + return None + lost_delta = max(0, packets_lost - self.prev_lost) + sent_delta = max(0, packets_sent - self.prev_sent) + self.prev_lost, self.prev_sent = packets_lost, packets_sent + return lost_delta / sent_delta if sent_delta else 0.0 + + def _publish(self, bitrate: float): + self.params.put(self.param_name, bitrate) + + def set_quality(self, quality): + if quality in self.label_to_bitrate: + self._publish(self.label_to_bitrate[quality]) + self._auto = False + elif quality == "auto": + self._auto = True + + class StreamSession: shared_pub_master = DynamicPubMaster([]) - def __init__(self, sdp: str, cameras: list[str], incoming_services: list[str], outgoing_services: list[str], debug_mode: bool = False): - from aiortc.mediastreams import VideoStreamTrack, AudioStreamTrack - from aiortc.contrib.media import MediaBlackhole + def __init__(self, body: StreamRequestBody, debug_mode: bool = False): + if debug_mode: + from aiortc.mediastreams import AudioStreamTrack, VideoStreamTrack + from aiortc.contrib.media import MediaBlackhole from openpilot.system.webrtc.device.video import LiveStreamVideoStreamTrack from openpilot.system.webrtc.device.audio import AudioInputStreamTrack, AudioOutputSpeaker - from teleoprtc import WebRTCAnswerBuilder + from teleoprtc.builder import WebRTCAnswerBuilder from teleoprtc.info import parse_info_from_offer - config = parse_info_from_offer(sdp) - builder = WebRTCAnswerBuilder(sdp) + self.identifier = str(uuid.uuid4()) + self.params = Params() + builder = WebRTCAnswerBuilder(body.sdp) + config = parse_info_from_offer(body.sdp) - assert len(cameras) == config.n_expected_camera_tracks, "Incoming stream has misconfigured number of video tracks" - for cam in cameras: - builder.add_video_stream(cam, LiveStreamVideoStreamTrack(cam) if not debug_mode else VideoStreamTrack()) + self.enabled = body.enabled + self.video_track = LiveStreamVideoStreamTrack(body.init_camera, self.enabled) if not debug_mode else VideoStreamTrack() + builder.add_video_stream(body.init_camera, self.video_track) if config.expected_audio_track: builder.add_audio_stream(AudioInputStreamTrack() if not debug_mode else AudioStreamTrack()) if config.incoming_audio_track: self.audio_output_cls = AudioOutputSpeaker if not debug_mode else MediaBlackhole builder.offer_to_receive_audio_stream() - self.stream = builder.stream() - self.identifier = str(uuid.uuid4()) self.incoming_bridge: CerealIncomingMessageProxy | None = None - self.incoming_bridge_services = incoming_services + self.incoming_bridge_services = body.bridge_services_in self.outgoing_bridge: CerealOutgoingMessageProxy | None = None - self.outgoing_bridge_runner: CerealProxyRunner | None = None - if len(incoming_services) > 0: - self.incoming_bridge = CerealIncomingMessageProxy(self.shared_pub_master) - if len(outgoing_services) > 0: - self.outgoing_bridge = CerealOutgoingMessageProxy(messaging.SubMaster(outgoing_services)) - self.outgoing_bridge_runner = CerealProxyRunner(self.outgoing_bridge) - + self.bitrate_controller: LivestreamBitrateController | None = None self.audio_output: AudioOutputSpeaker | MediaBlackhole | None = None + if len(body.bridge_services_in) > 0: + self.incoming_bridge = CerealIncomingMessageProxy(self.shared_pub_master) + if len(body.bridge_services_out) > 0: + self.outgoing_bridge = CerealOutgoingMessageProxy(body.bridge_services_out, self.enabled) + self.bitrate_controller = LivestreamBitrateController(self.stream.peer_connection, self.params, self.enabled) + self.run_task: asyncio.Task | None = None + self._cleanup_lock = asyncio.Lock() + self._cleanup_done = False self.logger = logging.getLogger("webrtcd") - self.logger.info("New stream session (%s), cameras %s, audio in %s out %s, incoming services %s, outgoing services %s", - self.identifier, cameras, config.incoming_audio_track, config.expected_audio_track, incoming_services, outgoing_services) + self.logger.info( + "New stream session (%s), init camera %s, video enabled %s, incoming services %s, outgoing services %s", + self.identifier, body.init_camera, body.enabled, body.bridge_services_in, body.bridge_services_out, + ) def start(self): self.run_task = asyncio.create_task(self.run()) - def stop(self): - if self.run_task.done(): - return - self.run_task.cancel() + async def stop(self): + if self.run_task is not None and not self.run_task.done() and self.run_task is not asyncio.current_task(): + self.run_task.cancel() + with contextlib.suppress(asyncio.CancelledError): + await self.run_task self.run_task = None - asyncio.run(self.post_run_cleanup()) + await self.post_run_cleanup() async def get_answer(self): return await self.stream.start() - async def message_handler(self, message: bytes): - assert self.incoming_bridge is not None + def message_handler(self, message: bytes): try: - self.incoming_bridge.send(message) + payload = json.loads(message) if isinstance(message, (bytes, str)) else None + if isinstance(payload, dict): + msg_type = payload.get("type") + + match msg_type: + case "livestreamCameraSwitch": + self.video_track.switch_camera(payload["data"]["camera"]) + case "livestreamSettings": + self.bitrate_controller.set_quality(payload["data"]["quality"]) + case "livestreamVideoEnable": + enabled = payload["data"]["enabled"] + self.enabled = enabled + self.video_track.enable(enabled) + if self.outgoing_bridge is not None: + self.outgoing_bridge.enable(enabled) + self.bitrate_controller.enable(enabled) + if not enabled: + self.params.put("LivestreamRequestKeyframe", True) + case "clockSync": + pong = json.dumps({"type": "clockSync", "data": { + "action": "pong", "browserSendTime": payload["data"]["browserSendTime"], "deviceTime": time.time() * 1000, # noqa: TID251 + }}) + self.stream.get_messaging_channel().send(pong) + case "enableTimingSei": + if hasattr(self.video_track, 'timing_sei_enabled'): + self.video_track.timing_sei_enabled = bool(payload["data"]["enabled"]) + case _: + if payload.get("type") not in self.incoming_bridge_services: + return + self.incoming_bridge.send(message) except Exception: self.logger.exception("Cereal incoming proxy failure") async def run(self): try: - await self.stream.wait_for_connection() + self.params.put("LivestreamRequestKeyframe", True) + await asyncio.wait_for(self.stream.wait_for_connection(), timeout=15) if self.stream.has_messaging_channel(): + self.stream.set_message_handler(self.message_handler) if self.incoming_bridge is not None: await self.shared_pub_master.add_services_if_needed(self.incoming_bridge_services) - self.stream.set_message_handler(self.message_handler) - if self.outgoing_bridge_runner is not None: + if self.outgoing_bridge is not None: channel = self.stream.get_messaging_channel() - self.outgoing_bridge_runner.proxy.add_channel(channel) - self.outgoing_bridge_runner.start() + self.outgoing_bridge.add_channel(channel) + self.outgoing_bridge.start() if self.stream.has_incoming_audio_track(): track = self.stream.get_incoming_audio_track(buffered=False) self.audio_output = self.audio_output_cls() self.audio_output.addTrack(track) self.audio_output.start() + self.bitrate_controller.start() + self.logger.info("Stream session (%s) connected", self.identifier) - await self.stream.wait_for_disconnection() - await self.post_run_cleanup() - self.logger.info("Stream session (%s) ended", self.identifier) except Exception: self.logger.exception("Stream session failure") + finally: + await self.post_run_cleanup() async def post_run_cleanup(self): - await self.stream.stop() - if self.outgoing_bridge is not None: - self.outgoing_bridge_runner.stop() - if self.audio_output: - self.audio_output.stop() + async with self._cleanup_lock: + if self._cleanup_done: + return + self._cleanup_done = True + self.params.put("LivestreamRequestKeyframe", False) + await self.bitrate_controller.stop() + if self.outgoing_bridge is not None: + await self.outgoing_bridge.stop() + if self.video_track is not None: + self.video_track.stop() + self.video_track = None + if self.audio_output is not None: + self.audio_output.stop() + self.audio_output = None + await self.stream.stop() -@dataclass -class StreamRequestBody: - sdp: str - cameras: list[str] - bridge_services_in: list[str] = field(default_factory=list) - bridge_services_out: list[str] = field(default_factory=list) +def schedule_teardown(app): + # if nothing connects for 5 seconds, tear down livestreaming processes + h = app.get('teardown') + if h: + h.cancel() + def clear(): + if not app['streams']: + Params().put_bool("IsLiveStreaming", False) + app['teardown'] = asyncio.get_running_loop().call_later(5.0, clear) async def get_stream(request: 'web.Request'): @@ -223,22 +398,49 @@ async def get_stream(request: 'web.Request'): raw_body = await request.json() body = StreamRequestBody(**raw_body) - session = StreamSession(body.sdp, body.cameras, body.bridge_services_in, body.bridge_services_out, debug_mode) - answer = await session.get_answer() - session.start() + async with request.app['stream_lock']: + # don't remove existing connection on prewarm request + enabled = any(s.run_task and not s.run_task.done() and s.enabled for s in stream_dict.values()) + if enabled and not body.enabled: + return web.json_response({"error": "busy", "message": "someone else is connected."}) - stream_dict[session.identifier] = session + for sid, s in list(stream_dict.items()): + if s.run_task and not s.run_task.done(): + try: + ch = s.stream.get_messaging_channel() + ch.send(json.dumps({"type": "disconnect", "data": "Another device has connected, closing this session."})) + except Exception: + pass + await s.stop() + stream_dict.pop(sid, None) + + session = StreamSession(body, debug_mode) + stream_dict[session.identifier] = session + try: + answer = await session.get_answer() + except Exception: + await session.stop() + stream_dict.pop(session.identifier, None) + logging.getLogger("webrtcd").exception("Failed to create stream answer") + raise + session.start() + + def remove_finished_session(_: asyncio.Task) -> None: + stream_dict.pop(session.identifier, None) + schedule_teardown(request.app) + session.run_task.add_done_callback(remove_finished_session) return web.json_response({"sdp": answer.sdp, "type": answer.type}) async def get_schema(request: 'web.Request'): - services = request.query["services"].split(",") + services = request.query.get("services", "").split(",") services = [s for s in services if s] assert all(s in log.Event.schema.fields and not s.endswith("DEPRECATED") for s in services), "Invalid service name" schema_dict = {s: generate_field(log.Event.schema.fields[s]) for s in services} return web.json_response(schema_dict) + async def post_notify(request: 'web.Request'): try: payload = await request.json() @@ -254,21 +456,48 @@ async def post_notify(request: 'web.Request'): return web.Response(status=200, text="OK") + async def on_shutdown(app: 'web.Application'): - for session in app['streams'].values(): - session.stop() + for session in list(app['streams'].values()): + try: + ch = session.stream.get_messaging_channel() + ch.send(json.dumps({"type": "disconnect", "data": "device streaming has been stopped."})) + except Exception: + pass + await session.stop() del app['streams'] +@web.middleware +async def error_middleware(request: 'web.Request', handler): + try: + return await handler(request) + except Exception as e: + logging.getLogger("webrtcd").exception("Unhandled error handling %s", request.path) + return web.json_response({"error": "exception", "message": f"{type(e).__name__}: {e}"}, status=500) + + +def prewarm_stream_session_imports(debug_mode: bool = False) -> None: + if debug_mode: + from aiortc.mediastreams import VideoStreamTrack + assert VideoStreamTrack + from openpilot.system.webrtc.device.video import LiveStreamVideoStreamTrack + from teleoprtc.builder import WebRTCAnswerBuilder + assert LiveStreamVideoStreamTrack + assert WebRTCAnswerBuilder + + def webrtcd_thread(host: str, port: int, debug: bool): logging.basicConfig(level=logging.CRITICAL, handlers=[logging.StreamHandler()]) - logging_level = logging.DEBUG if debug else logging.INFO - logging.getLogger("WebRTCStream").setLevel(logging_level) - logging.getLogger("webrtcd").setLevel(logging_level) + prewarm_start = time.monotonic() + prewarm_stream_session_imports(debug) + prewarm_end = time.monotonic() + logging.getLogger("webrtcd").info(f"webrtc prewarm finished in {(prewarm_end - prewarm_start) * 1000} ms") - app = web.Application() + app = web.Application(middlewares=[error_middleware]) app['streams'] = dict() + app['stream_lock'] = asyncio.Lock() app['debug'] = debug app.on_shutdown.append(on_shutdown) app.router.add_post("/stream", get_stream) diff --git a/tools/bodyteleop/web.py b/tools/bodyteleop/web.py index fd8f691d1..cfe00d321 100644 --- a/tools/bodyteleop/web.py +++ b/tools/bodyteleop/web.py @@ -12,7 +12,7 @@ from aiohttp import web from aiohttp import ClientSession from openpilot.common.basedir import BASEDIR -from openpilot.system.webrtc.webrtcd import StreamRequestBody +from openpilot.system.webrtc.helpers import StreamRequestBody from openpilot.common.params import Params logger = logging.getLogger("bodyteleop") @@ -96,7 +96,7 @@ async def sound(request: 'web.Request'): async def offer(request: 'web.Request'): params = await request.json() - body = StreamRequestBody(params["sdp"], ["driver"], ["testJoystick"], ["carState"]) + body = StreamRequestBody(params["sdp"], "driver", True, ["testJoystick"], ["carState"]) body_json = json.dumps(dataclasses.asdict(body)) logger.info("Sending offer to webrtcd...")