diff --git a/common/libcommon.a b/common/libcommon.a index b566e9dad..5c5dd0748 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 eab732600..b5f178c73 100644 --- a/common/params_keys.h +++ b/common/params_keys.h @@ -589,6 +589,7 @@ inline static std::unordered_map keys = { {"SteerOffsetStock", {PERSISTENT, FLOAT, "0.0", "0.0", 3}}, {"SteerRatio", {PERSISTENT, FLOAT, "0.0", "0.0", 3}}, {"SteerRatioStock", {PERSISTENT, FLOAT, "0.0", "0.0", 3}}, + {"EnableTorqueBarWidget", {PERSISTENT, BOOL, "1", "0", 0}}, {"StockConfidenceBallWidget", {PERSISTENT, BOOL, "0", "0", 0}}, {"StockDongleId", {PERSISTENT, STRING, "", ""}}, {"StopAccel", {PERSISTENT, FLOAT, "0.0", "0.0", 3}}, diff --git a/common/params_pyx.so b/common/params_pyx.so index 194c25da5..a79ba6947 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 cc0fb75c0..f4b6563f0 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 4531651ac..f48044375 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 191ae0d0d..c401163d1 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 3b47aa35b..61bedcf35 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 9f40e113e..b628a9f3c 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 e5d419561..49179d16c 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 e5d419561..49179d16c 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 c7cb64bc8..6c5c2d08b 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 c7cb64bc8..6c5c2d08b 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 c7cb64bc8..6c5c2d08b 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 c7cb64bc8..6c5c2d08b 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 c7cb64bc8..6c5c2d08b 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 c7cb64bc8..6c5c2d08b 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 e5d419561..49179d16c 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 e5d419561..49179d16c 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 9edac6832..a5ce034fa 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 e5d419561..49179d16c 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 e5d419561..49179d16c 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 b97bdc9b0..552ed520f 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-cb82059d-DEBUG"; +const uint8_t gitversion[19] = "DEV-231eb68f-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index d6595cc11..1b68dae6b 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 b69c1e5dc..0133315c3 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 1c7a6a9d3..e81675cd1 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 127a0808d..a8018cdb4 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 b02da5fa7..d36314410 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 ccb853073..9e7658a5d 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 459f11e10..b483d0296 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 898a9d38e..9a4139939 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 97e4b9ed4..eeb5384b1 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 a8e0b6825..175fc883a 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 ab1add88d..6b87db32d 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 3e8e46563..0a7bc63b0 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 a393d0a4c..5b5bb330d 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 3e304eaaa..cf46f7bc5 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 fe7ac0156..5300dd6c6 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 276828dbb..af943d8dc 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 05adfd60e..9204e38d2 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 b05ae50c2..c2f0adfc4 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 1b0427d19..2dc705934 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 848d6399d..6354aa7d1 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 bae030e42..ef81b0a89 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 2927616fa..2d7691dc4 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 eeee56275..45223ddb9 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 2abe5b283..e36c098d2 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 f3ff7cc11..be5ebbaa5 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 a4eec171d..f691443d8 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 7cec63ec0..519a2d188 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 6f779a833..40183fca5 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 b683b7011..41e562157 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 b2cefad95..d7e607be5 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 b25c26dd4..22ce7f618 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 77974cb2a..a489b7929 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 0f38d6077..46b767449 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 20a5e3d5e..f2a65c7c5 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 79145b903..d563cb94f 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 d452bdaa9..4fc45e52d 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 68d765cf7..7a15e6456 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 d244f15d5..4171dee70 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 cae0f2372..6600e4ca0 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 2bdb610c6..36d4ad78e 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 a4e00fc20..a832c69ae 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 eeb0afd78..23292f961 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 6dfbb045c..473456ca4 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 f644ce2dd..94ee0e12c 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 1c3341e70..4dab199ec 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 e0e046567..2c1906e2a 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 a0a33947a..a1cf76add 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 733dea575..22dd692ac 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 d861c7a83..ed7a4216f 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 1a35f90d8..234cbe9b9 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 b8f19a9bf..97bd21ddf 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 c9a1b79fa..753fcf508 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 611b4e279..59b33b480 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-cb82059d-DEBUG \ No newline at end of file +DEV-231eb68f-DEBUG \ No newline at end of file diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index c6707b34c..38485bd9a 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_6125726627663246664) { - out_6125726627663246664[0] = delta_x[0] + nom_x[0]; - out_6125726627663246664[1] = delta_x[1] + nom_x[1]; - out_6125726627663246664[2] = delta_x[2] + nom_x[2]; - out_6125726627663246664[3] = delta_x[3] + nom_x[3]; - out_6125726627663246664[4] = delta_x[4] + nom_x[4]; - out_6125726627663246664[5] = delta_x[5] + nom_x[5]; - out_6125726627663246664[6] = delta_x[6] + nom_x[6]; - out_6125726627663246664[7] = delta_x[7] + nom_x[7]; - out_6125726627663246664[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_1393219424364542669) { + out_1393219424364542669[0] = delta_x[0] + nom_x[0]; + out_1393219424364542669[1] = delta_x[1] + nom_x[1]; + out_1393219424364542669[2] = delta_x[2] + nom_x[2]; + out_1393219424364542669[3] = delta_x[3] + nom_x[3]; + out_1393219424364542669[4] = delta_x[4] + nom_x[4]; + out_1393219424364542669[5] = delta_x[5] + nom_x[5]; + out_1393219424364542669[6] = delta_x[6] + nom_x[6]; + out_1393219424364542669[7] = delta_x[7] + nom_x[7]; + out_1393219424364542669[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_87606233557452672) { - out_87606233557452672[0] = -nom_x[0] + true_x[0]; - out_87606233557452672[1] = -nom_x[1] + true_x[1]; - out_87606233557452672[2] = -nom_x[2] + true_x[2]; - out_87606233557452672[3] = -nom_x[3] + true_x[3]; - out_87606233557452672[4] = -nom_x[4] + true_x[4]; - out_87606233557452672[5] = -nom_x[5] + true_x[5]; - out_87606233557452672[6] = -nom_x[6] + true_x[6]; - out_87606233557452672[7] = -nom_x[7] + true_x[7]; - out_87606233557452672[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_8544281472794776121) { + out_8544281472794776121[0] = -nom_x[0] + true_x[0]; + out_8544281472794776121[1] = -nom_x[1] + true_x[1]; + out_8544281472794776121[2] = -nom_x[2] + true_x[2]; + out_8544281472794776121[3] = -nom_x[3] + true_x[3]; + out_8544281472794776121[4] = -nom_x[4] + true_x[4]; + out_8544281472794776121[5] = -nom_x[5] + true_x[5]; + out_8544281472794776121[6] = -nom_x[6] + true_x[6]; + out_8544281472794776121[7] = -nom_x[7] + true_x[7]; + out_8544281472794776121[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_1571740516032572076) { - out_1571740516032572076[0] = 1.0; - out_1571740516032572076[1] = 0.0; - out_1571740516032572076[2] = 0.0; - out_1571740516032572076[3] = 0.0; - out_1571740516032572076[4] = 0.0; - out_1571740516032572076[5] = 0.0; - out_1571740516032572076[6] = 0.0; - out_1571740516032572076[7] = 0.0; - out_1571740516032572076[8] = 0.0; - out_1571740516032572076[9] = 0.0; - out_1571740516032572076[10] = 1.0; - out_1571740516032572076[11] = 0.0; - out_1571740516032572076[12] = 0.0; - out_1571740516032572076[13] = 0.0; - out_1571740516032572076[14] = 0.0; - out_1571740516032572076[15] = 0.0; - out_1571740516032572076[16] = 0.0; - out_1571740516032572076[17] = 0.0; - out_1571740516032572076[18] = 0.0; - out_1571740516032572076[19] = 0.0; - out_1571740516032572076[20] = 1.0; - out_1571740516032572076[21] = 0.0; - out_1571740516032572076[22] = 0.0; - out_1571740516032572076[23] = 0.0; - out_1571740516032572076[24] = 0.0; - out_1571740516032572076[25] = 0.0; - out_1571740516032572076[26] = 0.0; - out_1571740516032572076[27] = 0.0; - out_1571740516032572076[28] = 0.0; - out_1571740516032572076[29] = 0.0; - out_1571740516032572076[30] = 1.0; - out_1571740516032572076[31] = 0.0; - out_1571740516032572076[32] = 0.0; - out_1571740516032572076[33] = 0.0; - out_1571740516032572076[34] = 0.0; - out_1571740516032572076[35] = 0.0; - out_1571740516032572076[36] = 0.0; - out_1571740516032572076[37] = 0.0; - out_1571740516032572076[38] = 0.0; - out_1571740516032572076[39] = 0.0; - out_1571740516032572076[40] = 1.0; - out_1571740516032572076[41] = 0.0; - out_1571740516032572076[42] = 0.0; - out_1571740516032572076[43] = 0.0; - out_1571740516032572076[44] = 0.0; - out_1571740516032572076[45] = 0.0; - out_1571740516032572076[46] = 0.0; - out_1571740516032572076[47] = 0.0; - out_1571740516032572076[48] = 0.0; - out_1571740516032572076[49] = 0.0; - out_1571740516032572076[50] = 1.0; - out_1571740516032572076[51] = 0.0; - out_1571740516032572076[52] = 0.0; - out_1571740516032572076[53] = 0.0; - out_1571740516032572076[54] = 0.0; - out_1571740516032572076[55] = 0.0; - out_1571740516032572076[56] = 0.0; - out_1571740516032572076[57] = 0.0; - out_1571740516032572076[58] = 0.0; - out_1571740516032572076[59] = 0.0; - out_1571740516032572076[60] = 1.0; - out_1571740516032572076[61] = 0.0; - out_1571740516032572076[62] = 0.0; - out_1571740516032572076[63] = 0.0; - out_1571740516032572076[64] = 0.0; - out_1571740516032572076[65] = 0.0; - out_1571740516032572076[66] = 0.0; - out_1571740516032572076[67] = 0.0; - out_1571740516032572076[68] = 0.0; - out_1571740516032572076[69] = 0.0; - out_1571740516032572076[70] = 1.0; - out_1571740516032572076[71] = 0.0; - out_1571740516032572076[72] = 0.0; - out_1571740516032572076[73] = 0.0; - out_1571740516032572076[74] = 0.0; - out_1571740516032572076[75] = 0.0; - out_1571740516032572076[76] = 0.0; - out_1571740516032572076[77] = 0.0; - out_1571740516032572076[78] = 0.0; - out_1571740516032572076[79] = 0.0; - out_1571740516032572076[80] = 1.0; +void H_mod_fun(double *state, double *out_542719001883324995) { + out_542719001883324995[0] = 1.0; + out_542719001883324995[1] = 0.0; + out_542719001883324995[2] = 0.0; + out_542719001883324995[3] = 0.0; + out_542719001883324995[4] = 0.0; + out_542719001883324995[5] = 0.0; + out_542719001883324995[6] = 0.0; + out_542719001883324995[7] = 0.0; + out_542719001883324995[8] = 0.0; + out_542719001883324995[9] = 0.0; + out_542719001883324995[10] = 1.0; + out_542719001883324995[11] = 0.0; + out_542719001883324995[12] = 0.0; + out_542719001883324995[13] = 0.0; + out_542719001883324995[14] = 0.0; + out_542719001883324995[15] = 0.0; + out_542719001883324995[16] = 0.0; + out_542719001883324995[17] = 0.0; + out_542719001883324995[18] = 0.0; + out_542719001883324995[19] = 0.0; + out_542719001883324995[20] = 1.0; + out_542719001883324995[21] = 0.0; + out_542719001883324995[22] = 0.0; + out_542719001883324995[23] = 0.0; + out_542719001883324995[24] = 0.0; + out_542719001883324995[25] = 0.0; + out_542719001883324995[26] = 0.0; + out_542719001883324995[27] = 0.0; + out_542719001883324995[28] = 0.0; + out_542719001883324995[29] = 0.0; + out_542719001883324995[30] = 1.0; + out_542719001883324995[31] = 0.0; + out_542719001883324995[32] = 0.0; + out_542719001883324995[33] = 0.0; + out_542719001883324995[34] = 0.0; + out_542719001883324995[35] = 0.0; + out_542719001883324995[36] = 0.0; + out_542719001883324995[37] = 0.0; + out_542719001883324995[38] = 0.0; + out_542719001883324995[39] = 0.0; + out_542719001883324995[40] = 1.0; + out_542719001883324995[41] = 0.0; + out_542719001883324995[42] = 0.0; + out_542719001883324995[43] = 0.0; + out_542719001883324995[44] = 0.0; + out_542719001883324995[45] = 0.0; + out_542719001883324995[46] = 0.0; + out_542719001883324995[47] = 0.0; + out_542719001883324995[48] = 0.0; + out_542719001883324995[49] = 0.0; + out_542719001883324995[50] = 1.0; + out_542719001883324995[51] = 0.0; + out_542719001883324995[52] = 0.0; + out_542719001883324995[53] = 0.0; + out_542719001883324995[54] = 0.0; + out_542719001883324995[55] = 0.0; + out_542719001883324995[56] = 0.0; + out_542719001883324995[57] = 0.0; + out_542719001883324995[58] = 0.0; + out_542719001883324995[59] = 0.0; + out_542719001883324995[60] = 1.0; + out_542719001883324995[61] = 0.0; + out_542719001883324995[62] = 0.0; + out_542719001883324995[63] = 0.0; + out_542719001883324995[64] = 0.0; + out_542719001883324995[65] = 0.0; + out_542719001883324995[66] = 0.0; + out_542719001883324995[67] = 0.0; + out_542719001883324995[68] = 0.0; + out_542719001883324995[69] = 0.0; + out_542719001883324995[70] = 1.0; + out_542719001883324995[71] = 0.0; + out_542719001883324995[72] = 0.0; + out_542719001883324995[73] = 0.0; + out_542719001883324995[74] = 0.0; + out_542719001883324995[75] = 0.0; + out_542719001883324995[76] = 0.0; + out_542719001883324995[77] = 0.0; + out_542719001883324995[78] = 0.0; + out_542719001883324995[79] = 0.0; + out_542719001883324995[80] = 1.0; } -void f_fun(double *state, double dt, double *out_7904413896065989389) { - out_7904413896065989389[0] = state[0]; - out_7904413896065989389[1] = state[1]; - out_7904413896065989389[2] = state[2]; - out_7904413896065989389[3] = state[3]; - out_7904413896065989389[4] = state[4]; - out_7904413896065989389[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_7904413896065989389[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_7904413896065989389[7] = state[7]; - out_7904413896065989389[8] = state[8]; +void f_fun(double *state, double dt, double *out_9203753821438188332) { + out_9203753821438188332[0] = state[0]; + out_9203753821438188332[1] = state[1]; + out_9203753821438188332[2] = state[2]; + out_9203753821438188332[3] = state[3]; + out_9203753821438188332[4] = state[4]; + out_9203753821438188332[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_9203753821438188332[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_9203753821438188332[7] = state[7]; + out_9203753821438188332[8] = state[8]; } -void F_fun(double *state, double dt, double *out_6516583174049416628) { - out_6516583174049416628[0] = 1; - out_6516583174049416628[1] = 0; - out_6516583174049416628[2] = 0; - out_6516583174049416628[3] = 0; - out_6516583174049416628[4] = 0; - out_6516583174049416628[5] = 0; - out_6516583174049416628[6] = 0; - out_6516583174049416628[7] = 0; - out_6516583174049416628[8] = 0; - out_6516583174049416628[9] = 0; - out_6516583174049416628[10] = 1; - out_6516583174049416628[11] = 0; - out_6516583174049416628[12] = 0; - out_6516583174049416628[13] = 0; - out_6516583174049416628[14] = 0; - out_6516583174049416628[15] = 0; - out_6516583174049416628[16] = 0; - out_6516583174049416628[17] = 0; - out_6516583174049416628[18] = 0; - out_6516583174049416628[19] = 0; - out_6516583174049416628[20] = 1; - out_6516583174049416628[21] = 0; - out_6516583174049416628[22] = 0; - out_6516583174049416628[23] = 0; - out_6516583174049416628[24] = 0; - out_6516583174049416628[25] = 0; - out_6516583174049416628[26] = 0; - out_6516583174049416628[27] = 0; - out_6516583174049416628[28] = 0; - out_6516583174049416628[29] = 0; - out_6516583174049416628[30] = 1; - out_6516583174049416628[31] = 0; - out_6516583174049416628[32] = 0; - out_6516583174049416628[33] = 0; - out_6516583174049416628[34] = 0; - out_6516583174049416628[35] = 0; - out_6516583174049416628[36] = 0; - out_6516583174049416628[37] = 0; - out_6516583174049416628[38] = 0; - out_6516583174049416628[39] = 0; - out_6516583174049416628[40] = 1; - out_6516583174049416628[41] = 0; - out_6516583174049416628[42] = 0; - out_6516583174049416628[43] = 0; - out_6516583174049416628[44] = 0; - out_6516583174049416628[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_6516583174049416628[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_6516583174049416628[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_6516583174049416628[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_6516583174049416628[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_6516583174049416628[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_6516583174049416628[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_6516583174049416628[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_6516583174049416628[53] = -9.8100000000000005*dt; - out_6516583174049416628[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_6516583174049416628[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_6516583174049416628[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6516583174049416628[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6516583174049416628[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_6516583174049416628[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_6516583174049416628[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_6516583174049416628[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_6516583174049416628[62] = 0; - out_6516583174049416628[63] = 0; - out_6516583174049416628[64] = 0; - out_6516583174049416628[65] = 0; - out_6516583174049416628[66] = 0; - out_6516583174049416628[67] = 0; - out_6516583174049416628[68] = 0; - out_6516583174049416628[69] = 0; - out_6516583174049416628[70] = 1; - out_6516583174049416628[71] = 0; - out_6516583174049416628[72] = 0; - out_6516583174049416628[73] = 0; - out_6516583174049416628[74] = 0; - out_6516583174049416628[75] = 0; - out_6516583174049416628[76] = 0; - out_6516583174049416628[77] = 0; - out_6516583174049416628[78] = 0; - out_6516583174049416628[79] = 0; - out_6516583174049416628[80] = 1; +void F_fun(double *state, double dt, double *out_1821220582526959893) { + out_1821220582526959893[0] = 1; + out_1821220582526959893[1] = 0; + out_1821220582526959893[2] = 0; + out_1821220582526959893[3] = 0; + out_1821220582526959893[4] = 0; + out_1821220582526959893[5] = 0; + out_1821220582526959893[6] = 0; + out_1821220582526959893[7] = 0; + out_1821220582526959893[8] = 0; + out_1821220582526959893[9] = 0; + out_1821220582526959893[10] = 1; + out_1821220582526959893[11] = 0; + out_1821220582526959893[12] = 0; + out_1821220582526959893[13] = 0; + out_1821220582526959893[14] = 0; + out_1821220582526959893[15] = 0; + out_1821220582526959893[16] = 0; + out_1821220582526959893[17] = 0; + out_1821220582526959893[18] = 0; + out_1821220582526959893[19] = 0; + out_1821220582526959893[20] = 1; + out_1821220582526959893[21] = 0; + out_1821220582526959893[22] = 0; + out_1821220582526959893[23] = 0; + out_1821220582526959893[24] = 0; + out_1821220582526959893[25] = 0; + out_1821220582526959893[26] = 0; + out_1821220582526959893[27] = 0; + out_1821220582526959893[28] = 0; + out_1821220582526959893[29] = 0; + out_1821220582526959893[30] = 1; + out_1821220582526959893[31] = 0; + out_1821220582526959893[32] = 0; + out_1821220582526959893[33] = 0; + out_1821220582526959893[34] = 0; + out_1821220582526959893[35] = 0; + out_1821220582526959893[36] = 0; + out_1821220582526959893[37] = 0; + out_1821220582526959893[38] = 0; + out_1821220582526959893[39] = 0; + out_1821220582526959893[40] = 1; + out_1821220582526959893[41] = 0; + out_1821220582526959893[42] = 0; + out_1821220582526959893[43] = 0; + out_1821220582526959893[44] = 0; + out_1821220582526959893[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_1821220582526959893[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_1821220582526959893[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_1821220582526959893[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_1821220582526959893[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_1821220582526959893[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_1821220582526959893[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_1821220582526959893[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_1821220582526959893[53] = -9.8100000000000005*dt; + out_1821220582526959893[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_1821220582526959893[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_1821220582526959893[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_1821220582526959893[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_1821220582526959893[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_1821220582526959893[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_1821220582526959893[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_1821220582526959893[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_1821220582526959893[62] = 0; + out_1821220582526959893[63] = 0; + out_1821220582526959893[64] = 0; + out_1821220582526959893[65] = 0; + out_1821220582526959893[66] = 0; + out_1821220582526959893[67] = 0; + out_1821220582526959893[68] = 0; + out_1821220582526959893[69] = 0; + out_1821220582526959893[70] = 1; + out_1821220582526959893[71] = 0; + out_1821220582526959893[72] = 0; + out_1821220582526959893[73] = 0; + out_1821220582526959893[74] = 0; + out_1821220582526959893[75] = 0; + out_1821220582526959893[76] = 0; + out_1821220582526959893[77] = 0; + out_1821220582526959893[78] = 0; + out_1821220582526959893[79] = 0; + out_1821220582526959893[80] = 1; } -void h_25(double *state, double *unused, double *out_6845996924949086281) { - out_6845996924949086281[0] = state[6]; +void h_25(double *state, double *unused, double *out_7848411301120641124) { + out_7848411301120641124[0] = state[6]; } -void H_25(double *state, double *unused, double *out_4925282443857457875) { - out_4925282443857457875[0] = 0; - out_4925282443857457875[1] = 0; - out_4925282443857457875[2] = 0; - out_4925282443857457875[3] = 0; - out_4925282443857457875[4] = 0; - out_4925282443857457875[5] = 0; - out_4925282443857457875[6] = 1; - out_4925282443857457875[7] = 0; - out_4925282443857457875[8] = 0; +void H_25(double *state, double *unused, double *out_6136977117952979916) { + out_6136977117952979916[0] = 0; + out_6136977117952979916[1] = 0; + out_6136977117952979916[2] = 0; + out_6136977117952979916[3] = 0; + out_6136977117952979916[4] = 0; + out_6136977117952979916[5] = 0; + out_6136977117952979916[6] = 1; + out_6136977117952979916[7] = 0; + out_6136977117952979916[8] = 0; } -void h_24(double *state, double *unused, double *out_76787194833890602) { - out_76787194833890602[0] = state[4]; - out_76787194833890602[1] = state[5]; +void h_24(double *state, double *unused, double *out_8292657587405145092) { + out_8292657587405145092[0] = state[4]; + out_8292657587405145092[1] = state[5]; } -void H_24(double *state, double *unused, double *out_2493109189819469018) { - out_2493109189819469018[0] = 0; - out_2493109189819469018[1] = 0; - out_2493109189819469018[2] = 0; - out_2493109189819469018[3] = 0; - out_2493109189819469018[4] = 1; - out_2493109189819469018[5] = 0; - out_2493109189819469018[6] = 0; - out_2493109189819469018[7] = 0; - out_2493109189819469018[8] = 0; - out_2493109189819469018[9] = 0; - out_2493109189819469018[10] = 0; - out_2493109189819469018[11] = 0; - out_2493109189819469018[12] = 0; - out_2493109189819469018[13] = 0; - out_2493109189819469018[14] = 1; - out_2493109189819469018[15] = 0; - out_2493109189819469018[16] = 0; - out_2493109189819469018[17] = 0; +void H_24(double *state, double *unused, double *out_3964327518947480350) { + out_3964327518947480350[0] = 0; + out_3964327518947480350[1] = 0; + out_3964327518947480350[2] = 0; + out_3964327518947480350[3] = 0; + out_3964327518947480350[4] = 1; + out_3964327518947480350[5] = 0; + out_3964327518947480350[6] = 0; + out_3964327518947480350[7] = 0; + out_3964327518947480350[8] = 0; + out_3964327518947480350[9] = 0; + out_3964327518947480350[10] = 0; + out_3964327518947480350[11] = 0; + out_3964327518947480350[12] = 0; + out_3964327518947480350[13] = 0; + out_3964327518947480350[14] = 1; + out_3964327518947480350[15] = 0; + out_3964327518947480350[16] = 0; + out_3964327518947480350[17] = 0; } -void h_30(double *state, double *unused, double *out_7304198287107647943) { - out_7304198287107647943[0] = state[4]; +void h_30(double *state, double *unused, double *out_1692964105279980995) { + out_1692964105279980995[0] = state[4]; } -void H_30(double *state, double *unused, double *out_8993765299724485543) { - out_8993765299724485543[0] = 0; - out_8993765299724485543[1] = 0; - out_8993765299724485543[2] = 0; - out_8993765299724485543[3] = 0; - out_8993765299724485543[4] = 1; - out_8993765299724485543[5] = 0; - out_8993765299724485543[6] = 0; - out_8993765299724485543[7] = 0; - out_8993765299724485543[8] = 0; +void H_30(double *state, double *unused, double *out_8655310076460228543) { + out_8655310076460228543[0] = 0; + out_8655310076460228543[1] = 0; + out_8655310076460228543[2] = 0; + out_8655310076460228543[3] = 0; + out_8655310076460228543[4] = 1; + out_8655310076460228543[5] = 0; + out_8655310076460228543[6] = 0; + out_8655310076460228543[7] = 0; + out_8655310076460228543[8] = 0; } -void h_26(double *state, double *unused, double *out_6360688434205985508) { - out_6360688434205985508[0] = state[7]; +void h_26(double *state, double *unused, double *out_6694637372940826640) { + out_6694637372940826640[0] = state[7]; } -void H_26(double *state, double *unused, double *out_8666785762731514099) { - out_8666785762731514099[0] = 0; - out_8666785762731514099[1] = 0; - out_8666785762731514099[2] = 0; - out_8666785762731514099[3] = 0; - out_8666785762731514099[4] = 0; - out_8666785762731514099[5] = 0; - out_8666785762731514099[6] = 0; - out_8666785762731514099[7] = 1; - out_8666785762731514099[8] = 0; +void H_26(double *state, double *unused, double *out_2395473799078923692) { + out_2395473799078923692[0] = 0; + out_2395473799078923692[1] = 0; + out_2395473799078923692[2] = 0; + out_2395473799078923692[3] = 0; + out_2395473799078923692[4] = 0; + out_2395473799078923692[5] = 0; + out_2395473799078923692[6] = 0; + out_2395473799078923692[7] = 1; + out_2395473799078923692[8] = 0; } -void h_27(double *state, double *unused, double *out_2448044843667457269) { - out_2448044843667457269[0] = state[3]; +void h_27(double *state, double *unused, double *out_3658606002300167844) { + out_3658606002300167844[0] = state[3]; } -void H_27(double *state, double *unused, double *out_6819001987924060632) { - out_6819001987924060632[0] = 0; - out_6819001987924060632[1] = 0; - out_6819001987924060632[2] = 0; - out_6819001987924060632[3] = 1; - out_6819001987924060632[4] = 0; - out_6819001987924060632[5] = 0; - out_6819001987924060632[6] = 0; - out_6819001987924060632[7] = 0; - out_6819001987924060632[8] = 0; +void H_27(double *state, double *unused, double *out_6480546764659803632) { + out_6480546764659803632[0] = 0; + out_6480546764659803632[1] = 0; + out_6480546764659803632[2] = 0; + out_6480546764659803632[3] = 1; + out_6480546764659803632[4] = 0; + out_6480546764659803632[5] = 0; + out_6480546764659803632[6] = 0; + out_6480546764659803632[7] = 0; + out_6480546764659803632[8] = 0; } -void h_29(double *state, double *unused, double *out_2302525019446802298) { - out_2302525019446802298[0] = state[1]; +void h_29(double *state, double *unused, double *out_3383411940015661955) { + out_3383411940015661955[0] = state[1]; } -void H_29(double *state, double *unused, double *out_8942747429670673889) { - out_8942747429670673889[0] = 0; - out_8942747429670673889[1] = 1; - out_8942747429670673889[2] = 0; - out_8942747429670673889[3] = 0; - out_8942747429670673889[4] = 0; - out_8942747429670673889[5] = 0; - out_8942747429670673889[6] = 0; - out_8942747429670673889[7] = 0; - out_8942747429670673889[8] = 0; +void H_29(double *state, double *unused, double *out_9165541420774620727) { + out_9165541420774620727[0] = 0; + out_9165541420774620727[1] = 1; + out_9165541420774620727[2] = 0; + out_9165541420774620727[3] = 0; + out_9165541420774620727[4] = 0; + out_9165541420774620727[5] = 0; + out_9165541420774620727[6] = 0; + out_9165541420774620727[7] = 0; + out_9165541420774620727[8] = 0; } -void h_28(double *state, double *unused, double *out_2553628423993388376) { - out_2553628423993388376[0] = state[0]; +void h_28(double *state, double *unused, double *out_2219679485258547244) { + out_2219679485258547244[0] = state[0]; } -void H_28(double *state, double *unused, double *out_4421597626969347153) { - out_4421597626969347153[0] = 1; - out_4421597626969347153[1] = 0; - out_4421597626969347153[2] = 0; - out_4421597626969347153[3] = 0; - out_4421597626969347153[4] = 0; - out_4421597626969347153[5] = 0; - out_4421597626969347153[6] = 0; - out_4421597626969347153[7] = 0; - out_4421597626969347153[8] = 0; +void H_28(double *state, double *unused, double *out_4083142403705090153) { + out_4083142403705090153[0] = 1; + out_4083142403705090153[1] = 0; + out_4083142403705090153[2] = 0; + out_4083142403705090153[3] = 0; + out_4083142403705090153[4] = 0; + out_4083142403705090153[5] = 0; + out_4083142403705090153[6] = 0; + out_4083142403705090153[7] = 0; + out_4083142403705090153[8] = 0; } -void h_31(double *state, double *unused, double *out_7359576434127226352) { - out_7359576434127226352[0] = state[8]; +void h_31(double *state, double *unused, double *out_8123605363405147013) { + out_8123605363405147013[0] = state[8]; } -void H_31(double *state, double *unused, double *out_9153750208744686041) { - out_9153750208744686041[0] = 0; - out_9153750208744686041[1] = 0; - out_9153750208744686041[2] = 0; - out_9153750208744686041[3] = 0; - out_9153750208744686041[4] = 0; - out_9153750208744686041[5] = 0; - out_9153750208744686041[6] = 0; - out_9153750208744686041[7] = 0; - out_9153750208744686041[8] = 1; +void H_31(double *state, double *unused, double *out_1769265696845572216) { + out_1769265696845572216[0] = 0; + out_1769265696845572216[1] = 0; + out_1769265696845572216[2] = 0; + out_1769265696845572216[3] = 0; + out_1769265696845572216[4] = 0; + out_1769265696845572216[5] = 0; + out_1769265696845572216[6] = 0; + out_1769265696845572216[7] = 0; + out_1769265696845572216[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_6125726627663246664) { - err_fun(nom_x, delta_x, out_6125726627663246664); +void car_err_fun(double *nom_x, double *delta_x, double *out_1393219424364542669) { + err_fun(nom_x, delta_x, out_1393219424364542669); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_87606233557452672) { - inv_err_fun(nom_x, true_x, out_87606233557452672); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_8544281472794776121) { + inv_err_fun(nom_x, true_x, out_8544281472794776121); } -void car_H_mod_fun(double *state, double *out_1571740516032572076) { - H_mod_fun(state, out_1571740516032572076); +void car_H_mod_fun(double *state, double *out_542719001883324995) { + H_mod_fun(state, out_542719001883324995); } -void car_f_fun(double *state, double dt, double *out_7904413896065989389) { - f_fun(state, dt, out_7904413896065989389); +void car_f_fun(double *state, double dt, double *out_9203753821438188332) { + f_fun(state, dt, out_9203753821438188332); } -void car_F_fun(double *state, double dt, double *out_6516583174049416628) { - F_fun(state, dt, out_6516583174049416628); +void car_F_fun(double *state, double dt, double *out_1821220582526959893) { + F_fun(state, dt, out_1821220582526959893); } -void car_h_25(double *state, double *unused, double *out_6845996924949086281) { - h_25(state, unused, out_6845996924949086281); +void car_h_25(double *state, double *unused, double *out_7848411301120641124) { + h_25(state, unused, out_7848411301120641124); } -void car_H_25(double *state, double *unused, double *out_4925282443857457875) { - H_25(state, unused, out_4925282443857457875); +void car_H_25(double *state, double *unused, double *out_6136977117952979916) { + H_25(state, unused, out_6136977117952979916); } -void car_h_24(double *state, double *unused, double *out_76787194833890602) { - h_24(state, unused, out_76787194833890602); +void car_h_24(double *state, double *unused, double *out_8292657587405145092) { + h_24(state, unused, out_8292657587405145092); } -void car_H_24(double *state, double *unused, double *out_2493109189819469018) { - H_24(state, unused, out_2493109189819469018); +void car_H_24(double *state, double *unused, double *out_3964327518947480350) { + H_24(state, unused, out_3964327518947480350); } -void car_h_30(double *state, double *unused, double *out_7304198287107647943) { - h_30(state, unused, out_7304198287107647943); +void car_h_30(double *state, double *unused, double *out_1692964105279980995) { + h_30(state, unused, out_1692964105279980995); } -void car_H_30(double *state, double *unused, double *out_8993765299724485543) { - H_30(state, unused, out_8993765299724485543); +void car_H_30(double *state, double *unused, double *out_8655310076460228543) { + H_30(state, unused, out_8655310076460228543); } -void car_h_26(double *state, double *unused, double *out_6360688434205985508) { - h_26(state, unused, out_6360688434205985508); +void car_h_26(double *state, double *unused, double *out_6694637372940826640) { + h_26(state, unused, out_6694637372940826640); } -void car_H_26(double *state, double *unused, double *out_8666785762731514099) { - H_26(state, unused, out_8666785762731514099); +void car_H_26(double *state, double *unused, double *out_2395473799078923692) { + H_26(state, unused, out_2395473799078923692); } -void car_h_27(double *state, double *unused, double *out_2448044843667457269) { - h_27(state, unused, out_2448044843667457269); +void car_h_27(double *state, double *unused, double *out_3658606002300167844) { + h_27(state, unused, out_3658606002300167844); } -void car_H_27(double *state, double *unused, double *out_6819001987924060632) { - H_27(state, unused, out_6819001987924060632); +void car_H_27(double *state, double *unused, double *out_6480546764659803632) { + H_27(state, unused, out_6480546764659803632); } -void car_h_29(double *state, double *unused, double *out_2302525019446802298) { - h_29(state, unused, out_2302525019446802298); +void car_h_29(double *state, double *unused, double *out_3383411940015661955) { + h_29(state, unused, out_3383411940015661955); } -void car_H_29(double *state, double *unused, double *out_8942747429670673889) { - H_29(state, unused, out_8942747429670673889); +void car_H_29(double *state, double *unused, double *out_9165541420774620727) { + H_29(state, unused, out_9165541420774620727); } -void car_h_28(double *state, double *unused, double *out_2553628423993388376) { - h_28(state, unused, out_2553628423993388376); +void car_h_28(double *state, double *unused, double *out_2219679485258547244) { + h_28(state, unused, out_2219679485258547244); } -void car_H_28(double *state, double *unused, double *out_4421597626969347153) { - H_28(state, unused, out_4421597626969347153); +void car_H_28(double *state, double *unused, double *out_4083142403705090153) { + H_28(state, unused, out_4083142403705090153); } -void car_h_31(double *state, double *unused, double *out_7359576434127226352) { - h_31(state, unused, out_7359576434127226352); +void car_h_31(double *state, double *unused, double *out_8123605363405147013) { + h_31(state, unused, out_8123605363405147013); } -void car_H_31(double *state, double *unused, double *out_9153750208744686041) { - H_31(state, unused, out_9153750208744686041); +void car_H_31(double *state, double *unused, double *out_1769265696845572216) { + H_31(state, unused, out_1769265696845572216); } 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 9578cbf44..6120c64f9 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_6125726627663246664); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_87606233557452672); -void car_H_mod_fun(double *state, double *out_1571740516032572076); -void car_f_fun(double *state, double dt, double *out_7904413896065989389); -void car_F_fun(double *state, double dt, double *out_6516583174049416628); -void car_h_25(double *state, double *unused, double *out_6845996924949086281); -void car_H_25(double *state, double *unused, double *out_4925282443857457875); -void car_h_24(double *state, double *unused, double *out_76787194833890602); -void car_H_24(double *state, double *unused, double *out_2493109189819469018); -void car_h_30(double *state, double *unused, double *out_7304198287107647943); -void car_H_30(double *state, double *unused, double *out_8993765299724485543); -void car_h_26(double *state, double *unused, double *out_6360688434205985508); -void car_H_26(double *state, double *unused, double *out_8666785762731514099); -void car_h_27(double *state, double *unused, double *out_2448044843667457269); -void car_H_27(double *state, double *unused, double *out_6819001987924060632); -void car_h_29(double *state, double *unused, double *out_2302525019446802298); -void car_H_29(double *state, double *unused, double *out_8942747429670673889); -void car_h_28(double *state, double *unused, double *out_2553628423993388376); -void car_H_28(double *state, double *unused, double *out_4421597626969347153); -void car_h_31(double *state, double *unused, double *out_7359576434127226352); -void car_H_31(double *state, double *unused, double *out_9153750208744686041); +void car_err_fun(double *nom_x, double *delta_x, double *out_1393219424364542669); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_8544281472794776121); +void car_H_mod_fun(double *state, double *out_542719001883324995); +void car_f_fun(double *state, double dt, double *out_9203753821438188332); +void car_F_fun(double *state, double dt, double *out_1821220582526959893); +void car_h_25(double *state, double *unused, double *out_7848411301120641124); +void car_H_25(double *state, double *unused, double *out_6136977117952979916); +void car_h_24(double *state, double *unused, double *out_8292657587405145092); +void car_H_24(double *state, double *unused, double *out_3964327518947480350); +void car_h_30(double *state, double *unused, double *out_1692964105279980995); +void car_H_30(double *state, double *unused, double *out_8655310076460228543); +void car_h_26(double *state, double *unused, double *out_6694637372940826640); +void car_H_26(double *state, double *unused, double *out_2395473799078923692); +void car_h_27(double *state, double *unused, double *out_3658606002300167844); +void car_H_27(double *state, double *unused, double *out_6480546764659803632); +void car_h_29(double *state, double *unused, double *out_3383411940015661955); +void car_H_29(double *state, double *unused, double *out_9165541420774620727); +void car_h_28(double *state, double *unused, double *out_2219679485258547244); +void car_H_28(double *state, double *unused, double *out_4083142403705090153); +void car_h_31(double *state, double *unused, double *out_8123605363405147013); +void car_H_31(double *state, double *unused, double *out_1769265696845572216); 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 0f40acd86..60c0e80a2 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_5340642756295943636) { - out_5340642756295943636[0] = delta_x[0] + nom_x[0]; - out_5340642756295943636[1] = delta_x[1] + nom_x[1]; - out_5340642756295943636[2] = delta_x[2] + nom_x[2]; - out_5340642756295943636[3] = delta_x[3] + nom_x[3]; - out_5340642756295943636[4] = delta_x[4] + nom_x[4]; - out_5340642756295943636[5] = delta_x[5] + nom_x[5]; - out_5340642756295943636[6] = delta_x[6] + nom_x[6]; - out_5340642756295943636[7] = delta_x[7] + nom_x[7]; - out_5340642756295943636[8] = delta_x[8] + nom_x[8]; - out_5340642756295943636[9] = delta_x[9] + nom_x[9]; - out_5340642756295943636[10] = delta_x[10] + nom_x[10]; - out_5340642756295943636[11] = delta_x[11] + nom_x[11]; - out_5340642756295943636[12] = delta_x[12] + nom_x[12]; - out_5340642756295943636[13] = delta_x[13] + nom_x[13]; - out_5340642756295943636[14] = delta_x[14] + nom_x[14]; - out_5340642756295943636[15] = delta_x[15] + nom_x[15]; - out_5340642756295943636[16] = delta_x[16] + nom_x[16]; - out_5340642756295943636[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_4967863734907407968) { + out_4967863734907407968[0] = delta_x[0] + nom_x[0]; + out_4967863734907407968[1] = delta_x[1] + nom_x[1]; + out_4967863734907407968[2] = delta_x[2] + nom_x[2]; + out_4967863734907407968[3] = delta_x[3] + nom_x[3]; + out_4967863734907407968[4] = delta_x[4] + nom_x[4]; + out_4967863734907407968[5] = delta_x[5] + nom_x[5]; + out_4967863734907407968[6] = delta_x[6] + nom_x[6]; + out_4967863734907407968[7] = delta_x[7] + nom_x[7]; + out_4967863734907407968[8] = delta_x[8] + nom_x[8]; + out_4967863734907407968[9] = delta_x[9] + nom_x[9]; + out_4967863734907407968[10] = delta_x[10] + nom_x[10]; + out_4967863734907407968[11] = delta_x[11] + nom_x[11]; + out_4967863734907407968[12] = delta_x[12] + nom_x[12]; + out_4967863734907407968[13] = delta_x[13] + nom_x[13]; + out_4967863734907407968[14] = delta_x[14] + nom_x[14]; + out_4967863734907407968[15] = delta_x[15] + nom_x[15]; + out_4967863734907407968[16] = delta_x[16] + nom_x[16]; + out_4967863734907407968[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_6968876690248087810) { - out_6968876690248087810[0] = -nom_x[0] + true_x[0]; - out_6968876690248087810[1] = -nom_x[1] + true_x[1]; - out_6968876690248087810[2] = -nom_x[2] + true_x[2]; - out_6968876690248087810[3] = -nom_x[3] + true_x[3]; - out_6968876690248087810[4] = -nom_x[4] + true_x[4]; - out_6968876690248087810[5] = -nom_x[5] + true_x[5]; - out_6968876690248087810[6] = -nom_x[6] + true_x[6]; - out_6968876690248087810[7] = -nom_x[7] + true_x[7]; - out_6968876690248087810[8] = -nom_x[8] + true_x[8]; - out_6968876690248087810[9] = -nom_x[9] + true_x[9]; - out_6968876690248087810[10] = -nom_x[10] + true_x[10]; - out_6968876690248087810[11] = -nom_x[11] + true_x[11]; - out_6968876690248087810[12] = -nom_x[12] + true_x[12]; - out_6968876690248087810[13] = -nom_x[13] + true_x[13]; - out_6968876690248087810[14] = -nom_x[14] + true_x[14]; - out_6968876690248087810[15] = -nom_x[15] + true_x[15]; - out_6968876690248087810[16] = -nom_x[16] + true_x[16]; - out_6968876690248087810[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_554301816764323460) { + out_554301816764323460[0] = -nom_x[0] + true_x[0]; + out_554301816764323460[1] = -nom_x[1] + true_x[1]; + out_554301816764323460[2] = -nom_x[2] + true_x[2]; + out_554301816764323460[3] = -nom_x[3] + true_x[3]; + out_554301816764323460[4] = -nom_x[4] + true_x[4]; + out_554301816764323460[5] = -nom_x[5] + true_x[5]; + out_554301816764323460[6] = -nom_x[6] + true_x[6]; + out_554301816764323460[7] = -nom_x[7] + true_x[7]; + out_554301816764323460[8] = -nom_x[8] + true_x[8]; + out_554301816764323460[9] = -nom_x[9] + true_x[9]; + out_554301816764323460[10] = -nom_x[10] + true_x[10]; + out_554301816764323460[11] = -nom_x[11] + true_x[11]; + out_554301816764323460[12] = -nom_x[12] + true_x[12]; + out_554301816764323460[13] = -nom_x[13] + true_x[13]; + out_554301816764323460[14] = -nom_x[14] + true_x[14]; + out_554301816764323460[15] = -nom_x[15] + true_x[15]; + out_554301816764323460[16] = -nom_x[16] + true_x[16]; + out_554301816764323460[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_3389862186663429553) { - out_3389862186663429553[0] = 1.0; - out_3389862186663429553[1] = 0.0; - out_3389862186663429553[2] = 0.0; - out_3389862186663429553[3] = 0.0; - out_3389862186663429553[4] = 0.0; - out_3389862186663429553[5] = 0.0; - out_3389862186663429553[6] = 0.0; - out_3389862186663429553[7] = 0.0; - out_3389862186663429553[8] = 0.0; - out_3389862186663429553[9] = 0.0; - out_3389862186663429553[10] = 0.0; - out_3389862186663429553[11] = 0.0; - out_3389862186663429553[12] = 0.0; - out_3389862186663429553[13] = 0.0; - out_3389862186663429553[14] = 0.0; - out_3389862186663429553[15] = 0.0; - out_3389862186663429553[16] = 0.0; - out_3389862186663429553[17] = 0.0; - out_3389862186663429553[18] = 0.0; - out_3389862186663429553[19] = 1.0; - out_3389862186663429553[20] = 0.0; - out_3389862186663429553[21] = 0.0; - out_3389862186663429553[22] = 0.0; - out_3389862186663429553[23] = 0.0; - out_3389862186663429553[24] = 0.0; - out_3389862186663429553[25] = 0.0; - out_3389862186663429553[26] = 0.0; - out_3389862186663429553[27] = 0.0; - out_3389862186663429553[28] = 0.0; - out_3389862186663429553[29] = 0.0; - out_3389862186663429553[30] = 0.0; - out_3389862186663429553[31] = 0.0; - out_3389862186663429553[32] = 0.0; - out_3389862186663429553[33] = 0.0; - out_3389862186663429553[34] = 0.0; - out_3389862186663429553[35] = 0.0; - out_3389862186663429553[36] = 0.0; - out_3389862186663429553[37] = 0.0; - out_3389862186663429553[38] = 1.0; - out_3389862186663429553[39] = 0.0; - out_3389862186663429553[40] = 0.0; - out_3389862186663429553[41] = 0.0; - out_3389862186663429553[42] = 0.0; - out_3389862186663429553[43] = 0.0; - out_3389862186663429553[44] = 0.0; - out_3389862186663429553[45] = 0.0; - out_3389862186663429553[46] = 0.0; - out_3389862186663429553[47] = 0.0; - out_3389862186663429553[48] = 0.0; - out_3389862186663429553[49] = 0.0; - out_3389862186663429553[50] = 0.0; - out_3389862186663429553[51] = 0.0; - out_3389862186663429553[52] = 0.0; - out_3389862186663429553[53] = 0.0; - out_3389862186663429553[54] = 0.0; - out_3389862186663429553[55] = 0.0; - out_3389862186663429553[56] = 0.0; - out_3389862186663429553[57] = 1.0; - out_3389862186663429553[58] = 0.0; - out_3389862186663429553[59] = 0.0; - out_3389862186663429553[60] = 0.0; - out_3389862186663429553[61] = 0.0; - out_3389862186663429553[62] = 0.0; - out_3389862186663429553[63] = 0.0; - out_3389862186663429553[64] = 0.0; - out_3389862186663429553[65] = 0.0; - out_3389862186663429553[66] = 0.0; - out_3389862186663429553[67] = 0.0; - out_3389862186663429553[68] = 0.0; - out_3389862186663429553[69] = 0.0; - out_3389862186663429553[70] = 0.0; - out_3389862186663429553[71] = 0.0; - out_3389862186663429553[72] = 0.0; - out_3389862186663429553[73] = 0.0; - out_3389862186663429553[74] = 0.0; - out_3389862186663429553[75] = 0.0; - out_3389862186663429553[76] = 1.0; - out_3389862186663429553[77] = 0.0; - out_3389862186663429553[78] = 0.0; - out_3389862186663429553[79] = 0.0; - out_3389862186663429553[80] = 0.0; - out_3389862186663429553[81] = 0.0; - out_3389862186663429553[82] = 0.0; - out_3389862186663429553[83] = 0.0; - out_3389862186663429553[84] = 0.0; - out_3389862186663429553[85] = 0.0; - out_3389862186663429553[86] = 0.0; - out_3389862186663429553[87] = 0.0; - out_3389862186663429553[88] = 0.0; - out_3389862186663429553[89] = 0.0; - out_3389862186663429553[90] = 0.0; - out_3389862186663429553[91] = 0.0; - out_3389862186663429553[92] = 0.0; - out_3389862186663429553[93] = 0.0; - out_3389862186663429553[94] = 0.0; - out_3389862186663429553[95] = 1.0; - out_3389862186663429553[96] = 0.0; - out_3389862186663429553[97] = 0.0; - out_3389862186663429553[98] = 0.0; - out_3389862186663429553[99] = 0.0; - out_3389862186663429553[100] = 0.0; - out_3389862186663429553[101] = 0.0; - out_3389862186663429553[102] = 0.0; - out_3389862186663429553[103] = 0.0; - out_3389862186663429553[104] = 0.0; - out_3389862186663429553[105] = 0.0; - out_3389862186663429553[106] = 0.0; - out_3389862186663429553[107] = 0.0; - out_3389862186663429553[108] = 0.0; - out_3389862186663429553[109] = 0.0; - out_3389862186663429553[110] = 0.0; - out_3389862186663429553[111] = 0.0; - out_3389862186663429553[112] = 0.0; - out_3389862186663429553[113] = 0.0; - out_3389862186663429553[114] = 1.0; - out_3389862186663429553[115] = 0.0; - out_3389862186663429553[116] = 0.0; - out_3389862186663429553[117] = 0.0; - out_3389862186663429553[118] = 0.0; - out_3389862186663429553[119] = 0.0; - out_3389862186663429553[120] = 0.0; - out_3389862186663429553[121] = 0.0; - out_3389862186663429553[122] = 0.0; - out_3389862186663429553[123] = 0.0; - out_3389862186663429553[124] = 0.0; - out_3389862186663429553[125] = 0.0; - out_3389862186663429553[126] = 0.0; - out_3389862186663429553[127] = 0.0; - out_3389862186663429553[128] = 0.0; - out_3389862186663429553[129] = 0.0; - out_3389862186663429553[130] = 0.0; - out_3389862186663429553[131] = 0.0; - out_3389862186663429553[132] = 0.0; - out_3389862186663429553[133] = 1.0; - out_3389862186663429553[134] = 0.0; - out_3389862186663429553[135] = 0.0; - out_3389862186663429553[136] = 0.0; - out_3389862186663429553[137] = 0.0; - out_3389862186663429553[138] = 0.0; - out_3389862186663429553[139] = 0.0; - out_3389862186663429553[140] = 0.0; - out_3389862186663429553[141] = 0.0; - out_3389862186663429553[142] = 0.0; - out_3389862186663429553[143] = 0.0; - out_3389862186663429553[144] = 0.0; - out_3389862186663429553[145] = 0.0; - out_3389862186663429553[146] = 0.0; - out_3389862186663429553[147] = 0.0; - out_3389862186663429553[148] = 0.0; - out_3389862186663429553[149] = 0.0; - out_3389862186663429553[150] = 0.0; - out_3389862186663429553[151] = 0.0; - out_3389862186663429553[152] = 1.0; - out_3389862186663429553[153] = 0.0; - out_3389862186663429553[154] = 0.0; - out_3389862186663429553[155] = 0.0; - out_3389862186663429553[156] = 0.0; - out_3389862186663429553[157] = 0.0; - out_3389862186663429553[158] = 0.0; - out_3389862186663429553[159] = 0.0; - out_3389862186663429553[160] = 0.0; - out_3389862186663429553[161] = 0.0; - out_3389862186663429553[162] = 0.0; - out_3389862186663429553[163] = 0.0; - out_3389862186663429553[164] = 0.0; - out_3389862186663429553[165] = 0.0; - out_3389862186663429553[166] = 0.0; - out_3389862186663429553[167] = 0.0; - out_3389862186663429553[168] = 0.0; - out_3389862186663429553[169] = 0.0; - out_3389862186663429553[170] = 0.0; - out_3389862186663429553[171] = 1.0; - out_3389862186663429553[172] = 0.0; - out_3389862186663429553[173] = 0.0; - out_3389862186663429553[174] = 0.0; - out_3389862186663429553[175] = 0.0; - out_3389862186663429553[176] = 0.0; - out_3389862186663429553[177] = 0.0; - out_3389862186663429553[178] = 0.0; - out_3389862186663429553[179] = 0.0; - out_3389862186663429553[180] = 0.0; - out_3389862186663429553[181] = 0.0; - out_3389862186663429553[182] = 0.0; - out_3389862186663429553[183] = 0.0; - out_3389862186663429553[184] = 0.0; - out_3389862186663429553[185] = 0.0; - out_3389862186663429553[186] = 0.0; - out_3389862186663429553[187] = 0.0; - out_3389862186663429553[188] = 0.0; - out_3389862186663429553[189] = 0.0; - out_3389862186663429553[190] = 1.0; - out_3389862186663429553[191] = 0.0; - out_3389862186663429553[192] = 0.0; - out_3389862186663429553[193] = 0.0; - out_3389862186663429553[194] = 0.0; - out_3389862186663429553[195] = 0.0; - out_3389862186663429553[196] = 0.0; - out_3389862186663429553[197] = 0.0; - out_3389862186663429553[198] = 0.0; - out_3389862186663429553[199] = 0.0; - out_3389862186663429553[200] = 0.0; - out_3389862186663429553[201] = 0.0; - out_3389862186663429553[202] = 0.0; - out_3389862186663429553[203] = 0.0; - out_3389862186663429553[204] = 0.0; - out_3389862186663429553[205] = 0.0; - out_3389862186663429553[206] = 0.0; - out_3389862186663429553[207] = 0.0; - out_3389862186663429553[208] = 0.0; - out_3389862186663429553[209] = 1.0; - out_3389862186663429553[210] = 0.0; - out_3389862186663429553[211] = 0.0; - out_3389862186663429553[212] = 0.0; - out_3389862186663429553[213] = 0.0; - out_3389862186663429553[214] = 0.0; - out_3389862186663429553[215] = 0.0; - out_3389862186663429553[216] = 0.0; - out_3389862186663429553[217] = 0.0; - out_3389862186663429553[218] = 0.0; - out_3389862186663429553[219] = 0.0; - out_3389862186663429553[220] = 0.0; - out_3389862186663429553[221] = 0.0; - out_3389862186663429553[222] = 0.0; - out_3389862186663429553[223] = 0.0; - out_3389862186663429553[224] = 0.0; - out_3389862186663429553[225] = 0.0; - out_3389862186663429553[226] = 0.0; - out_3389862186663429553[227] = 0.0; - out_3389862186663429553[228] = 1.0; - out_3389862186663429553[229] = 0.0; - out_3389862186663429553[230] = 0.0; - out_3389862186663429553[231] = 0.0; - out_3389862186663429553[232] = 0.0; - out_3389862186663429553[233] = 0.0; - out_3389862186663429553[234] = 0.0; - out_3389862186663429553[235] = 0.0; - out_3389862186663429553[236] = 0.0; - out_3389862186663429553[237] = 0.0; - out_3389862186663429553[238] = 0.0; - out_3389862186663429553[239] = 0.0; - out_3389862186663429553[240] = 0.0; - out_3389862186663429553[241] = 0.0; - out_3389862186663429553[242] = 0.0; - out_3389862186663429553[243] = 0.0; - out_3389862186663429553[244] = 0.0; - out_3389862186663429553[245] = 0.0; - out_3389862186663429553[246] = 0.0; - out_3389862186663429553[247] = 1.0; - out_3389862186663429553[248] = 0.0; - out_3389862186663429553[249] = 0.0; - out_3389862186663429553[250] = 0.0; - out_3389862186663429553[251] = 0.0; - out_3389862186663429553[252] = 0.0; - out_3389862186663429553[253] = 0.0; - out_3389862186663429553[254] = 0.0; - out_3389862186663429553[255] = 0.0; - out_3389862186663429553[256] = 0.0; - out_3389862186663429553[257] = 0.0; - out_3389862186663429553[258] = 0.0; - out_3389862186663429553[259] = 0.0; - out_3389862186663429553[260] = 0.0; - out_3389862186663429553[261] = 0.0; - out_3389862186663429553[262] = 0.0; - out_3389862186663429553[263] = 0.0; - out_3389862186663429553[264] = 0.0; - out_3389862186663429553[265] = 0.0; - out_3389862186663429553[266] = 1.0; - out_3389862186663429553[267] = 0.0; - out_3389862186663429553[268] = 0.0; - out_3389862186663429553[269] = 0.0; - out_3389862186663429553[270] = 0.0; - out_3389862186663429553[271] = 0.0; - out_3389862186663429553[272] = 0.0; - out_3389862186663429553[273] = 0.0; - out_3389862186663429553[274] = 0.0; - out_3389862186663429553[275] = 0.0; - out_3389862186663429553[276] = 0.0; - out_3389862186663429553[277] = 0.0; - out_3389862186663429553[278] = 0.0; - out_3389862186663429553[279] = 0.0; - out_3389862186663429553[280] = 0.0; - out_3389862186663429553[281] = 0.0; - out_3389862186663429553[282] = 0.0; - out_3389862186663429553[283] = 0.0; - out_3389862186663429553[284] = 0.0; - out_3389862186663429553[285] = 1.0; - out_3389862186663429553[286] = 0.0; - out_3389862186663429553[287] = 0.0; - out_3389862186663429553[288] = 0.0; - out_3389862186663429553[289] = 0.0; - out_3389862186663429553[290] = 0.0; - out_3389862186663429553[291] = 0.0; - out_3389862186663429553[292] = 0.0; - out_3389862186663429553[293] = 0.0; - out_3389862186663429553[294] = 0.0; - out_3389862186663429553[295] = 0.0; - out_3389862186663429553[296] = 0.0; - out_3389862186663429553[297] = 0.0; - out_3389862186663429553[298] = 0.0; - out_3389862186663429553[299] = 0.0; - out_3389862186663429553[300] = 0.0; - out_3389862186663429553[301] = 0.0; - out_3389862186663429553[302] = 0.0; - out_3389862186663429553[303] = 0.0; - out_3389862186663429553[304] = 1.0; - out_3389862186663429553[305] = 0.0; - out_3389862186663429553[306] = 0.0; - out_3389862186663429553[307] = 0.0; - out_3389862186663429553[308] = 0.0; - out_3389862186663429553[309] = 0.0; - out_3389862186663429553[310] = 0.0; - out_3389862186663429553[311] = 0.0; - out_3389862186663429553[312] = 0.0; - out_3389862186663429553[313] = 0.0; - out_3389862186663429553[314] = 0.0; - out_3389862186663429553[315] = 0.0; - out_3389862186663429553[316] = 0.0; - out_3389862186663429553[317] = 0.0; - out_3389862186663429553[318] = 0.0; - out_3389862186663429553[319] = 0.0; - out_3389862186663429553[320] = 0.0; - out_3389862186663429553[321] = 0.0; - out_3389862186663429553[322] = 0.0; - out_3389862186663429553[323] = 1.0; +void H_mod_fun(double *state, double *out_3047027985808074405) { + out_3047027985808074405[0] = 1.0; + out_3047027985808074405[1] = 0.0; + out_3047027985808074405[2] = 0.0; + out_3047027985808074405[3] = 0.0; + out_3047027985808074405[4] = 0.0; + out_3047027985808074405[5] = 0.0; + out_3047027985808074405[6] = 0.0; + out_3047027985808074405[7] = 0.0; + out_3047027985808074405[8] = 0.0; + out_3047027985808074405[9] = 0.0; + out_3047027985808074405[10] = 0.0; + out_3047027985808074405[11] = 0.0; + out_3047027985808074405[12] = 0.0; + out_3047027985808074405[13] = 0.0; + out_3047027985808074405[14] = 0.0; + out_3047027985808074405[15] = 0.0; + out_3047027985808074405[16] = 0.0; + out_3047027985808074405[17] = 0.0; + out_3047027985808074405[18] = 0.0; + out_3047027985808074405[19] = 1.0; + out_3047027985808074405[20] = 0.0; + out_3047027985808074405[21] = 0.0; + out_3047027985808074405[22] = 0.0; + out_3047027985808074405[23] = 0.0; + out_3047027985808074405[24] = 0.0; + out_3047027985808074405[25] = 0.0; + out_3047027985808074405[26] = 0.0; + out_3047027985808074405[27] = 0.0; + out_3047027985808074405[28] = 0.0; + out_3047027985808074405[29] = 0.0; + out_3047027985808074405[30] = 0.0; + out_3047027985808074405[31] = 0.0; + out_3047027985808074405[32] = 0.0; + out_3047027985808074405[33] = 0.0; + out_3047027985808074405[34] = 0.0; + out_3047027985808074405[35] = 0.0; + out_3047027985808074405[36] = 0.0; + out_3047027985808074405[37] = 0.0; + out_3047027985808074405[38] = 1.0; + out_3047027985808074405[39] = 0.0; + out_3047027985808074405[40] = 0.0; + out_3047027985808074405[41] = 0.0; + out_3047027985808074405[42] = 0.0; + out_3047027985808074405[43] = 0.0; + out_3047027985808074405[44] = 0.0; + out_3047027985808074405[45] = 0.0; + out_3047027985808074405[46] = 0.0; + out_3047027985808074405[47] = 0.0; + out_3047027985808074405[48] = 0.0; + out_3047027985808074405[49] = 0.0; + out_3047027985808074405[50] = 0.0; + out_3047027985808074405[51] = 0.0; + out_3047027985808074405[52] = 0.0; + out_3047027985808074405[53] = 0.0; + out_3047027985808074405[54] = 0.0; + out_3047027985808074405[55] = 0.0; + out_3047027985808074405[56] = 0.0; + out_3047027985808074405[57] = 1.0; + out_3047027985808074405[58] = 0.0; + out_3047027985808074405[59] = 0.0; + out_3047027985808074405[60] = 0.0; + out_3047027985808074405[61] = 0.0; + out_3047027985808074405[62] = 0.0; + out_3047027985808074405[63] = 0.0; + out_3047027985808074405[64] = 0.0; + out_3047027985808074405[65] = 0.0; + out_3047027985808074405[66] = 0.0; + out_3047027985808074405[67] = 0.0; + out_3047027985808074405[68] = 0.0; + out_3047027985808074405[69] = 0.0; + out_3047027985808074405[70] = 0.0; + out_3047027985808074405[71] = 0.0; + out_3047027985808074405[72] = 0.0; + out_3047027985808074405[73] = 0.0; + out_3047027985808074405[74] = 0.0; + out_3047027985808074405[75] = 0.0; + out_3047027985808074405[76] = 1.0; + out_3047027985808074405[77] = 0.0; + out_3047027985808074405[78] = 0.0; + out_3047027985808074405[79] = 0.0; + out_3047027985808074405[80] = 0.0; + out_3047027985808074405[81] = 0.0; + out_3047027985808074405[82] = 0.0; + out_3047027985808074405[83] = 0.0; + out_3047027985808074405[84] = 0.0; + out_3047027985808074405[85] = 0.0; + out_3047027985808074405[86] = 0.0; + out_3047027985808074405[87] = 0.0; + out_3047027985808074405[88] = 0.0; + out_3047027985808074405[89] = 0.0; + out_3047027985808074405[90] = 0.0; + out_3047027985808074405[91] = 0.0; + out_3047027985808074405[92] = 0.0; + out_3047027985808074405[93] = 0.0; + out_3047027985808074405[94] = 0.0; + out_3047027985808074405[95] = 1.0; + out_3047027985808074405[96] = 0.0; + out_3047027985808074405[97] = 0.0; + out_3047027985808074405[98] = 0.0; + out_3047027985808074405[99] = 0.0; + out_3047027985808074405[100] = 0.0; + out_3047027985808074405[101] = 0.0; + out_3047027985808074405[102] = 0.0; + out_3047027985808074405[103] = 0.0; + out_3047027985808074405[104] = 0.0; + out_3047027985808074405[105] = 0.0; + out_3047027985808074405[106] = 0.0; + out_3047027985808074405[107] = 0.0; + out_3047027985808074405[108] = 0.0; + out_3047027985808074405[109] = 0.0; + out_3047027985808074405[110] = 0.0; + out_3047027985808074405[111] = 0.0; + out_3047027985808074405[112] = 0.0; + out_3047027985808074405[113] = 0.0; + out_3047027985808074405[114] = 1.0; + out_3047027985808074405[115] = 0.0; + out_3047027985808074405[116] = 0.0; + out_3047027985808074405[117] = 0.0; + out_3047027985808074405[118] = 0.0; + out_3047027985808074405[119] = 0.0; + out_3047027985808074405[120] = 0.0; + out_3047027985808074405[121] = 0.0; + out_3047027985808074405[122] = 0.0; + out_3047027985808074405[123] = 0.0; + out_3047027985808074405[124] = 0.0; + out_3047027985808074405[125] = 0.0; + out_3047027985808074405[126] = 0.0; + out_3047027985808074405[127] = 0.0; + out_3047027985808074405[128] = 0.0; + out_3047027985808074405[129] = 0.0; + out_3047027985808074405[130] = 0.0; + out_3047027985808074405[131] = 0.0; + out_3047027985808074405[132] = 0.0; + out_3047027985808074405[133] = 1.0; + out_3047027985808074405[134] = 0.0; + out_3047027985808074405[135] = 0.0; + out_3047027985808074405[136] = 0.0; + out_3047027985808074405[137] = 0.0; + out_3047027985808074405[138] = 0.0; + out_3047027985808074405[139] = 0.0; + out_3047027985808074405[140] = 0.0; + out_3047027985808074405[141] = 0.0; + out_3047027985808074405[142] = 0.0; + out_3047027985808074405[143] = 0.0; + out_3047027985808074405[144] = 0.0; + out_3047027985808074405[145] = 0.0; + out_3047027985808074405[146] = 0.0; + out_3047027985808074405[147] = 0.0; + out_3047027985808074405[148] = 0.0; + out_3047027985808074405[149] = 0.0; + out_3047027985808074405[150] = 0.0; + out_3047027985808074405[151] = 0.0; + out_3047027985808074405[152] = 1.0; + out_3047027985808074405[153] = 0.0; + out_3047027985808074405[154] = 0.0; + out_3047027985808074405[155] = 0.0; + out_3047027985808074405[156] = 0.0; + out_3047027985808074405[157] = 0.0; + out_3047027985808074405[158] = 0.0; + out_3047027985808074405[159] = 0.0; + out_3047027985808074405[160] = 0.0; + out_3047027985808074405[161] = 0.0; + out_3047027985808074405[162] = 0.0; + out_3047027985808074405[163] = 0.0; + out_3047027985808074405[164] = 0.0; + out_3047027985808074405[165] = 0.0; + out_3047027985808074405[166] = 0.0; + out_3047027985808074405[167] = 0.0; + out_3047027985808074405[168] = 0.0; + out_3047027985808074405[169] = 0.0; + out_3047027985808074405[170] = 0.0; + out_3047027985808074405[171] = 1.0; + out_3047027985808074405[172] = 0.0; + out_3047027985808074405[173] = 0.0; + out_3047027985808074405[174] = 0.0; + out_3047027985808074405[175] = 0.0; + out_3047027985808074405[176] = 0.0; + out_3047027985808074405[177] = 0.0; + out_3047027985808074405[178] = 0.0; + out_3047027985808074405[179] = 0.0; + out_3047027985808074405[180] = 0.0; + out_3047027985808074405[181] = 0.0; + out_3047027985808074405[182] = 0.0; + out_3047027985808074405[183] = 0.0; + out_3047027985808074405[184] = 0.0; + out_3047027985808074405[185] = 0.0; + out_3047027985808074405[186] = 0.0; + out_3047027985808074405[187] = 0.0; + out_3047027985808074405[188] = 0.0; + out_3047027985808074405[189] = 0.0; + out_3047027985808074405[190] = 1.0; + out_3047027985808074405[191] = 0.0; + out_3047027985808074405[192] = 0.0; + out_3047027985808074405[193] = 0.0; + out_3047027985808074405[194] = 0.0; + out_3047027985808074405[195] = 0.0; + out_3047027985808074405[196] = 0.0; + out_3047027985808074405[197] = 0.0; + out_3047027985808074405[198] = 0.0; + out_3047027985808074405[199] = 0.0; + out_3047027985808074405[200] = 0.0; + out_3047027985808074405[201] = 0.0; + out_3047027985808074405[202] = 0.0; + out_3047027985808074405[203] = 0.0; + out_3047027985808074405[204] = 0.0; + out_3047027985808074405[205] = 0.0; + out_3047027985808074405[206] = 0.0; + out_3047027985808074405[207] = 0.0; + out_3047027985808074405[208] = 0.0; + out_3047027985808074405[209] = 1.0; + out_3047027985808074405[210] = 0.0; + out_3047027985808074405[211] = 0.0; + out_3047027985808074405[212] = 0.0; + out_3047027985808074405[213] = 0.0; + out_3047027985808074405[214] = 0.0; + out_3047027985808074405[215] = 0.0; + out_3047027985808074405[216] = 0.0; + out_3047027985808074405[217] = 0.0; + out_3047027985808074405[218] = 0.0; + out_3047027985808074405[219] = 0.0; + out_3047027985808074405[220] = 0.0; + out_3047027985808074405[221] = 0.0; + out_3047027985808074405[222] = 0.0; + out_3047027985808074405[223] = 0.0; + out_3047027985808074405[224] = 0.0; + out_3047027985808074405[225] = 0.0; + out_3047027985808074405[226] = 0.0; + out_3047027985808074405[227] = 0.0; + out_3047027985808074405[228] = 1.0; + out_3047027985808074405[229] = 0.0; + out_3047027985808074405[230] = 0.0; + out_3047027985808074405[231] = 0.0; + out_3047027985808074405[232] = 0.0; + out_3047027985808074405[233] = 0.0; + out_3047027985808074405[234] = 0.0; + out_3047027985808074405[235] = 0.0; + out_3047027985808074405[236] = 0.0; + out_3047027985808074405[237] = 0.0; + out_3047027985808074405[238] = 0.0; + out_3047027985808074405[239] = 0.0; + out_3047027985808074405[240] = 0.0; + out_3047027985808074405[241] = 0.0; + out_3047027985808074405[242] = 0.0; + out_3047027985808074405[243] = 0.0; + out_3047027985808074405[244] = 0.0; + out_3047027985808074405[245] = 0.0; + out_3047027985808074405[246] = 0.0; + out_3047027985808074405[247] = 1.0; + out_3047027985808074405[248] = 0.0; + out_3047027985808074405[249] = 0.0; + out_3047027985808074405[250] = 0.0; + out_3047027985808074405[251] = 0.0; + out_3047027985808074405[252] = 0.0; + out_3047027985808074405[253] = 0.0; + out_3047027985808074405[254] = 0.0; + out_3047027985808074405[255] = 0.0; + out_3047027985808074405[256] = 0.0; + out_3047027985808074405[257] = 0.0; + out_3047027985808074405[258] = 0.0; + out_3047027985808074405[259] = 0.0; + out_3047027985808074405[260] = 0.0; + out_3047027985808074405[261] = 0.0; + out_3047027985808074405[262] = 0.0; + out_3047027985808074405[263] = 0.0; + out_3047027985808074405[264] = 0.0; + out_3047027985808074405[265] = 0.0; + out_3047027985808074405[266] = 1.0; + out_3047027985808074405[267] = 0.0; + out_3047027985808074405[268] = 0.0; + out_3047027985808074405[269] = 0.0; + out_3047027985808074405[270] = 0.0; + out_3047027985808074405[271] = 0.0; + out_3047027985808074405[272] = 0.0; + out_3047027985808074405[273] = 0.0; + out_3047027985808074405[274] = 0.0; + out_3047027985808074405[275] = 0.0; + out_3047027985808074405[276] = 0.0; + out_3047027985808074405[277] = 0.0; + out_3047027985808074405[278] = 0.0; + out_3047027985808074405[279] = 0.0; + out_3047027985808074405[280] = 0.0; + out_3047027985808074405[281] = 0.0; + out_3047027985808074405[282] = 0.0; + out_3047027985808074405[283] = 0.0; + out_3047027985808074405[284] = 0.0; + out_3047027985808074405[285] = 1.0; + out_3047027985808074405[286] = 0.0; + out_3047027985808074405[287] = 0.0; + out_3047027985808074405[288] = 0.0; + out_3047027985808074405[289] = 0.0; + out_3047027985808074405[290] = 0.0; + out_3047027985808074405[291] = 0.0; + out_3047027985808074405[292] = 0.0; + out_3047027985808074405[293] = 0.0; + out_3047027985808074405[294] = 0.0; + out_3047027985808074405[295] = 0.0; + out_3047027985808074405[296] = 0.0; + out_3047027985808074405[297] = 0.0; + out_3047027985808074405[298] = 0.0; + out_3047027985808074405[299] = 0.0; + out_3047027985808074405[300] = 0.0; + out_3047027985808074405[301] = 0.0; + out_3047027985808074405[302] = 0.0; + out_3047027985808074405[303] = 0.0; + out_3047027985808074405[304] = 1.0; + out_3047027985808074405[305] = 0.0; + out_3047027985808074405[306] = 0.0; + out_3047027985808074405[307] = 0.0; + out_3047027985808074405[308] = 0.0; + out_3047027985808074405[309] = 0.0; + out_3047027985808074405[310] = 0.0; + out_3047027985808074405[311] = 0.0; + out_3047027985808074405[312] = 0.0; + out_3047027985808074405[313] = 0.0; + out_3047027985808074405[314] = 0.0; + out_3047027985808074405[315] = 0.0; + out_3047027985808074405[316] = 0.0; + out_3047027985808074405[317] = 0.0; + out_3047027985808074405[318] = 0.0; + out_3047027985808074405[319] = 0.0; + out_3047027985808074405[320] = 0.0; + out_3047027985808074405[321] = 0.0; + out_3047027985808074405[322] = 0.0; + out_3047027985808074405[323] = 1.0; } -void f_fun(double *state, double dt, double *out_4802700997878092668) { - out_4802700997878092668[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_4802700997878092668[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_4802700997878092668[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_4802700997878092668[3] = dt*state[12] + state[3]; - out_4802700997878092668[4] = dt*state[13] + state[4]; - out_4802700997878092668[5] = dt*state[14] + state[5]; - out_4802700997878092668[6] = state[6]; - out_4802700997878092668[7] = state[7]; - out_4802700997878092668[8] = state[8]; - out_4802700997878092668[9] = state[9]; - out_4802700997878092668[10] = state[10]; - out_4802700997878092668[11] = state[11]; - out_4802700997878092668[12] = state[12]; - out_4802700997878092668[13] = state[13]; - out_4802700997878092668[14] = state[14]; - out_4802700997878092668[15] = state[15]; - out_4802700997878092668[16] = state[16]; - out_4802700997878092668[17] = state[17]; +void f_fun(double *state, double dt, double *out_986492010673501731) { + out_986492010673501731[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_986492010673501731[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_986492010673501731[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_986492010673501731[3] = dt*state[12] + state[3]; + out_986492010673501731[4] = dt*state[13] + state[4]; + out_986492010673501731[5] = dt*state[14] + state[5]; + out_986492010673501731[6] = state[6]; + out_986492010673501731[7] = state[7]; + out_986492010673501731[8] = state[8]; + out_986492010673501731[9] = state[9]; + out_986492010673501731[10] = state[10]; + out_986492010673501731[11] = state[11]; + out_986492010673501731[12] = state[12]; + out_986492010673501731[13] = state[13]; + out_986492010673501731[14] = state[14]; + out_986492010673501731[15] = state[15]; + out_986492010673501731[16] = state[16]; + out_986492010673501731[17] = state[17]; } -void F_fun(double *state, double dt, double *out_8725044315227912670) { - out_8725044315227912670[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_8725044315227912670[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_8725044315227912670[2] = 0; - out_8725044315227912670[3] = 0; - out_8725044315227912670[4] = 0; - out_8725044315227912670[5] = 0; - out_8725044315227912670[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_8725044315227912670[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_8725044315227912670[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_8725044315227912670[9] = 0; - out_8725044315227912670[10] = 0; - out_8725044315227912670[11] = 0; - out_8725044315227912670[12] = 0; - out_8725044315227912670[13] = 0; - out_8725044315227912670[14] = 0; - out_8725044315227912670[15] = 0; - out_8725044315227912670[16] = 0; - out_8725044315227912670[17] = 0; - out_8725044315227912670[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_8725044315227912670[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_8725044315227912670[20] = 0; - out_8725044315227912670[21] = 0; - out_8725044315227912670[22] = 0; - out_8725044315227912670[23] = 0; - out_8725044315227912670[24] = 0; - out_8725044315227912670[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_8725044315227912670[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_8725044315227912670[27] = 0; - out_8725044315227912670[28] = 0; - out_8725044315227912670[29] = 0; - out_8725044315227912670[30] = 0; - out_8725044315227912670[31] = 0; - out_8725044315227912670[32] = 0; - out_8725044315227912670[33] = 0; - out_8725044315227912670[34] = 0; - out_8725044315227912670[35] = 0; - out_8725044315227912670[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_8725044315227912670[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_8725044315227912670[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_8725044315227912670[39] = 0; - out_8725044315227912670[40] = 0; - out_8725044315227912670[41] = 0; - out_8725044315227912670[42] = 0; - out_8725044315227912670[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_8725044315227912670[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_8725044315227912670[45] = 0; - out_8725044315227912670[46] = 0; - out_8725044315227912670[47] = 0; - out_8725044315227912670[48] = 0; - out_8725044315227912670[49] = 0; - out_8725044315227912670[50] = 0; - out_8725044315227912670[51] = 0; - out_8725044315227912670[52] = 0; - out_8725044315227912670[53] = 0; - out_8725044315227912670[54] = 0; - out_8725044315227912670[55] = 0; - out_8725044315227912670[56] = 0; - out_8725044315227912670[57] = 1; - out_8725044315227912670[58] = 0; - out_8725044315227912670[59] = 0; - out_8725044315227912670[60] = 0; - out_8725044315227912670[61] = 0; - out_8725044315227912670[62] = 0; - out_8725044315227912670[63] = 0; - out_8725044315227912670[64] = 0; - out_8725044315227912670[65] = 0; - out_8725044315227912670[66] = dt; - out_8725044315227912670[67] = 0; - out_8725044315227912670[68] = 0; - out_8725044315227912670[69] = 0; - out_8725044315227912670[70] = 0; - out_8725044315227912670[71] = 0; - out_8725044315227912670[72] = 0; - out_8725044315227912670[73] = 0; - out_8725044315227912670[74] = 0; - out_8725044315227912670[75] = 0; - out_8725044315227912670[76] = 1; - out_8725044315227912670[77] = 0; - out_8725044315227912670[78] = 0; - out_8725044315227912670[79] = 0; - out_8725044315227912670[80] = 0; - out_8725044315227912670[81] = 0; - out_8725044315227912670[82] = 0; - out_8725044315227912670[83] = 0; - out_8725044315227912670[84] = 0; - out_8725044315227912670[85] = dt; - out_8725044315227912670[86] = 0; - out_8725044315227912670[87] = 0; - out_8725044315227912670[88] = 0; - out_8725044315227912670[89] = 0; - out_8725044315227912670[90] = 0; - out_8725044315227912670[91] = 0; - out_8725044315227912670[92] = 0; - out_8725044315227912670[93] = 0; - out_8725044315227912670[94] = 0; - out_8725044315227912670[95] = 1; - out_8725044315227912670[96] = 0; - out_8725044315227912670[97] = 0; - out_8725044315227912670[98] = 0; - out_8725044315227912670[99] = 0; - out_8725044315227912670[100] = 0; - out_8725044315227912670[101] = 0; - out_8725044315227912670[102] = 0; - out_8725044315227912670[103] = 0; - out_8725044315227912670[104] = dt; - out_8725044315227912670[105] = 0; - out_8725044315227912670[106] = 0; - out_8725044315227912670[107] = 0; - out_8725044315227912670[108] = 0; - out_8725044315227912670[109] = 0; - out_8725044315227912670[110] = 0; - out_8725044315227912670[111] = 0; - out_8725044315227912670[112] = 0; - out_8725044315227912670[113] = 0; - out_8725044315227912670[114] = 1; - out_8725044315227912670[115] = 0; - out_8725044315227912670[116] = 0; - out_8725044315227912670[117] = 0; - out_8725044315227912670[118] = 0; - out_8725044315227912670[119] = 0; - out_8725044315227912670[120] = 0; - out_8725044315227912670[121] = 0; - out_8725044315227912670[122] = 0; - out_8725044315227912670[123] = 0; - out_8725044315227912670[124] = 0; - out_8725044315227912670[125] = 0; - out_8725044315227912670[126] = 0; - out_8725044315227912670[127] = 0; - out_8725044315227912670[128] = 0; - out_8725044315227912670[129] = 0; - out_8725044315227912670[130] = 0; - out_8725044315227912670[131] = 0; - out_8725044315227912670[132] = 0; - out_8725044315227912670[133] = 1; - out_8725044315227912670[134] = 0; - out_8725044315227912670[135] = 0; - out_8725044315227912670[136] = 0; - out_8725044315227912670[137] = 0; - out_8725044315227912670[138] = 0; - out_8725044315227912670[139] = 0; - out_8725044315227912670[140] = 0; - out_8725044315227912670[141] = 0; - out_8725044315227912670[142] = 0; - out_8725044315227912670[143] = 0; - out_8725044315227912670[144] = 0; - out_8725044315227912670[145] = 0; - out_8725044315227912670[146] = 0; - out_8725044315227912670[147] = 0; - out_8725044315227912670[148] = 0; - out_8725044315227912670[149] = 0; - out_8725044315227912670[150] = 0; - out_8725044315227912670[151] = 0; - out_8725044315227912670[152] = 1; - out_8725044315227912670[153] = 0; - out_8725044315227912670[154] = 0; - out_8725044315227912670[155] = 0; - out_8725044315227912670[156] = 0; - out_8725044315227912670[157] = 0; - out_8725044315227912670[158] = 0; - out_8725044315227912670[159] = 0; - out_8725044315227912670[160] = 0; - out_8725044315227912670[161] = 0; - out_8725044315227912670[162] = 0; - out_8725044315227912670[163] = 0; - out_8725044315227912670[164] = 0; - out_8725044315227912670[165] = 0; - out_8725044315227912670[166] = 0; - out_8725044315227912670[167] = 0; - out_8725044315227912670[168] = 0; - out_8725044315227912670[169] = 0; - out_8725044315227912670[170] = 0; - out_8725044315227912670[171] = 1; - out_8725044315227912670[172] = 0; - out_8725044315227912670[173] = 0; - out_8725044315227912670[174] = 0; - out_8725044315227912670[175] = 0; - out_8725044315227912670[176] = 0; - out_8725044315227912670[177] = 0; - out_8725044315227912670[178] = 0; - out_8725044315227912670[179] = 0; - out_8725044315227912670[180] = 0; - out_8725044315227912670[181] = 0; - out_8725044315227912670[182] = 0; - out_8725044315227912670[183] = 0; - out_8725044315227912670[184] = 0; - out_8725044315227912670[185] = 0; - out_8725044315227912670[186] = 0; - out_8725044315227912670[187] = 0; - out_8725044315227912670[188] = 0; - out_8725044315227912670[189] = 0; - out_8725044315227912670[190] = 1; - out_8725044315227912670[191] = 0; - out_8725044315227912670[192] = 0; - out_8725044315227912670[193] = 0; - out_8725044315227912670[194] = 0; - out_8725044315227912670[195] = 0; - out_8725044315227912670[196] = 0; - out_8725044315227912670[197] = 0; - out_8725044315227912670[198] = 0; - out_8725044315227912670[199] = 0; - out_8725044315227912670[200] = 0; - out_8725044315227912670[201] = 0; - out_8725044315227912670[202] = 0; - out_8725044315227912670[203] = 0; - out_8725044315227912670[204] = 0; - out_8725044315227912670[205] = 0; - out_8725044315227912670[206] = 0; - out_8725044315227912670[207] = 0; - out_8725044315227912670[208] = 0; - out_8725044315227912670[209] = 1; - out_8725044315227912670[210] = 0; - out_8725044315227912670[211] = 0; - out_8725044315227912670[212] = 0; - out_8725044315227912670[213] = 0; - out_8725044315227912670[214] = 0; - out_8725044315227912670[215] = 0; - out_8725044315227912670[216] = 0; - out_8725044315227912670[217] = 0; - out_8725044315227912670[218] = 0; - out_8725044315227912670[219] = 0; - out_8725044315227912670[220] = 0; - out_8725044315227912670[221] = 0; - out_8725044315227912670[222] = 0; - out_8725044315227912670[223] = 0; - out_8725044315227912670[224] = 0; - out_8725044315227912670[225] = 0; - out_8725044315227912670[226] = 0; - out_8725044315227912670[227] = 0; - out_8725044315227912670[228] = 1; - out_8725044315227912670[229] = 0; - out_8725044315227912670[230] = 0; - out_8725044315227912670[231] = 0; - out_8725044315227912670[232] = 0; - out_8725044315227912670[233] = 0; - out_8725044315227912670[234] = 0; - out_8725044315227912670[235] = 0; - out_8725044315227912670[236] = 0; - out_8725044315227912670[237] = 0; - out_8725044315227912670[238] = 0; - out_8725044315227912670[239] = 0; - out_8725044315227912670[240] = 0; - out_8725044315227912670[241] = 0; - out_8725044315227912670[242] = 0; - out_8725044315227912670[243] = 0; - out_8725044315227912670[244] = 0; - out_8725044315227912670[245] = 0; - out_8725044315227912670[246] = 0; - out_8725044315227912670[247] = 1; - out_8725044315227912670[248] = 0; - out_8725044315227912670[249] = 0; - out_8725044315227912670[250] = 0; - out_8725044315227912670[251] = 0; - out_8725044315227912670[252] = 0; - out_8725044315227912670[253] = 0; - out_8725044315227912670[254] = 0; - out_8725044315227912670[255] = 0; - out_8725044315227912670[256] = 0; - out_8725044315227912670[257] = 0; - out_8725044315227912670[258] = 0; - out_8725044315227912670[259] = 0; - out_8725044315227912670[260] = 0; - out_8725044315227912670[261] = 0; - out_8725044315227912670[262] = 0; - out_8725044315227912670[263] = 0; - out_8725044315227912670[264] = 0; - out_8725044315227912670[265] = 0; - out_8725044315227912670[266] = 1; - out_8725044315227912670[267] = 0; - out_8725044315227912670[268] = 0; - out_8725044315227912670[269] = 0; - out_8725044315227912670[270] = 0; - out_8725044315227912670[271] = 0; - out_8725044315227912670[272] = 0; - out_8725044315227912670[273] = 0; - out_8725044315227912670[274] = 0; - out_8725044315227912670[275] = 0; - out_8725044315227912670[276] = 0; - out_8725044315227912670[277] = 0; - out_8725044315227912670[278] = 0; - out_8725044315227912670[279] = 0; - out_8725044315227912670[280] = 0; - out_8725044315227912670[281] = 0; - out_8725044315227912670[282] = 0; - out_8725044315227912670[283] = 0; - out_8725044315227912670[284] = 0; - out_8725044315227912670[285] = 1; - out_8725044315227912670[286] = 0; - out_8725044315227912670[287] = 0; - out_8725044315227912670[288] = 0; - out_8725044315227912670[289] = 0; - out_8725044315227912670[290] = 0; - out_8725044315227912670[291] = 0; - out_8725044315227912670[292] = 0; - out_8725044315227912670[293] = 0; - out_8725044315227912670[294] = 0; - out_8725044315227912670[295] = 0; - out_8725044315227912670[296] = 0; - out_8725044315227912670[297] = 0; - out_8725044315227912670[298] = 0; - out_8725044315227912670[299] = 0; - out_8725044315227912670[300] = 0; - out_8725044315227912670[301] = 0; - out_8725044315227912670[302] = 0; - out_8725044315227912670[303] = 0; - out_8725044315227912670[304] = 1; - out_8725044315227912670[305] = 0; - out_8725044315227912670[306] = 0; - out_8725044315227912670[307] = 0; - out_8725044315227912670[308] = 0; - out_8725044315227912670[309] = 0; - out_8725044315227912670[310] = 0; - out_8725044315227912670[311] = 0; - out_8725044315227912670[312] = 0; - out_8725044315227912670[313] = 0; - out_8725044315227912670[314] = 0; - out_8725044315227912670[315] = 0; - out_8725044315227912670[316] = 0; - out_8725044315227912670[317] = 0; - out_8725044315227912670[318] = 0; - out_8725044315227912670[319] = 0; - out_8725044315227912670[320] = 0; - out_8725044315227912670[321] = 0; - out_8725044315227912670[322] = 0; - out_8725044315227912670[323] = 1; +void F_fun(double *state, double dt, double *out_4336070434649428420) { + out_4336070434649428420[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_4336070434649428420[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_4336070434649428420[2] = 0; + out_4336070434649428420[3] = 0; + out_4336070434649428420[4] = 0; + out_4336070434649428420[5] = 0; + out_4336070434649428420[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_4336070434649428420[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_4336070434649428420[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_4336070434649428420[9] = 0; + out_4336070434649428420[10] = 0; + out_4336070434649428420[11] = 0; + out_4336070434649428420[12] = 0; + out_4336070434649428420[13] = 0; + out_4336070434649428420[14] = 0; + out_4336070434649428420[15] = 0; + out_4336070434649428420[16] = 0; + out_4336070434649428420[17] = 0; + out_4336070434649428420[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_4336070434649428420[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_4336070434649428420[20] = 0; + out_4336070434649428420[21] = 0; + out_4336070434649428420[22] = 0; + out_4336070434649428420[23] = 0; + out_4336070434649428420[24] = 0; + out_4336070434649428420[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_4336070434649428420[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_4336070434649428420[27] = 0; + out_4336070434649428420[28] = 0; + out_4336070434649428420[29] = 0; + out_4336070434649428420[30] = 0; + out_4336070434649428420[31] = 0; + out_4336070434649428420[32] = 0; + out_4336070434649428420[33] = 0; + out_4336070434649428420[34] = 0; + out_4336070434649428420[35] = 0; + out_4336070434649428420[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_4336070434649428420[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_4336070434649428420[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_4336070434649428420[39] = 0; + out_4336070434649428420[40] = 0; + out_4336070434649428420[41] = 0; + out_4336070434649428420[42] = 0; + out_4336070434649428420[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_4336070434649428420[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_4336070434649428420[45] = 0; + out_4336070434649428420[46] = 0; + out_4336070434649428420[47] = 0; + out_4336070434649428420[48] = 0; + out_4336070434649428420[49] = 0; + out_4336070434649428420[50] = 0; + out_4336070434649428420[51] = 0; + out_4336070434649428420[52] = 0; + out_4336070434649428420[53] = 0; + out_4336070434649428420[54] = 0; + out_4336070434649428420[55] = 0; + out_4336070434649428420[56] = 0; + out_4336070434649428420[57] = 1; + out_4336070434649428420[58] = 0; + out_4336070434649428420[59] = 0; + out_4336070434649428420[60] = 0; + out_4336070434649428420[61] = 0; + out_4336070434649428420[62] = 0; + out_4336070434649428420[63] = 0; + out_4336070434649428420[64] = 0; + out_4336070434649428420[65] = 0; + out_4336070434649428420[66] = dt; + out_4336070434649428420[67] = 0; + out_4336070434649428420[68] = 0; + out_4336070434649428420[69] = 0; + out_4336070434649428420[70] = 0; + out_4336070434649428420[71] = 0; + out_4336070434649428420[72] = 0; + out_4336070434649428420[73] = 0; + out_4336070434649428420[74] = 0; + out_4336070434649428420[75] = 0; + out_4336070434649428420[76] = 1; + out_4336070434649428420[77] = 0; + out_4336070434649428420[78] = 0; + out_4336070434649428420[79] = 0; + out_4336070434649428420[80] = 0; + out_4336070434649428420[81] = 0; + out_4336070434649428420[82] = 0; + out_4336070434649428420[83] = 0; + out_4336070434649428420[84] = 0; + out_4336070434649428420[85] = dt; + out_4336070434649428420[86] = 0; + out_4336070434649428420[87] = 0; + out_4336070434649428420[88] = 0; + out_4336070434649428420[89] = 0; + out_4336070434649428420[90] = 0; + out_4336070434649428420[91] = 0; + out_4336070434649428420[92] = 0; + out_4336070434649428420[93] = 0; + out_4336070434649428420[94] = 0; + out_4336070434649428420[95] = 1; + out_4336070434649428420[96] = 0; + out_4336070434649428420[97] = 0; + out_4336070434649428420[98] = 0; + out_4336070434649428420[99] = 0; + out_4336070434649428420[100] = 0; + out_4336070434649428420[101] = 0; + out_4336070434649428420[102] = 0; + out_4336070434649428420[103] = 0; + out_4336070434649428420[104] = dt; + out_4336070434649428420[105] = 0; + out_4336070434649428420[106] = 0; + out_4336070434649428420[107] = 0; + out_4336070434649428420[108] = 0; + out_4336070434649428420[109] = 0; + out_4336070434649428420[110] = 0; + out_4336070434649428420[111] = 0; + out_4336070434649428420[112] = 0; + out_4336070434649428420[113] = 0; + out_4336070434649428420[114] = 1; + out_4336070434649428420[115] = 0; + out_4336070434649428420[116] = 0; + out_4336070434649428420[117] = 0; + out_4336070434649428420[118] = 0; + out_4336070434649428420[119] = 0; + out_4336070434649428420[120] = 0; + out_4336070434649428420[121] = 0; + out_4336070434649428420[122] = 0; + out_4336070434649428420[123] = 0; + out_4336070434649428420[124] = 0; + out_4336070434649428420[125] = 0; + out_4336070434649428420[126] = 0; + out_4336070434649428420[127] = 0; + out_4336070434649428420[128] = 0; + out_4336070434649428420[129] = 0; + out_4336070434649428420[130] = 0; + out_4336070434649428420[131] = 0; + out_4336070434649428420[132] = 0; + out_4336070434649428420[133] = 1; + out_4336070434649428420[134] = 0; + out_4336070434649428420[135] = 0; + out_4336070434649428420[136] = 0; + out_4336070434649428420[137] = 0; + out_4336070434649428420[138] = 0; + out_4336070434649428420[139] = 0; + out_4336070434649428420[140] = 0; + out_4336070434649428420[141] = 0; + out_4336070434649428420[142] = 0; + out_4336070434649428420[143] = 0; + out_4336070434649428420[144] = 0; + out_4336070434649428420[145] = 0; + out_4336070434649428420[146] = 0; + out_4336070434649428420[147] = 0; + out_4336070434649428420[148] = 0; + out_4336070434649428420[149] = 0; + out_4336070434649428420[150] = 0; + out_4336070434649428420[151] = 0; + out_4336070434649428420[152] = 1; + out_4336070434649428420[153] = 0; + out_4336070434649428420[154] = 0; + out_4336070434649428420[155] = 0; + out_4336070434649428420[156] = 0; + out_4336070434649428420[157] = 0; + out_4336070434649428420[158] = 0; + out_4336070434649428420[159] = 0; + out_4336070434649428420[160] = 0; + out_4336070434649428420[161] = 0; + out_4336070434649428420[162] = 0; + out_4336070434649428420[163] = 0; + out_4336070434649428420[164] = 0; + out_4336070434649428420[165] = 0; + out_4336070434649428420[166] = 0; + out_4336070434649428420[167] = 0; + out_4336070434649428420[168] = 0; + out_4336070434649428420[169] = 0; + out_4336070434649428420[170] = 0; + out_4336070434649428420[171] = 1; + out_4336070434649428420[172] = 0; + out_4336070434649428420[173] = 0; + out_4336070434649428420[174] = 0; + out_4336070434649428420[175] = 0; + out_4336070434649428420[176] = 0; + out_4336070434649428420[177] = 0; + out_4336070434649428420[178] = 0; + out_4336070434649428420[179] = 0; + out_4336070434649428420[180] = 0; + out_4336070434649428420[181] = 0; + out_4336070434649428420[182] = 0; + out_4336070434649428420[183] = 0; + out_4336070434649428420[184] = 0; + out_4336070434649428420[185] = 0; + out_4336070434649428420[186] = 0; + out_4336070434649428420[187] = 0; + out_4336070434649428420[188] = 0; + out_4336070434649428420[189] = 0; + out_4336070434649428420[190] = 1; + out_4336070434649428420[191] = 0; + out_4336070434649428420[192] = 0; + out_4336070434649428420[193] = 0; + out_4336070434649428420[194] = 0; + out_4336070434649428420[195] = 0; + out_4336070434649428420[196] = 0; + out_4336070434649428420[197] = 0; + out_4336070434649428420[198] = 0; + out_4336070434649428420[199] = 0; + out_4336070434649428420[200] = 0; + out_4336070434649428420[201] = 0; + out_4336070434649428420[202] = 0; + out_4336070434649428420[203] = 0; + out_4336070434649428420[204] = 0; + out_4336070434649428420[205] = 0; + out_4336070434649428420[206] = 0; + out_4336070434649428420[207] = 0; + out_4336070434649428420[208] = 0; + out_4336070434649428420[209] = 1; + out_4336070434649428420[210] = 0; + out_4336070434649428420[211] = 0; + out_4336070434649428420[212] = 0; + out_4336070434649428420[213] = 0; + out_4336070434649428420[214] = 0; + out_4336070434649428420[215] = 0; + out_4336070434649428420[216] = 0; + out_4336070434649428420[217] = 0; + out_4336070434649428420[218] = 0; + out_4336070434649428420[219] = 0; + out_4336070434649428420[220] = 0; + out_4336070434649428420[221] = 0; + out_4336070434649428420[222] = 0; + out_4336070434649428420[223] = 0; + out_4336070434649428420[224] = 0; + out_4336070434649428420[225] = 0; + out_4336070434649428420[226] = 0; + out_4336070434649428420[227] = 0; + out_4336070434649428420[228] = 1; + out_4336070434649428420[229] = 0; + out_4336070434649428420[230] = 0; + out_4336070434649428420[231] = 0; + out_4336070434649428420[232] = 0; + out_4336070434649428420[233] = 0; + out_4336070434649428420[234] = 0; + out_4336070434649428420[235] = 0; + out_4336070434649428420[236] = 0; + out_4336070434649428420[237] = 0; + out_4336070434649428420[238] = 0; + out_4336070434649428420[239] = 0; + out_4336070434649428420[240] = 0; + out_4336070434649428420[241] = 0; + out_4336070434649428420[242] = 0; + out_4336070434649428420[243] = 0; + out_4336070434649428420[244] = 0; + out_4336070434649428420[245] = 0; + out_4336070434649428420[246] = 0; + out_4336070434649428420[247] = 1; + out_4336070434649428420[248] = 0; + out_4336070434649428420[249] = 0; + out_4336070434649428420[250] = 0; + out_4336070434649428420[251] = 0; + out_4336070434649428420[252] = 0; + out_4336070434649428420[253] = 0; + out_4336070434649428420[254] = 0; + out_4336070434649428420[255] = 0; + out_4336070434649428420[256] = 0; + out_4336070434649428420[257] = 0; + out_4336070434649428420[258] = 0; + out_4336070434649428420[259] = 0; + out_4336070434649428420[260] = 0; + out_4336070434649428420[261] = 0; + out_4336070434649428420[262] = 0; + out_4336070434649428420[263] = 0; + out_4336070434649428420[264] = 0; + out_4336070434649428420[265] = 0; + out_4336070434649428420[266] = 1; + out_4336070434649428420[267] = 0; + out_4336070434649428420[268] = 0; + out_4336070434649428420[269] = 0; + out_4336070434649428420[270] = 0; + out_4336070434649428420[271] = 0; + out_4336070434649428420[272] = 0; + out_4336070434649428420[273] = 0; + out_4336070434649428420[274] = 0; + out_4336070434649428420[275] = 0; + out_4336070434649428420[276] = 0; + out_4336070434649428420[277] = 0; + out_4336070434649428420[278] = 0; + out_4336070434649428420[279] = 0; + out_4336070434649428420[280] = 0; + out_4336070434649428420[281] = 0; + out_4336070434649428420[282] = 0; + out_4336070434649428420[283] = 0; + out_4336070434649428420[284] = 0; + out_4336070434649428420[285] = 1; + out_4336070434649428420[286] = 0; + out_4336070434649428420[287] = 0; + out_4336070434649428420[288] = 0; + out_4336070434649428420[289] = 0; + out_4336070434649428420[290] = 0; + out_4336070434649428420[291] = 0; + out_4336070434649428420[292] = 0; + out_4336070434649428420[293] = 0; + out_4336070434649428420[294] = 0; + out_4336070434649428420[295] = 0; + out_4336070434649428420[296] = 0; + out_4336070434649428420[297] = 0; + out_4336070434649428420[298] = 0; + out_4336070434649428420[299] = 0; + out_4336070434649428420[300] = 0; + out_4336070434649428420[301] = 0; + out_4336070434649428420[302] = 0; + out_4336070434649428420[303] = 0; + out_4336070434649428420[304] = 1; + out_4336070434649428420[305] = 0; + out_4336070434649428420[306] = 0; + out_4336070434649428420[307] = 0; + out_4336070434649428420[308] = 0; + out_4336070434649428420[309] = 0; + out_4336070434649428420[310] = 0; + out_4336070434649428420[311] = 0; + out_4336070434649428420[312] = 0; + out_4336070434649428420[313] = 0; + out_4336070434649428420[314] = 0; + out_4336070434649428420[315] = 0; + out_4336070434649428420[316] = 0; + out_4336070434649428420[317] = 0; + out_4336070434649428420[318] = 0; + out_4336070434649428420[319] = 0; + out_4336070434649428420[320] = 0; + out_4336070434649428420[321] = 0; + out_4336070434649428420[322] = 0; + out_4336070434649428420[323] = 1; } -void h_4(double *state, double *unused, double *out_1359340648735360631) { - out_1359340648735360631[0] = state[6] + state[9]; - out_1359340648735360631[1] = state[7] + state[10]; - out_1359340648735360631[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_5526642384709914880) { + out_5526642384709914880[0] = state[6] + state[9]; + out_5526642384709914880[1] = state[7] + state[10]; + out_5526642384709914880[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_4587809668546327657) { - out_4587809668546327657[0] = 0; - out_4587809668546327657[1] = 0; - out_4587809668546327657[2] = 0; - out_4587809668546327657[3] = 0; - out_4587809668546327657[4] = 0; - out_4587809668546327657[5] = 0; - out_4587809668546327657[6] = 1; - out_4587809668546327657[7] = 0; - out_4587809668546327657[8] = 0; - out_4587809668546327657[9] = 1; - out_4587809668546327657[10] = 0; - out_4587809668546327657[11] = 0; - out_4587809668546327657[12] = 0; - out_4587809668546327657[13] = 0; - out_4587809668546327657[14] = 0; - out_4587809668546327657[15] = 0; - out_4587809668546327657[16] = 0; - out_4587809668546327657[17] = 0; - out_4587809668546327657[18] = 0; - out_4587809668546327657[19] = 0; - out_4587809668546327657[20] = 0; - out_4587809668546327657[21] = 0; - out_4587809668546327657[22] = 0; - out_4587809668546327657[23] = 0; - out_4587809668546327657[24] = 0; - out_4587809668546327657[25] = 1; - out_4587809668546327657[26] = 0; - out_4587809668546327657[27] = 0; - out_4587809668546327657[28] = 1; - out_4587809668546327657[29] = 0; - out_4587809668546327657[30] = 0; - out_4587809668546327657[31] = 0; - out_4587809668546327657[32] = 0; - out_4587809668546327657[33] = 0; - out_4587809668546327657[34] = 0; - out_4587809668546327657[35] = 0; - out_4587809668546327657[36] = 0; - out_4587809668546327657[37] = 0; - out_4587809668546327657[38] = 0; - out_4587809668546327657[39] = 0; - out_4587809668546327657[40] = 0; - out_4587809668546327657[41] = 0; - out_4587809668546327657[42] = 0; - out_4587809668546327657[43] = 0; - out_4587809668546327657[44] = 1; - out_4587809668546327657[45] = 0; - out_4587809668546327657[46] = 0; - out_4587809668546327657[47] = 1; - out_4587809668546327657[48] = 0; - out_4587809668546327657[49] = 0; - out_4587809668546327657[50] = 0; - out_4587809668546327657[51] = 0; - out_4587809668546327657[52] = 0; - out_4587809668546327657[53] = 0; +void H_4(double *state, double *unused, double *out_5691168246274914733) { + out_5691168246274914733[0] = 0; + out_5691168246274914733[1] = 0; + out_5691168246274914733[2] = 0; + out_5691168246274914733[3] = 0; + out_5691168246274914733[4] = 0; + out_5691168246274914733[5] = 0; + out_5691168246274914733[6] = 1; + out_5691168246274914733[7] = 0; + out_5691168246274914733[8] = 0; + out_5691168246274914733[9] = 1; + out_5691168246274914733[10] = 0; + out_5691168246274914733[11] = 0; + out_5691168246274914733[12] = 0; + out_5691168246274914733[13] = 0; + out_5691168246274914733[14] = 0; + out_5691168246274914733[15] = 0; + out_5691168246274914733[16] = 0; + out_5691168246274914733[17] = 0; + out_5691168246274914733[18] = 0; + out_5691168246274914733[19] = 0; + out_5691168246274914733[20] = 0; + out_5691168246274914733[21] = 0; + out_5691168246274914733[22] = 0; + out_5691168246274914733[23] = 0; + out_5691168246274914733[24] = 0; + out_5691168246274914733[25] = 1; + out_5691168246274914733[26] = 0; + out_5691168246274914733[27] = 0; + out_5691168246274914733[28] = 1; + out_5691168246274914733[29] = 0; + out_5691168246274914733[30] = 0; + out_5691168246274914733[31] = 0; + out_5691168246274914733[32] = 0; + out_5691168246274914733[33] = 0; + out_5691168246274914733[34] = 0; + out_5691168246274914733[35] = 0; + out_5691168246274914733[36] = 0; + out_5691168246274914733[37] = 0; + out_5691168246274914733[38] = 0; + out_5691168246274914733[39] = 0; + out_5691168246274914733[40] = 0; + out_5691168246274914733[41] = 0; + out_5691168246274914733[42] = 0; + out_5691168246274914733[43] = 0; + out_5691168246274914733[44] = 1; + out_5691168246274914733[45] = 0; + out_5691168246274914733[46] = 0; + out_5691168246274914733[47] = 1; + out_5691168246274914733[48] = 0; + out_5691168246274914733[49] = 0; + out_5691168246274914733[50] = 0; + out_5691168246274914733[51] = 0; + out_5691168246274914733[52] = 0; + out_5691168246274914733[53] = 0; } -void h_10(double *state, double *unused, double *out_6036145492114111302) { - out_6036145492114111302[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_6036145492114111302[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_6036145492114111302[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_6728942981811315921) { + out_6728942981811315921[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_6728942981811315921[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_6728942981811315921[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_7466880812702133914) { - out_7466880812702133914[0] = 0; - out_7466880812702133914[1] = 9.8100000000000005*cos(state[1]); - out_7466880812702133914[2] = 0; - out_7466880812702133914[3] = 0; - out_7466880812702133914[4] = -state[8]; - out_7466880812702133914[5] = state[7]; - out_7466880812702133914[6] = 0; - out_7466880812702133914[7] = state[5]; - out_7466880812702133914[8] = -state[4]; - out_7466880812702133914[9] = 0; - out_7466880812702133914[10] = 0; - out_7466880812702133914[11] = 0; - out_7466880812702133914[12] = 1; - out_7466880812702133914[13] = 0; - out_7466880812702133914[14] = 0; - out_7466880812702133914[15] = 1; - out_7466880812702133914[16] = 0; - out_7466880812702133914[17] = 0; - out_7466880812702133914[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_7466880812702133914[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_7466880812702133914[20] = 0; - out_7466880812702133914[21] = state[8]; - out_7466880812702133914[22] = 0; - out_7466880812702133914[23] = -state[6]; - out_7466880812702133914[24] = -state[5]; - out_7466880812702133914[25] = 0; - out_7466880812702133914[26] = state[3]; - out_7466880812702133914[27] = 0; - out_7466880812702133914[28] = 0; - out_7466880812702133914[29] = 0; - out_7466880812702133914[30] = 0; - out_7466880812702133914[31] = 1; - out_7466880812702133914[32] = 0; - out_7466880812702133914[33] = 0; - out_7466880812702133914[34] = 1; - out_7466880812702133914[35] = 0; - out_7466880812702133914[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_7466880812702133914[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_7466880812702133914[38] = 0; - out_7466880812702133914[39] = -state[7]; - out_7466880812702133914[40] = state[6]; - out_7466880812702133914[41] = 0; - out_7466880812702133914[42] = state[4]; - out_7466880812702133914[43] = -state[3]; - out_7466880812702133914[44] = 0; - out_7466880812702133914[45] = 0; - out_7466880812702133914[46] = 0; - out_7466880812702133914[47] = 0; - out_7466880812702133914[48] = 0; - out_7466880812702133914[49] = 0; - out_7466880812702133914[50] = 1; - out_7466880812702133914[51] = 0; - out_7466880812702133914[52] = 0; - out_7466880812702133914[53] = 1; +void H_10(double *state, double *unused, double *out_4278506402956144046) { + out_4278506402956144046[0] = 0; + out_4278506402956144046[1] = 9.8100000000000005*cos(state[1]); + out_4278506402956144046[2] = 0; + out_4278506402956144046[3] = 0; + out_4278506402956144046[4] = -state[8]; + out_4278506402956144046[5] = state[7]; + out_4278506402956144046[6] = 0; + out_4278506402956144046[7] = state[5]; + out_4278506402956144046[8] = -state[4]; + out_4278506402956144046[9] = 0; + out_4278506402956144046[10] = 0; + out_4278506402956144046[11] = 0; + out_4278506402956144046[12] = 1; + out_4278506402956144046[13] = 0; + out_4278506402956144046[14] = 0; + out_4278506402956144046[15] = 1; + out_4278506402956144046[16] = 0; + out_4278506402956144046[17] = 0; + out_4278506402956144046[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_4278506402956144046[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_4278506402956144046[20] = 0; + out_4278506402956144046[21] = state[8]; + out_4278506402956144046[22] = 0; + out_4278506402956144046[23] = -state[6]; + out_4278506402956144046[24] = -state[5]; + out_4278506402956144046[25] = 0; + out_4278506402956144046[26] = state[3]; + out_4278506402956144046[27] = 0; + out_4278506402956144046[28] = 0; + out_4278506402956144046[29] = 0; + out_4278506402956144046[30] = 0; + out_4278506402956144046[31] = 1; + out_4278506402956144046[32] = 0; + out_4278506402956144046[33] = 0; + out_4278506402956144046[34] = 1; + out_4278506402956144046[35] = 0; + out_4278506402956144046[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_4278506402956144046[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_4278506402956144046[38] = 0; + out_4278506402956144046[39] = -state[7]; + out_4278506402956144046[40] = state[6]; + out_4278506402956144046[41] = 0; + out_4278506402956144046[42] = state[4]; + out_4278506402956144046[43] = -state[3]; + out_4278506402956144046[44] = 0; + out_4278506402956144046[45] = 0; + out_4278506402956144046[46] = 0; + out_4278506402956144046[47] = 0; + out_4278506402956144046[48] = 0; + out_4278506402956144046[49] = 0; + out_4278506402956144046[50] = 1; + out_4278506402956144046[51] = 0; + out_4278506402956144046[52] = 0; + out_4278506402956144046[53] = 1; } -void h_13(double *state, double *unused, double *out_863073697323037062) { - out_863073697323037062[0] = state[3]; - out_863073697323037062[1] = state[4]; - out_863073697323037062[2] = state[5]; +void h_13(double *state, double *unused, double *out_1205451540469147746) { + out_1205451540469147746[0] = state[3]; + out_1205451540469147746[1] = state[4]; + out_1205451540469147746[2] = state[5]; } -void H_13(double *state, double *unused, double *out_7800083493878660458) { - out_7800083493878660458[0] = 0; - out_7800083493878660458[1] = 0; - out_7800083493878660458[2] = 0; - out_7800083493878660458[3] = 1; - out_7800083493878660458[4] = 0; - out_7800083493878660458[5] = 0; - out_7800083493878660458[6] = 0; - out_7800083493878660458[7] = 0; - out_7800083493878660458[8] = 0; - out_7800083493878660458[9] = 0; - out_7800083493878660458[10] = 0; - out_7800083493878660458[11] = 0; - out_7800083493878660458[12] = 0; - out_7800083493878660458[13] = 0; - out_7800083493878660458[14] = 0; - out_7800083493878660458[15] = 0; - out_7800083493878660458[16] = 0; - out_7800083493878660458[17] = 0; - out_7800083493878660458[18] = 0; - out_7800083493878660458[19] = 0; - out_7800083493878660458[20] = 0; - out_7800083493878660458[21] = 0; - out_7800083493878660458[22] = 1; - out_7800083493878660458[23] = 0; - out_7800083493878660458[24] = 0; - out_7800083493878660458[25] = 0; - out_7800083493878660458[26] = 0; - out_7800083493878660458[27] = 0; - out_7800083493878660458[28] = 0; - out_7800083493878660458[29] = 0; - out_7800083493878660458[30] = 0; - out_7800083493878660458[31] = 0; - out_7800083493878660458[32] = 0; - out_7800083493878660458[33] = 0; - out_7800083493878660458[34] = 0; - out_7800083493878660458[35] = 0; - out_7800083493878660458[36] = 0; - out_7800083493878660458[37] = 0; - out_7800083493878660458[38] = 0; - out_7800083493878660458[39] = 0; - out_7800083493878660458[40] = 0; - out_7800083493878660458[41] = 1; - out_7800083493878660458[42] = 0; - out_7800083493878660458[43] = 0; - out_7800083493878660458[44] = 0; - out_7800083493878660458[45] = 0; - out_7800083493878660458[46] = 0; - out_7800083493878660458[47] = 0; - out_7800083493878660458[48] = 0; - out_7800083493878660458[49] = 0; - out_7800083493878660458[50] = 0; - out_7800083493878660458[51] = 0; - out_7800083493878660458[52] = 0; - out_7800083493878660458[53] = 0; +void H_13(double *state, double *unused, double *out_8903442071607247534) { + out_8903442071607247534[0] = 0; + out_8903442071607247534[1] = 0; + out_8903442071607247534[2] = 0; + out_8903442071607247534[3] = 1; + out_8903442071607247534[4] = 0; + out_8903442071607247534[5] = 0; + out_8903442071607247534[6] = 0; + out_8903442071607247534[7] = 0; + out_8903442071607247534[8] = 0; + out_8903442071607247534[9] = 0; + out_8903442071607247534[10] = 0; + out_8903442071607247534[11] = 0; + out_8903442071607247534[12] = 0; + out_8903442071607247534[13] = 0; + out_8903442071607247534[14] = 0; + out_8903442071607247534[15] = 0; + out_8903442071607247534[16] = 0; + out_8903442071607247534[17] = 0; + out_8903442071607247534[18] = 0; + out_8903442071607247534[19] = 0; + out_8903442071607247534[20] = 0; + out_8903442071607247534[21] = 0; + out_8903442071607247534[22] = 1; + out_8903442071607247534[23] = 0; + out_8903442071607247534[24] = 0; + out_8903442071607247534[25] = 0; + out_8903442071607247534[26] = 0; + out_8903442071607247534[27] = 0; + out_8903442071607247534[28] = 0; + out_8903442071607247534[29] = 0; + out_8903442071607247534[30] = 0; + out_8903442071607247534[31] = 0; + out_8903442071607247534[32] = 0; + out_8903442071607247534[33] = 0; + out_8903442071607247534[34] = 0; + out_8903442071607247534[35] = 0; + out_8903442071607247534[36] = 0; + out_8903442071607247534[37] = 0; + out_8903442071607247534[38] = 0; + out_8903442071607247534[39] = 0; + out_8903442071607247534[40] = 0; + out_8903442071607247534[41] = 1; + out_8903442071607247534[42] = 0; + out_8903442071607247534[43] = 0; + out_8903442071607247534[44] = 0; + out_8903442071607247534[45] = 0; + out_8903442071607247534[46] = 0; + out_8903442071607247534[47] = 0; + out_8903442071607247534[48] = 0; + out_8903442071607247534[49] = 0; + out_8903442071607247534[50] = 0; + out_8903442071607247534[51] = 0; + out_8903442071607247534[52] = 0; + out_8903442071607247534[53] = 0; } -void h_14(double *state, double *unused, double *out_1770197321873156194) { - out_1770197321873156194[0] = state[6]; - out_1770197321873156194[1] = state[7]; - out_1770197321873156194[2] = state[8]; +void h_14(double *state, double *unused, double *out_3303339494882182614) { + out_3303339494882182614[0] = state[6]; + out_3303339494882182614[1] = state[7]; + out_3303339494882182614[2] = state[8]; } -void H_14(double *state, double *unused, double *out_1505021236250955361) { - out_1505021236250955361[0] = 0; - out_1505021236250955361[1] = 0; - out_1505021236250955361[2] = 0; - out_1505021236250955361[3] = 0; - out_1505021236250955361[4] = 0; - out_1505021236250955361[5] = 0; - out_1505021236250955361[6] = 1; - out_1505021236250955361[7] = 0; - out_1505021236250955361[8] = 0; - out_1505021236250955361[9] = 0; - out_1505021236250955361[10] = 0; - out_1505021236250955361[11] = 0; - out_1505021236250955361[12] = 0; - out_1505021236250955361[13] = 0; - out_1505021236250955361[14] = 0; - out_1505021236250955361[15] = 0; - out_1505021236250955361[16] = 0; - out_1505021236250955361[17] = 0; - out_1505021236250955361[18] = 0; - out_1505021236250955361[19] = 0; - out_1505021236250955361[20] = 0; - out_1505021236250955361[21] = 0; - out_1505021236250955361[22] = 0; - out_1505021236250955361[23] = 0; - out_1505021236250955361[24] = 0; - out_1505021236250955361[25] = 1; - out_1505021236250955361[26] = 0; - out_1505021236250955361[27] = 0; - out_1505021236250955361[28] = 0; - out_1505021236250955361[29] = 0; - out_1505021236250955361[30] = 0; - out_1505021236250955361[31] = 0; - out_1505021236250955361[32] = 0; - out_1505021236250955361[33] = 0; - out_1505021236250955361[34] = 0; - out_1505021236250955361[35] = 0; - out_1505021236250955361[36] = 0; - out_1505021236250955361[37] = 0; - out_1505021236250955361[38] = 0; - out_1505021236250955361[39] = 0; - out_1505021236250955361[40] = 0; - out_1505021236250955361[41] = 0; - out_1505021236250955361[42] = 0; - out_1505021236250955361[43] = 0; - out_1505021236250955361[44] = 1; - out_1505021236250955361[45] = 0; - out_1505021236250955361[46] = 0; - out_1505021236250955361[47] = 0; - out_1505021236250955361[48] = 0; - out_1505021236250955361[49] = 0; - out_1505021236250955361[50] = 0; - out_1505021236250955361[51] = 0; - out_1505021236250955361[52] = 0; - out_1505021236250955361[53] = 0; +void H_14(double *state, double *unused, double *out_8792334971095152354) { + out_8792334971095152354[0] = 0; + out_8792334971095152354[1] = 0; + out_8792334971095152354[2] = 0; + out_8792334971095152354[3] = 0; + out_8792334971095152354[4] = 0; + out_8792334971095152354[5] = 0; + out_8792334971095152354[6] = 1; + out_8792334971095152354[7] = 0; + out_8792334971095152354[8] = 0; + out_8792334971095152354[9] = 0; + out_8792334971095152354[10] = 0; + out_8792334971095152354[11] = 0; + out_8792334971095152354[12] = 0; + out_8792334971095152354[13] = 0; + out_8792334971095152354[14] = 0; + out_8792334971095152354[15] = 0; + out_8792334971095152354[16] = 0; + out_8792334971095152354[17] = 0; + out_8792334971095152354[18] = 0; + out_8792334971095152354[19] = 0; + out_8792334971095152354[20] = 0; + out_8792334971095152354[21] = 0; + out_8792334971095152354[22] = 0; + out_8792334971095152354[23] = 0; + out_8792334971095152354[24] = 0; + out_8792334971095152354[25] = 1; + out_8792334971095152354[26] = 0; + out_8792334971095152354[27] = 0; + out_8792334971095152354[28] = 0; + out_8792334971095152354[29] = 0; + out_8792334971095152354[30] = 0; + out_8792334971095152354[31] = 0; + out_8792334971095152354[32] = 0; + out_8792334971095152354[33] = 0; + out_8792334971095152354[34] = 0; + out_8792334971095152354[35] = 0; + out_8792334971095152354[36] = 0; + out_8792334971095152354[37] = 0; + out_8792334971095152354[38] = 0; + out_8792334971095152354[39] = 0; + out_8792334971095152354[40] = 0; + out_8792334971095152354[41] = 0; + out_8792334971095152354[42] = 0; + out_8792334971095152354[43] = 0; + out_8792334971095152354[44] = 1; + out_8792334971095152354[45] = 0; + out_8792334971095152354[46] = 0; + out_8792334971095152354[47] = 0; + out_8792334971095152354[48] = 0; + out_8792334971095152354[49] = 0; + out_8792334971095152354[50] = 0; + out_8792334971095152354[51] = 0; + out_8792334971095152354[52] = 0; + out_8792334971095152354[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_5340642756295943636) { - err_fun(nom_x, delta_x, out_5340642756295943636); +void pose_err_fun(double *nom_x, double *delta_x, double *out_4967863734907407968) { + err_fun(nom_x, delta_x, out_4967863734907407968); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6968876690248087810) { - inv_err_fun(nom_x, true_x, out_6968876690248087810); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_554301816764323460) { + inv_err_fun(nom_x, true_x, out_554301816764323460); } -void pose_H_mod_fun(double *state, double *out_3389862186663429553) { - H_mod_fun(state, out_3389862186663429553); +void pose_H_mod_fun(double *state, double *out_3047027985808074405) { + H_mod_fun(state, out_3047027985808074405); } -void pose_f_fun(double *state, double dt, double *out_4802700997878092668) { - f_fun(state, dt, out_4802700997878092668); +void pose_f_fun(double *state, double dt, double *out_986492010673501731) { + f_fun(state, dt, out_986492010673501731); } -void pose_F_fun(double *state, double dt, double *out_8725044315227912670) { - F_fun(state, dt, out_8725044315227912670); +void pose_F_fun(double *state, double dt, double *out_4336070434649428420) { + F_fun(state, dt, out_4336070434649428420); } -void pose_h_4(double *state, double *unused, double *out_1359340648735360631) { - h_4(state, unused, out_1359340648735360631); +void pose_h_4(double *state, double *unused, double *out_5526642384709914880) { + h_4(state, unused, out_5526642384709914880); } -void pose_H_4(double *state, double *unused, double *out_4587809668546327657) { - H_4(state, unused, out_4587809668546327657); +void pose_H_4(double *state, double *unused, double *out_5691168246274914733) { + H_4(state, unused, out_5691168246274914733); } -void pose_h_10(double *state, double *unused, double *out_6036145492114111302) { - h_10(state, unused, out_6036145492114111302); +void pose_h_10(double *state, double *unused, double *out_6728942981811315921) { + h_10(state, unused, out_6728942981811315921); } -void pose_H_10(double *state, double *unused, double *out_7466880812702133914) { - H_10(state, unused, out_7466880812702133914); +void pose_H_10(double *state, double *unused, double *out_4278506402956144046) { + H_10(state, unused, out_4278506402956144046); } -void pose_h_13(double *state, double *unused, double *out_863073697323037062) { - h_13(state, unused, out_863073697323037062); +void pose_h_13(double *state, double *unused, double *out_1205451540469147746) { + h_13(state, unused, out_1205451540469147746); } -void pose_H_13(double *state, double *unused, double *out_7800083493878660458) { - H_13(state, unused, out_7800083493878660458); +void pose_H_13(double *state, double *unused, double *out_8903442071607247534) { + H_13(state, unused, out_8903442071607247534); } -void pose_h_14(double *state, double *unused, double *out_1770197321873156194) { - h_14(state, unused, out_1770197321873156194); +void pose_h_14(double *state, double *unused, double *out_3303339494882182614) { + h_14(state, unused, out_3303339494882182614); } -void pose_H_14(double *state, double *unused, double *out_1505021236250955361) { - H_14(state, unused, out_1505021236250955361); +void pose_H_14(double *state, double *unused, double *out_8792334971095152354) { + H_14(state, unused, out_8792334971095152354); } 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 8fb658bc0..b74fe6f27 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_5340642756295943636); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6968876690248087810); -void pose_H_mod_fun(double *state, double *out_3389862186663429553); -void pose_f_fun(double *state, double dt, double *out_4802700997878092668); -void pose_F_fun(double *state, double dt, double *out_8725044315227912670); -void pose_h_4(double *state, double *unused, double *out_1359340648735360631); -void pose_H_4(double *state, double *unused, double *out_4587809668546327657); -void pose_h_10(double *state, double *unused, double *out_6036145492114111302); -void pose_H_10(double *state, double *unused, double *out_7466880812702133914); -void pose_h_13(double *state, double *unused, double *out_863073697323037062); -void pose_H_13(double *state, double *unused, double *out_7800083493878660458); -void pose_h_14(double *state, double *unused, double *out_1770197321873156194); -void pose_H_14(double *state, double *unused, double *out_1505021236250955361); +void pose_err_fun(double *nom_x, double *delta_x, double *out_4967863734907407968); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_554301816764323460); +void pose_H_mod_fun(double *state, double *out_3047027985808074405); +void pose_f_fun(double *state, double dt, double *out_986492010673501731); +void pose_F_fun(double *state, double dt, double *out_4336070434649428420); +void pose_h_4(double *state, double *unused, double *out_5526642384709914880); +void pose_H_4(double *state, double *unused, double *out_5691168246274914733); +void pose_h_10(double *state, double *unused, double *out_6728942981811315921); +void pose_H_10(double *state, double *unused, double *out_4278506402956144046); +void pose_h_13(double *state, double *unused, double *out_1205451540469147746); +void pose_H_13(double *state, double *unused, double *out_8903442071607247534); +void pose_h_14(double *state, double *unused, double *out_3303339494882182614); +void pose_H_14(double *state, double *unused, double *out_8792334971095152354); 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 10df21213..a91796473 100755 Binary files a/selfdrive/pandad/pandad and b/selfdrive/pandad/pandad differ diff --git a/selfdrive/ui/layouts/settings/starpilot/appearance.py b/selfdrive/ui/layouts/settings/starpilot/appearance.py index 16e324da8..13af52420 100644 --- a/selfdrive/ui/layouts/settings/starpilot/appearance.py +++ b/selfdrive/ui/layouts/settings/starpilot/appearance.py @@ -309,6 +309,11 @@ class StarPilotAppearanceLayout(_SettingsPage): get_state=lambda: self._params.get_bool("ShowSteering"), set_state=lambda s: self._params.put_bool("ShowSteering", s), visible=hud_on), + SettingRow("EnableTorqueBarWidget", "toggle", tr_noop("Torque Bar"), + subtitle=tr_noop("Show a curved torque-utilization indicator at the bottom of the driving screen."), + get_state=lambda: self._params.get_bool("EnableTorqueBarWidget"), + set_state=lambda s: self._params.put_bool("EnableTorqueBarWidget", s), + visible=hud_on), SettingRow("SignalMetrics", "toggle", tr_noop("Turn Signal Borders"), subtitle="", get_state=lambda: self._params.get_bool("SignalMetrics"), diff --git a/selfdrive/ui/lib/starpilot_visuals.py b/selfdrive/ui/lib/starpilot_visuals.py index c73d9962f..eb29d1dbe 100644 --- a/selfdrive/ui/lib/starpilot_visuals.py +++ b/selfdrive/ui/lib/starpilot_visuals.py @@ -2,9 +2,20 @@ from __future__ import annotations import math +import pyray as rl + from openpilot.common.params import Params +def blend_colors(a: rl.Color, b: rl.Color, f: float) -> rl.Color: + h0, s0, v0 = (hsv0 := rl.color_to_hsv(a)).x, hsv0.y, hsv0.z + h1, s1, v1 = (hsv1 := rl.color_to_hsv(b)).x, hsv1.y, hsv1.z + dh = ((h1 - h0 + 180) % 360) - 180 # shortest hue delta + return rl.color_from_hsv((h0 + f * dh) % 360, + s0 + f * (s1 - s0), + v0 + f * (v1 - v0)) + + def get_border_width(base_width: int, params: Params | None = None) -> int: active_params = params if params is not None else Params() diff --git a/selfdrive/ui/mici/layouts/settings/visuals.py b/selfdrive/ui/mici/layouts/settings/visuals.py index 8f55abb31..84f9edf6e 100644 --- a/selfdrive/ui/mici/layouts/settings/visuals.py +++ b/selfdrive/ui/mici/layouts/settings/visuals.py @@ -60,6 +60,7 @@ class VisualsLayoutMici(NavScroller): self._driver_camera_btn = BigParamControl("driver camera on reverse", "DriverCamera") self._stopped_timer_btn = BigParamControl("stopped timer", "StoppedTimer") self._stock_confidence_ball_btn = BigParamControl("stock confidence ball", "StockConfidenceBallWidget") + self._torque_bar_btn = BigParamControl("torque bar", "EnableTorqueBarWidget") self._rainbow_path_btn = BigParamControl("rainbow road", "RainbowPath") self._lead_indicator_btn = LeadIndicatorBigButton() self._speed_limit_signs_btn = BigParamControl("speed limit signs", "ShowSpeedLimits") @@ -72,6 +73,7 @@ class VisualsLayoutMici(NavScroller): self._driver_camera_btn, self._stopped_timer_btn, self._stock_confidence_ball_btn, + self._torque_bar_btn, self._rainbow_path_btn, self._lead_indicator_btn, self._speed_limit_signs_btn, diff --git a/selfdrive/ui/mici/onroad/__init__.py b/selfdrive/ui/mici/onroad/__init__.py index bb45117b9..098a6e45f 100644 --- a/selfdrive/ui/mici/onroad/__init__.py +++ b/selfdrive/ui/mici/onroad/__init__.py @@ -1,12 +1 @@ -import pyray as rl - SIDE_PANEL_WIDTH = 60 - - -def blend_colors(a: rl.Color, b: rl.Color, f: float) -> rl.Color: - h0, s0, v0 = (hsv0 := rl.color_to_hsv(a)).x, hsv0.y, hsv0.z - h1, s1, v1 = (hsv1 := rl.color_to_hsv(b)).x, hsv1.y, hsv1.z - dh = ((h1 - h0 + 180) % 360) - 180 # shortest hue delta - return rl.color_from_hsv((h0 + f * dh) % 360, - s0 + f * (s1 - s0), - v0 + f * (v1 - v0)) diff --git a/selfdrive/ui/mici/onroad/hud_renderer.py b/selfdrive/ui/mici/onroad/hud_renderer.py index 3e0003e44..8ee4cb886 100644 --- a/selfdrive/ui/mici/onroad/hud_renderer.py +++ b/selfdrive/ui/mici/onroad/hud_renderer.py @@ -3,7 +3,7 @@ import math import pyray as rl from dataclasses import dataclass from openpilot.common.constants import CV -from openpilot.selfdrive.ui.mici.onroad.torque_bar import TorqueBar +from openpilot.selfdrive.ui.onroad.starpilot.torque_bar import TorqueBar from openpilot.selfdrive.ui.mici.onroad.speed_limit_utils import resolve_display_speed_limit_ms from openpilot.selfdrive.ui.onroad.starpilot.navigation_card import NavigationCardRenderer from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus @@ -269,7 +269,8 @@ class HudRenderer(Widget): def render_foreground(self) -> None: """Draw HUD elements that should sit above alerts.""" - if ui_state.sm['controlsState'].lateralControlState.which() != 'angleState': + if ui_state.sm['controlsState'].lateralControlState.which() != 'angleState' and \ + ui_state.params.get_bool("EnableTorqueBarWidget", default=True): self._torque_bar.render(self._rect) if self.is_cruise_set: diff --git a/selfdrive/ui/mici/onroad/model_renderer.py b/selfdrive/ui/mici/onroad/model_renderer.py index c8bf5cb7d..e8336727a 100644 --- a/selfdrive/ui/mici/onroad/model_renderer.py +++ b/selfdrive/ui/mici/onroad/model_renderer.py @@ -9,9 +9,8 @@ from openpilot.common.filter_simple import FirstOrderFilter from openpilot.selfdrive.locationd.calibrationd import HEIGHT_INIT from openpilot.selfdrive.ui.lib.starpilot_theme import get_param_color, get_theme_color, get_visual_color, is_stock_color_scheme, with_alpha from openpilot.selfdrive.ui.onroad.starpilot.rainbow_path import RainbowPath -from openpilot.selfdrive.ui.lib.starpilot_visuals import lead_indicator_enabled +from openpilot.selfdrive.ui.lib.starpilot_visuals import blend_colors, lead_indicator_enabled from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus -from openpilot.selfdrive.ui.mici.onroad import blend_colors from openpilot.selfdrive.ui.mici.onroad.starpilot_status import get_border_color from openpilot.system.ui.lib.application import gui_app from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient diff --git a/selfdrive/ui/onroad/starpilot/navigation_card.py b/selfdrive/ui/onroad/starpilot/navigation_card.py index 0de37cab0..3ad18dff8 100644 --- a/selfdrive/ui/onroad/starpilot/navigation_card.py +++ b/selfdrive/ui/onroad/starpilot/navigation_card.py @@ -15,7 +15,6 @@ from openpilot.system.ui.widgets import Widget ASSETS_PATH = Path(__file__).resolve().parents[4] / "starpilot" / "assets" / "navigation" FALLBACK_ICON = "direction_turn_straight.png" NAV_CANCEL_HOLD_SECONDS = 0.65 -_LEFT_UI_EXTENT = 424 # left UI (DM icon + personality button) extent from content_rect.x: offset(126) + x_shift(202) + radius(96) def _format_distance(distance_m: float, is_metric: bool) -> str: @@ -58,6 +57,7 @@ def _normalize_maneuver_type(maneuver_type: str) -> str: class NavigationCardRenderer(Widget): def __init__(self, layout_variant: str = "default"): + """layout_variant: "default" (tici/tizi) or "mici".""" super().__init__() self._font_bold = gui_app.font(FontWeight.BOLD) self._font_medium = gui_app.font(FontWeight.MEDIUM) @@ -278,31 +278,31 @@ class NavigationCardRenderer(Widget): line_y = y + index * (font_size + line_gap) rl.draw_text_ex(self._font_bold, line, rl.Vector2(x, line_y), font_size, 0, rl.WHITE) - def _render_default(self, rect: rl.Rectangle) -> None: - container_width = min(int(rect.width - 2 * _LEFT_UI_EXTENT - 20), 1080) - container_width = max(container_width, 760) - container_height = 225 if rect.width >= 1200 else 195 - container_x = int(rect.x + (rect.width - container_width) / 2) - container_y = int(rect.y + rect.height - container_height - 80) - border_radius = 42 if container_height == 225 else 34 - icon_size = 150 if container_height == 225 else 122 - icon_padding = 30 if container_height == 225 else 24 - then_section_width = 180 if container_height == 225 else 144 - then_icon_size = 105 if container_height == 225 else 82 - preferred_font_size = 75 if container_height == 225 else 58 - minimum_font_size = 60 if container_height == 225 else 46 - distance_font_size = 48 if container_height == 225 else 40 + def _render_default(self, rect: rl.Rectangle) -> None: # tici/tizi expanded card + right_margin = 40 + card_width = 560 + card_height = 195 + container_x = int(rect.x + rect.width - card_width - right_margin) + container_y = int(rect.y + rect.height - card_height - 440) - container = rl.Rectangle(container_x, container_y, container_width, container_height) + icon_size = 100 + icon_padding = 24 + then_section_width = 130 + then_icon_size = 72 + preferred_font_size = 48 + minimum_font_size = 38 + distance_font_size = 32 + + container = rl.Rectangle(container_x, container_y, card_width, card_height) self._interactive_rect = container - rl.draw_rectangle_rounded(container, border_radius / container_height, 10, rl.Color(0, 0, 0, 192)) + rl.draw_rectangle_rounded(container, 34 / card_height, 10, rl.Color(0, 0, 0, 192)) icon_x = container_x + icon_padding icon = self._get_icon(self._maneuver_type, self._modifier) distance_size = measure_text_cached(self._font_bold, self._distance, distance_font_size) icon_block_height = icon_size + 8 + int(distance_size.y) - icon_y = container_y + (container_height - icon_block_height) // 2 + icon_y = container_y + (card_height - icon_block_height) // 2 dest_rect = rl.Rectangle(icon_x, icon_y, icon_size, icon_size) rl.draw_texture_pro(icon, rl.Rectangle(0, 0, icon.width, icon.height), dest_rect, rl.Vector2(0, 0), 0, rl.WHITE) @@ -311,16 +311,17 @@ class NavigationCardRenderer(Widget): rl.draw_text_ex(self._font_bold, self._distance, rl.Vector2(distance_x, distance_y), distance_font_size, 0, rl.WHITE) show_next = self._has_next - text_x = icon_x + icon_size + 53 + gap_after_icon = 32 + text_x = icon_x + icon_size + gap_after_icon right_gutter = icon_padding + (then_section_width if show_next else 0) - text_area_width = container_width - (text_x - container_x) - right_gutter + text_area_width = card_width - (text_x - container_x) - right_gutter title_lines, title_font_size = self._fit_title(self._primary_text, text_area_width, preferred_font_size, minimum_font_size) - secondary_font_size = 38 if container_height == 225 else 30 + secondary_font_size = 26 text_block_h = len(title_lines) * title_font_size if self._secondary_text: text_block_h += 8 + secondary_font_size - text_block_y = container_y + (container_height - text_block_h) // 2 + text_block_y = container_y + (card_height - text_block_h) // 2 for index, line in enumerate(title_lines[:2]): rl.draw_text_ex(self._font_bold, line, rl.Vector2(text_x, text_block_y + index * title_font_size), title_font_size, 0, rl.WHITE) @@ -340,24 +341,24 @@ class NavigationCardRenderer(Widget): if not show_next: return - divider_x = container_x + container_width - then_section_width - 8 + divider_x = container_x + card_width - then_section_width - 8 rl.draw_line_ex( rl.Vector2(divider_x, container_y + 23), - rl.Vector2(divider_x, container_y + container_height - 23), + rl.Vector2(divider_x, container_y + card_height - 23), 2, rl.Color(255, 255, 255, 50), ) then_x = divider_x + 15 then_label = "Then" - then_font_size = 53 if container_height == 225 else 40 + then_font_size = 36 then_size = measure_text_cached(self._font_medium, then_label, then_font_size) then_label_x = then_x + (then_section_width - 23 - then_size.x) / 2 rl.draw_text_ex(self._font_medium, then_label, rl.Vector2(then_label_x, container_y + 30), then_font_size, 0, rl.WHITE) next_icon = self._get_icon(self._next_maneuver_type, self._next_modifier) then_icon_x = then_x + (then_section_width - 23 - then_icon_size) / 2 - then_icon_y = container_y + (84 if container_height == 225 else 72) + then_icon_y = container_y + 72 rl.draw_texture_pro( next_icon, rl.Rectangle(0, 0, next_icon.width, next_icon.height), @@ -367,10 +368,10 @@ class NavigationCardRenderer(Widget): rl.WHITE, ) - def _render_collapsed_default(self, rect: rl.Rectangle) -> None: + def _render_collapsed_default(self, rect: rl.Rectangle) -> None: # tici/tizi collapsed chip chip_size = 94 - chip_x = int(rect.x + rect.width - 126 - chip_size / 2) # center-aligned with steering wheel icon horizontally - chip_y = int(rect.y + (rect.height - chip_size) / 2) # half way down vertically + chip_x = int(rect.x + rect.width - chip_size - 40) # right-aligned, 40px margin + chip_y = int(rect.y + 340) # above expanded card chip_rect = rl.Rectangle(chip_x, chip_y, chip_size, chip_size) self._interactive_rect = chip_rect @@ -390,7 +391,7 @@ class NavigationCardRenderer(Widget): rl.WHITE, ) - def _render_mici(self, rect: rl.Rectangle) -> None: + def _render_mici(self, rect: rl.Rectangle) -> None: # MICI expanded card left_safe = 96 right_margin = 12 available_width = max(260, int(rect.width - left_safe - right_margin)) @@ -485,7 +486,7 @@ class NavigationCardRenderer(Widget): rl.WHITE, ) - def _render_collapsed_mici(self, rect: rl.Rectangle) -> None: + def _render_collapsed_mici(self, rect: rl.Rectangle) -> None: # MICI collapsed chip chip_size = 72 right_reserved = 160 chip_x = int(rect.x + rect.width - chip_size - right_reserved) @@ -515,11 +516,11 @@ class NavigationCardRenderer(Widget): return if self._collapsed: if self._layout_variant == "mici": - self._render_collapsed_mici(rect) + self._render_collapsed_mici(rect) # MICI: top-right chip else: - self._render_collapsed_default(rect) + self._render_collapsed_default(rect) # tici/tizi: right edge return if self._layout_variant == "mici": - self._render_mici(rect) + self._render_mici(rect) # MICI: top-left card else: - self._render_default(rect) + self._render_default(rect) # tici/tizi: right edge, bottom-aligned diff --git a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py index 64cd8d5cc..f7f0aea89 100644 --- a/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py +++ b/selfdrive/ui/onroad/starpilot/starpilot_onroad_view.py @@ -12,6 +12,7 @@ from openpilot.selfdrive.ui.onroad.starpilot.slc_speed_limit import ( ) from openpilot.selfdrive.ui.ui_state import ui_state from openpilot.selfdrive.ui.onroad.starpilot.aethergauge import AetherGauge +from openpilot.selfdrive.ui.onroad.starpilot.torque_bar import TorqueBar from openpilot.selfdrive.ui.lib.starpilot_status import ( get_screen_edge_color, ENGAGED_COLOR, EXPERIMENTAL_COLOR, TRAFFIC_COLOR, @@ -30,6 +31,7 @@ class StarPilotOnroadView(AugmentedRoadView): self._font_medium = gui_app.font(FontWeight.MEDIUM) self._standstill_started_at = 0.0 self._aethergauge = AetherGauge() + self._torque_bar = TorqueBar() def _render(self, rect: rl.Rectangle): self._position_personality_button() @@ -72,9 +74,23 @@ class StarPilotOnroadView(AugmentedRoadView): ) else: self._aethergauge.render(self._content_rect, self._font_bold, self._font_medium, current_speed=self._hud_renderer.speed) + self._render_torque_bar() self._render_bottom_row_widgets() self._render_pedals() + def _render_torque_bar(self) -> None: + """Draw the curved torque-utilization indicator at the bottom of the screen.""" + if not self._params.get_bool("EnableTorqueBarWidget", default=True): + return + if ui_state.sm['controlsState'].lateralControlState.which() == 'angleState': + return + rl.begin_scissor_mode( + int(self._content_rect.x), int(self._content_rect.y), + int(self._content_rect.width), int(self._content_rect.height), + ) + self._torque_bar.render(self._content_rect) + rl.end_scissor_mode() + def _render_path_features(self, rect: rl.Rectangle): """Render path-related features (adjacent paths, blind spot, path edges).""" mr = self.model_renderer diff --git a/selfdrive/ui/mici/onroad/torque_bar.py b/selfdrive/ui/onroad/starpilot/torque_bar.py similarity index 97% rename from selfdrive/ui/mici/onroad/torque_bar.py rename to selfdrive/ui/onroad/starpilot/torque_bar.py index 079a9fcac..5916ada1d 100644 --- a/selfdrive/ui/mici/onroad/torque_bar.py +++ b/selfdrive/ui/onroad/starpilot/torque_bar.py @@ -6,7 +6,7 @@ from collections import OrderedDict import numpy as np import pyray as rl from opendbc.car import ACCELERATION_DUE_TO_GRAVITY -from openpilot.selfdrive.ui.mici.onroad import blend_colors +from openpilot.selfdrive.ui.lib.starpilot_visuals import blend_colors from openpilot.selfdrive.ui.ui_state import ui_state, UIStatus from openpilot.system.ui.lib.application import gui_app from openpilot.system.ui.lib.shader_polygon import draw_polygon, Gradient @@ -189,9 +189,10 @@ class TorqueBar(Widget): self._torque_filter.update(-ui_state.sm['carOutput'].actuatorsOutput.torque) def _render(self, rect: rl.Rectangle) -> None: + SCALE = rect.height / 240.0 # adjust y pos with torque - torque_line_offset = np.interp(abs(self._torque_filter.x), [0.5, 1], [22, 26]) - torque_line_height = np.interp(abs(self._torque_filter.x), [0.5, 1], [14, 56]) + torque_line_offset = np.interp(abs(self._torque_filter.x), [0.5, 1], [22 * SCALE, 26 * SCALE]) + torque_line_height = np.interp(abs(self._torque_filter.x), [0.5, 1], [14 * SCALE, 56 * SCALE]) lateral_ui_active = ui_state.status == UIStatus.ENGAGED or ui_state.always_on_lateral_active # animate alpha and angle span @@ -206,7 +207,7 @@ class TorqueBar(Widget): torque_line_bg_color = rl.Color(255, 255, 255, int(255 * 0.15 * self._torque_line_alpha_filter.x)) # draw curved line polygon torque bar - torque_line_radius = 1200 + torque_line_radius = 1200 * SCALE top_angle = -90 torque_bg_angle_span = self._torque_line_alpha_filter.x * TORQUE_ANGLE_SPAN torque_start_angle = top_angle - torque_bg_angle_span / 2 @@ -263,5 +264,5 @@ class TorqueBar(Widget): # draw center torque bar dot if abs(self._torque_filter.x) < 0.5: dot_y = self._rect.y + self._rect.height - torque_line_offset - torque_line_height / 2 - rl.draw_circle(int(cx), int(dot_y), 10 // 2, + rl.draw_circle(int(cx), int(dot_y), (10 * SCALE) / 2, rl.Color(182, 182, 182, int(255 * 0.9 * self._torque_line_alpha_filter.x))) diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index 5551573cf..7f0556c39 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ diff --git a/system/camerad/camerad b/system/camerad/camerad index 29be30341..5dbfea0eb 100755 Binary files a/system/camerad/camerad and b/system/camerad/camerad differ diff --git a/system/loggerd/bootlog b/system/loggerd/bootlog index 550d8bb3a..40dbb3942 100755 Binary files a/system/loggerd/bootlog and b/system/loggerd/bootlog differ diff --git a/system/loggerd/encoderd b/system/loggerd/encoderd index 6bdb7f22f..679dec91e 100755 Binary files a/system/loggerd/encoderd and b/system/loggerd/encoderd differ diff --git a/system/loggerd/loggerd b/system/loggerd/loggerd index 0ce1c5d1a..fa20c54e4 100755 Binary files a/system/loggerd/loggerd and b/system/loggerd/loggerd differ