diff --git a/openpilot/common/SConscript b/openpilot/common/SConscript index 4e06ae88a1..847d16446a 100644 --- a/openpilot/common/SConscript +++ b/openpilot/common/SConscript @@ -17,3 +17,6 @@ Export('common_python') if GetOption('extras'): env.Program('tests/test_swaglog', 'tests/test_swaglog.cc', LIBS=[_common, 'json11', 'zmq', 'pthread']) + + env.Program('tests/bin/test_params_fork_lock', 'tests/test_params_fork_lock.cc', + LIBS=[_common, 'json11', 'zmq', 'capnp', 'kj', 'pthread']) diff --git a/openpilot/common/libcommon.a b/openpilot/common/libcommon.a index 4c2bb0c1ea..44186118ea 100644 Binary files a/openpilot/common/libcommon.a and b/openpilot/common/libcommon.a differ diff --git a/openpilot/common/libparams_c.so b/openpilot/common/libparams_c.so index 84cd8600cd..674b3a801e 100755 Binary files a/openpilot/common/libparams_c.so and b/openpilot/common/libparams_c.so differ diff --git a/openpilot/common/params.cc b/openpilot/common/params.cc index 495feb0a99..229754cf62 100644 --- a/openpilot/common/params.cc +++ b/openpilot/common/params.cc @@ -76,16 +76,28 @@ std::string ensure_params_path(const std::string &prefix, const std::string &pat class FileLock { public: - FileLock(const std::string &fn) { - fd_ = HANDLE_EINTR(open(fn.c_str(), O_CREAT, 0775)); - if (fd_ < 0 || HANDLE_EINTR(flock(fd_, LOCK_EX)) < 0) { + FileLock(const std::string &fn) : owner_pid_(getpid()) { + fd_ = HANDLE_EINTR(open(fn.c_str(), O_CREAT | O_CLOEXEC, 0775)); + locked_ = fd_ >= 0 && HANDLE_EINTR(flock(fd_, LOCK_EX)) == 0; + if (!locked_) { LOGE("Failed to lock file %s, errno=%d", fn.c_str(), errno); } } - ~FileLock() { close(fd_); } + ~FileLock() { + if (fd_ >= 0) { + // flock follows the open file description across fork. Release explicitly + // in the acquiring process even while children retain their copies. + if (locked_ && owner_pid_ == getpid()) { + HANDLE_EINTR(flock(fd_, LOCK_UN)); + } + close(fd_); + } + } private: + const pid_t owner_pid_; int fd_ = -1; + bool locked_ = false; }; } // namespace diff --git a/openpilot/common/tests/test_params_fork_lock.cc b/openpilot/common/tests/test_params_fork_lock.cc new file mode 100644 index 0000000000..e83e816687 --- /dev/null +++ b/openpilot/common/tests/test_params_fork_lock.cc @@ -0,0 +1,205 @@ +// Native regression tests exercise the actual Params/FileLock implementation. +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "common/tests/native_test.h" + +static int fail_ex = 0; +static std::atomic unlocks{0}; +static int observed_flock(int fd, int op) { + if (op == LOCK_UN) ++unlocks; + if (op == LOCK_EX && fail_ex != 0) { + errno = fail_ex; + fail_ex = 0; + return -1; + } + return ::flock(fd, op); +} +// Fault injection applies only to actual FileLock's syscall and retains all +// production declarations/method bodies. Other test probes call real flock. +#define flock observed_flock +#ifdef PARAMS_IMPLEMENTATION +#include PARAMS_IMPLEMENTATION +#else +#include "common/params.cc" +#endif +#undef flock + +static void check(bool good, const char *message) { + native_test_check(good, message, __FILE__, __LINE__); +} +static void send_byte(int fd) { check(write(fd, "x", 1) == 1, "pipe write"); } +static bool receive_byte(int fd, int ms) { + pollfd p{fd, POLLIN, 0}; + if (poll(&p, 1, ms) != 1) return false; + char c; + return read(fd, &c, 1) == 1; +} +static bool available(const std::string &path) { + int fd = open(path.c_str(), O_RDONLY | O_CLOEXEC); + check(fd >= 0, "probe open"); + bool good = ::flock(fd, LOCK_EX | LOCK_NB) == 0; + if (good) check(::flock(fd, LOCK_UN) == 0, "probe unlock"); + else check(errno == EWOULDBLOCK || errno == EAGAIN, "probe expected contention"); + close(fd); + return good; +} +static void finish(pid_t child, int release, bool ready) { + if (ready) send_byte(release); + else kill(child, SIGKILL); + int status = 0; + check(waitpid(child, &status, 0) == child, "reap child"); + if (ready) check(WIFEXITED(status) && WEXITSTATUS(status) == 0, "child clean exit"); +} +static void inherited_release(const std::string &path) { + int ready[2], release[2]; check(pipe(ready) == 0 && pipe(release) == 0, "pipes"); + auto lock = new FileLock(path); + pid_t child = fork(); check(child >= 0, "fork"); + if (child == 0) { + close(ready[0]); close(release[1]); send_byte(ready[1]); + bool done = receive_byte(release[0], 3000); _exit(done ? 0 : 2); + } + close(ready[1]); close(release[0]); + bool live = receive_byte(ready[0], 1500); + delete lock; + bool released_while_child_alive = live && available(path); + finish(child, release[1], live); close(ready[0]); close(release[1]); + check(released_while_child_alive, "parent destructor must release while fork child retains FD"); +} +static void child_destructor_preserves_parent(const std::string &path) { + int ready[2], release[2]; check(pipe(ready) == 0 && pipe(release) == 0, "pipes"); + auto lock = new FileLock(path); + pid_t child = fork(); check(child >= 0, "fork"); + if (child == 0) { + close(ready[0]); close(release[1]); delete lock; send_byte(ready[1]); + bool done = receive_byte(release[0], 3000); _exit(done ? 0 : 2); + } + close(ready[1]); close(release[0]); + bool live = receive_byte(ready[0], 1500); + bool exclusive = live && !available(path); + delete lock; + bool released = available(path); + finish(child, release[1], live); close(ready[0]); close(release[1]); + check(exclusive && released, "child destructor must not unlock acquiring parent"); +} +static int exec_probe(const std::string &path) { + struct stat target{}; if (stat(path.c_str(), &target) != 0) return 2; + for (const auto &entry : std::filesystem::directory_iterator("/proc/self/fd")) { + struct stat st{}; + if (stat(entry.path().c_str(), &st) == 0 && st.st_dev == target.st_dev && st.st_ino == target.st_ino) return 3; + } + return 0; +} +static void no_exec_leak(const std::string &path) { + FileLock lock(path); + pid_t child = fork(); check(child >= 0, "fork"); + if (child == 0) { + execl("/proc/self/exe", "test_params_fork_lock", "exec-probe", path.c_str(), nullptr); + _exit(127); + } + int status = 0; check(waitpid(child, &status, 0) == child, "reap exec"); + check(WIFEXITED(status) && WEXITSTATUS(status) == 0, "no lock descriptor inherited across exec"); + check(!available(path), "parent remains exclusive after child exec"); +} +static void competing_writer(const std::string &path) { + int ready[2]; check(pipe(ready) == 0, "pipe"); + auto lock = new FileLock(path); + pid_t child = fork(); check(child >= 0, "fork"); + if (child == 0) { + close(ready[0]); + { FileLock competitor(path); send_byte(ready[1]); } + _exit(0); + } + close(ready[1]); + bool premature = receive_byte(ready[0], 150); + delete lock; + bool progressed = receive_byte(ready[0], 1500); + if (!progressed) kill(child, SIGKILL); + int status = 0; check(waitpid(child, &status, 0) == child, "reap competitor"); close(ready[0]); + check(!premature && progressed && WIFEXITED(status) && WEXITSTATUS(status) == 0, + "competitor excluded during parent scope and proceeds after owning unlock"); +} +static void failures_and_eintr(const std::string &path) { + int before = unlocks.load(); + fail_ex = EIO; + { FileLock failed(path); } + check(unlocks == before, "failed lock must not issue unlock"); + { FileLock failed(path + "/absent/lock"); } + check(unlocks == before, "failed open must not issue unlock"); + fail_ex = EINTR; + { FileLock retried(path); check(!available(path), "EINTR acquisition retries"); } + check(available(path), "retried lock released"); +} +static void params_semantics(const std::string &root) { + Params params(root); + check(!params.allKeys().empty(), "actual product registry is available"); + check(params.remove("LastManagerExitReason") == 0 || errno == ENOENT, "remove fixture value"); + std::string got; + std::thread reader([&] { got = params.get("LastManagerExitReason", true); }); + std::this_thread::sleep_for(std::chrono::milliseconds(30)); + check(params.put("LastManagerExitReason", "blocking-read-write") == 0, "blocking put"); + reader.join(); check(got == "blocking-read-write", "actual blocking get/put"); + const std::vector keys{"GitCommit", "GitBranch", "GitRemote", "Version"}; + std::vector writers; + std::atomic running{int(keys.size())}; + for (size_t k = 0; k < keys.size(); ++k) writers.emplace_back([&, k] { + { + Params queued(root); + for (int n = 0; n < 30; ++n) queued.putNonBlocking(keys[k], std::string(4096, char('A' + k)) + std::to_string(n)); + // Actual Params destructor drains the nonblocking queue. + } + --running; + }); + bool complete_reads = true; + while (running > 0) { + auto live_batch = params.readAll(); + for (size_t k = 0; k < keys.size(); ++k) { + auto item = live_batch.find(keys[k]); + if (item != live_batch.end()) { + const auto &value = item->second; + complete_reads &= (value.size() == 4097 || value.size() == 4098) && + value.substr(0, 4096) == std::string(4096, char('A' + k)); + } + } + } + for (auto &writer : writers) writer.join(); + check(complete_reads, "concurrent readAll never observes a partial atomic write"); + auto batch = params.readAll(); + for (size_t k = 0; k < keys.size(); ++k) + check(batch.at(keys[k]) == std::string(4096, char('A' + k)) + "29", "queued multiwriter drain and batch visibility"); + check(params.putBool("IsOffroad", true) == 0 && params.getBool("IsOffroad"), "actual bool put/get"); + params.clearAll(ParamKeyFlag::CLEAR_ON_MANAGER_START); + check(params.get("IsOffroad").empty(), "actual clearAll under lock"); +} +int main(int argc, char **argv) { + if (argc == 3 && std::string(argv[1]) == "exec-probe") return exec_probe(argv[2]); + if (argc != 3) { std::cerr << "CASE ABS_PRIVATE_TEMP_DIRECTORY\n"; return 2; } + std::string name = argv[1], root = argv[2]; + if (root.empty() || root[0] != '/') return 2; + std::filesystem::create_directories(root); + std::string lock = root + "/fixture.lock"; + try { + if (name == "inherited_release") inherited_release(lock); + else if (name == "child_destructor") child_destructor_preserves_parent(lock); + else if (name == "exec") no_exec_leak(lock); + else if (name == "competitor") competing_writer(lock); + else if (name == "failures_eintr") failures_and_eintr(lock); + else if (name == "params_semantics") params_semantics(root + "/params"); + else return 2; + std::cout << "PASS " << name << '\n'; return 0; + } catch (const std::exception &e) { + std::cerr << "FAIL " << name << ": " << e.what() << '\n'; return 1; + } +} diff --git a/openpilot/common/tests/test_params_fork_lock.py b/openpilot/common/tests/test_params_fork_lock.py new file mode 100644 index 0000000000..972d776016 --- /dev/null +++ b/openpilot/common/tests/test_params_fork_lock.py @@ -0,0 +1,38 @@ +"""Exercise native inherited Params locks on the Linux host CI filesystem.""" +from pathlib import Path +import subprocess +import sys +import tempfile +import unittest + +ROOT = Path(__file__).resolve().parents[3] +BINARY = ROOT / "openpilot/common/tests/bin/test_params_fork_lock" + + +@unittest.skipUnless(sys.platform == "linux", "Linux fork/exec and /proc descriptor contract") +class TestParamsForkLock(unittest.TestCase): + def run_case(self, case): + self.assertTrue(BINARY.is_file(), "Build the native regression target with existing SCons host dependencies") + # Kernel-local /tmp is required: remote/macOS bind mounts can implement flock differently. + with tempfile.TemporaryDirectory(prefix="params-fork-lock-", dir="/tmp") as directory: + result = subprocess.run([str(BINARY), case, directory], cwd=ROOT, + capture_output=True, text=True, timeout=15, check=False) + self.assertEqual(result.returncode, 0, result.stdout + result.stderr) + + def test_parent_releases_with_live_fork_child(self): + self.run_case("inherited_release") + + def test_child_destructor_keeps_parent_exclusive(self): + self.run_case("child_destructor") + + def test_exec_closes_lock_descriptor(self): + self.run_case("exec") + + def test_competing_writer_exclusion_and_progress(self): + self.run_case("competitor") + + def test_failed_lock_and_eintr_retry(self): + self.run_case("failures_eintr") + + def test_real_params_async_batch_and_blocking_io(self): + self.run_case("params_semantics") diff --git a/openpilot/selfdrive/car/card.py b/openpilot/selfdrive/car/card.py index c0c96ec2a2..5f0f4a2b99 100755 --- a/openpilot/selfdrive/car/card.py +++ b/openpilot/selfdrive/car/card.py @@ -823,6 +823,9 @@ class Car: from openpilot.starpilot.aol.transport_pause import AVAILABILITY_EVENTS events = [event for event in events if not (event.name in AVAILABILITY_EVENTS and event.softDisable and not event.immediateDisable)] + if getattr(self.aol_transport_feedback, 'temporary_host_qualified', False): + events = [event for event in events if not (event.name == log.OnroadEvent.EventName.locationdTemporaryError and + event.softDisable and not event.immediateDisable)] return intent_disarming_fault(self.CP, events, CS, temporary_ui_process_failure=self.aol_process_fault_context.temporary_ui_failure(event_ns, now_ns), temporary_selfdrive_lagging=(qualified_gm(self.CP) or aol_policy_for(self.CP).intent_supported), diff --git a/openpilot/selfdrive/locationd/models/generated/car.cpp b/openpilot/selfdrive/locationd/models/generated/car.cpp index 696830b496..8892902ee3 100644 --- a/openpilot/selfdrive/locationd/models/generated/car.cpp +++ b/openpilot/selfdrive/locationd/models/generated/car.cpp @@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227; * * * This file is part of 'ekf' * ******************************************************************************/ -void err_fun(double *nom_x, double *delta_x, double *out_4635167675190451527) { - out_4635167675190451527[0] = delta_x[0] + nom_x[0]; - out_4635167675190451527[1] = delta_x[1] + nom_x[1]; - out_4635167675190451527[2] = delta_x[2] + nom_x[2]; - out_4635167675190451527[3] = delta_x[3] + nom_x[3]; - out_4635167675190451527[4] = delta_x[4] + nom_x[4]; - out_4635167675190451527[5] = delta_x[5] + nom_x[5]; - out_4635167675190451527[6] = delta_x[6] + nom_x[6]; - out_4635167675190451527[7] = delta_x[7] + nom_x[7]; - out_4635167675190451527[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_8385818388315926896) { + out_8385818388315926896[0] = delta_x[0] + nom_x[0]; + out_8385818388315926896[1] = delta_x[1] + nom_x[1]; + out_8385818388315926896[2] = delta_x[2] + nom_x[2]; + out_8385818388315926896[3] = delta_x[3] + nom_x[3]; + out_8385818388315926896[4] = delta_x[4] + nom_x[4]; + out_8385818388315926896[5] = delta_x[5] + nom_x[5]; + out_8385818388315926896[6] = delta_x[6] + nom_x[6]; + out_8385818388315926896[7] = delta_x[7] + nom_x[7]; + out_8385818388315926896[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_3517853178278344902) { - out_3517853178278344902[0] = -nom_x[0] + true_x[0]; - out_3517853178278344902[1] = -nom_x[1] + true_x[1]; - out_3517853178278344902[2] = -nom_x[2] + true_x[2]; - out_3517853178278344902[3] = -nom_x[3] + true_x[3]; - out_3517853178278344902[4] = -nom_x[4] + true_x[4]; - out_3517853178278344902[5] = -nom_x[5] + true_x[5]; - out_3517853178278344902[6] = -nom_x[6] + true_x[6]; - out_3517853178278344902[7] = -nom_x[7] + true_x[7]; - out_3517853178278344902[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_3457977650484525251) { + out_3457977650484525251[0] = -nom_x[0] + true_x[0]; + out_3457977650484525251[1] = -nom_x[1] + true_x[1]; + out_3457977650484525251[2] = -nom_x[2] + true_x[2]; + out_3457977650484525251[3] = -nom_x[3] + true_x[3]; + out_3457977650484525251[4] = -nom_x[4] + true_x[4]; + out_3457977650484525251[5] = -nom_x[5] + true_x[5]; + out_3457977650484525251[6] = -nom_x[6] + true_x[6]; + out_3457977650484525251[7] = -nom_x[7] + true_x[7]; + out_3457977650484525251[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_602949557029320551) { - out_602949557029320551[0] = 1.0; - out_602949557029320551[1] = 0.0; - out_602949557029320551[2] = 0.0; - out_602949557029320551[3] = 0.0; - out_602949557029320551[4] = 0.0; - out_602949557029320551[5] = 0.0; - out_602949557029320551[6] = 0.0; - out_602949557029320551[7] = 0.0; - out_602949557029320551[8] = 0.0; - out_602949557029320551[9] = 0.0; - out_602949557029320551[10] = 1.0; - out_602949557029320551[11] = 0.0; - out_602949557029320551[12] = 0.0; - out_602949557029320551[13] = 0.0; - out_602949557029320551[14] = 0.0; - out_602949557029320551[15] = 0.0; - out_602949557029320551[16] = 0.0; - out_602949557029320551[17] = 0.0; - out_602949557029320551[18] = 0.0; - out_602949557029320551[19] = 0.0; - out_602949557029320551[20] = 1.0; - out_602949557029320551[21] = 0.0; - out_602949557029320551[22] = 0.0; - out_602949557029320551[23] = 0.0; - out_602949557029320551[24] = 0.0; - out_602949557029320551[25] = 0.0; - out_602949557029320551[26] = 0.0; - out_602949557029320551[27] = 0.0; - out_602949557029320551[28] = 0.0; - out_602949557029320551[29] = 0.0; - out_602949557029320551[30] = 1.0; - out_602949557029320551[31] = 0.0; - out_602949557029320551[32] = 0.0; - out_602949557029320551[33] = 0.0; - out_602949557029320551[34] = 0.0; - out_602949557029320551[35] = 0.0; - out_602949557029320551[36] = 0.0; - out_602949557029320551[37] = 0.0; - out_602949557029320551[38] = 0.0; - out_602949557029320551[39] = 0.0; - out_602949557029320551[40] = 1.0; - out_602949557029320551[41] = 0.0; - out_602949557029320551[42] = 0.0; - out_602949557029320551[43] = 0.0; - out_602949557029320551[44] = 0.0; - out_602949557029320551[45] = 0.0; - out_602949557029320551[46] = 0.0; - out_602949557029320551[47] = 0.0; - out_602949557029320551[48] = 0.0; - out_602949557029320551[49] = 0.0; - out_602949557029320551[50] = 1.0; - out_602949557029320551[51] = 0.0; - out_602949557029320551[52] = 0.0; - out_602949557029320551[53] = 0.0; - out_602949557029320551[54] = 0.0; - out_602949557029320551[55] = 0.0; - out_602949557029320551[56] = 0.0; - out_602949557029320551[57] = 0.0; - out_602949557029320551[58] = 0.0; - out_602949557029320551[59] = 0.0; - out_602949557029320551[60] = 1.0; - out_602949557029320551[61] = 0.0; - out_602949557029320551[62] = 0.0; - out_602949557029320551[63] = 0.0; - out_602949557029320551[64] = 0.0; - out_602949557029320551[65] = 0.0; - out_602949557029320551[66] = 0.0; - out_602949557029320551[67] = 0.0; - out_602949557029320551[68] = 0.0; - out_602949557029320551[69] = 0.0; - out_602949557029320551[70] = 1.0; - out_602949557029320551[71] = 0.0; - out_602949557029320551[72] = 0.0; - out_602949557029320551[73] = 0.0; - out_602949557029320551[74] = 0.0; - out_602949557029320551[75] = 0.0; - out_602949557029320551[76] = 0.0; - out_602949557029320551[77] = 0.0; - out_602949557029320551[78] = 0.0; - out_602949557029320551[79] = 0.0; - out_602949557029320551[80] = 1.0; +void H_mod_fun(double *state, double *out_1868311171103980123) { + out_1868311171103980123[0] = 1.0; + out_1868311171103980123[1] = 0.0; + out_1868311171103980123[2] = 0.0; + out_1868311171103980123[3] = 0.0; + out_1868311171103980123[4] = 0.0; + out_1868311171103980123[5] = 0.0; + out_1868311171103980123[6] = 0.0; + out_1868311171103980123[7] = 0.0; + out_1868311171103980123[8] = 0.0; + out_1868311171103980123[9] = 0.0; + out_1868311171103980123[10] = 1.0; + out_1868311171103980123[11] = 0.0; + out_1868311171103980123[12] = 0.0; + out_1868311171103980123[13] = 0.0; + out_1868311171103980123[14] = 0.0; + out_1868311171103980123[15] = 0.0; + out_1868311171103980123[16] = 0.0; + out_1868311171103980123[17] = 0.0; + out_1868311171103980123[18] = 0.0; + out_1868311171103980123[19] = 0.0; + out_1868311171103980123[20] = 1.0; + out_1868311171103980123[21] = 0.0; + out_1868311171103980123[22] = 0.0; + out_1868311171103980123[23] = 0.0; + out_1868311171103980123[24] = 0.0; + out_1868311171103980123[25] = 0.0; + out_1868311171103980123[26] = 0.0; + out_1868311171103980123[27] = 0.0; + out_1868311171103980123[28] = 0.0; + out_1868311171103980123[29] = 0.0; + out_1868311171103980123[30] = 1.0; + out_1868311171103980123[31] = 0.0; + out_1868311171103980123[32] = 0.0; + out_1868311171103980123[33] = 0.0; + out_1868311171103980123[34] = 0.0; + out_1868311171103980123[35] = 0.0; + out_1868311171103980123[36] = 0.0; + out_1868311171103980123[37] = 0.0; + out_1868311171103980123[38] = 0.0; + out_1868311171103980123[39] = 0.0; + out_1868311171103980123[40] = 1.0; + out_1868311171103980123[41] = 0.0; + out_1868311171103980123[42] = 0.0; + out_1868311171103980123[43] = 0.0; + out_1868311171103980123[44] = 0.0; + out_1868311171103980123[45] = 0.0; + out_1868311171103980123[46] = 0.0; + out_1868311171103980123[47] = 0.0; + out_1868311171103980123[48] = 0.0; + out_1868311171103980123[49] = 0.0; + out_1868311171103980123[50] = 1.0; + out_1868311171103980123[51] = 0.0; + out_1868311171103980123[52] = 0.0; + out_1868311171103980123[53] = 0.0; + out_1868311171103980123[54] = 0.0; + out_1868311171103980123[55] = 0.0; + out_1868311171103980123[56] = 0.0; + out_1868311171103980123[57] = 0.0; + out_1868311171103980123[58] = 0.0; + out_1868311171103980123[59] = 0.0; + out_1868311171103980123[60] = 1.0; + out_1868311171103980123[61] = 0.0; + out_1868311171103980123[62] = 0.0; + out_1868311171103980123[63] = 0.0; + out_1868311171103980123[64] = 0.0; + out_1868311171103980123[65] = 0.0; + out_1868311171103980123[66] = 0.0; + out_1868311171103980123[67] = 0.0; + out_1868311171103980123[68] = 0.0; + out_1868311171103980123[69] = 0.0; + out_1868311171103980123[70] = 1.0; + out_1868311171103980123[71] = 0.0; + out_1868311171103980123[72] = 0.0; + out_1868311171103980123[73] = 0.0; + out_1868311171103980123[74] = 0.0; + out_1868311171103980123[75] = 0.0; + out_1868311171103980123[76] = 0.0; + out_1868311171103980123[77] = 0.0; + out_1868311171103980123[78] = 0.0; + out_1868311171103980123[79] = 0.0; + out_1868311171103980123[80] = 1.0; } -void f_fun(double *state, double dt, double *out_113004381451582801) { - out_113004381451582801[0] = state[0]; - out_113004381451582801[1] = state[1]; - out_113004381451582801[2] = state[2]; - out_113004381451582801[3] = state[3]; - out_113004381451582801[4] = state[4]; - out_113004381451582801[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_113004381451582801[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_113004381451582801[7] = state[7]; - out_113004381451582801[8] = state[8]; +void f_fun(double *state, double dt, double *out_1499231844596104733) { + out_1499231844596104733[0] = state[0]; + out_1499231844596104733[1] = state[1]; + out_1499231844596104733[2] = state[2]; + out_1499231844596104733[3] = state[3]; + out_1499231844596104733[4] = state[4]; + out_1499231844596104733[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_1499231844596104733[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_1499231844596104733[7] = state[7]; + out_1499231844596104733[8] = state[8]; } -void F_fun(double *state, double dt, double *out_696983711093396292) { - out_696983711093396292[0] = 1; - out_696983711093396292[1] = 0; - out_696983711093396292[2] = 0; - out_696983711093396292[3] = 0; - out_696983711093396292[4] = 0; - out_696983711093396292[5] = 0; - out_696983711093396292[6] = 0; - out_696983711093396292[7] = 0; - out_696983711093396292[8] = 0; - out_696983711093396292[9] = 0; - out_696983711093396292[10] = 1; - out_696983711093396292[11] = 0; - out_696983711093396292[12] = 0; - out_696983711093396292[13] = 0; - out_696983711093396292[14] = 0; - out_696983711093396292[15] = 0; - out_696983711093396292[16] = 0; - out_696983711093396292[17] = 0; - out_696983711093396292[18] = 0; - out_696983711093396292[19] = 0; - out_696983711093396292[20] = 1; - out_696983711093396292[21] = 0; - out_696983711093396292[22] = 0; - out_696983711093396292[23] = 0; - out_696983711093396292[24] = 0; - out_696983711093396292[25] = 0; - out_696983711093396292[26] = 0; - out_696983711093396292[27] = 0; - out_696983711093396292[28] = 0; - out_696983711093396292[29] = 0; - out_696983711093396292[30] = 1; - out_696983711093396292[31] = 0; - out_696983711093396292[32] = 0; - out_696983711093396292[33] = 0; - out_696983711093396292[34] = 0; - out_696983711093396292[35] = 0; - out_696983711093396292[36] = 0; - out_696983711093396292[37] = 0; - out_696983711093396292[38] = 0; - out_696983711093396292[39] = 0; - out_696983711093396292[40] = 1; - out_696983711093396292[41] = 0; - out_696983711093396292[42] = 0; - out_696983711093396292[43] = 0; - out_696983711093396292[44] = 0; - out_696983711093396292[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_696983711093396292[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_696983711093396292[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_696983711093396292[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_696983711093396292[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_696983711093396292[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_696983711093396292[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_696983711093396292[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_696983711093396292[53] = -9.8100000000000005*dt; - out_696983711093396292[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_696983711093396292[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_696983711093396292[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_696983711093396292[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_696983711093396292[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_696983711093396292[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_696983711093396292[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_696983711093396292[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_696983711093396292[62] = 0; - out_696983711093396292[63] = 0; - out_696983711093396292[64] = 0; - out_696983711093396292[65] = 0; - out_696983711093396292[66] = 0; - out_696983711093396292[67] = 0; - out_696983711093396292[68] = 0; - out_696983711093396292[69] = 0; - out_696983711093396292[70] = 1; - out_696983711093396292[71] = 0; - out_696983711093396292[72] = 0; - out_696983711093396292[73] = 0; - out_696983711093396292[74] = 0; - out_696983711093396292[75] = 0; - out_696983711093396292[76] = 0; - out_696983711093396292[77] = 0; - out_696983711093396292[78] = 0; - out_696983711093396292[79] = 0; - out_696983711093396292[80] = 1; +void F_fun(double *state, double dt, double *out_7736119846340090296) { + out_7736119846340090296[0] = 1; + out_7736119846340090296[1] = 0; + out_7736119846340090296[2] = 0; + out_7736119846340090296[3] = 0; + out_7736119846340090296[4] = 0; + out_7736119846340090296[5] = 0; + out_7736119846340090296[6] = 0; + out_7736119846340090296[7] = 0; + out_7736119846340090296[8] = 0; + out_7736119846340090296[9] = 0; + out_7736119846340090296[10] = 1; + out_7736119846340090296[11] = 0; + out_7736119846340090296[12] = 0; + out_7736119846340090296[13] = 0; + out_7736119846340090296[14] = 0; + out_7736119846340090296[15] = 0; + out_7736119846340090296[16] = 0; + out_7736119846340090296[17] = 0; + out_7736119846340090296[18] = 0; + out_7736119846340090296[19] = 0; + out_7736119846340090296[20] = 1; + out_7736119846340090296[21] = 0; + out_7736119846340090296[22] = 0; + out_7736119846340090296[23] = 0; + out_7736119846340090296[24] = 0; + out_7736119846340090296[25] = 0; + out_7736119846340090296[26] = 0; + out_7736119846340090296[27] = 0; + out_7736119846340090296[28] = 0; + out_7736119846340090296[29] = 0; + out_7736119846340090296[30] = 1; + out_7736119846340090296[31] = 0; + out_7736119846340090296[32] = 0; + out_7736119846340090296[33] = 0; + out_7736119846340090296[34] = 0; + out_7736119846340090296[35] = 0; + out_7736119846340090296[36] = 0; + out_7736119846340090296[37] = 0; + out_7736119846340090296[38] = 0; + out_7736119846340090296[39] = 0; + out_7736119846340090296[40] = 1; + out_7736119846340090296[41] = 0; + out_7736119846340090296[42] = 0; + out_7736119846340090296[43] = 0; + out_7736119846340090296[44] = 0; + out_7736119846340090296[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_7736119846340090296[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_7736119846340090296[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_7736119846340090296[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_7736119846340090296[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_7736119846340090296[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_7736119846340090296[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_7736119846340090296[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_7736119846340090296[53] = -9.8100000000000005*dt; + out_7736119846340090296[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_7736119846340090296[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_7736119846340090296[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_7736119846340090296[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_7736119846340090296[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_7736119846340090296[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_7736119846340090296[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_7736119846340090296[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_7736119846340090296[62] = 0; + out_7736119846340090296[63] = 0; + out_7736119846340090296[64] = 0; + out_7736119846340090296[65] = 0; + out_7736119846340090296[66] = 0; + out_7736119846340090296[67] = 0; + out_7736119846340090296[68] = 0; + out_7736119846340090296[69] = 0; + out_7736119846340090296[70] = 1; + out_7736119846340090296[71] = 0; + out_7736119846340090296[72] = 0; + out_7736119846340090296[73] = 0; + out_7736119846340090296[74] = 0; + out_7736119846340090296[75] = 0; + out_7736119846340090296[76] = 0; + out_7736119846340090296[77] = 0; + out_7736119846340090296[78] = 0; + out_7736119846340090296[79] = 0; + out_7736119846340090296[80] = 1; } -void h_25(double *state, double *unused, double *out_8706394861897294349) { - out_8706394861897294349[0] = state[6]; +void h_25(double *state, double *unused, double *out_5130465691248679820) { + out_5130465691248679820[0] = state[6]; } -void H_25(double *state, double *unused, double *out_4300742268155344289) { - out_4300742268155344289[0] = 0; - out_4300742268155344289[1] = 0; - out_4300742268155344289[2] = 0; - out_4300742268155344289[3] = 0; - out_4300742268155344289[4] = 0; - out_4300742268155344289[5] = 0; - out_4300742268155344289[6] = 1; - out_4300742268155344289[7] = 0; - out_4300742268155344289[8] = 0; +void H_25(double *state, double *unused, double *out_993002426764977143) { + out_993002426764977143[0] = 0; + out_993002426764977143[1] = 0; + out_993002426764977143[2] = 0; + out_993002426764977143[3] = 0; + out_993002426764977143[4] = 0; + out_993002426764977143[5] = 0; + out_993002426764977143[6] = 1; + out_993002426764977143[7] = 0; + out_993002426764977143[8] = 0; } -void h_24(double *state, double *unused, double *out_2168250296672775669) { - out_2168250296672775669[0] = state[4]; - out_2168250296672775669[1] = state[5]; +void h_24(double *state, double *unused, double *out_3830047811733214319) { + out_3830047811733214319[0] = state[4]; + out_3830047811733214319[1] = state[5]; } -void H_24(double *state, double *unused, double *out_4474150139656826605) { - out_4474150139656826605[0] = 0; - out_4474150139656826605[1] = 0; - out_4474150139656826605[2] = 0; - out_4474150139656826605[3] = 0; - out_4474150139656826605[4] = 1; - out_4474150139656826605[5] = 0; - out_4474150139656826605[6] = 0; - out_4474150139656826605[7] = 0; - out_4474150139656826605[8] = 0; - out_4474150139656826605[9] = 0; - out_4474150139656826605[10] = 0; - out_4474150139656826605[11] = 0; - out_4474150139656826605[12] = 0; - out_4474150139656826605[13] = 0; - out_4474150139656826605[14] = 1; - out_4474150139656826605[15] = 0; - out_4474150139656826605[16] = 0; - out_4474150139656826605[17] = 0; +void H_24(double *state, double *unused, double *out_5739511753731486177) { + out_5739511753731486177[0] = 0; + out_5739511753731486177[1] = 0; + out_5739511753731486177[2] = 0; + out_5739511753731486177[3] = 0; + out_5739511753731486177[4] = 1; + out_5739511753731486177[5] = 0; + out_5739511753731486177[6] = 0; + out_5739511753731486177[7] = 0; + out_5739511753731486177[8] = 0; + out_5739511753731486177[9] = 0; + out_5739511753731486177[10] = 0; + out_5739511753731486177[11] = 0; + out_5739511753731486177[12] = 0; + out_5739511753731486177[13] = 0; + out_5739511753731486177[14] = 1; + out_5739511753731486177[15] = 0; + out_5739511753731486177[16] = 0; + out_5739511753731486177[17] = 0; } -void h_30(double *state, double *unused, double *out_3180337425346271236) { - out_3180337425346271236[0] = state[4]; +void h_30(double *state, double *unused, double *out_4178561097777020614) { + out_4178561097777020614[0] = state[4]; } -void H_30(double *state, double *unused, double *out_7229311464062590572) { - out_7229311464062590572[0] = 0; - out_7229311464062590572[1] = 0; - out_7229311464062590572[2] = 0; - out_7229311464062590572[3] = 0; - out_7229311464062590572[4] = 1; - out_7229311464062590572[5] = 0; - out_7229311464062590572[6] = 0; - out_7229311464062590572[7] = 0; - out_7229311464062590572[8] = 0; +void H_30(double *state, double *unused, double *out_1525330531742271484) { + out_1525330531742271484[0] = 0; + out_1525330531742271484[1] = 0; + out_1525330531742271484[2] = 0; + out_1525330531742271484[3] = 0; + out_1525330531742271484[4] = 1; + out_1525330531742271484[5] = 0; + out_1525330531742271484[6] = 0; + out_1525330531742271484[7] = 0; + out_1525330531742271484[8] = 0; } -void h_26(double *state, double *unused, double *out_3755837170012439621) { - out_3755837170012439621[0] = state[7]; +void h_26(double *state, double *unused, double *out_4041436014126362414) { + out_4041436014126362414[0] = state[7]; } -void H_26(double *state, double *unused, double *out_559238949281288065) { - out_559238949281288065[0] = 0; - out_559238949281288065[1] = 0; - out_559238949281288065[2] = 0; - out_559238949281288065[3] = 0; - out_559238949281288065[4] = 0; - out_559238949281288065[5] = 0; - out_559238949281288065[6] = 0; - out_559238949281288065[7] = 1; - out_559238949281288065[8] = 0; +void H_26(double *state, double *unused, double *out_2311523542995823458) { + out_2311523542995823458[0] = 0; + out_2311523542995823458[1] = 0; + out_2311523542995823458[2] = 0; + out_2311523542995823458[3] = 0; + out_2311523542995823458[4] = 0; + out_2311523542995823458[5] = 0; + out_2311523542995823458[6] = 0; + out_2311523542995823458[7] = 1; + out_2311523542995823458[8] = 0; } -void h_27(double *state, double *unused, double *out_7762885008995631914) { - out_7762885008995631914[0] = state[3]; +void h_27(double *state, double *unused, double *out_128792423587834175) { + out_128792423587834175[0] = state[3]; } -void H_27(double *state, double *unused, double *out_9042669297846536133) { - out_9042669297846536133[0] = 0; - out_9042669297846536133[1] = 0; - out_9042669297846536133[2] = 0; - out_9042669297846536133[3] = 1; - out_9042669297846536133[4] = 0; - out_9042669297846536133[5] = 0; - out_9042669297846536133[6] = 0; - out_9042669297846536133[7] = 0; - out_9042669297846536133[8] = 0; +void H_27(double *state, double *unused, double *out_3748924602926214701) { + out_3748924602926214701[0] = 0; + out_3748924602926214701[1] = 0; + out_3748924602926214701[2] = 0; + out_3748924602926214701[3] = 1; + out_3748924602926214701[4] = 0; + out_3748924602926214701[5] = 0; + out_3748924602926214701[6] = 0; + out_3748924602926214701[7] = 0; + out_3748924602926214701[8] = 0; } -void h_29(double *state, double *unused, double *out_6312253616366748747) { - out_6312253616366748747[0] = state[1]; +void h_29(double *state, double *unused, double *out_1579423816216717342) { + out_1579423816216717342[0] = state[1]; } -void H_29(double *state, double *unused, double *out_7329306570976985100) { - out_7329306570976985100[0] = 0; - out_7329306570976985100[1] = 1; - out_7329306570976985100[2] = 0; - out_7329306570976985100[3] = 0; - out_7329306570976985100[4] = 0; - out_7329306570976985100[5] = 0; - out_7329306570976985100[6] = 0; - out_7329306570976985100[7] = 0; - out_7329306570976985100[8] = 0; +void H_29(double *state, double *unused, double *out_2035561876056663668) { + out_2035561876056663668[0] = 0; + out_2035561876056663668[1] = 1; + out_2035561876056663668[2] = 0; + out_2035561876056663668[3] = 0; + out_2035561876056663668[4] = 0; + out_2035561876056663668[5] = 0; + out_2035561876056663668[6] = 0; + out_2035561876056663668[7] = 0; + out_2035561876056663668[8] = 0; } -void h_28(double *state, double *unused, double *out_2004900095001893958) { - out_2004900095001893958[0] = state[0]; +void h_28(double *state, double *unused, double *out_7563935858016534988) { + out_7563935858016534988[0] = state[0]; } -void H_28(double *state, double *unused, double *out_2246907553907454526) { - out_2246907553907454526[0] = 1; - out_2246907553907454526[1] = 0; - out_2246907553907454526[2] = 0; - out_2246907553907454526[3] = 0; - out_2246907553907454526[4] = 0; - out_2246907553907454526[5] = 0; - out_2246907553907454526[6] = 0; - out_2246907553907454526[7] = 0; - out_2246907553907454526[8] = 0; +void H_28(double *state, double *unused, double *out_3046837141012866906) { + out_3046837141012866906[0] = 1; + out_3046837141012866906[1] = 0; + out_3046837141012866906[2] = 0; + out_3046837141012866906[3] = 0; + out_3046837141012866906[4] = 0; + out_3046837141012866906[5] = 0; + out_3046837141012866906[6] = 0; + out_3046837141012866906[7] = 0; + out_3046837141012866906[8] = 0; } -void h_31(double *state, double *unused, double *out_1100471735093715383) { - out_1100471735093715383[0] = state[8]; +void h_31(double *state, double *unused, double *out_8001670333771886589) { + out_8001670333771886589[0] = state[8]; } -void H_31(double *state, double *unused, double *out_4331388230032304717) { - out_4331388230032304717[0] = 0; - out_4331388230032304717[1] = 0; - out_4331388230032304717[2] = 0; - out_4331388230032304717[3] = 0; - out_4331388230032304717[4] = 0; - out_4331388230032304717[5] = 0; - out_4331388230032304717[6] = 0; - out_4331388230032304717[7] = 0; - out_4331388230032304717[8] = 1; +void H_31(double *state, double *unused, double *out_1685315440762471982) { + out_1685315440762471982[0] = 0; + out_1685315440762471982[1] = 0; + out_1685315440762471982[2] = 0; + out_1685315440762471982[3] = 0; + out_1685315440762471982[4] = 0; + out_1685315440762471982[5] = 0; + out_1685315440762471982[6] = 0; + out_1685315440762471982[7] = 0; + out_1685315440762471982[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_4635167675190451527) { - err_fun(nom_x, delta_x, out_4635167675190451527); +void car_err_fun(double *nom_x, double *delta_x, double *out_8385818388315926896) { + err_fun(nom_x, delta_x, out_8385818388315926896); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3517853178278344902) { - inv_err_fun(nom_x, true_x, out_3517853178278344902); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3457977650484525251) { + inv_err_fun(nom_x, true_x, out_3457977650484525251); } -void car_H_mod_fun(double *state, double *out_602949557029320551) { - H_mod_fun(state, out_602949557029320551); +void car_H_mod_fun(double *state, double *out_1868311171103980123) { + H_mod_fun(state, out_1868311171103980123); } -void car_f_fun(double *state, double dt, double *out_113004381451582801) { - f_fun(state, dt, out_113004381451582801); +void car_f_fun(double *state, double dt, double *out_1499231844596104733) { + f_fun(state, dt, out_1499231844596104733); } -void car_F_fun(double *state, double dt, double *out_696983711093396292) { - F_fun(state, dt, out_696983711093396292); +void car_F_fun(double *state, double dt, double *out_7736119846340090296) { + F_fun(state, dt, out_7736119846340090296); } -void car_h_25(double *state, double *unused, double *out_8706394861897294349) { - h_25(state, unused, out_8706394861897294349); +void car_h_25(double *state, double *unused, double *out_5130465691248679820) { + h_25(state, unused, out_5130465691248679820); } -void car_H_25(double *state, double *unused, double *out_4300742268155344289) { - H_25(state, unused, out_4300742268155344289); +void car_H_25(double *state, double *unused, double *out_993002426764977143) { + H_25(state, unused, out_993002426764977143); } -void car_h_24(double *state, double *unused, double *out_2168250296672775669) { - h_24(state, unused, out_2168250296672775669); +void car_h_24(double *state, double *unused, double *out_3830047811733214319) { + h_24(state, unused, out_3830047811733214319); } -void car_H_24(double *state, double *unused, double *out_4474150139656826605) { - H_24(state, unused, out_4474150139656826605); +void car_H_24(double *state, double *unused, double *out_5739511753731486177) { + H_24(state, unused, out_5739511753731486177); } -void car_h_30(double *state, double *unused, double *out_3180337425346271236) { - h_30(state, unused, out_3180337425346271236); +void car_h_30(double *state, double *unused, double *out_4178561097777020614) { + h_30(state, unused, out_4178561097777020614); } -void car_H_30(double *state, double *unused, double *out_7229311464062590572) { - H_30(state, unused, out_7229311464062590572); +void car_H_30(double *state, double *unused, double *out_1525330531742271484) { + H_30(state, unused, out_1525330531742271484); } -void car_h_26(double *state, double *unused, double *out_3755837170012439621) { - h_26(state, unused, out_3755837170012439621); +void car_h_26(double *state, double *unused, double *out_4041436014126362414) { + h_26(state, unused, out_4041436014126362414); } -void car_H_26(double *state, double *unused, double *out_559238949281288065) { - H_26(state, unused, out_559238949281288065); +void car_H_26(double *state, double *unused, double *out_2311523542995823458) { + H_26(state, unused, out_2311523542995823458); } -void car_h_27(double *state, double *unused, double *out_7762885008995631914) { - h_27(state, unused, out_7762885008995631914); +void car_h_27(double *state, double *unused, double *out_128792423587834175) { + h_27(state, unused, out_128792423587834175); } -void car_H_27(double *state, double *unused, double *out_9042669297846536133) { - H_27(state, unused, out_9042669297846536133); +void car_H_27(double *state, double *unused, double *out_3748924602926214701) { + H_27(state, unused, out_3748924602926214701); } -void car_h_29(double *state, double *unused, double *out_6312253616366748747) { - h_29(state, unused, out_6312253616366748747); +void car_h_29(double *state, double *unused, double *out_1579423816216717342) { + h_29(state, unused, out_1579423816216717342); } -void car_H_29(double *state, double *unused, double *out_7329306570976985100) { - H_29(state, unused, out_7329306570976985100); +void car_H_29(double *state, double *unused, double *out_2035561876056663668) { + H_29(state, unused, out_2035561876056663668); } -void car_h_28(double *state, double *unused, double *out_2004900095001893958) { - h_28(state, unused, out_2004900095001893958); +void car_h_28(double *state, double *unused, double *out_7563935858016534988) { + h_28(state, unused, out_7563935858016534988); } -void car_H_28(double *state, double *unused, double *out_2246907553907454526) { - H_28(state, unused, out_2246907553907454526); +void car_H_28(double *state, double *unused, double *out_3046837141012866906) { + H_28(state, unused, out_3046837141012866906); } -void car_h_31(double *state, double *unused, double *out_1100471735093715383) { - h_31(state, unused, out_1100471735093715383); +void car_h_31(double *state, double *unused, double *out_8001670333771886589) { + h_31(state, unused, out_8001670333771886589); } -void car_H_31(double *state, double *unused, double *out_4331388230032304717) { - H_31(state, unused, out_4331388230032304717); +void car_H_31(double *state, double *unused, double *out_1685315440762471982) { + H_31(state, unused, out_1685315440762471982); } void car_predict(double *in_x, double *in_P, double *in_Q, double dt) { predict(in_x, in_P, in_Q, dt); diff --git a/openpilot/selfdrive/locationd/models/generated/car.h b/openpilot/selfdrive/locationd/models/generated/car.h index 6ca1495ce2..93607a6318 100644 --- a/openpilot/selfdrive/locationd/models/generated/car.h +++ b/openpilot/selfdrive/locationd/models/generated/car.h @@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); -void car_err_fun(double *nom_x, double *delta_x, double *out_4635167675190451527); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_3517853178278344902); -void car_H_mod_fun(double *state, double *out_602949557029320551); -void car_f_fun(double *state, double dt, double *out_113004381451582801); -void car_F_fun(double *state, double dt, double *out_696983711093396292); -void car_h_25(double *state, double *unused, double *out_8706394861897294349); -void car_H_25(double *state, double *unused, double *out_4300742268155344289); -void car_h_24(double *state, double *unused, double *out_2168250296672775669); -void car_H_24(double *state, double *unused, double *out_4474150139656826605); -void car_h_30(double *state, double *unused, double *out_3180337425346271236); -void car_H_30(double *state, double *unused, double *out_7229311464062590572); -void car_h_26(double *state, double *unused, double *out_3755837170012439621); -void car_H_26(double *state, double *unused, double *out_559238949281288065); -void car_h_27(double *state, double *unused, double *out_7762885008995631914); -void car_H_27(double *state, double *unused, double *out_9042669297846536133); -void car_h_29(double *state, double *unused, double *out_6312253616366748747); -void car_H_29(double *state, double *unused, double *out_7329306570976985100); -void car_h_28(double *state, double *unused, double *out_2004900095001893958); -void car_H_28(double *state, double *unused, double *out_2246907553907454526); -void car_h_31(double *state, double *unused, double *out_1100471735093715383); -void car_H_31(double *state, double *unused, double *out_4331388230032304717); +void car_err_fun(double *nom_x, double *delta_x, double *out_8385818388315926896); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3457977650484525251); +void car_H_mod_fun(double *state, double *out_1868311171103980123); +void car_f_fun(double *state, double dt, double *out_1499231844596104733); +void car_F_fun(double *state, double dt, double *out_7736119846340090296); +void car_h_25(double *state, double *unused, double *out_5130465691248679820); +void car_H_25(double *state, double *unused, double *out_993002426764977143); +void car_h_24(double *state, double *unused, double *out_3830047811733214319); +void car_H_24(double *state, double *unused, double *out_5739511753731486177); +void car_h_30(double *state, double *unused, double *out_4178561097777020614); +void car_H_30(double *state, double *unused, double *out_1525330531742271484); +void car_h_26(double *state, double *unused, double *out_4041436014126362414); +void car_H_26(double *state, double *unused, double *out_2311523542995823458); +void car_h_27(double *state, double *unused, double *out_128792423587834175); +void car_H_27(double *state, double *unused, double *out_3748924602926214701); +void car_h_29(double *state, double *unused, double *out_1579423816216717342); +void car_H_29(double *state, double *unused, double *out_2035561876056663668); +void car_h_28(double *state, double *unused, double *out_7563935858016534988); +void car_H_28(double *state, double *unused, double *out_3046837141012866906); +void car_h_31(double *state, double *unused, double *out_8001670333771886589); +void car_H_31(double *state, double *unused, double *out_1685315440762471982); void car_predict(double *in_x, double *in_P, double *in_Q, double dt); void car_set_mass(double x); void car_set_rotational_inertia(double x); diff --git a/openpilot/selfdrive/locationd/models/generated/libcar.so b/openpilot/selfdrive/locationd/models/generated/libcar.so index 2b48045333..c502ffcd6e 100755 Binary files a/openpilot/selfdrive/locationd/models/generated/libcar.so and b/openpilot/selfdrive/locationd/models/generated/libcar.so differ diff --git a/openpilot/selfdrive/locationd/models/generated/libpose.so b/openpilot/selfdrive/locationd/models/generated/libpose.so index 01336c98c7..2d40f589dc 100755 Binary files a/openpilot/selfdrive/locationd/models/generated/libpose.so and b/openpilot/selfdrive/locationd/models/generated/libpose.so differ diff --git a/openpilot/selfdrive/locationd/models/generated/pose.cpp b/openpilot/selfdrive/locationd/models/generated/pose.cpp index 6b2e624159..4c5e3e01e2 100644 --- a/openpilot/selfdrive/locationd/models/generated/pose.cpp +++ b/openpilot/selfdrive/locationd/models/generated/pose.cpp @@ -17,961 +17,961 @@ const static double MAHA_THRESH_14 = 7.814727903251177; * * * This file is part of 'ekf' * ******************************************************************************/ -void err_fun(double *nom_x, double *delta_x, double *out_6769094018143345471) { - out_6769094018143345471[0] = delta_x[0] + nom_x[0]; - out_6769094018143345471[1] = delta_x[1] + nom_x[1]; - out_6769094018143345471[2] = delta_x[2] + nom_x[2]; - out_6769094018143345471[3] = delta_x[3] + nom_x[3]; - out_6769094018143345471[4] = delta_x[4] + nom_x[4]; - out_6769094018143345471[5] = delta_x[5] + nom_x[5]; - out_6769094018143345471[6] = delta_x[6] + nom_x[6]; - out_6769094018143345471[7] = delta_x[7] + nom_x[7]; - out_6769094018143345471[8] = delta_x[8] + nom_x[8]; - out_6769094018143345471[9] = delta_x[9] + nom_x[9]; - out_6769094018143345471[10] = delta_x[10] + nom_x[10]; - out_6769094018143345471[11] = delta_x[11] + nom_x[11]; - out_6769094018143345471[12] = delta_x[12] + nom_x[12]; - out_6769094018143345471[13] = delta_x[13] + nom_x[13]; - out_6769094018143345471[14] = delta_x[14] + nom_x[14]; - out_6769094018143345471[15] = delta_x[15] + nom_x[15]; - out_6769094018143345471[16] = delta_x[16] + nom_x[16]; - out_6769094018143345471[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_8587424029422482613) { + out_8587424029422482613[0] = delta_x[0] + nom_x[0]; + out_8587424029422482613[1] = delta_x[1] + nom_x[1]; + out_8587424029422482613[2] = delta_x[2] + nom_x[2]; + out_8587424029422482613[3] = delta_x[3] + nom_x[3]; + out_8587424029422482613[4] = delta_x[4] + nom_x[4]; + out_8587424029422482613[5] = delta_x[5] + nom_x[5]; + out_8587424029422482613[6] = delta_x[6] + nom_x[6]; + out_8587424029422482613[7] = delta_x[7] + nom_x[7]; + out_8587424029422482613[8] = delta_x[8] + nom_x[8]; + out_8587424029422482613[9] = delta_x[9] + nom_x[9]; + out_8587424029422482613[10] = delta_x[10] + nom_x[10]; + out_8587424029422482613[11] = delta_x[11] + nom_x[11]; + out_8587424029422482613[12] = delta_x[12] + nom_x[12]; + out_8587424029422482613[13] = delta_x[13] + nom_x[13]; + out_8587424029422482613[14] = delta_x[14] + nom_x[14]; + out_8587424029422482613[15] = delta_x[15] + nom_x[15]; + out_8587424029422482613[16] = delta_x[16] + nom_x[16]; + out_8587424029422482613[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_7105507959452432978) { - out_7105507959452432978[0] = -nom_x[0] + true_x[0]; - out_7105507959452432978[1] = -nom_x[1] + true_x[1]; - out_7105507959452432978[2] = -nom_x[2] + true_x[2]; - out_7105507959452432978[3] = -nom_x[3] + true_x[3]; - out_7105507959452432978[4] = -nom_x[4] + true_x[4]; - out_7105507959452432978[5] = -nom_x[5] + true_x[5]; - out_7105507959452432978[6] = -nom_x[6] + true_x[6]; - out_7105507959452432978[7] = -nom_x[7] + true_x[7]; - out_7105507959452432978[8] = -nom_x[8] + true_x[8]; - out_7105507959452432978[9] = -nom_x[9] + true_x[9]; - out_7105507959452432978[10] = -nom_x[10] + true_x[10]; - out_7105507959452432978[11] = -nom_x[11] + true_x[11]; - out_7105507959452432978[12] = -nom_x[12] + true_x[12]; - out_7105507959452432978[13] = -nom_x[13] + true_x[13]; - out_7105507959452432978[14] = -nom_x[14] + true_x[14]; - out_7105507959452432978[15] = -nom_x[15] + true_x[15]; - out_7105507959452432978[16] = -nom_x[16] + true_x[16]; - out_7105507959452432978[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_8786163150320395038) { + out_8786163150320395038[0] = -nom_x[0] + true_x[0]; + out_8786163150320395038[1] = -nom_x[1] + true_x[1]; + out_8786163150320395038[2] = -nom_x[2] + true_x[2]; + out_8786163150320395038[3] = -nom_x[3] + true_x[3]; + out_8786163150320395038[4] = -nom_x[4] + true_x[4]; + out_8786163150320395038[5] = -nom_x[5] + true_x[5]; + out_8786163150320395038[6] = -nom_x[6] + true_x[6]; + out_8786163150320395038[7] = -nom_x[7] + true_x[7]; + out_8786163150320395038[8] = -nom_x[8] + true_x[8]; + out_8786163150320395038[9] = -nom_x[9] + true_x[9]; + out_8786163150320395038[10] = -nom_x[10] + true_x[10]; + out_8786163150320395038[11] = -nom_x[11] + true_x[11]; + out_8786163150320395038[12] = -nom_x[12] + true_x[12]; + out_8786163150320395038[13] = -nom_x[13] + true_x[13]; + out_8786163150320395038[14] = -nom_x[14] + true_x[14]; + out_8786163150320395038[15] = -nom_x[15] + true_x[15]; + out_8786163150320395038[16] = -nom_x[16] + true_x[16]; + out_8786163150320395038[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_6619985577834044559) { - out_6619985577834044559[0] = 1.0; - out_6619985577834044559[1] = 0.0; - out_6619985577834044559[2] = 0.0; - out_6619985577834044559[3] = 0.0; - out_6619985577834044559[4] = 0.0; - out_6619985577834044559[5] = 0.0; - out_6619985577834044559[6] = 0.0; - out_6619985577834044559[7] = 0.0; - out_6619985577834044559[8] = 0.0; - out_6619985577834044559[9] = 0.0; - out_6619985577834044559[10] = 0.0; - out_6619985577834044559[11] = 0.0; - out_6619985577834044559[12] = 0.0; - out_6619985577834044559[13] = 0.0; - out_6619985577834044559[14] = 0.0; - out_6619985577834044559[15] = 0.0; - out_6619985577834044559[16] = 0.0; - out_6619985577834044559[17] = 0.0; - out_6619985577834044559[18] = 0.0; - out_6619985577834044559[19] = 1.0; - out_6619985577834044559[20] = 0.0; - out_6619985577834044559[21] = 0.0; - out_6619985577834044559[22] = 0.0; - out_6619985577834044559[23] = 0.0; - out_6619985577834044559[24] = 0.0; - out_6619985577834044559[25] = 0.0; - out_6619985577834044559[26] = 0.0; - out_6619985577834044559[27] = 0.0; - out_6619985577834044559[28] = 0.0; - out_6619985577834044559[29] = 0.0; - out_6619985577834044559[30] = 0.0; - out_6619985577834044559[31] = 0.0; - out_6619985577834044559[32] = 0.0; - out_6619985577834044559[33] = 0.0; - out_6619985577834044559[34] = 0.0; - out_6619985577834044559[35] = 0.0; - out_6619985577834044559[36] = 0.0; - out_6619985577834044559[37] = 0.0; - out_6619985577834044559[38] = 1.0; - out_6619985577834044559[39] = 0.0; - out_6619985577834044559[40] = 0.0; - out_6619985577834044559[41] = 0.0; - out_6619985577834044559[42] = 0.0; - out_6619985577834044559[43] = 0.0; - out_6619985577834044559[44] = 0.0; - out_6619985577834044559[45] = 0.0; - out_6619985577834044559[46] = 0.0; - out_6619985577834044559[47] = 0.0; - out_6619985577834044559[48] = 0.0; - out_6619985577834044559[49] = 0.0; - out_6619985577834044559[50] = 0.0; - out_6619985577834044559[51] = 0.0; - out_6619985577834044559[52] = 0.0; - out_6619985577834044559[53] = 0.0; - out_6619985577834044559[54] = 0.0; - out_6619985577834044559[55] = 0.0; - out_6619985577834044559[56] = 0.0; - out_6619985577834044559[57] = 1.0; - out_6619985577834044559[58] = 0.0; - out_6619985577834044559[59] = 0.0; - out_6619985577834044559[60] = 0.0; - out_6619985577834044559[61] = 0.0; - out_6619985577834044559[62] = 0.0; - out_6619985577834044559[63] = 0.0; - out_6619985577834044559[64] = 0.0; - out_6619985577834044559[65] = 0.0; - out_6619985577834044559[66] = 0.0; - out_6619985577834044559[67] = 0.0; - out_6619985577834044559[68] = 0.0; - out_6619985577834044559[69] = 0.0; - out_6619985577834044559[70] = 0.0; - out_6619985577834044559[71] = 0.0; - out_6619985577834044559[72] = 0.0; - out_6619985577834044559[73] = 0.0; - out_6619985577834044559[74] = 0.0; - out_6619985577834044559[75] = 0.0; - out_6619985577834044559[76] = 1.0; - out_6619985577834044559[77] = 0.0; - out_6619985577834044559[78] = 0.0; - out_6619985577834044559[79] = 0.0; - out_6619985577834044559[80] = 0.0; - out_6619985577834044559[81] = 0.0; - out_6619985577834044559[82] = 0.0; - out_6619985577834044559[83] = 0.0; - out_6619985577834044559[84] = 0.0; - out_6619985577834044559[85] = 0.0; - out_6619985577834044559[86] = 0.0; - out_6619985577834044559[87] = 0.0; - out_6619985577834044559[88] = 0.0; - out_6619985577834044559[89] = 0.0; - out_6619985577834044559[90] = 0.0; - out_6619985577834044559[91] = 0.0; - out_6619985577834044559[92] = 0.0; - out_6619985577834044559[93] = 0.0; - out_6619985577834044559[94] = 0.0; - out_6619985577834044559[95] = 1.0; - out_6619985577834044559[96] = 0.0; - out_6619985577834044559[97] = 0.0; - out_6619985577834044559[98] = 0.0; - out_6619985577834044559[99] = 0.0; - out_6619985577834044559[100] = 0.0; - out_6619985577834044559[101] = 0.0; - out_6619985577834044559[102] = 0.0; - out_6619985577834044559[103] = 0.0; - out_6619985577834044559[104] = 0.0; - out_6619985577834044559[105] = 0.0; - out_6619985577834044559[106] = 0.0; - out_6619985577834044559[107] = 0.0; - out_6619985577834044559[108] = 0.0; - out_6619985577834044559[109] = 0.0; - out_6619985577834044559[110] = 0.0; - out_6619985577834044559[111] = 0.0; - out_6619985577834044559[112] = 0.0; - out_6619985577834044559[113] = 0.0; - out_6619985577834044559[114] = 1.0; - out_6619985577834044559[115] = 0.0; - out_6619985577834044559[116] = 0.0; - out_6619985577834044559[117] = 0.0; - out_6619985577834044559[118] = 0.0; - out_6619985577834044559[119] = 0.0; - out_6619985577834044559[120] = 0.0; - out_6619985577834044559[121] = 0.0; - out_6619985577834044559[122] = 0.0; - out_6619985577834044559[123] = 0.0; - out_6619985577834044559[124] = 0.0; - out_6619985577834044559[125] = 0.0; - out_6619985577834044559[126] = 0.0; - out_6619985577834044559[127] = 0.0; - out_6619985577834044559[128] = 0.0; - out_6619985577834044559[129] = 0.0; - out_6619985577834044559[130] = 0.0; - out_6619985577834044559[131] = 0.0; - out_6619985577834044559[132] = 0.0; - out_6619985577834044559[133] = 1.0; - out_6619985577834044559[134] = 0.0; - out_6619985577834044559[135] = 0.0; - out_6619985577834044559[136] = 0.0; - out_6619985577834044559[137] = 0.0; - out_6619985577834044559[138] = 0.0; - out_6619985577834044559[139] = 0.0; - out_6619985577834044559[140] = 0.0; - out_6619985577834044559[141] = 0.0; - out_6619985577834044559[142] = 0.0; - out_6619985577834044559[143] = 0.0; - out_6619985577834044559[144] = 0.0; - out_6619985577834044559[145] = 0.0; - out_6619985577834044559[146] = 0.0; - out_6619985577834044559[147] = 0.0; - out_6619985577834044559[148] = 0.0; - out_6619985577834044559[149] = 0.0; - out_6619985577834044559[150] = 0.0; - out_6619985577834044559[151] = 0.0; - out_6619985577834044559[152] = 1.0; - out_6619985577834044559[153] = 0.0; - out_6619985577834044559[154] = 0.0; - out_6619985577834044559[155] = 0.0; - out_6619985577834044559[156] = 0.0; - out_6619985577834044559[157] = 0.0; - out_6619985577834044559[158] = 0.0; - out_6619985577834044559[159] = 0.0; - out_6619985577834044559[160] = 0.0; - out_6619985577834044559[161] = 0.0; - out_6619985577834044559[162] = 0.0; - out_6619985577834044559[163] = 0.0; - out_6619985577834044559[164] = 0.0; - out_6619985577834044559[165] = 0.0; - out_6619985577834044559[166] = 0.0; - out_6619985577834044559[167] = 0.0; - out_6619985577834044559[168] = 0.0; - out_6619985577834044559[169] = 0.0; - out_6619985577834044559[170] = 0.0; - out_6619985577834044559[171] = 1.0; - out_6619985577834044559[172] = 0.0; - out_6619985577834044559[173] = 0.0; - out_6619985577834044559[174] = 0.0; - out_6619985577834044559[175] = 0.0; - out_6619985577834044559[176] = 0.0; - out_6619985577834044559[177] = 0.0; - out_6619985577834044559[178] = 0.0; - out_6619985577834044559[179] = 0.0; - out_6619985577834044559[180] = 0.0; - out_6619985577834044559[181] = 0.0; - out_6619985577834044559[182] = 0.0; - out_6619985577834044559[183] = 0.0; - out_6619985577834044559[184] = 0.0; - out_6619985577834044559[185] = 0.0; - out_6619985577834044559[186] = 0.0; - out_6619985577834044559[187] = 0.0; - out_6619985577834044559[188] = 0.0; - out_6619985577834044559[189] = 0.0; - out_6619985577834044559[190] = 1.0; - out_6619985577834044559[191] = 0.0; - out_6619985577834044559[192] = 0.0; - out_6619985577834044559[193] = 0.0; - out_6619985577834044559[194] = 0.0; - out_6619985577834044559[195] = 0.0; - out_6619985577834044559[196] = 0.0; - out_6619985577834044559[197] = 0.0; - out_6619985577834044559[198] = 0.0; - out_6619985577834044559[199] = 0.0; - out_6619985577834044559[200] = 0.0; - out_6619985577834044559[201] = 0.0; - out_6619985577834044559[202] = 0.0; - out_6619985577834044559[203] = 0.0; - out_6619985577834044559[204] = 0.0; - out_6619985577834044559[205] = 0.0; - out_6619985577834044559[206] = 0.0; - out_6619985577834044559[207] = 0.0; - out_6619985577834044559[208] = 0.0; - out_6619985577834044559[209] = 1.0; - out_6619985577834044559[210] = 0.0; - out_6619985577834044559[211] = 0.0; - out_6619985577834044559[212] = 0.0; - out_6619985577834044559[213] = 0.0; - out_6619985577834044559[214] = 0.0; - out_6619985577834044559[215] = 0.0; - out_6619985577834044559[216] = 0.0; - out_6619985577834044559[217] = 0.0; - out_6619985577834044559[218] = 0.0; - out_6619985577834044559[219] = 0.0; - out_6619985577834044559[220] = 0.0; - out_6619985577834044559[221] = 0.0; - out_6619985577834044559[222] = 0.0; - out_6619985577834044559[223] = 0.0; - out_6619985577834044559[224] = 0.0; - out_6619985577834044559[225] = 0.0; - out_6619985577834044559[226] = 0.0; - out_6619985577834044559[227] = 0.0; - out_6619985577834044559[228] = 1.0; - out_6619985577834044559[229] = 0.0; - out_6619985577834044559[230] = 0.0; - out_6619985577834044559[231] = 0.0; - out_6619985577834044559[232] = 0.0; - out_6619985577834044559[233] = 0.0; - out_6619985577834044559[234] = 0.0; - out_6619985577834044559[235] = 0.0; - out_6619985577834044559[236] = 0.0; - out_6619985577834044559[237] = 0.0; - out_6619985577834044559[238] = 0.0; - out_6619985577834044559[239] = 0.0; - out_6619985577834044559[240] = 0.0; - out_6619985577834044559[241] = 0.0; - out_6619985577834044559[242] = 0.0; - out_6619985577834044559[243] = 0.0; - out_6619985577834044559[244] = 0.0; - out_6619985577834044559[245] = 0.0; - out_6619985577834044559[246] = 0.0; - out_6619985577834044559[247] = 1.0; - out_6619985577834044559[248] = 0.0; - out_6619985577834044559[249] = 0.0; - out_6619985577834044559[250] = 0.0; - out_6619985577834044559[251] = 0.0; - out_6619985577834044559[252] = 0.0; - out_6619985577834044559[253] = 0.0; - out_6619985577834044559[254] = 0.0; - out_6619985577834044559[255] = 0.0; - out_6619985577834044559[256] = 0.0; - out_6619985577834044559[257] = 0.0; - out_6619985577834044559[258] = 0.0; - out_6619985577834044559[259] = 0.0; - out_6619985577834044559[260] = 0.0; - out_6619985577834044559[261] = 0.0; - out_6619985577834044559[262] = 0.0; - out_6619985577834044559[263] = 0.0; - out_6619985577834044559[264] = 0.0; - out_6619985577834044559[265] = 0.0; - out_6619985577834044559[266] = 1.0; - out_6619985577834044559[267] = 0.0; - out_6619985577834044559[268] = 0.0; - out_6619985577834044559[269] = 0.0; - out_6619985577834044559[270] = 0.0; - out_6619985577834044559[271] = 0.0; - out_6619985577834044559[272] = 0.0; - out_6619985577834044559[273] = 0.0; - out_6619985577834044559[274] = 0.0; - out_6619985577834044559[275] = 0.0; - out_6619985577834044559[276] = 0.0; - out_6619985577834044559[277] = 0.0; - out_6619985577834044559[278] = 0.0; - out_6619985577834044559[279] = 0.0; - out_6619985577834044559[280] = 0.0; - out_6619985577834044559[281] = 0.0; - out_6619985577834044559[282] = 0.0; - out_6619985577834044559[283] = 0.0; - out_6619985577834044559[284] = 0.0; - out_6619985577834044559[285] = 1.0; - out_6619985577834044559[286] = 0.0; - out_6619985577834044559[287] = 0.0; - out_6619985577834044559[288] = 0.0; - out_6619985577834044559[289] = 0.0; - out_6619985577834044559[290] = 0.0; - out_6619985577834044559[291] = 0.0; - out_6619985577834044559[292] = 0.0; - out_6619985577834044559[293] = 0.0; - out_6619985577834044559[294] = 0.0; - out_6619985577834044559[295] = 0.0; - out_6619985577834044559[296] = 0.0; - out_6619985577834044559[297] = 0.0; - out_6619985577834044559[298] = 0.0; - out_6619985577834044559[299] = 0.0; - out_6619985577834044559[300] = 0.0; - out_6619985577834044559[301] = 0.0; - out_6619985577834044559[302] = 0.0; - out_6619985577834044559[303] = 0.0; - out_6619985577834044559[304] = 1.0; - out_6619985577834044559[305] = 0.0; - out_6619985577834044559[306] = 0.0; - out_6619985577834044559[307] = 0.0; - out_6619985577834044559[308] = 0.0; - out_6619985577834044559[309] = 0.0; - out_6619985577834044559[310] = 0.0; - out_6619985577834044559[311] = 0.0; - out_6619985577834044559[312] = 0.0; - out_6619985577834044559[313] = 0.0; - out_6619985577834044559[314] = 0.0; - out_6619985577834044559[315] = 0.0; - out_6619985577834044559[316] = 0.0; - out_6619985577834044559[317] = 0.0; - out_6619985577834044559[318] = 0.0; - out_6619985577834044559[319] = 0.0; - out_6619985577834044559[320] = 0.0; - out_6619985577834044559[321] = 0.0; - out_6619985577834044559[322] = 0.0; - out_6619985577834044559[323] = 1.0; +void H_mod_fun(double *state, double *out_2404492370394702667) { + out_2404492370394702667[0] = 1.0; + out_2404492370394702667[1] = 0.0; + out_2404492370394702667[2] = 0.0; + out_2404492370394702667[3] = 0.0; + out_2404492370394702667[4] = 0.0; + out_2404492370394702667[5] = 0.0; + out_2404492370394702667[6] = 0.0; + out_2404492370394702667[7] = 0.0; + out_2404492370394702667[8] = 0.0; + out_2404492370394702667[9] = 0.0; + out_2404492370394702667[10] = 0.0; + out_2404492370394702667[11] = 0.0; + out_2404492370394702667[12] = 0.0; + out_2404492370394702667[13] = 0.0; + out_2404492370394702667[14] = 0.0; + out_2404492370394702667[15] = 0.0; + out_2404492370394702667[16] = 0.0; + out_2404492370394702667[17] = 0.0; + out_2404492370394702667[18] = 0.0; + out_2404492370394702667[19] = 1.0; + out_2404492370394702667[20] = 0.0; + out_2404492370394702667[21] = 0.0; + out_2404492370394702667[22] = 0.0; + out_2404492370394702667[23] = 0.0; + out_2404492370394702667[24] = 0.0; + out_2404492370394702667[25] = 0.0; + out_2404492370394702667[26] = 0.0; + out_2404492370394702667[27] = 0.0; + out_2404492370394702667[28] = 0.0; + out_2404492370394702667[29] = 0.0; + out_2404492370394702667[30] = 0.0; + out_2404492370394702667[31] = 0.0; + out_2404492370394702667[32] = 0.0; + out_2404492370394702667[33] = 0.0; + out_2404492370394702667[34] = 0.0; + out_2404492370394702667[35] = 0.0; + out_2404492370394702667[36] = 0.0; + out_2404492370394702667[37] = 0.0; + out_2404492370394702667[38] = 1.0; + out_2404492370394702667[39] = 0.0; + out_2404492370394702667[40] = 0.0; + out_2404492370394702667[41] = 0.0; + out_2404492370394702667[42] = 0.0; + out_2404492370394702667[43] = 0.0; + out_2404492370394702667[44] = 0.0; + out_2404492370394702667[45] = 0.0; + out_2404492370394702667[46] = 0.0; + out_2404492370394702667[47] = 0.0; + out_2404492370394702667[48] = 0.0; + out_2404492370394702667[49] = 0.0; + out_2404492370394702667[50] = 0.0; + out_2404492370394702667[51] = 0.0; + out_2404492370394702667[52] = 0.0; + out_2404492370394702667[53] = 0.0; + out_2404492370394702667[54] = 0.0; + out_2404492370394702667[55] = 0.0; + out_2404492370394702667[56] = 0.0; + out_2404492370394702667[57] = 1.0; + out_2404492370394702667[58] = 0.0; + out_2404492370394702667[59] = 0.0; + out_2404492370394702667[60] = 0.0; + out_2404492370394702667[61] = 0.0; + out_2404492370394702667[62] = 0.0; + out_2404492370394702667[63] = 0.0; + out_2404492370394702667[64] = 0.0; + out_2404492370394702667[65] = 0.0; + out_2404492370394702667[66] = 0.0; + out_2404492370394702667[67] = 0.0; + out_2404492370394702667[68] = 0.0; + out_2404492370394702667[69] = 0.0; + out_2404492370394702667[70] = 0.0; + out_2404492370394702667[71] = 0.0; + out_2404492370394702667[72] = 0.0; + out_2404492370394702667[73] = 0.0; + out_2404492370394702667[74] = 0.0; + out_2404492370394702667[75] = 0.0; + out_2404492370394702667[76] = 1.0; + out_2404492370394702667[77] = 0.0; + out_2404492370394702667[78] = 0.0; + out_2404492370394702667[79] = 0.0; + out_2404492370394702667[80] = 0.0; + out_2404492370394702667[81] = 0.0; + out_2404492370394702667[82] = 0.0; + out_2404492370394702667[83] = 0.0; + out_2404492370394702667[84] = 0.0; + out_2404492370394702667[85] = 0.0; + out_2404492370394702667[86] = 0.0; + out_2404492370394702667[87] = 0.0; + out_2404492370394702667[88] = 0.0; + out_2404492370394702667[89] = 0.0; + out_2404492370394702667[90] = 0.0; + out_2404492370394702667[91] = 0.0; + out_2404492370394702667[92] = 0.0; + out_2404492370394702667[93] = 0.0; + out_2404492370394702667[94] = 0.0; + out_2404492370394702667[95] = 1.0; + out_2404492370394702667[96] = 0.0; + out_2404492370394702667[97] = 0.0; + out_2404492370394702667[98] = 0.0; + out_2404492370394702667[99] = 0.0; + out_2404492370394702667[100] = 0.0; + out_2404492370394702667[101] = 0.0; + out_2404492370394702667[102] = 0.0; + out_2404492370394702667[103] = 0.0; + out_2404492370394702667[104] = 0.0; + out_2404492370394702667[105] = 0.0; + out_2404492370394702667[106] = 0.0; + out_2404492370394702667[107] = 0.0; + out_2404492370394702667[108] = 0.0; + out_2404492370394702667[109] = 0.0; + out_2404492370394702667[110] = 0.0; + out_2404492370394702667[111] = 0.0; + out_2404492370394702667[112] = 0.0; + out_2404492370394702667[113] = 0.0; + out_2404492370394702667[114] = 1.0; + out_2404492370394702667[115] = 0.0; + out_2404492370394702667[116] = 0.0; + out_2404492370394702667[117] = 0.0; + out_2404492370394702667[118] = 0.0; + out_2404492370394702667[119] = 0.0; + out_2404492370394702667[120] = 0.0; + out_2404492370394702667[121] = 0.0; + out_2404492370394702667[122] = 0.0; + out_2404492370394702667[123] = 0.0; + out_2404492370394702667[124] = 0.0; + out_2404492370394702667[125] = 0.0; + out_2404492370394702667[126] = 0.0; + out_2404492370394702667[127] = 0.0; + out_2404492370394702667[128] = 0.0; + out_2404492370394702667[129] = 0.0; + out_2404492370394702667[130] = 0.0; + out_2404492370394702667[131] = 0.0; + out_2404492370394702667[132] = 0.0; + out_2404492370394702667[133] = 1.0; + out_2404492370394702667[134] = 0.0; + out_2404492370394702667[135] = 0.0; + out_2404492370394702667[136] = 0.0; + out_2404492370394702667[137] = 0.0; + out_2404492370394702667[138] = 0.0; + out_2404492370394702667[139] = 0.0; + out_2404492370394702667[140] = 0.0; + out_2404492370394702667[141] = 0.0; + out_2404492370394702667[142] = 0.0; + out_2404492370394702667[143] = 0.0; + out_2404492370394702667[144] = 0.0; + out_2404492370394702667[145] = 0.0; + out_2404492370394702667[146] = 0.0; + out_2404492370394702667[147] = 0.0; + out_2404492370394702667[148] = 0.0; + out_2404492370394702667[149] = 0.0; + out_2404492370394702667[150] = 0.0; + out_2404492370394702667[151] = 0.0; + out_2404492370394702667[152] = 1.0; + out_2404492370394702667[153] = 0.0; + out_2404492370394702667[154] = 0.0; + out_2404492370394702667[155] = 0.0; + out_2404492370394702667[156] = 0.0; + out_2404492370394702667[157] = 0.0; + out_2404492370394702667[158] = 0.0; + out_2404492370394702667[159] = 0.0; + out_2404492370394702667[160] = 0.0; + out_2404492370394702667[161] = 0.0; + out_2404492370394702667[162] = 0.0; + out_2404492370394702667[163] = 0.0; + out_2404492370394702667[164] = 0.0; + out_2404492370394702667[165] = 0.0; + out_2404492370394702667[166] = 0.0; + out_2404492370394702667[167] = 0.0; + out_2404492370394702667[168] = 0.0; + out_2404492370394702667[169] = 0.0; + out_2404492370394702667[170] = 0.0; + out_2404492370394702667[171] = 1.0; + out_2404492370394702667[172] = 0.0; + out_2404492370394702667[173] = 0.0; + out_2404492370394702667[174] = 0.0; + out_2404492370394702667[175] = 0.0; + out_2404492370394702667[176] = 0.0; + out_2404492370394702667[177] = 0.0; + out_2404492370394702667[178] = 0.0; + out_2404492370394702667[179] = 0.0; + out_2404492370394702667[180] = 0.0; + out_2404492370394702667[181] = 0.0; + out_2404492370394702667[182] = 0.0; + out_2404492370394702667[183] = 0.0; + out_2404492370394702667[184] = 0.0; + out_2404492370394702667[185] = 0.0; + out_2404492370394702667[186] = 0.0; + out_2404492370394702667[187] = 0.0; + out_2404492370394702667[188] = 0.0; + out_2404492370394702667[189] = 0.0; + out_2404492370394702667[190] = 1.0; + out_2404492370394702667[191] = 0.0; + out_2404492370394702667[192] = 0.0; + out_2404492370394702667[193] = 0.0; + out_2404492370394702667[194] = 0.0; + out_2404492370394702667[195] = 0.0; + out_2404492370394702667[196] = 0.0; + out_2404492370394702667[197] = 0.0; + out_2404492370394702667[198] = 0.0; + out_2404492370394702667[199] = 0.0; + out_2404492370394702667[200] = 0.0; + out_2404492370394702667[201] = 0.0; + out_2404492370394702667[202] = 0.0; + out_2404492370394702667[203] = 0.0; + out_2404492370394702667[204] = 0.0; + out_2404492370394702667[205] = 0.0; + out_2404492370394702667[206] = 0.0; + out_2404492370394702667[207] = 0.0; + out_2404492370394702667[208] = 0.0; + out_2404492370394702667[209] = 1.0; + out_2404492370394702667[210] = 0.0; + out_2404492370394702667[211] = 0.0; + out_2404492370394702667[212] = 0.0; + out_2404492370394702667[213] = 0.0; + out_2404492370394702667[214] = 0.0; + out_2404492370394702667[215] = 0.0; + out_2404492370394702667[216] = 0.0; + out_2404492370394702667[217] = 0.0; + out_2404492370394702667[218] = 0.0; + out_2404492370394702667[219] = 0.0; + out_2404492370394702667[220] = 0.0; + out_2404492370394702667[221] = 0.0; + out_2404492370394702667[222] = 0.0; + out_2404492370394702667[223] = 0.0; + out_2404492370394702667[224] = 0.0; + out_2404492370394702667[225] = 0.0; + out_2404492370394702667[226] = 0.0; + out_2404492370394702667[227] = 0.0; + out_2404492370394702667[228] = 1.0; + out_2404492370394702667[229] = 0.0; + out_2404492370394702667[230] = 0.0; + out_2404492370394702667[231] = 0.0; + out_2404492370394702667[232] = 0.0; + out_2404492370394702667[233] = 0.0; + out_2404492370394702667[234] = 0.0; + out_2404492370394702667[235] = 0.0; + out_2404492370394702667[236] = 0.0; + out_2404492370394702667[237] = 0.0; + out_2404492370394702667[238] = 0.0; + out_2404492370394702667[239] = 0.0; + out_2404492370394702667[240] = 0.0; + out_2404492370394702667[241] = 0.0; + out_2404492370394702667[242] = 0.0; + out_2404492370394702667[243] = 0.0; + out_2404492370394702667[244] = 0.0; + out_2404492370394702667[245] = 0.0; + out_2404492370394702667[246] = 0.0; + out_2404492370394702667[247] = 1.0; + out_2404492370394702667[248] = 0.0; + out_2404492370394702667[249] = 0.0; + out_2404492370394702667[250] = 0.0; + out_2404492370394702667[251] = 0.0; + out_2404492370394702667[252] = 0.0; + out_2404492370394702667[253] = 0.0; + out_2404492370394702667[254] = 0.0; + out_2404492370394702667[255] = 0.0; + out_2404492370394702667[256] = 0.0; + out_2404492370394702667[257] = 0.0; + out_2404492370394702667[258] = 0.0; + out_2404492370394702667[259] = 0.0; + out_2404492370394702667[260] = 0.0; + out_2404492370394702667[261] = 0.0; + out_2404492370394702667[262] = 0.0; + out_2404492370394702667[263] = 0.0; + out_2404492370394702667[264] = 0.0; + out_2404492370394702667[265] = 0.0; + out_2404492370394702667[266] = 1.0; + out_2404492370394702667[267] = 0.0; + out_2404492370394702667[268] = 0.0; + out_2404492370394702667[269] = 0.0; + out_2404492370394702667[270] = 0.0; + out_2404492370394702667[271] = 0.0; + out_2404492370394702667[272] = 0.0; + out_2404492370394702667[273] = 0.0; + out_2404492370394702667[274] = 0.0; + out_2404492370394702667[275] = 0.0; + out_2404492370394702667[276] = 0.0; + out_2404492370394702667[277] = 0.0; + out_2404492370394702667[278] = 0.0; + out_2404492370394702667[279] = 0.0; + out_2404492370394702667[280] = 0.0; + out_2404492370394702667[281] = 0.0; + out_2404492370394702667[282] = 0.0; + out_2404492370394702667[283] = 0.0; + out_2404492370394702667[284] = 0.0; + out_2404492370394702667[285] = 1.0; + out_2404492370394702667[286] = 0.0; + out_2404492370394702667[287] = 0.0; + out_2404492370394702667[288] = 0.0; + out_2404492370394702667[289] = 0.0; + out_2404492370394702667[290] = 0.0; + out_2404492370394702667[291] = 0.0; + out_2404492370394702667[292] = 0.0; + out_2404492370394702667[293] = 0.0; + out_2404492370394702667[294] = 0.0; + out_2404492370394702667[295] = 0.0; + out_2404492370394702667[296] = 0.0; + out_2404492370394702667[297] = 0.0; + out_2404492370394702667[298] = 0.0; + out_2404492370394702667[299] = 0.0; + out_2404492370394702667[300] = 0.0; + out_2404492370394702667[301] = 0.0; + out_2404492370394702667[302] = 0.0; + out_2404492370394702667[303] = 0.0; + out_2404492370394702667[304] = 1.0; + out_2404492370394702667[305] = 0.0; + out_2404492370394702667[306] = 0.0; + out_2404492370394702667[307] = 0.0; + out_2404492370394702667[308] = 0.0; + out_2404492370394702667[309] = 0.0; + out_2404492370394702667[310] = 0.0; + out_2404492370394702667[311] = 0.0; + out_2404492370394702667[312] = 0.0; + out_2404492370394702667[313] = 0.0; + out_2404492370394702667[314] = 0.0; + out_2404492370394702667[315] = 0.0; + out_2404492370394702667[316] = 0.0; + out_2404492370394702667[317] = 0.0; + out_2404492370394702667[318] = 0.0; + out_2404492370394702667[319] = 0.0; + out_2404492370394702667[320] = 0.0; + out_2404492370394702667[321] = 0.0; + out_2404492370394702667[322] = 0.0; + out_2404492370394702667[323] = 1.0; } -void f_fun(double *state, double dt, double *out_8911446702564060395) { - out_8911446702564060395[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_8911446702564060395[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_8911446702564060395[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_8911446702564060395[3] = dt*state[12] + state[3]; - out_8911446702564060395[4] = dt*state[13] + state[4]; - out_8911446702564060395[5] = dt*state[14] + state[5]; - out_8911446702564060395[6] = state[6]; - out_8911446702564060395[7] = state[7]; - out_8911446702564060395[8] = state[8]; - out_8911446702564060395[9] = state[9]; - out_8911446702564060395[10] = state[10]; - out_8911446702564060395[11] = state[11]; - out_8911446702564060395[12] = state[12]; - out_8911446702564060395[13] = state[13]; - out_8911446702564060395[14] = state[14]; - out_8911446702564060395[15] = state[15]; - out_8911446702564060395[16] = state[16]; - out_8911446702564060395[17] = state[17]; +void f_fun(double *state, double dt, double *out_442464745730678105) { + out_442464745730678105[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_442464745730678105[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_442464745730678105[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_442464745730678105[3] = dt*state[12] + state[3]; + out_442464745730678105[4] = dt*state[13] + state[4]; + out_442464745730678105[5] = dt*state[14] + state[5]; + out_442464745730678105[6] = state[6]; + out_442464745730678105[7] = state[7]; + out_442464745730678105[8] = state[8]; + out_442464745730678105[9] = state[9]; + out_442464745730678105[10] = state[10]; + out_442464745730678105[11] = state[11]; + out_442464745730678105[12] = state[12]; + out_442464745730678105[13] = state[13]; + out_442464745730678105[14] = state[14]; + out_442464745730678105[15] = state[15]; + out_442464745730678105[16] = state[16]; + out_442464745730678105[17] = state[17]; } -void F_fun(double *state, double dt, double *out_8001086797165012903) { - out_8001086797165012903[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_8001086797165012903[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_8001086797165012903[2] = 0; - out_8001086797165012903[3] = 0; - out_8001086797165012903[4] = 0; - out_8001086797165012903[5] = 0; - out_8001086797165012903[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_8001086797165012903[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_8001086797165012903[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_8001086797165012903[9] = 0; - out_8001086797165012903[10] = 0; - out_8001086797165012903[11] = 0; - out_8001086797165012903[12] = 0; - out_8001086797165012903[13] = 0; - out_8001086797165012903[14] = 0; - out_8001086797165012903[15] = 0; - out_8001086797165012903[16] = 0; - out_8001086797165012903[17] = 0; - out_8001086797165012903[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_8001086797165012903[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_8001086797165012903[20] = 0; - out_8001086797165012903[21] = 0; - out_8001086797165012903[22] = 0; - out_8001086797165012903[23] = 0; - out_8001086797165012903[24] = 0; - out_8001086797165012903[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_8001086797165012903[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_8001086797165012903[27] = 0; - out_8001086797165012903[28] = 0; - out_8001086797165012903[29] = 0; - out_8001086797165012903[30] = 0; - out_8001086797165012903[31] = 0; - out_8001086797165012903[32] = 0; - out_8001086797165012903[33] = 0; - out_8001086797165012903[34] = 0; - out_8001086797165012903[35] = 0; - out_8001086797165012903[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_8001086797165012903[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_8001086797165012903[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_8001086797165012903[39] = 0; - out_8001086797165012903[40] = 0; - out_8001086797165012903[41] = 0; - out_8001086797165012903[42] = 0; - out_8001086797165012903[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_8001086797165012903[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_8001086797165012903[45] = 0; - out_8001086797165012903[46] = 0; - out_8001086797165012903[47] = 0; - out_8001086797165012903[48] = 0; - out_8001086797165012903[49] = 0; - out_8001086797165012903[50] = 0; - out_8001086797165012903[51] = 0; - out_8001086797165012903[52] = 0; - out_8001086797165012903[53] = 0; - out_8001086797165012903[54] = 0; - out_8001086797165012903[55] = 0; - out_8001086797165012903[56] = 0; - out_8001086797165012903[57] = 1; - out_8001086797165012903[58] = 0; - out_8001086797165012903[59] = 0; - out_8001086797165012903[60] = 0; - out_8001086797165012903[61] = 0; - out_8001086797165012903[62] = 0; - out_8001086797165012903[63] = 0; - out_8001086797165012903[64] = 0; - out_8001086797165012903[65] = 0; - out_8001086797165012903[66] = dt; - out_8001086797165012903[67] = 0; - out_8001086797165012903[68] = 0; - out_8001086797165012903[69] = 0; - out_8001086797165012903[70] = 0; - out_8001086797165012903[71] = 0; - out_8001086797165012903[72] = 0; - out_8001086797165012903[73] = 0; - out_8001086797165012903[74] = 0; - out_8001086797165012903[75] = 0; - out_8001086797165012903[76] = 1; - out_8001086797165012903[77] = 0; - out_8001086797165012903[78] = 0; - out_8001086797165012903[79] = 0; - out_8001086797165012903[80] = 0; - out_8001086797165012903[81] = 0; - out_8001086797165012903[82] = 0; - out_8001086797165012903[83] = 0; - out_8001086797165012903[84] = 0; - out_8001086797165012903[85] = dt; - out_8001086797165012903[86] = 0; - out_8001086797165012903[87] = 0; - out_8001086797165012903[88] = 0; - out_8001086797165012903[89] = 0; - out_8001086797165012903[90] = 0; - out_8001086797165012903[91] = 0; - out_8001086797165012903[92] = 0; - out_8001086797165012903[93] = 0; - out_8001086797165012903[94] = 0; - out_8001086797165012903[95] = 1; - out_8001086797165012903[96] = 0; - out_8001086797165012903[97] = 0; - out_8001086797165012903[98] = 0; - out_8001086797165012903[99] = 0; - out_8001086797165012903[100] = 0; - out_8001086797165012903[101] = 0; - out_8001086797165012903[102] = 0; - out_8001086797165012903[103] = 0; - out_8001086797165012903[104] = dt; - out_8001086797165012903[105] = 0; - out_8001086797165012903[106] = 0; - out_8001086797165012903[107] = 0; - out_8001086797165012903[108] = 0; - out_8001086797165012903[109] = 0; - out_8001086797165012903[110] = 0; - out_8001086797165012903[111] = 0; - out_8001086797165012903[112] = 0; - out_8001086797165012903[113] = 0; - out_8001086797165012903[114] = 1; - out_8001086797165012903[115] = 0; - out_8001086797165012903[116] = 0; - out_8001086797165012903[117] = 0; - out_8001086797165012903[118] = 0; - out_8001086797165012903[119] = 0; - out_8001086797165012903[120] = 0; - out_8001086797165012903[121] = 0; - out_8001086797165012903[122] = 0; - out_8001086797165012903[123] = 0; - out_8001086797165012903[124] = 0; - out_8001086797165012903[125] = 0; - out_8001086797165012903[126] = 0; - out_8001086797165012903[127] = 0; - out_8001086797165012903[128] = 0; - out_8001086797165012903[129] = 0; - out_8001086797165012903[130] = 0; - out_8001086797165012903[131] = 0; - out_8001086797165012903[132] = 0; - out_8001086797165012903[133] = 1; - out_8001086797165012903[134] = 0; - out_8001086797165012903[135] = 0; - out_8001086797165012903[136] = 0; - out_8001086797165012903[137] = 0; - out_8001086797165012903[138] = 0; - out_8001086797165012903[139] = 0; - out_8001086797165012903[140] = 0; - out_8001086797165012903[141] = 0; - out_8001086797165012903[142] = 0; - out_8001086797165012903[143] = 0; - out_8001086797165012903[144] = 0; - out_8001086797165012903[145] = 0; - out_8001086797165012903[146] = 0; - out_8001086797165012903[147] = 0; - out_8001086797165012903[148] = 0; - out_8001086797165012903[149] = 0; - out_8001086797165012903[150] = 0; - out_8001086797165012903[151] = 0; - out_8001086797165012903[152] = 1; - out_8001086797165012903[153] = 0; - out_8001086797165012903[154] = 0; - out_8001086797165012903[155] = 0; - out_8001086797165012903[156] = 0; - out_8001086797165012903[157] = 0; - out_8001086797165012903[158] = 0; - out_8001086797165012903[159] = 0; - out_8001086797165012903[160] = 0; - out_8001086797165012903[161] = 0; - out_8001086797165012903[162] = 0; - out_8001086797165012903[163] = 0; - out_8001086797165012903[164] = 0; - out_8001086797165012903[165] = 0; - out_8001086797165012903[166] = 0; - out_8001086797165012903[167] = 0; - out_8001086797165012903[168] = 0; - out_8001086797165012903[169] = 0; - out_8001086797165012903[170] = 0; - out_8001086797165012903[171] = 1; - out_8001086797165012903[172] = 0; - out_8001086797165012903[173] = 0; - out_8001086797165012903[174] = 0; - out_8001086797165012903[175] = 0; - out_8001086797165012903[176] = 0; - out_8001086797165012903[177] = 0; - out_8001086797165012903[178] = 0; - out_8001086797165012903[179] = 0; - out_8001086797165012903[180] = 0; - out_8001086797165012903[181] = 0; - out_8001086797165012903[182] = 0; - out_8001086797165012903[183] = 0; - out_8001086797165012903[184] = 0; - out_8001086797165012903[185] = 0; - out_8001086797165012903[186] = 0; - out_8001086797165012903[187] = 0; - out_8001086797165012903[188] = 0; - out_8001086797165012903[189] = 0; - out_8001086797165012903[190] = 1; - out_8001086797165012903[191] = 0; - out_8001086797165012903[192] = 0; - out_8001086797165012903[193] = 0; - out_8001086797165012903[194] = 0; - out_8001086797165012903[195] = 0; - out_8001086797165012903[196] = 0; - out_8001086797165012903[197] = 0; - out_8001086797165012903[198] = 0; - out_8001086797165012903[199] = 0; - out_8001086797165012903[200] = 0; - out_8001086797165012903[201] = 0; - out_8001086797165012903[202] = 0; - out_8001086797165012903[203] = 0; - out_8001086797165012903[204] = 0; - out_8001086797165012903[205] = 0; - out_8001086797165012903[206] = 0; - out_8001086797165012903[207] = 0; - out_8001086797165012903[208] = 0; - out_8001086797165012903[209] = 1; - out_8001086797165012903[210] = 0; - out_8001086797165012903[211] = 0; - out_8001086797165012903[212] = 0; - out_8001086797165012903[213] = 0; - out_8001086797165012903[214] = 0; - out_8001086797165012903[215] = 0; - out_8001086797165012903[216] = 0; - out_8001086797165012903[217] = 0; - out_8001086797165012903[218] = 0; - out_8001086797165012903[219] = 0; - out_8001086797165012903[220] = 0; - out_8001086797165012903[221] = 0; - out_8001086797165012903[222] = 0; - out_8001086797165012903[223] = 0; - out_8001086797165012903[224] = 0; - out_8001086797165012903[225] = 0; - out_8001086797165012903[226] = 0; - out_8001086797165012903[227] = 0; - out_8001086797165012903[228] = 1; - out_8001086797165012903[229] = 0; - out_8001086797165012903[230] = 0; - out_8001086797165012903[231] = 0; - out_8001086797165012903[232] = 0; - out_8001086797165012903[233] = 0; - out_8001086797165012903[234] = 0; - out_8001086797165012903[235] = 0; - out_8001086797165012903[236] = 0; - out_8001086797165012903[237] = 0; - out_8001086797165012903[238] = 0; - out_8001086797165012903[239] = 0; - out_8001086797165012903[240] = 0; - out_8001086797165012903[241] = 0; - out_8001086797165012903[242] = 0; - out_8001086797165012903[243] = 0; - out_8001086797165012903[244] = 0; - out_8001086797165012903[245] = 0; - out_8001086797165012903[246] = 0; - out_8001086797165012903[247] = 1; - out_8001086797165012903[248] = 0; - out_8001086797165012903[249] = 0; - out_8001086797165012903[250] = 0; - out_8001086797165012903[251] = 0; - out_8001086797165012903[252] = 0; - out_8001086797165012903[253] = 0; - out_8001086797165012903[254] = 0; - out_8001086797165012903[255] = 0; - out_8001086797165012903[256] = 0; - out_8001086797165012903[257] = 0; - out_8001086797165012903[258] = 0; - out_8001086797165012903[259] = 0; - out_8001086797165012903[260] = 0; - out_8001086797165012903[261] = 0; - out_8001086797165012903[262] = 0; - out_8001086797165012903[263] = 0; - out_8001086797165012903[264] = 0; - out_8001086797165012903[265] = 0; - out_8001086797165012903[266] = 1; - out_8001086797165012903[267] = 0; - out_8001086797165012903[268] = 0; - out_8001086797165012903[269] = 0; - out_8001086797165012903[270] = 0; - out_8001086797165012903[271] = 0; - out_8001086797165012903[272] = 0; - out_8001086797165012903[273] = 0; - out_8001086797165012903[274] = 0; - out_8001086797165012903[275] = 0; - out_8001086797165012903[276] = 0; - out_8001086797165012903[277] = 0; - out_8001086797165012903[278] = 0; - out_8001086797165012903[279] = 0; - out_8001086797165012903[280] = 0; - out_8001086797165012903[281] = 0; - out_8001086797165012903[282] = 0; - out_8001086797165012903[283] = 0; - out_8001086797165012903[284] = 0; - out_8001086797165012903[285] = 1; - out_8001086797165012903[286] = 0; - out_8001086797165012903[287] = 0; - out_8001086797165012903[288] = 0; - out_8001086797165012903[289] = 0; - out_8001086797165012903[290] = 0; - out_8001086797165012903[291] = 0; - out_8001086797165012903[292] = 0; - out_8001086797165012903[293] = 0; - out_8001086797165012903[294] = 0; - out_8001086797165012903[295] = 0; - out_8001086797165012903[296] = 0; - out_8001086797165012903[297] = 0; - out_8001086797165012903[298] = 0; - out_8001086797165012903[299] = 0; - out_8001086797165012903[300] = 0; - out_8001086797165012903[301] = 0; - out_8001086797165012903[302] = 0; - out_8001086797165012903[303] = 0; - out_8001086797165012903[304] = 1; - out_8001086797165012903[305] = 0; - out_8001086797165012903[306] = 0; - out_8001086797165012903[307] = 0; - out_8001086797165012903[308] = 0; - out_8001086797165012903[309] = 0; - out_8001086797165012903[310] = 0; - out_8001086797165012903[311] = 0; - out_8001086797165012903[312] = 0; - out_8001086797165012903[313] = 0; - out_8001086797165012903[314] = 0; - out_8001086797165012903[315] = 0; - out_8001086797165012903[316] = 0; - out_8001086797165012903[317] = 0; - out_8001086797165012903[318] = 0; - out_8001086797165012903[319] = 0; - out_8001086797165012903[320] = 0; - out_8001086797165012903[321] = 0; - out_8001086797165012903[322] = 0; - out_8001086797165012903[323] = 1; +void F_fun(double *state, double dt, double *out_68862599699564370) { + out_68862599699564370[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_68862599699564370[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_68862599699564370[2] = 0; + out_68862599699564370[3] = 0; + out_68862599699564370[4] = 0; + out_68862599699564370[5] = 0; + out_68862599699564370[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_68862599699564370[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_68862599699564370[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_68862599699564370[9] = 0; + out_68862599699564370[10] = 0; + out_68862599699564370[11] = 0; + out_68862599699564370[12] = 0; + out_68862599699564370[13] = 0; + out_68862599699564370[14] = 0; + out_68862599699564370[15] = 0; + out_68862599699564370[16] = 0; + out_68862599699564370[17] = 0; + out_68862599699564370[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_68862599699564370[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_68862599699564370[20] = 0; + out_68862599699564370[21] = 0; + out_68862599699564370[22] = 0; + out_68862599699564370[23] = 0; + out_68862599699564370[24] = 0; + out_68862599699564370[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_68862599699564370[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_68862599699564370[27] = 0; + out_68862599699564370[28] = 0; + out_68862599699564370[29] = 0; + out_68862599699564370[30] = 0; + out_68862599699564370[31] = 0; + out_68862599699564370[32] = 0; + out_68862599699564370[33] = 0; + out_68862599699564370[34] = 0; + out_68862599699564370[35] = 0; + out_68862599699564370[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_68862599699564370[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_68862599699564370[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_68862599699564370[39] = 0; + out_68862599699564370[40] = 0; + out_68862599699564370[41] = 0; + out_68862599699564370[42] = 0; + out_68862599699564370[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_68862599699564370[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_68862599699564370[45] = 0; + out_68862599699564370[46] = 0; + out_68862599699564370[47] = 0; + out_68862599699564370[48] = 0; + out_68862599699564370[49] = 0; + out_68862599699564370[50] = 0; + out_68862599699564370[51] = 0; + out_68862599699564370[52] = 0; + out_68862599699564370[53] = 0; + out_68862599699564370[54] = 0; + out_68862599699564370[55] = 0; + out_68862599699564370[56] = 0; + out_68862599699564370[57] = 1; + out_68862599699564370[58] = 0; + out_68862599699564370[59] = 0; + out_68862599699564370[60] = 0; + out_68862599699564370[61] = 0; + out_68862599699564370[62] = 0; + out_68862599699564370[63] = 0; + out_68862599699564370[64] = 0; + out_68862599699564370[65] = 0; + out_68862599699564370[66] = dt; + out_68862599699564370[67] = 0; + out_68862599699564370[68] = 0; + out_68862599699564370[69] = 0; + out_68862599699564370[70] = 0; + out_68862599699564370[71] = 0; + out_68862599699564370[72] = 0; + out_68862599699564370[73] = 0; + out_68862599699564370[74] = 0; + out_68862599699564370[75] = 0; + out_68862599699564370[76] = 1; + out_68862599699564370[77] = 0; + out_68862599699564370[78] = 0; + out_68862599699564370[79] = 0; + out_68862599699564370[80] = 0; + out_68862599699564370[81] = 0; + out_68862599699564370[82] = 0; + out_68862599699564370[83] = 0; + out_68862599699564370[84] = 0; + out_68862599699564370[85] = dt; + out_68862599699564370[86] = 0; + out_68862599699564370[87] = 0; + out_68862599699564370[88] = 0; + out_68862599699564370[89] = 0; + out_68862599699564370[90] = 0; + out_68862599699564370[91] = 0; + out_68862599699564370[92] = 0; + out_68862599699564370[93] = 0; + out_68862599699564370[94] = 0; + out_68862599699564370[95] = 1; + out_68862599699564370[96] = 0; + out_68862599699564370[97] = 0; + out_68862599699564370[98] = 0; + out_68862599699564370[99] = 0; + out_68862599699564370[100] = 0; + out_68862599699564370[101] = 0; + out_68862599699564370[102] = 0; + out_68862599699564370[103] = 0; + out_68862599699564370[104] = dt; + out_68862599699564370[105] = 0; + out_68862599699564370[106] = 0; + out_68862599699564370[107] = 0; + out_68862599699564370[108] = 0; + out_68862599699564370[109] = 0; + out_68862599699564370[110] = 0; + out_68862599699564370[111] = 0; + out_68862599699564370[112] = 0; + out_68862599699564370[113] = 0; + out_68862599699564370[114] = 1; + out_68862599699564370[115] = 0; + out_68862599699564370[116] = 0; + out_68862599699564370[117] = 0; + out_68862599699564370[118] = 0; + out_68862599699564370[119] = 0; + out_68862599699564370[120] = 0; + out_68862599699564370[121] = 0; + out_68862599699564370[122] = 0; + out_68862599699564370[123] = 0; + out_68862599699564370[124] = 0; + out_68862599699564370[125] = 0; + out_68862599699564370[126] = 0; + out_68862599699564370[127] = 0; + out_68862599699564370[128] = 0; + out_68862599699564370[129] = 0; + out_68862599699564370[130] = 0; + out_68862599699564370[131] = 0; + out_68862599699564370[132] = 0; + out_68862599699564370[133] = 1; + out_68862599699564370[134] = 0; + out_68862599699564370[135] = 0; + out_68862599699564370[136] = 0; + out_68862599699564370[137] = 0; + out_68862599699564370[138] = 0; + out_68862599699564370[139] = 0; + out_68862599699564370[140] = 0; + out_68862599699564370[141] = 0; + out_68862599699564370[142] = 0; + out_68862599699564370[143] = 0; + out_68862599699564370[144] = 0; + out_68862599699564370[145] = 0; + out_68862599699564370[146] = 0; + out_68862599699564370[147] = 0; + out_68862599699564370[148] = 0; + out_68862599699564370[149] = 0; + out_68862599699564370[150] = 0; + out_68862599699564370[151] = 0; + out_68862599699564370[152] = 1; + out_68862599699564370[153] = 0; + out_68862599699564370[154] = 0; + out_68862599699564370[155] = 0; + out_68862599699564370[156] = 0; + out_68862599699564370[157] = 0; + out_68862599699564370[158] = 0; + out_68862599699564370[159] = 0; + out_68862599699564370[160] = 0; + out_68862599699564370[161] = 0; + out_68862599699564370[162] = 0; + out_68862599699564370[163] = 0; + out_68862599699564370[164] = 0; + out_68862599699564370[165] = 0; + out_68862599699564370[166] = 0; + out_68862599699564370[167] = 0; + out_68862599699564370[168] = 0; + out_68862599699564370[169] = 0; + out_68862599699564370[170] = 0; + out_68862599699564370[171] = 1; + out_68862599699564370[172] = 0; + out_68862599699564370[173] = 0; + out_68862599699564370[174] = 0; + out_68862599699564370[175] = 0; + out_68862599699564370[176] = 0; + out_68862599699564370[177] = 0; + out_68862599699564370[178] = 0; + out_68862599699564370[179] = 0; + out_68862599699564370[180] = 0; + out_68862599699564370[181] = 0; + out_68862599699564370[182] = 0; + out_68862599699564370[183] = 0; + out_68862599699564370[184] = 0; + out_68862599699564370[185] = 0; + out_68862599699564370[186] = 0; + out_68862599699564370[187] = 0; + out_68862599699564370[188] = 0; + out_68862599699564370[189] = 0; + out_68862599699564370[190] = 1; + out_68862599699564370[191] = 0; + out_68862599699564370[192] = 0; + out_68862599699564370[193] = 0; + out_68862599699564370[194] = 0; + out_68862599699564370[195] = 0; + out_68862599699564370[196] = 0; + out_68862599699564370[197] = 0; + out_68862599699564370[198] = 0; + out_68862599699564370[199] = 0; + out_68862599699564370[200] = 0; + out_68862599699564370[201] = 0; + out_68862599699564370[202] = 0; + out_68862599699564370[203] = 0; + out_68862599699564370[204] = 0; + out_68862599699564370[205] = 0; + out_68862599699564370[206] = 0; + out_68862599699564370[207] = 0; + out_68862599699564370[208] = 0; + out_68862599699564370[209] = 1; + out_68862599699564370[210] = 0; + out_68862599699564370[211] = 0; + out_68862599699564370[212] = 0; + out_68862599699564370[213] = 0; + out_68862599699564370[214] = 0; + out_68862599699564370[215] = 0; + out_68862599699564370[216] = 0; + out_68862599699564370[217] = 0; + out_68862599699564370[218] = 0; + out_68862599699564370[219] = 0; + out_68862599699564370[220] = 0; + out_68862599699564370[221] = 0; + out_68862599699564370[222] = 0; + out_68862599699564370[223] = 0; + out_68862599699564370[224] = 0; + out_68862599699564370[225] = 0; + out_68862599699564370[226] = 0; + out_68862599699564370[227] = 0; + out_68862599699564370[228] = 1; + out_68862599699564370[229] = 0; + out_68862599699564370[230] = 0; + out_68862599699564370[231] = 0; + out_68862599699564370[232] = 0; + out_68862599699564370[233] = 0; + out_68862599699564370[234] = 0; + out_68862599699564370[235] = 0; + out_68862599699564370[236] = 0; + out_68862599699564370[237] = 0; + out_68862599699564370[238] = 0; + out_68862599699564370[239] = 0; + out_68862599699564370[240] = 0; + out_68862599699564370[241] = 0; + out_68862599699564370[242] = 0; + out_68862599699564370[243] = 0; + out_68862599699564370[244] = 0; + out_68862599699564370[245] = 0; + out_68862599699564370[246] = 0; + out_68862599699564370[247] = 1; + out_68862599699564370[248] = 0; + out_68862599699564370[249] = 0; + out_68862599699564370[250] = 0; + out_68862599699564370[251] = 0; + out_68862599699564370[252] = 0; + out_68862599699564370[253] = 0; + out_68862599699564370[254] = 0; + out_68862599699564370[255] = 0; + out_68862599699564370[256] = 0; + out_68862599699564370[257] = 0; + out_68862599699564370[258] = 0; + out_68862599699564370[259] = 0; + out_68862599699564370[260] = 0; + out_68862599699564370[261] = 0; + out_68862599699564370[262] = 0; + out_68862599699564370[263] = 0; + out_68862599699564370[264] = 0; + out_68862599699564370[265] = 0; + out_68862599699564370[266] = 1; + out_68862599699564370[267] = 0; + out_68862599699564370[268] = 0; + out_68862599699564370[269] = 0; + out_68862599699564370[270] = 0; + out_68862599699564370[271] = 0; + out_68862599699564370[272] = 0; + out_68862599699564370[273] = 0; + out_68862599699564370[274] = 0; + out_68862599699564370[275] = 0; + out_68862599699564370[276] = 0; + out_68862599699564370[277] = 0; + out_68862599699564370[278] = 0; + out_68862599699564370[279] = 0; + out_68862599699564370[280] = 0; + out_68862599699564370[281] = 0; + out_68862599699564370[282] = 0; + out_68862599699564370[283] = 0; + out_68862599699564370[284] = 0; + out_68862599699564370[285] = 1; + out_68862599699564370[286] = 0; + out_68862599699564370[287] = 0; + out_68862599699564370[288] = 0; + out_68862599699564370[289] = 0; + out_68862599699564370[290] = 0; + out_68862599699564370[291] = 0; + out_68862599699564370[292] = 0; + out_68862599699564370[293] = 0; + out_68862599699564370[294] = 0; + out_68862599699564370[295] = 0; + out_68862599699564370[296] = 0; + out_68862599699564370[297] = 0; + out_68862599699564370[298] = 0; + out_68862599699564370[299] = 0; + out_68862599699564370[300] = 0; + out_68862599699564370[301] = 0; + out_68862599699564370[302] = 0; + out_68862599699564370[303] = 0; + out_68862599699564370[304] = 1; + out_68862599699564370[305] = 0; + out_68862599699564370[306] = 0; + out_68862599699564370[307] = 0; + out_68862599699564370[308] = 0; + out_68862599699564370[309] = 0; + out_68862599699564370[310] = 0; + out_68862599699564370[311] = 0; + out_68862599699564370[312] = 0; + out_68862599699564370[313] = 0; + out_68862599699564370[314] = 0; + out_68862599699564370[315] = 0; + out_68862599699564370[316] = 0; + out_68862599699564370[317] = 0; + out_68862599699564370[318] = 0; + out_68862599699564370[319] = 0; + out_68862599699564370[320] = 0; + out_68862599699564370[321] = 0; + out_68862599699564370[322] = 0; + out_68862599699564370[323] = 1; } -void h_4(double *state, double *unused, double *out_700157376215369055) { - out_700157376215369055[0] = state[6] + state[9]; - out_700157376215369055[1] = state[7] + state[10]; - out_700157376215369055[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_6968799398651119641) { + out_6968799398651119641[0] = state[6] + state[9]; + out_6968799398651119641[1] = state[7] + state[10]; + out_6968799398651119641[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_5040670747730128630) { - out_5040670747730128630[0] = 0; - out_5040670747730128630[1] = 0; - out_5040670747730128630[2] = 0; - out_5040670747730128630[3] = 0; - out_5040670747730128630[4] = 0; - out_5040670747730128630[5] = 0; - out_5040670747730128630[6] = 1; - out_5040670747730128630[7] = 0; - out_5040670747730128630[8] = 0; - out_5040670747730128630[9] = 1; - out_5040670747730128630[10] = 0; - out_5040670747730128630[11] = 0; - out_5040670747730128630[12] = 0; - out_5040670747730128630[13] = 0; - out_5040670747730128630[14] = 0; - out_5040670747730128630[15] = 0; - out_5040670747730128630[16] = 0; - out_5040670747730128630[17] = 0; - out_5040670747730128630[18] = 0; - out_5040670747730128630[19] = 0; - out_5040670747730128630[20] = 0; - out_5040670747730128630[21] = 0; - out_5040670747730128630[22] = 0; - out_5040670747730128630[23] = 0; - out_5040670747730128630[24] = 0; - out_5040670747730128630[25] = 1; - out_5040670747730128630[26] = 0; - out_5040670747730128630[27] = 0; - out_5040670747730128630[28] = 1; - out_5040670747730128630[29] = 0; - out_5040670747730128630[30] = 0; - out_5040670747730128630[31] = 0; - out_5040670747730128630[32] = 0; - out_5040670747730128630[33] = 0; - out_5040670747730128630[34] = 0; - out_5040670747730128630[35] = 0; - out_5040670747730128630[36] = 0; - out_5040670747730128630[37] = 0; - out_5040670747730128630[38] = 0; - out_5040670747730128630[39] = 0; - out_5040670747730128630[40] = 0; - out_5040670747730128630[41] = 0; - out_5040670747730128630[42] = 0; - out_5040670747730128630[43] = 0; - out_5040670747730128630[44] = 1; - out_5040670747730128630[45] = 0; - out_5040670747730128630[46] = 0; - out_5040670747730128630[47] = 1; - out_5040670747730128630[48] = 0; - out_5040670747730128630[49] = 0; - out_5040670747730128630[50] = 0; - out_5040670747730128630[51] = 0; - out_5040670747730128630[52] = 0; - out_5040670747730128630[53] = 0; +void H_4(double *state, double *unused, double *out_6333703861688286471) { + out_6333703861688286471[0] = 0; + out_6333703861688286471[1] = 0; + out_6333703861688286471[2] = 0; + out_6333703861688286471[3] = 0; + out_6333703861688286471[4] = 0; + out_6333703861688286471[5] = 0; + out_6333703861688286471[6] = 1; + out_6333703861688286471[7] = 0; + out_6333703861688286471[8] = 0; + out_6333703861688286471[9] = 1; + out_6333703861688286471[10] = 0; + out_6333703861688286471[11] = 0; + out_6333703861688286471[12] = 0; + out_6333703861688286471[13] = 0; + out_6333703861688286471[14] = 0; + out_6333703861688286471[15] = 0; + out_6333703861688286471[16] = 0; + out_6333703861688286471[17] = 0; + out_6333703861688286471[18] = 0; + out_6333703861688286471[19] = 0; + out_6333703861688286471[20] = 0; + out_6333703861688286471[21] = 0; + out_6333703861688286471[22] = 0; + out_6333703861688286471[23] = 0; + out_6333703861688286471[24] = 0; + out_6333703861688286471[25] = 1; + out_6333703861688286471[26] = 0; + out_6333703861688286471[27] = 0; + out_6333703861688286471[28] = 1; + out_6333703861688286471[29] = 0; + out_6333703861688286471[30] = 0; + out_6333703861688286471[31] = 0; + out_6333703861688286471[32] = 0; + out_6333703861688286471[33] = 0; + out_6333703861688286471[34] = 0; + out_6333703861688286471[35] = 0; + out_6333703861688286471[36] = 0; + out_6333703861688286471[37] = 0; + out_6333703861688286471[38] = 0; + out_6333703861688286471[39] = 0; + out_6333703861688286471[40] = 0; + out_6333703861688286471[41] = 0; + out_6333703861688286471[42] = 0; + out_6333703861688286471[43] = 0; + out_6333703861688286471[44] = 1; + out_6333703861688286471[45] = 0; + out_6333703861688286471[46] = 0; + out_6333703861688286471[47] = 1; + out_6333703861688286471[48] = 0; + out_6333703861688286471[49] = 0; + out_6333703861688286471[50] = 0; + out_6333703861688286471[51] = 0; + out_6333703861688286471[52] = 0; + out_6333703861688286471[53] = 0; } -void h_10(double *state, double *unused, double *out_157308656343122283) { - out_157308656343122283[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_157308656343122283[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_157308656343122283[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_5068008641526453532) { + out_5068008641526453532[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_5068008641526453532[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_5068008641526453532[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_6282647945703286816) { - out_6282647945703286816[0] = 0; - out_6282647945703286816[1] = 9.8100000000000005*cos(state[1]); - out_6282647945703286816[2] = 0; - out_6282647945703286816[3] = 0; - out_6282647945703286816[4] = -state[8]; - out_6282647945703286816[5] = state[7]; - out_6282647945703286816[6] = 0; - out_6282647945703286816[7] = state[5]; - out_6282647945703286816[8] = -state[4]; - out_6282647945703286816[9] = 0; - out_6282647945703286816[10] = 0; - out_6282647945703286816[11] = 0; - out_6282647945703286816[12] = 1; - out_6282647945703286816[13] = 0; - out_6282647945703286816[14] = 0; - out_6282647945703286816[15] = 1; - out_6282647945703286816[16] = 0; - out_6282647945703286816[17] = 0; - out_6282647945703286816[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_6282647945703286816[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_6282647945703286816[20] = 0; - out_6282647945703286816[21] = state[8]; - out_6282647945703286816[22] = 0; - out_6282647945703286816[23] = -state[6]; - out_6282647945703286816[24] = -state[5]; - out_6282647945703286816[25] = 0; - out_6282647945703286816[26] = state[3]; - out_6282647945703286816[27] = 0; - out_6282647945703286816[28] = 0; - out_6282647945703286816[29] = 0; - out_6282647945703286816[30] = 0; - out_6282647945703286816[31] = 1; - out_6282647945703286816[32] = 0; - out_6282647945703286816[33] = 0; - out_6282647945703286816[34] = 1; - out_6282647945703286816[35] = 0; - out_6282647945703286816[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_6282647945703286816[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_6282647945703286816[38] = 0; - out_6282647945703286816[39] = -state[7]; - out_6282647945703286816[40] = state[6]; - out_6282647945703286816[41] = 0; - out_6282647945703286816[42] = state[4]; - out_6282647945703286816[43] = -state[3]; - out_6282647945703286816[44] = 0; - out_6282647945703286816[45] = 0; - out_6282647945703286816[46] = 0; - out_6282647945703286816[47] = 0; - out_6282647945703286816[48] = 0; - out_6282647945703286816[49] = 0; - out_6282647945703286816[50] = 1; - out_6282647945703286816[51] = 0; - out_6282647945703286816[52] = 0; - out_6282647945703286816[53] = 1; +void H_10(double *state, double *unused, double *out_3796470789070264043) { + out_3796470789070264043[0] = 0; + out_3796470789070264043[1] = 9.8100000000000005*cos(state[1]); + out_3796470789070264043[2] = 0; + out_3796470789070264043[3] = 0; + out_3796470789070264043[4] = -state[8]; + out_3796470789070264043[5] = state[7]; + out_3796470789070264043[6] = 0; + out_3796470789070264043[7] = state[5]; + out_3796470789070264043[8] = -state[4]; + out_3796470789070264043[9] = 0; + out_3796470789070264043[10] = 0; + out_3796470789070264043[11] = 0; + out_3796470789070264043[12] = 1; + out_3796470789070264043[13] = 0; + out_3796470789070264043[14] = 0; + out_3796470789070264043[15] = 1; + out_3796470789070264043[16] = 0; + out_3796470789070264043[17] = 0; + out_3796470789070264043[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_3796470789070264043[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_3796470789070264043[20] = 0; + out_3796470789070264043[21] = state[8]; + out_3796470789070264043[22] = 0; + out_3796470789070264043[23] = -state[6]; + out_3796470789070264043[24] = -state[5]; + out_3796470789070264043[25] = 0; + out_3796470789070264043[26] = state[3]; + out_3796470789070264043[27] = 0; + out_3796470789070264043[28] = 0; + out_3796470789070264043[29] = 0; + out_3796470789070264043[30] = 0; + out_3796470789070264043[31] = 1; + out_3796470789070264043[32] = 0; + out_3796470789070264043[33] = 0; + out_3796470789070264043[34] = 1; + out_3796470789070264043[35] = 0; + out_3796470789070264043[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_3796470789070264043[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_3796470789070264043[38] = 0; + out_3796470789070264043[39] = -state[7]; + out_3796470789070264043[40] = state[6]; + out_3796470789070264043[41] = 0; + out_3796470789070264043[42] = state[4]; + out_3796470789070264043[43] = -state[3]; + out_3796470789070264043[44] = 0; + out_3796470789070264043[45] = 0; + out_3796470789070264043[46] = 0; + out_3796470789070264043[47] = 0; + out_3796470789070264043[48] = 0; + out_3796470789070264043[49] = 0; + out_3796470789070264043[50] = 1; + out_3796470789070264043[51] = 0; + out_3796470789070264043[52] = 0; + out_3796470789070264043[53] = 1; } -void h_13(double *state, double *unused, double *out_8562780767398180943) { - out_8562780767398180943[0] = state[3]; - out_8562780767398180943[1] = state[4]; - out_8562780767398180943[2] = state[5]; +void h_13(double *state, double *unused, double *out_1215417763549020361) { + out_1215417763549020361[0] = state[3]; + out_1215417763549020361[1] = state[4]; + out_1215417763549020361[2] = state[5]; } -void H_13(double *state, double *unused, double *out_5795442117662722057) { - out_5795442117662722057[0] = 0; - out_5795442117662722057[1] = 0; - out_5795442117662722057[2] = 0; - out_5795442117662722057[3] = 1; - out_5795442117662722057[4] = 0; - out_5795442117662722057[5] = 0; - out_5795442117662722057[6] = 0; - out_5795442117662722057[7] = 0; - out_5795442117662722057[8] = 0; - out_5795442117662722057[9] = 0; - out_5795442117662722057[10] = 0; - out_5795442117662722057[11] = 0; - out_5795442117662722057[12] = 0; - out_5795442117662722057[13] = 0; - out_5795442117662722057[14] = 0; - out_5795442117662722057[15] = 0; - out_5795442117662722057[16] = 0; - out_5795442117662722057[17] = 0; - out_5795442117662722057[18] = 0; - out_5795442117662722057[19] = 0; - out_5795442117662722057[20] = 0; - out_5795442117662722057[21] = 0; - out_5795442117662722057[22] = 1; - out_5795442117662722057[23] = 0; - out_5795442117662722057[24] = 0; - out_5795442117662722057[25] = 0; - out_5795442117662722057[26] = 0; - out_5795442117662722057[27] = 0; - out_5795442117662722057[28] = 0; - out_5795442117662722057[29] = 0; - out_5795442117662722057[30] = 0; - out_5795442117662722057[31] = 0; - out_5795442117662722057[32] = 0; - out_5795442117662722057[33] = 0; - out_5795442117662722057[34] = 0; - out_5795442117662722057[35] = 0; - out_5795442117662722057[36] = 0; - out_5795442117662722057[37] = 0; - out_5795442117662722057[38] = 0; - out_5795442117662722057[39] = 0; - out_5795442117662722057[40] = 0; - out_5795442117662722057[41] = 1; - out_5795442117662722057[42] = 0; - out_5795442117662722057[43] = 0; - out_5795442117662722057[44] = 0; - out_5795442117662722057[45] = 0; - out_5795442117662722057[46] = 0; - out_5795442117662722057[47] = 0; - out_5795442117662722057[48] = 0; - out_5795442117662722057[49] = 0; - out_5795442117662722057[50] = 0; - out_5795442117662722057[51] = 0; - out_5795442117662722057[52] = 0; - out_5795442117662722057[53] = 0; +void H_13(double *state, double *unused, double *out_8900766386688932344) { + out_8900766386688932344[0] = 0; + out_8900766386688932344[1] = 0; + out_8900766386688932344[2] = 0; + out_8900766386688932344[3] = 1; + out_8900766386688932344[4] = 0; + out_8900766386688932344[5] = 0; + out_8900766386688932344[6] = 0; + out_8900766386688932344[7] = 0; + out_8900766386688932344[8] = 0; + out_8900766386688932344[9] = 0; + out_8900766386688932344[10] = 0; + out_8900766386688932344[11] = 0; + out_8900766386688932344[12] = 0; + out_8900766386688932344[13] = 0; + out_8900766386688932344[14] = 0; + out_8900766386688932344[15] = 0; + out_8900766386688932344[16] = 0; + out_8900766386688932344[17] = 0; + out_8900766386688932344[18] = 0; + out_8900766386688932344[19] = 0; + out_8900766386688932344[20] = 0; + out_8900766386688932344[21] = 0; + out_8900766386688932344[22] = 1; + out_8900766386688932344[23] = 0; + out_8900766386688932344[24] = 0; + out_8900766386688932344[25] = 0; + out_8900766386688932344[26] = 0; + out_8900766386688932344[27] = 0; + out_8900766386688932344[28] = 0; + out_8900766386688932344[29] = 0; + out_8900766386688932344[30] = 0; + out_8900766386688932344[31] = 0; + out_8900766386688932344[32] = 0; + out_8900766386688932344[33] = 0; + out_8900766386688932344[34] = 0; + out_8900766386688932344[35] = 0; + out_8900766386688932344[36] = 0; + out_8900766386688932344[37] = 0; + out_8900766386688932344[38] = 0; + out_8900766386688932344[39] = 0; + out_8900766386688932344[40] = 0; + out_8900766386688932344[41] = 1; + out_8900766386688932344[42] = 0; + out_8900766386688932344[43] = 0; + out_8900766386688932344[44] = 0; + out_8900766386688932344[45] = 0; + out_8900766386688932344[46] = 0; + out_8900766386688932344[47] = 0; + out_8900766386688932344[48] = 0; + out_8900766386688932344[49] = 0; + out_8900766386688932344[50] = 0; + out_8900766386688932344[51] = 0; + out_8900766386688932344[52] = 0; + out_8900766386688932344[53] = 0; } -void h_14(double *state, double *unused, double *out_8175075787359245864) { - out_8175075787359245864[0] = state[6]; - out_8175075787359245864[1] = state[7]; - out_8175075787359245864[2] = state[8]; +void h_14(double *state, double *unused, double *out_1254666721235298923) { + out_1254666721235298923[0] = state[6]; + out_1254666721235298923[1] = state[7]; + out_1254666721235298923[2] = state[8]; } -void H_14(double *state, double *unused, double *out_1957882315434756334) { - out_1957882315434756334[0] = 0; - out_1957882315434756334[1] = 0; - out_1957882315434756334[2] = 0; - out_1957882315434756334[3] = 0; - out_1957882315434756334[4] = 0; - out_1957882315434756334[5] = 0; - out_1957882315434756334[6] = 1; - out_1957882315434756334[7] = 0; - out_1957882315434756334[8] = 0; - out_1957882315434756334[9] = 0; - out_1957882315434756334[10] = 0; - out_1957882315434756334[11] = 0; - out_1957882315434756334[12] = 0; - out_1957882315434756334[13] = 0; - out_1957882315434756334[14] = 0; - out_1957882315434756334[15] = 0; - out_1957882315434756334[16] = 0; - out_1957882315434756334[17] = 0; - out_1957882315434756334[18] = 0; - out_1957882315434756334[19] = 0; - out_1957882315434756334[20] = 0; - out_1957882315434756334[21] = 0; - out_1957882315434756334[22] = 0; - out_1957882315434756334[23] = 0; - out_1957882315434756334[24] = 0; - out_1957882315434756334[25] = 1; - out_1957882315434756334[26] = 0; - out_1957882315434756334[27] = 0; - out_1957882315434756334[28] = 0; - out_1957882315434756334[29] = 0; - out_1957882315434756334[30] = 0; - out_1957882315434756334[31] = 0; - out_1957882315434756334[32] = 0; - out_1957882315434756334[33] = 0; - out_1957882315434756334[34] = 0; - out_1957882315434756334[35] = 0; - out_1957882315434756334[36] = 0; - out_1957882315434756334[37] = 0; - out_1957882315434756334[38] = 0; - out_1957882315434756334[39] = 0; - out_1957882315434756334[40] = 0; - out_1957882315434756334[41] = 0; - out_1957882315434756334[42] = 0; - out_1957882315434756334[43] = 0; - out_1957882315434756334[44] = 1; - out_1957882315434756334[45] = 0; - out_1957882315434756334[46] = 0; - out_1957882315434756334[47] = 0; - out_1957882315434756334[48] = 0; - out_1957882315434756334[49] = 0; - out_1957882315434756334[50] = 0; - out_1957882315434756334[51] = 0; - out_1957882315434756334[52] = 0; - out_1957882315434756334[53] = 0; +void H_14(double *state, double *unused, double *out_8149799355681780616) { + out_8149799355681780616[0] = 0; + out_8149799355681780616[1] = 0; + out_8149799355681780616[2] = 0; + out_8149799355681780616[3] = 0; + out_8149799355681780616[4] = 0; + out_8149799355681780616[5] = 0; + out_8149799355681780616[6] = 1; + out_8149799355681780616[7] = 0; + out_8149799355681780616[8] = 0; + out_8149799355681780616[9] = 0; + out_8149799355681780616[10] = 0; + out_8149799355681780616[11] = 0; + out_8149799355681780616[12] = 0; + out_8149799355681780616[13] = 0; + out_8149799355681780616[14] = 0; + out_8149799355681780616[15] = 0; + out_8149799355681780616[16] = 0; + out_8149799355681780616[17] = 0; + out_8149799355681780616[18] = 0; + out_8149799355681780616[19] = 0; + out_8149799355681780616[20] = 0; + out_8149799355681780616[21] = 0; + out_8149799355681780616[22] = 0; + out_8149799355681780616[23] = 0; + out_8149799355681780616[24] = 0; + out_8149799355681780616[25] = 1; + out_8149799355681780616[26] = 0; + out_8149799355681780616[27] = 0; + out_8149799355681780616[28] = 0; + out_8149799355681780616[29] = 0; + out_8149799355681780616[30] = 0; + out_8149799355681780616[31] = 0; + out_8149799355681780616[32] = 0; + out_8149799355681780616[33] = 0; + out_8149799355681780616[34] = 0; + out_8149799355681780616[35] = 0; + out_8149799355681780616[36] = 0; + out_8149799355681780616[37] = 0; + out_8149799355681780616[38] = 0; + out_8149799355681780616[39] = 0; + out_8149799355681780616[40] = 0; + out_8149799355681780616[41] = 0; + out_8149799355681780616[42] = 0; + out_8149799355681780616[43] = 0; + out_8149799355681780616[44] = 1; + out_8149799355681780616[45] = 0; + out_8149799355681780616[46] = 0; + out_8149799355681780616[47] = 0; + out_8149799355681780616[48] = 0; + out_8149799355681780616[49] = 0; + out_8149799355681780616[50] = 0; + out_8149799355681780616[51] = 0; + out_8149799355681780616[52] = 0; + out_8149799355681780616[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_6769094018143345471) { - err_fun(nom_x, delta_x, out_6769094018143345471); +void pose_err_fun(double *nom_x, double *delta_x, double *out_8587424029422482613) { + err_fun(nom_x, delta_x, out_8587424029422482613); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7105507959452432978) { - inv_err_fun(nom_x, true_x, out_7105507959452432978); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8786163150320395038) { + inv_err_fun(nom_x, true_x, out_8786163150320395038); } -void pose_H_mod_fun(double *state, double *out_6619985577834044559) { - H_mod_fun(state, out_6619985577834044559); +void pose_H_mod_fun(double *state, double *out_2404492370394702667) { + H_mod_fun(state, out_2404492370394702667); } -void pose_f_fun(double *state, double dt, double *out_8911446702564060395) { - f_fun(state, dt, out_8911446702564060395); +void pose_f_fun(double *state, double dt, double *out_442464745730678105) { + f_fun(state, dt, out_442464745730678105); } -void pose_F_fun(double *state, double dt, double *out_8001086797165012903) { - F_fun(state, dt, out_8001086797165012903); +void pose_F_fun(double *state, double dt, double *out_68862599699564370) { + F_fun(state, dt, out_68862599699564370); } -void pose_h_4(double *state, double *unused, double *out_700157376215369055) { - h_4(state, unused, out_700157376215369055); +void pose_h_4(double *state, double *unused, double *out_6968799398651119641) { + h_4(state, unused, out_6968799398651119641); } -void pose_H_4(double *state, double *unused, double *out_5040670747730128630) { - H_4(state, unused, out_5040670747730128630); +void pose_H_4(double *state, double *unused, double *out_6333703861688286471) { + H_4(state, unused, out_6333703861688286471); } -void pose_h_10(double *state, double *unused, double *out_157308656343122283) { - h_10(state, unused, out_157308656343122283); +void pose_h_10(double *state, double *unused, double *out_5068008641526453532) { + h_10(state, unused, out_5068008641526453532); } -void pose_H_10(double *state, double *unused, double *out_6282647945703286816) { - H_10(state, unused, out_6282647945703286816); +void pose_H_10(double *state, double *unused, double *out_3796470789070264043) { + H_10(state, unused, out_3796470789070264043); } -void pose_h_13(double *state, double *unused, double *out_8562780767398180943) { - h_13(state, unused, out_8562780767398180943); +void pose_h_13(double *state, double *unused, double *out_1215417763549020361) { + h_13(state, unused, out_1215417763549020361); } -void pose_H_13(double *state, double *unused, double *out_5795442117662722057) { - H_13(state, unused, out_5795442117662722057); +void pose_H_13(double *state, double *unused, double *out_8900766386688932344) { + H_13(state, unused, out_8900766386688932344); } -void pose_h_14(double *state, double *unused, double *out_8175075787359245864) { - h_14(state, unused, out_8175075787359245864); +void pose_h_14(double *state, double *unused, double *out_1254666721235298923) { + h_14(state, unused, out_1254666721235298923); } -void pose_H_14(double *state, double *unused, double *out_1957882315434756334) { - H_14(state, unused, out_1957882315434756334); +void pose_H_14(double *state, double *unused, double *out_8149799355681780616) { + H_14(state, unused, out_8149799355681780616); } void pose_predict(double *in_x, double *in_P, double *in_Q, double dt) { predict(in_x, in_P, in_Q, dt); diff --git a/openpilot/selfdrive/locationd/models/generated/pose.h b/openpilot/selfdrive/locationd/models/generated/pose.h index 175ab6ab09..31fc6d2813 100644 --- a/openpilot/selfdrive/locationd/models/generated/pose.h +++ b/openpilot/selfdrive/locationd/models/generated/pose.h @@ -5,18 +5,18 @@ void pose_update_4(double *in_x, double *in_P, double *in_z, double *in_R, doubl void pose_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); -void pose_err_fun(double *nom_x, double *delta_x, double *out_6769094018143345471); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_7105507959452432978); -void pose_H_mod_fun(double *state, double *out_6619985577834044559); -void pose_f_fun(double *state, double dt, double *out_8911446702564060395); -void pose_F_fun(double *state, double dt, double *out_8001086797165012903); -void pose_h_4(double *state, double *unused, double *out_700157376215369055); -void pose_H_4(double *state, double *unused, double *out_5040670747730128630); -void pose_h_10(double *state, double *unused, double *out_157308656343122283); -void pose_H_10(double *state, double *unused, double *out_6282647945703286816); -void pose_h_13(double *state, double *unused, double *out_8562780767398180943); -void pose_H_13(double *state, double *unused, double *out_5795442117662722057); -void pose_h_14(double *state, double *unused, double *out_8175075787359245864); -void pose_H_14(double *state, double *unused, double *out_1957882315434756334); +void pose_err_fun(double *nom_x, double *delta_x, double *out_8587424029422482613); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_8786163150320395038); +void pose_H_mod_fun(double *state, double *out_2404492370394702667); +void pose_f_fun(double *state, double dt, double *out_442464745730678105); +void pose_F_fun(double *state, double dt, double *out_68862599699564370); +void pose_h_4(double *state, double *unused, double *out_6968799398651119641); +void pose_H_4(double *state, double *unused, double *out_6333703861688286471); +void pose_h_10(double *state, double *unused, double *out_5068008641526453532); +void pose_H_10(double *state, double *unused, double *out_3796470789070264043); +void pose_h_13(double *state, double *unused, double *out_1215417763549020361); +void pose_H_13(double *state, double *unused, double *out_8900766386688932344); +void pose_h_14(double *state, double *unused, double *out_1254666721235298923); +void pose_H_14(double *state, double *unused, double *out_8149799355681780616); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/openpilot/selfdrive/pandad/pandad b/openpilot/selfdrive/pandad/pandad index 0da0a54894..8e4971f703 100755 Binary files a/openpilot/selfdrive/pandad/pandad and b/openpilot/selfdrive/pandad/pandad differ diff --git a/openpilot/starpilot/aol/tests/test_recorded_temporary_intent.py b/openpilot/starpilot/aol/tests/test_recorded_temporary_intent.py new file mode 100644 index 0000000000..520ad586d4 --- /dev/null +++ b/openpilot/starpilot/aol/tests/test_recorded_temporary_intent.py @@ -0,0 +1,314 @@ +from contextlib import ExitStack +from unittest import mock + +import pytest + +from openpilot.cereal import log +from openpilot.common.prefix import OpenpilotPrefix +from openpilot.starpilot.aol.tests import test_ioniq_availability as fixture +from openpilot.starpilot.aol.tests.test_temporary_steering_sequence import baseline, leases + + +def test_actual_eps_recovered_car_state_before_withdrawal_feedback(): + case = fixture.IoniqAvailabilityTests(methodName='runTest') + with ExitStack() as cleanup: + cleanup.callback(case.doCleanups) + cleanup.enter_context(OpenpilotPrefix()) + sd, _, drive, card, receive = baseline(case) + original = leases(card.aol_transport_feedback) + temporary = fixture.car_state() + temporary.gasPressed = temporary.steerFaultTemporary = True + for tick in range(152, 166): + axis, _, command = drive(tick, gas=True, observed_state=temporary, allowed=False, + native_request_lat=False, native_request_long=False) + assert not command.latActive and not command.longActive + assert not axis.lateralActive and not axis.longitudinalActive + assert receive(10_000_000_000 + tick * 10_000_000, axis=sd.pm.messages['aolAxisState'], + event=sd.pm.messages['onroadEvents'], state=temporary) + recovered = fixture.car_state() + recovered.gasPressed = True + assert receive(11_655_000_000, state=recovered) + assert card.aol_card_intent.allowed_latch + assert leases(card.aol_transport_feedback) == original + assert not card.sm['aolAxisState'].lateralActive and not card.sm['carControl'].longActive + + +@pytest.mark.parametrize('event_name', ['selfdrivedLagging', 'locationdTemporaryError']) +def test_actual_temporary_host_event_keeps_only_armed_intent(event_name): + case = fixture.IoniqAvailabilityTests(methodName='runTest') + with ExitStack() as cleanup: + cleanup.callback(case.doCleanups) + cleanup.enter_context(OpenpilotPrefix()) + sd, _, drive, card, receive = case._ioniq_availability_callers(full_op=False) + original = leases(card.aol_transport_feedback) + axis, state, command = drive(2, gas=True, critical_event=getattr(log.OnroadEvent.EventName, event_name), + native_request_long=False) + assert not axis.lateralActive and not axis.longitudinalActive + assert not command.latActive and not command.longActive and not state.enabled + assert receive(10_020_000_000, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents']) + assert card.aol_card_intent.allowed_latch + assert leases(card.aol_transport_feedback) == original + + +@pytest.mark.parametrize('fault', ['native_denial', 'panda_fault', 'permanent_eps', 'invalid_can', 'hard_event']) +def test_recorded_temporary_host_path_never_masks_native_or_hard_fault(fault): + case = fixture.IoniqAvailabilityTests(methodName='runTest') + with ExitStack() as cleanup: + cleanup.callback(case.doCleanups) + cleanup.enter_context(OpenpilotPrefix()) + sd, _, drive, card, receive = case._ioniq_availability_callers(full_op=False) + axis, _, command = drive(2, gas=True, critical_event=log.OnroadEvent.EventName.locationdTemporaryError, + allowed=fault != 'native_denial', panda_fault=fault == 'panda_fault', native_request_long=False) + state = fixture.car_state() + state.steerFaultPermanent = fault == 'permanent_eps' + state.canValid = fault != 'invalid_can' + proof, event = sd.pm.messages['aolAxisState'], sd.pm.messages['onroadEvents'] + if fault == 'hard_event': + event = event.as_builder() + event.onroadEvents[0].immediateDisable = True + assert not receive(10_020_000_000, axis=proof, event=event, state=state) + assert not command.longActive + + +def test_actual_ordinary_brake_with_current_lateral_grant_never_reenables_long(): + case = fixture.IoniqAvailabilityTests(methodName='runTest') + with ExitStack() as cleanup: + cleanup.callback(case.doCleanups) + cleanup.enter_context(OpenpilotPrefix()) + sd, controls, drive, card, receive = case._ioniq_availability_callers(full_op=True) + axis, state, command = drive(2, brake=True, native_request_long=False) + assert axis.lateralActive and not axis.longitudinalActive + assert command.latActive and not command.longActive + assert not state.enabled + actual_control = controls.pm.messages['carControl'] + assert actual_control.logMonoTime == 10_020_000_000 + assert not actual_control.carControl.longActive + sd.sm.update_msgs(10.02, [actual_control]) + observed = fixture.car_state(brake=True) + assert receive(10_020_000_000, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'], state=observed) + assert card.aol_card_intent.allowed_latch + assert not card.sm['carControl'].longActive + + +@pytest.mark.parametrize('event_first', [False, True]) +def test_actual_repeated_lag_after_healthy_pulse_and_native_withdrawal(event_first): + case = fixture.IoniqAvailabilityTests(methodName='runTest') + with ExitStack() as cleanup: + cleanup.callback(case.doCleanups) + cleanup.enter_context(OpenpilotPrefix()) + sd, _, drive, card, receive = case._ioniq_availability_callers(full_op=False) + with mock.patch.object(type(sd.rk), 'lagging', new_callable=mock.PropertyMock, return_value=True): + axis, _, command = drive(2, gas=True, native_request_long=False) + assert not axis.lateralActive and not command.latActive and not command.longActive + assert receive(10_020_000_000, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents']) + healthy, _, command = drive(3, gas=True, native_request_long=False) + assert healthy.lateralActive and command.latActive and not command.longActive + healthy_proof = sd.pm.messages['aolAxisState'] + assert receive(10_030_000_000, axis=healthy_proof, event=sd.pm.messages['onroadEvents']) + original = leases(card.aol_transport_feedback) + with mock.patch.object(type(sd.rk), 'lagging', new_callable=mock.PropertyMock, return_value=True): + axis, _, command = drive(4, gas=True, native_request_long=False) + proof, event = sd.pm.messages['aolAxisState'], sd.pm.messages['onroadEvents'] + assert not axis.lateralActive and not command.latActive and not command.longActive + if event_first: + assert receive(10_040_000_000, axis=healthy_proof, event=event) + assert receive(10_040_100_000, axis=proof, event=event) + assert leases(card.aol_transport_feedback) == original + with mock.patch.object(type(sd.rk), 'lagging', new_callable=mock.PropertyMock, return_value=True): + axis, state, command = drive(5, gas=True, allowed=False, native_request_lat=False, native_request_long=False) + assert not axis.lateralActive and not axis.longitudinalActive + assert not command.latActive and not command.longActive and not state.enabled + assert receive(10_050_000_000, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents']) + assert card.aol_card_intent.allowed_latch + + +@pytest.mark.parametrize('fault', ['none', 'native_lat_denial', 'extra_user', 'hard', 'source_reorder', 'epoch', 'session', 'expired']) +def test_actual_brake_old_combined_native_and_new_panda_long_withdrawal(fault): + case = fixture.IoniqAvailabilityTests(methodName='runTest') + with ExitStack() as cleanup: + cleanup.callback(case.doCleanups) + cleanup.enter_context(OpenpilotPrefix()) + sd, controls, drive, card, receive = case._ioniq_availability_callers(full_op=True) + old_native = sd.sm['aolSafetyWire'] + old_native_stamp = sd.sm.logMonoTime['aolSafetyWire'] + assert card.aol_transport_feedback.signature[10] + axis, state, command = drive(2, brake=True, native_request_long=False) + assert axis.lateralActive and not axis.longitudinalActive + assert command.latActive and not command.longActive and not state.enabled + proof = sd.pm.messages['aolAxisState'].as_builder() + event = sd.pm.messages['onroadEvents'].as_builder() + if fault == 'source_reorder': + proof.aolAxisState.sourceCarStateMonoTime = card.aol_transport_feedback.lateral_source_ns - 5_000_000 + elif fault == 'epoch': + proof.aolAxisState.intentIntegrityEpoch += 1 + elif fault == 'session': + proof.aolAxisState.sessionId = proof.aolAxisState.faultSessionId = 'wrong' + elif fault == 'expired': + proof.aolAxisState.validUntilMonoTime = 10_019_999_999 + elif fault in ('extra_user', 'hard'): + event.onroadEvents[0].name = log.OnroadEvent.EventName.buttonCancel if fault == 'extra_user' else log.OnroadEvent.EventName.controlsMismatch + event.onroadEvents[0].userDisable = fault == 'extra_user' + event.onroadEvents[0].immediateDisable = fault == 'hard' + from openpilot.cereal import messaging + native = messaging.new_message('aolSafetyWire', len(old_native), valid=True, logMonoTime=old_native_stamp) + native.aolSafetyWire = old_native + if fault == 'native_lat_denial': + from dataclasses import replace + from openpilot.starpilot.aol.wire import decode_safety, encode_safety + decoded = decode_safety(native.aolSafetyWire) + native.aolSafetyWire = encode_safety(replace(decoded, lateralAllowed=False)) + panda = messaging.new_message('pandaStates', len(sd.sm['pandaStates']), valid=True, logMonoTime=10_020_000_000) + panda.pandaStates = sd.sm['pandaStates'] + for slot in panda.pandaStates: + slot.controlsAllowed = False + sd.sm.update_msgs(10.02, [native.as_reader(), panda.as_reader()]) + actual_control = controls.pm.messages['carControl'] + assert actual_control.logMonoTime == 10_020_000_000 + assert not actual_control.carControl.longActive + sd.sm.update_msgs(10.02, [actual_control]) + observed = fixture.car_state(brake=True) + latched = receive(10_020_000_000, axis=proof.as_reader(), event=event.as_reader(), state=observed) + assert latched == (fault == 'none') + assert not card.sm['carControl'].longActive + if fault != 'none': + assert not card.aol_transport_feedback.temporary_host_qualified + + +@pytest.mark.parametrize('reset', ['session', 'configuration', 'rearm']) +def test_verified_eps_marker_cannot_cross_owner_reset(reset): + case = fixture.IoniqAvailabilityTests(methodName='runTest') + with ExitStack() as cleanup: + cleanup.callback(case.doCleanups) + cleanup.enter_context(OpenpilotPrefix()) + sd, _, drive, card, receive = baseline(case) + temporary = fixture.car_state() + temporary.steerFaultTemporary = True + axis, _, command = drive(152, gas=True, observed_state=temporary, allowed=False, + native_request_lat=False, native_request_long=False) + proof = sd.pm.messages['aolAxisState'].as_builder() + event = sd.pm.messages['onroadEvents'] + assert receive(11_520_000_000, axis=proof.as_reader(), event=event, state=temporary) + assert card.aol_transport_feedback.temporary_eps_event_ns + if reset == 'session': + proof.aolAxisState.sessionId = proof.aolAxisState.faultSessionId = 'different-session' + elif reset == 'configuration': + cp = card.CP.as_reader().as_builder() + cp.alternativeExperience ^= 1 + card.CP = cp.as_reader() + else: + from opendbc.car.structs import car + cancel = fixture.car_state() + cancel.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.cancel, pressed=True)] + card.aol_card_intent.update(cancel, now_ns=11_520_050_000) + assert not card.aol_card_intent.allowed_latch + receive(11_520_100_000, axis=proof.as_reader(), event=event, state=temporary) + assert card.aol_transport_feedback.temporary_eps_event_ns == 0 + assert not command.latActive and not command.longActive + + +def test_full_long_location_error_event_first_withdraw_recover_brake_and_lkas_off(): + case = fixture.IoniqAvailabilityTests(methodName='runTest') + with ExitStack() as cleanup: + cleanup.callback(case.doCleanups) + cleanup.enter_context(OpenpilotPrefix()) + sd, controls, drive, card, receive = case._ioniq_availability_callers(full_op=True) + actual_events = sd.update_events + location_error = [True] + + def events(cs): + result = actual_events(cs) + if location_error[0]: + sd.events.add(log.OnroadEvent.EventName.locationdTemporaryError) + return result + + def publish_to_card(tick, *, axis=None, event=None, state=None): + actual_control = controls.pm.messages['carControl'] + assert actual_control.logMonoTime == 10_000_000_000 + tick * 10_000_000 + actual_update = card.sm.update_msgs + + def transport(timestamp, frames): + return actual_update(timestamp, [actual_control if frame.which() == 'carControl' else frame for frame in frames]) + + with mock.patch.object(card.sm, 'update_msgs', side_effect=transport): + return receive(actual_control.logMonoTime, axis=axis, event=event, state=state) + + from openpilot.cereal import messaging + from openpilot.cereal.services import SERVICE_LIST + optional_due = {} + optional = [name for name in ('alertDebug', 'lateralManeuverPlan', 'laneChangeAssistWire', + 'starpilotLongitudinalPlan', 'slcState', sd.gps_location_service) if name in sd.sm.data] + + def drive_with_sources(tick, **kwargs): + now = 10_000_000_000 + tick * 10_000_000 + frames = [] + for name in optional: + frequency = SERVICE_LIST[name].frequency + if frequency <= 0 or now < optional_due.get(name, 0): + continue + optional_due[name] = now + round(1e9 / frequency) + if name == 'laneChangeAssistWire': + from openpilot.starpilot.lateral.lane_change_status_wire import LaneChangeStatus, Phase, Direction, encode + payload = encode(LaneChangeStatus('fixture-model', tick, tick * 5, now, now, now + 100_000_000, + Phase.MANUAL_REQUIRED, Direction.NONE, False, False)) + sample = messaging.new_message(name, len(payload), valid=True, logMonoTime=now) + sample.laneChangeAssistWire = payload + else: + sample = messaging.new_message(name, valid=True, logMonoTime=now) + frames.append(sample.as_reader()) + actual_update = sd.sm.update_msgs + delivered = [False] + + def transport(timestamp, messages): + if not delivered[0]: + delivered[0] = True + messages = list(messages) + frames + return actual_update(timestamp, messages) + + with mock.patch.object(sd.sm, 'update_msgs', side_effect=transport): + return drive(tick, **kwargs) + + old_proof = sd.pm.messages['aolAxisState'] + with mock.patch.object(sd, 'update_events', side_effect=events): + axis, state, command = drive_with_sources(2) + assert state.enabled and state.active + assert state.state == log.SelfdriveState.OpenpilotState.softDisabling + assert state.alertType == 'locationdTemporaryError/softDisable' + assert not axis.lateralActive and not axis.longitudinalActive + assert not command.latActive and not command.longActive + assert sd.state_machine.soft_disable_timer == 300 + # The actual newer event/command arrive before the new axis publication. + assert publish_to_card(2, axis=old_proof, event=sd.pm.messages['onroadEvents']) + assert card.aol_card_intent.allowed_latch + assert card.aol_transport_feedback.signature[4] == old_proof.aolAxisState.validUntilMonoTime + assert receive(10_020_100_000, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents']) + assert card.aol_card_intent.allowed_latch + # Follow the unchanged ordinary SOFT countdown, not a forced fixture disable. + for tick in range(3, 303): + axis, state, command = drive_with_sources(tick) + assert publish_to_card(tick, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents']) + assert not state.enabled and not state.active + assert not axis.lateralActive and not axis.longitudinalActive + assert not command.latActive and not command.longActive + assert card.aol_card_intent.allowed_latch + location_error[0] = False + axis, state, command = drive_with_sources(303, native_request_long=False) + assert not state.enabled and not state.active + assert axis.lateralActive and not axis.longitudinalActive + assert command.latActive and not command.longActive + assert publish_to_card(303, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents']) + axis, state, command = drive_with_sources(304, brake=True, native_request_long=False) + assert not state.enabled and not axis.longitudinalActive and not command.longActive + assert publish_to_card(304, axis=sd.pm.messages['aolAxisState'], event=sd.pm.messages['onroadEvents'], + state=fixture.car_state(brake=True)) + from opendbc.car.structs import car + release = fixture.car_state() + release.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.lkas, pressed=False)] + assert receive(13_040_050_000, state=release) + assert not card.aol_card_intent._lkas_held + off = fixture.car_state() + off.buttonEvents = [car.CarState.ButtonEvent(type=car.CarState.ButtonEvent.Type.lkas, pressed=True)] + assert not receive(13_040_100_000, state=off) + assert not card.aol_card_intent.allowed_latch + axis, state, command = drive_with_sources(305, native_request_long=False) + assert not axis.lateralActive and not axis.longitudinalActive + assert not command.latActive and not command.longActive and not state.enabled diff --git a/openpilot/starpilot/aol/transport_pause.py b/openpilot/starpilot/aol/transport_pause.py index c6422e5939..923ce6d286 100644 --- a/openpilot/starpilot/aol/transport_pause.py +++ b/openpilot/starpilot/aol/transport_pause.py @@ -267,6 +267,67 @@ class TransportPauseFeedback: self.healthy_native_ns = 0 self.healthy_inventory = None + def _temporary_host_intent_current(self, sm, cp, cs, axis, stamp, now_ns, *, latched, producer): + """A fresh inactive proof may retain intent, never native/output authority.""" + from openpilot.starpilot.nostalgia import physical_cancel + if physical_cancel(cs) or cs.steeringDisengage: + return False + events = sm['onroadEvents'] + faults = [event for event in events if event.immediateDisable or event.softDisable] + names = (log.OnroadEvent.EventName.selfdrivedLagging, log.OnroadEvent.EventName.locationdTemporaryError) + temporary = bool(len(faults) == 1 and faults[0].name in names and faults[0].softDisable and not faults[0].immediateDisable) + eps_recovery = bool(not cs.steerFaultTemporary and getattr(self, 'temporary_eps_event_ns', 0) and + axis.faultEventMonoTime == self.temporary_eps_event_ns and + all(not event.immediateDisable and + (not event.softDisable or event.name == log.OnroadEvent.EventName.steerTempUnavailable) for event in events)) + users = [event for event in events if event.userDisable] + pedal = bool(cs.brakePressed and users and all(event.name == log.OnroadEvent.EventName.pedalPressed and + not event.softDisable and not event.immediateDisable for event in users) and not faults) + location_only = bool(temporary and faults[0].name == log.OnroadEvent.EventName.locationdTemporaryError) + if axis.longitudinalActive and not location_only or not (temporary or eps_recovery or pedal): + return False + if (not latched or self.failed or self.paused or self.pending_event or self.retention_started_ns or + not self.session or str(axis.sessionId) != self.session or + not producer or producer != self.availability_producer or + str(getattr(axis, 'intentProducerSessionId', '')) != producer or + int(getattr(axis, 'intentIntegrityEpoch', 0)) != self.availability_epoch or + not self.lateral_inventory or not self.lateral_baseline_ns or + not cs.canValid or cs.canTimeout or cs.steerFaultPermanent or cs.accFaulted or cs.steerFaultTemporary or + users and not pedal or str(axis.faultReason) not in ('none', 'critical') or + not 0 < self.lateral_source_ns <= axis.sourceCarStateMonoTime <= stamp <= now_ns or + now_ns - axis.sourceCarStateMonoTime > AXIS_MAX_AGE_NS or + not sm.seen['onroadEvents'] or not sm.valid['onroadEvents'] or not sm.alive['onroadEvents']): + return False + event_ns = int(sm.logMonoTime['onroadEvents']) + if not 0 < event_ns <= now_ns or now_ns - event_ns > 1_500_000_000: + return False + withdrawn = not axis.desiredLateral and not axis.lateralActive and not axis.desiredLongitudinal and not axis.longitudinalActive + event_first = bool(temporary and str(axis.faultReason) == 'none' and + self.signature is not None and self.signature[9] and self.signature[11] and self.signature[13] and + axis.sequence >= self.sequence and stamp >= self.signature[2] and + temporary and axis.desiredLateral and axis.lateralActive and axis.nativeAcknowledged and + 0 < axis.faultEventMonoTime < event_ns and now_ns - event_ns <= AXIS_MAX_AGE_NS) + if not (event_first or temporary and axis.faultEventMonoTime == event_ns and + (withdrawn or str(axis.faultReason) == 'none' and axis.desiredLateral and + axis.lateralActive and axis.nativeAcknowledged) or eps_recovery and withdrawn or + pedal and str(axis.faultReason) == 'none' and axis.desiredLateral and axis.lateralActive and + not axis.desiredLongitudinal and not axis.longitudinalActive and + axis.nativeAcknowledged and axis.faultEventMonoTime == event_ns): + return False + native = _configured_transport_native(sm, cp, self.session, now_ns, require_longitudinal=not pedal) + if (native is None or not native.pandaInventory or native.observedMonoTime < self.lateral_native_ns or + (native.pandaSerial, tuple(p.hardwareSerial for p in native.pandaInventory)) != self.lateral_inventory or + native.requestedLateral and not native.lateralAllowed or + pedal and not (native.requestedLateral and native.lateralAllowed) or + axis.lateralActive and not (native.requestedLateral and native.lateralAllowed) or + axis.longitudinalActive and not (native.requestedLongitudinal and native.longitudinalAllowed)): + return False + from openpilot.starpilot.aol.vehicle import policy_for + if not policy_for(cp).intent_supported: + return False + self.temporary_host_qualified = True + return True + def _temporary_lag_intent_current(self, sm, cp, cs, axis, stamp, now_ns, *, latched, producer, signature): """Retain intent only through isolated lag or its bounded critical/event join.""" reason = str(axis.faultReason) @@ -413,6 +474,8 @@ class TransportPauseFeedback: native.observedMonoTime < self.lateral_native_ns or (native.pandaSerial, tuple(p.hardwareSerial for p in native.pandaInventory)) != self.lateral_inventory): return False + if cs.steerFaultTemporary and matched_withdrawal: + self.temporary_eps_event_ns = int(axis.faultEventMonoTime) if pending_withdrawal: # Keep only intent while this exact physical-EPS occurrence awaits its # event socket. Duplicate/newer proofs cannot move the original anchor. @@ -782,12 +845,15 @@ class TransportPauseFeedback: return True def observe(self, sm, cp, cs, now_ns, *, latched=True, cancel_source_ns=0, brake_source_ns=0, gm_withdrawal_source_ns=0, intent_producer_session=''): + self.temporary_host_qualified = False self.qualified = False self.awaiting = False self.retention_qualified = False self.retention_pending = False try: axis = sm['aolAxisState'] + if not latched or cp_identity(cp) != self.cp or self.session and str(axis.sessionId) != self.session: + self.temporary_eps_event_ns = 0 stamp = int(sm.logMonoTime['aolAxisState']) reason = str(axis.faultReason) signature = (str(axis.sessionId), int(axis.sequence), stamp, int(axis.observedMonoTime), @@ -809,6 +875,10 @@ class TransportPauseFeedback: # healthy baseline timestamp remains unchanged. self.sequence, self.signature = int(axis.sequence), signature return False + if current and continuity and self._temporary_host_intent_current(sm, cp, cs, axis, stamp, now_ns, + latched=latched, producer=intent_producer_session): + self.sequence, self.signature = int(axis.sequence), signature + return False if current and continuity and self._temporary_steering_intent_current(sm, cp, cs, axis, stamp, now_ns, latched=latched, producer=intent_producer_session, signature=signature): # Advance only received sequence identity; do not renew the prior output, @@ -873,6 +943,7 @@ class TransportPauseFeedback: return False current = bool(current and (transport_healthy or cancel_native is not None)) if current and baseline and not latched: + self.temporary_eps_event_ns = 0 self.session = '' self.pending_event = 0 self.failed = False @@ -966,6 +1037,7 @@ class TransportPauseFeedback: self.lateral_only_baseline = bool(lateral_baseline and self.availability_producer and not axis.desiredLongitudinal and not axis.longitudinalActive and not native.requestedLongitudinal) if lateral_baseline and native is not None: + self.temporary_eps_event_ns = 0 self.lateral_native_ns = int(native.observedMonoTime) self.lateral_inventory = (native.pandaSerial, tuple(p.hardwareSerial for p in native.pandaInventory)) else: diff --git a/openpilot/starpilot/conditional_mode/tests/test_paired_clock.py b/openpilot/starpilot/conditional_mode/tests/test_paired_clock.py index dee22a6a14..d07ce32b4b 100644 --- a/openpilot/starpilot/conditional_mode/tests/test_paired_clock.py +++ b/openpilot/starpilot/conditional_mode/tests/test_paired_clock.py @@ -98,13 +98,13 @@ class TestPairedClock(unittest.TestCase): self.assertTrue(self.sample(event, expiry, message_ns=now, receipt_ns=now, model_ns=expiry, car_state_ns=expiry).accepted) self.assertEqual(self.fixture.consumer.last, original) - self.assertEqual(event.to_bytes(), original_envelope) + self.assertEqual(event.as_reader().as_builder().to_bytes(), original_envelope) self.assertFalse(self.sample(event, expiry + 1, message_ns=now, receipt_ns=now, model_ns=expiry + 1, car_state_ns=expiry + 1).accepted) self.assertEqual(self.fixture.consumer.rejection, 'proposal_invalid') self.assertEqual(self.fixture.consumer.last, original) self.assertEqual(self.fixture.consumer.last.expires_ns, expiry) - self.assertEqual(event.to_bytes(), original_envelope) + self.assertEqual(event.as_reader().as_builder().to_bytes(), original_envelope) def test_monotonic_rollback_and_drive_change_still_require_new_sources(self): now = MONO + 50_000_000 diff --git a/openpilot/starpilot/conditional_mode/tests/test_status_consumer.py b/openpilot/starpilot/conditional_mode/tests/test_status_consumer.py index e7748d7308..b523d3b2f2 100644 --- a/openpilot/starpilot/conditional_mode/tests/test_status_consumer.py +++ b/openpilot/starpilot/conditional_mode/tests/test_status_consumer.py @@ -218,10 +218,10 @@ class TestModeTransport(unittest.TestCase): changed = messaging.log_from_bytes(event.as_reader().as_builder().to_bytes()).as_builder() changed.slcState.conditionalMode.experimental = False self.assertFalse(self.consume(changed, now + 20_000_000, message_ns=now, receipt_ns=now).accepted) - self.assertFalse(self.consume(event, now + 101_000_000, message_ns=now, receipt_ns=now).accepted) - later, _, _ = self.produce(now + 150_000_000) - self.assertTrue(self.consume(later, now + 150_000_000).accepted) - self.assertFalse(self.consume(event, now + 150_000_000).accepted) + self.assertFalse(self.consume(event, now + 151_000_000, message_ns=now, receipt_ns=now).accepted) + later, _, _ = self.produce(now + 200_000_000) + self.assertTrue(self.consume(later, now + 200_000_000).accepted) + self.assertFalse(self.consume(event, now + 200_000_000).accepted) def test_new_session_resume_and_drive_wait_for_new_sources(self): now = MONO + 50_000_000 diff --git a/openpilot/starpilot/system/android_auto/projection_layout.py b/openpilot/starpilot/system/android_auto/projection_layout.py index ae6d78c4ec..e42c24d727 100644 --- a/openpilot/starpilot/system/android_auto/projection_layout.py +++ b/openpilot/starpilot/system/android_auto/projection_layout.py @@ -6,7 +6,7 @@ from pathlib import Path from openpilot.starpilot.system.android_auto.identity import DATA_DIR from openpilot.starpilot.system.android_auto.display_profile import screen_geometry -from openpilot.starpilot.ui.onroad_customization import (CLOCK_WIDGET, MODE_WIDGET, STOPPED_TIMER_WIDGET, WHEEL_SIZES, +from openpilot.starpilot.ui.onroad_customization import (CEM_GAUGE_WIDGET, CLOCK_WIDGET, MODE_WIDGET, STOPPED_TIMER_WIDGET, WHEEL_SIZES, customization_metadata, validate_widget_order) from openpilot.starpilot.ui.onroad_stopped_timer import timer_size from openpilot.starpilot.ui.onroad_torque_geometry import maximum_footprint @@ -182,7 +182,7 @@ def validate_layout_for_viewport(value, viewport, *, _legacy_geometry=False): type(value['version']) is not int or value['version'] not in (1, 2) or value['canvas'] != {key: metadata[key] for key in ('width', 'height')} or type(value['widgets']) is not dict or not set(value['widgets']) <= set(metadata['widgets']) or - not set(metadata['widgets']) - set(value['widgets']) <= {MODE_WIDGET, CLOCK_WIDGET, *PROJECTION_WIDGETS}): + not set(metadata['widgets']) - set(value['widgets']) <= {CEM_GAUGE_WIDGET, MODE_WIDGET, CLOCK_WIDGET, *PROJECTION_WIDGETS}): raise ValueError('Projection layout does not match saved screen') result = copy.deepcopy(value) if 'clock24Hour' in value and type(value['clock24Hour']) is not bool: @@ -200,7 +200,7 @@ def validate_layout_for_viewport(value, viewport, *, _legacy_geometry=False): if 'widgetOrder' in value: order = value['widgetOrder'] if type(order) is list: - order = [*order, *(key for key in (MODE_WIDGET, CLOCK_WIDGET, *PROJECTION_WIDGETS) if key not in order)] + order = [*order, *(key for key in (CEM_GAUGE_WIDGET, MODE_WIDGET, CLOCK_WIDGET, *PROJECTION_WIDGETS) if key not in order)] order = validate_widget_order(order, metadata['widgets']) # Older or hand-edited documents may put the escape control underneath # another widget. Preserve their order while restoring this safety layer. diff --git a/openpilot/starpilot/ui/layout_preview_renderer.py b/openpilot/starpilot/ui/layout_preview_renderer.py index b1e40524bb..de37232c44 100644 --- a/openpilot/starpilot/ui/layout_preview_renderer.py +++ b/openpilot/starpilot/ui/layout_preview_renderer.py @@ -15,6 +15,8 @@ from openpilot.starpilot.ui.layout_preview_transport import SCENES from openpilot.starpilot.ui.developer_preview import OnroadVisualPreview from openpilot.starpilot.ui.onroad_customization import CAMERA_WIDGETS, PROFILES, placement, validate_document from openpilot.starpilot.ui.onroad_widget_style import draw_widget_frame +from openpilot.starpilot.curve_speed.status import CurveObservation +from openpilot.starpilot.ui.onroad_state import ConditionalLead from openpilot.starpilot.ui.onroad_state import AlertSize, ObservationKind, OnroadAlert, OnroadState, SourceReading, SpeedLimitObservation from openpilot.starpilot.ui.appearance_preferences import CameraViewChoice, OnroadAppearance from openpilot.starpilot.ui.onroad_dm import monitor_visible @@ -91,6 +93,11 @@ def sample_state(scene: str, document: dict) -> OnroadState: customization=document, visual_preview=OnroadVisualPreview(cem_reason=CEM_SCENE_REASONS[scene]) if scene in CEM_SCENE_REASONS else None) + if scene == 'cem_lead': + state = replace(state, conditional_lead=ConditionalLead(12.0, 0.0)) + elif scene == 'cem_curve': + state = replace(state, curve=CurveObservation(False, None, None, False, False, False, 0.0, 2.0, 0.0, + 'preview', 'idle', 'none', False, False, 0.005)) observation = state.speed_limit if scene == 'slc_max': state = replace(state, cruise_kph=65) diff --git a/openpilot/starpilot/ui/onroad.py b/openpilot/starpilot/ui/onroad.py index e8b980e024..58b9e46625 100644 --- a/openpilot/starpilot/ui/onroad.py +++ b/openpilot/starpilot/ui/onroad.py @@ -28,6 +28,7 @@ from openpilot.starpilot.ui.onroad_curve import controlling as curve_controlling from openpilot.starpilot.ui.onroad_conditional import status as conditional_status, stop_active, reason as conditional_reason from openpilot.starpilot.ui.onroad_corner import CornerHintCache, render_corner_hint from openpilot.starpilot.ui.onroad_large_widgets import CurrentSpeedHud, UnifiedSpeedWidget, SteeringWheelWidget +from openpilot.starpilot.ui.onroad_cem_gauge import AetherGauge from openpilot.starpilot.ui.live_developer_sidebar import render_sidebar as render_live_sidebar from openpilot.starpilot.ui.onroad_state import AlertSize, OnroadState, slc_controls from openpilot.starpilot.ui.onroad_stopped_timer import duration_color, duration_text, timer_size @@ -120,6 +121,7 @@ class OnroadView: if fonts.profile == Profile.LARGE: self.unified_speed = UnifiedSpeedWidget(fonts) self.current_speed = CurrentSpeedHud(fonts) + self.cem_gauge = AetherGauge(fonts) self.steering_wheel = SteeringWheelWidget(asset_directory) else: self.compact_hud = CompactHudRenderer(fonts, asset_directory) @@ -295,6 +297,8 @@ class OnroadView: if placement(state.customization, profile, "cruise_limits")["enabled"]: submit("cruise_limits", lambda: self.unified_speed.render(content, state)) submit("speed_limit_actions", lambda: self._slc_actions(state)) + if gauge := getattr(self, "cem_gauge", None): + submit("cem_gauge", lambda: gauge.render(state)) if placement(state.customization, profile, "current_speed")["enabled"]: speed_content = rl.Rectangle(content.x + (width - 1860) / 2, content.y, content.width, content.height) submit("current_speed", lambda: self.current_speed.render(speed_content, state)) @@ -363,6 +367,8 @@ class OnroadView: if placement(state.customization, "large", "cruise_limits")["enabled"]: self.unified_speed.render(content, state) self._slc_actions(state) + if gauge := getattr(self, "cem_gauge", None): + gauge.render(state) if placement(state.customization, "large", "current_speed")["enabled"]: speed_content = rl.Rectangle(content.x + right_shift / 2, content.y, content.width, content.height) self.current_speed.render(speed_content, state) diff --git a/openpilot/starpilot/ui/onroad_cem_gauge.py b/openpilot/starpilot/ui/onroad_cem_gauge.py new file mode 100644 index 0000000000..ccd470acea --- /dev/null +++ b/openpilot/starpilot/ui/onroad_cem_gauge.py @@ -0,0 +1,457 @@ +"""Dom's perspective CEM road gauge, driven only by current UI observations. + +Geometry and animation preserve the original AetherGauge. This renderer never +reads IPC, settings, or control globals and never caches a missing CEM receipt. +""" +from dataclasses import dataclass +from enum import Enum +import math + +import pyray as rl + +from openpilot.common.filter_simple import FirstOrderFilter +from openpilot.starpilot.ui.appearance_preferences import CameraViewChoice +from openpilot.starpilot.ui.onroad_conditional import reason, status +from openpilot.starpilot.ui.onroad_curve import controlling, glow_color, intensity +from openpilot.starpilot.ui.onroad_customization import placement +from openpilot.starpilot.ui.onroad_state import AlertSize, OnroadState +from openpilot.starpilot.ui.presentation import BitmapFonts, FontRole + +SCALE = 1.5 + + +X_MIN = 3.0 +X_MAX = 60.0 +PATH_Y_FAR_SCALE = 1800.0 +PERSPECTIVE_GAIN = 0.35 +PERSPECTIVE_MAX_OFFSET = 26.0 * SCALE +PERSPECTIVE_EXPONENT = 1.8 + +ROAD_HEIGHT = 80.0 * SCALE +ROAD_SEGMENTS = 24 +ROAD_W_BOTTOM = 32.0 * SCALE +ROAD_W_TOP = 18.0 * SCALE +ROAD_THICKNESS = 4.0 * SCALE +ROAD_HALF_SIZE = 40.0 * SCALE +ROAD_EDGE_INSET = 2.0 * SCALE +FILL_ALPHA = 90 + +STOP_DISTANCE_MAX = 60.0 + +CHEVRON_COUNT = 6 +CHEVRON_SPACING = 0.3 +CHEVRON_STEP = 0.05 + + + +COLOR_FORCE_STOP = rl.Color(255, 30, 60, 255) +COLOR_LEAD_STOPPED = rl.Color(255, 60, 60, 255) +COLOR_LEAD_SLOWER = rl.Color(255, 191, 0, 255) +COLOR_SHADOW = rl.Color(0, 0, 0, 100) +COLOR_STOP_LINE_GLOW = rl.Color(255, 30, 60, 255) +COLOR_STOP_LINE_CORE = rl.Color(255, 200, 200, 255) +COLOR_CEM_SPEED = rl.Color(112, 192, 216, 255) +COLOR_REDUCTION = rl.Color(255, 191, 0, 220) + + +def _channels(color): + """Extract (r, g, b, a) from either an rl.Color struct or a 4-tuple.""" + if hasattr(color, "r"): + return color.r, color.g, color.b, color.a + return color[0], color[1], color[2], color[3] + +def _with_alpha(color, a: int) -> rl.Color: + r, g, b, _ = _channels(color) + return rl.Color(r, g, b, max(0, min(255, a))) + +def _fade(color, alpha: float) -> rl.Color: + r, g, b, a = _channels(color) + return rl.Color(r, g, b, max(0, min(255, int(a * alpha)))) + +def _lerp_color(a, b, t: float) -> rl.Color: + t = max(0.0, min(1.0, t)) + ar, ag, ab, _ = _channels(a) + br, bg, bb, _ = _channels(b) + return rl.Color( + ar + int((br - ar) * t), + ag + int((bg - ag) * t), + ab + int((bb - ab) * t), + 255, + ) + +def _pulse(freq: float) -> float: + return 0.5 + 0.5 * math.sin(rl.get_time() * freq) + +def _get_road_height(data: 'AetherGaugeData | None') -> float: + is_curve = data.indicator_type == IndicatorType.ROAD_CURVE if data else False + return ROAD_HEIGHT if is_curve else (45.0 * SCALE) + +def _road_xy(distance: float, icx: float, bottom: float, data: 'AetherGaugeData', road_h: float | None = None) -> tuple[float, float, float]: + t = max(0.0, min(1.0, _get_t_from_x(distance))) + offset = _get_perspective_offset(t, data) + h = road_h if road_h is not None else _get_road_height(data) + return t, icx + offset, bottom - t * h + +def _get_t_from_x(x: float) -> float: + x_clamped = max(X_MIN, min(X_MAX, x)) + return (1.0 - X_MIN / x_clamped) / (1.0 - X_MIN / X_MAX) + +def _get_perspective_offset(t: float, data: 'AetherGaugeData | None') -> float: + curvature = 0.0 + if data and data.indicator_type == IndicatorType.ROAD_CURVE: + curvature = data.indicator_value + + path_y_far = PATH_Y_FAR_SCALE * curvature + max_offset_top = math.tanh(path_y_far * PERSPECTIVE_GAIN) * PERSPECTIVE_MAX_OFFSET + return max_offset_top * (t ** PERSPECTIVE_EXPONENT) + +def _draw_text_with_shadow(fonts: BitmapFonts, role: FontRole, text: str, pos: rl.Vector2, size: int, + color: rl.Color, alpha: float = 1.0): + for dx, dy in ((-1, -1), (1, -1), (-1, 1), (1, 1)): + fonts.draw(text, role, size, pos.x + dx, pos.y + dy, _fade(rl.BLACK, alpha)) + fonts.draw(text, role, size, pos.x, pos.y, _fade(color, alpha)) + + +class IndicatorType(Enum): + ROAD_CURVE = "road_curve" + STOP_LIGHT = "stop_light" + LEAD = "lead" + + +@dataclass(frozen=True) +class AetherGaugeData: + text: str + unit: str = "" + color: rl.Color = rl.WHITE + indicator_type: IndicatorType = IndicatorType.ROAD_CURVE + indicator_value: float = 0.0 + indicator_extra: str = "" + reduction_text: str = "" + is_numeric: bool = False + + +def gauge_data(state: OnroadState, *, lead_stopped: bool = False) -> AetherGaugeData | None: + """Use the selected reason; never infer a CEM condition from raw geometry.""" + if (state.alert.size == AlertSize.FULL or state.longitudinal_overridden or + state.appearance.camera_view in (CameraViewChoice.DRIVER, CameraViewChoice.NONE) or state.reverse_driver_camera): + return None + current = reason(state) if status(state) is not None else None + if current == 'cem_stop': + # The current receipt has no committed distance. Preserve the light/road + # graphic without claiming a numeric distance from an unrelated model path. + return AetherGaugeData('STOP', color=COLOR_FORCE_STOP, indicator_type=IndicatorType.STOP_LIGHT, + indicator_extra='red') + curve = state.curve + if controlling(state) or current == 'cem_curve': + speed = state.speed_mps + target = curve.ceiling_mps if controlling(state) and curve is not None else None + shown = min(speed, target) if speed is not None and target is not None else speed + curvature = curve.road_curvature if curve is not None else None + preview = state.visual_preview + if preview is not None and preview.curve_curvature is not None: + curvature = preview.curve_curvature + conversion = 3.6 if state.metric else 2.2369362921 + reduction = (round((state.cruise_kph / 3.6 - target) * conversion) + if target is not None and state.cruise_kph is not None and state.cruise_kph / 3.6 > target else 0) + return AetherGaugeData(str(round(shown * conversion)) if shown is not None else 'CURVE', + unit=('km/h' if state.metric else 'mph') if shown is not None else '', + color=glow_color(intensity(curvature)) if curvature is not None else COLOR_CEM_SPEED, + indicator_type=IndicatorType.ROAD_CURVE, indicator_value=curvature or 0.0, + reduction_text=f'-{reduction}' if reduction > 0 else '', is_numeric=shown is not None) + if current in ('cem_lead', 'ccm_lead'): + lead = state.conditional_lead + # Only current radar metrics may classify the lead as stopped/slower. + stopped = lead is not None and lead.speed_mps < (1.2 if lead_stopped else 0.8) + return AetherGaugeData('STOPPED' if stopped else 'SLOW' if lead is not None else 'LEAD', + color=COLOR_LEAD_STOPPED if stopped else COLOR_LEAD_SLOWER, + indicator_type=IndicatorType.LEAD, indicator_value=lead.distance_m if lead is not None else 0.0, + indicator_extra='stopped' if stopped else 'slower' if lead is not None else '') + return None + + +class AetherGauge: + def __init__(self, fonts: BitmapFonts): + self.fonts = fonts + self._chevron_accum = 0.0 + self._lead_chevron_accum = 0.0 + self._road_h_filter = FirstOrderFilter(ROAD_HEIGHT, 0.06, 1 / 60) + self._current_road_h = ROAD_HEIGHT + self._speed_mps = 0.0 + self._lead_stopped = False + self._drive_frame = None + + def render(self, state: OnroadState) -> None: + saved = placement(state.customization, 'large', 'cem_gauge') + if state.drive_frame != self._drive_frame: + self._road_h_filter.x = ROAD_HEIGHT + self._chevron_accum = self._lead_chevron_accum = 0.0 + self._lead_stopped = False + self._drive_frame = state.drive_frame + data = gauge_data(state, lead_stopped=self._lead_stopped) if saved['enabled'] else None + if data is None: + self._lead_stopped = False + self._road_h_filter.x = ROAD_HEIGHT + return + self._lead_stopped = data.indicator_extra == 'stopped' + self._speed_mps = state.speed_mps or 0.0 + rect = rl.Rectangle(saved['x'], saved['y'], 344, 300) + self._render_unified_road(rect, rect.x + rect.width / 2, rect.y + 145 - ROAD_HALF_SIZE, data) + + def _render_unified_road(self, rect, icx, icy, data, alpha=1.0): + bottom = icy + ROAD_HALF_SIZE + + if data.indicator_type == IndicatorType.STOP_LIGHT: + # Frozen decorative road anchor, not a reported stop distance. + distance = 15.0 + else: + distance = data.indicator_value + + points_left = [] + points_right = [] + self._current_road_h = self._road_h_filter.update(_get_road_height(data)) + road_h = self._current_road_h + + for i in range(ROAD_SEGMENTS + 1): + t = i / ROAD_SEGMENTS + offset = _get_perspective_offset(t, data) + cx_t = icx + offset + y_t = bottom - t * road_h + w_t = ROAD_W_BOTTOM - t * (ROAD_W_BOTTOM - ROAD_W_TOP) + + points_left.append(rl.Vector2(cx_t - w_t, y_t)) + points_right.append(rl.Vector2(cx_t + w_t, y_t)) + + fill_color = _fade(_with_alpha(data.color, FILL_ALPHA), alpha) + for i in range(ROAD_SEGMENTS): + t = i / ROAD_SEGMENTS + stroke = ROAD_THICKNESS * (1.0 - 0.6 * t) + rl.draw_triangle(points_left[i], points_right[i], points_left[i+1], fill_color) + rl.draw_triangle(points_right[i], points_right[i+1], points_left[i+1], fill_color) + rl.draw_line_ex(points_left[i], points_left[i+1], stroke, _fade(COLOR_SHADOW, alpha)) + rl.draw_line_ex(points_right[i], points_right[i+1], stroke, _fade(COLOR_SHADOW, alpha)) + rl.draw_line_ex(points_left[i], points_left[i+1], stroke, _fade(data.color, alpha)) + rl.draw_line_ex(points_right[i], points_right[i+1], stroke, _fade(data.color, alpha)) + + it = data.indicator_type + + if it == IndicatorType.STOP_LIGHT: + self._draw_stop_line(icx, bottom, distance, data, alpha) + + if it == IndicatorType.LEAD: + self._draw_lead_car(icx, bottom, data, alpha) + + if it in (IndicatorType.ROAD_CURVE, IndicatorType.STOP_LIGHT): + self._draw_standard_chevrons(icx, bottom, distance, data, alpha) + + if it == IndicatorType.STOP_LIGHT: + self._draw_traffic_light(icx, icy, distance, data, alpha) + + + self._draw_mini_cradle(icx, bottom, data, alpha) + + def _draw_stop_line(self, icx, bottom, distance, data, alpha=1.0): + t, cx_line, cy_line = _road_xy(distance, icx, bottom, data, self._current_road_h) + w_line = ROAD_W_BOTTOM - t * (ROAD_W_BOTTOM - ROAD_W_TOP) + + p_left = rl.Vector2(cx_line - w_line + ROAD_EDGE_INSET, cy_line) + p_right = rl.Vector2(cx_line + w_line - ROAD_EDGE_INSET, cy_line) + + fade = 1.0 - t * 0.5 + glow_a = int(150 * fade) + + rl.draw_line_ex(p_left, p_right, max(3.0 * SCALE, 7.0 * SCALE * fade), _fade(_with_alpha(COLOR_STOP_LINE_GLOW, glow_a), alpha)) + rl.draw_line_ex(p_left, p_right, max(1.5 * SCALE, 3.5 * SCALE * fade), _fade(COLOR_STOP_LINE_CORE, alpha)) + + def _draw_lead_car(self, icx, bottom, data, alpha=1.0): + t_lead, cx_lead, cy_lead = _road_xy(6.5, icx, bottom, data, self._current_road_h) + t_lead = max(0.15, min(0.85, t_lead)) + + car_scale = 0.45 + (1.0 - t_lead) * 0.55 + W = 38.0 * SCALE * car_scale + H = 22.0 * SCALE * car_scale + + is_stopped = data.indicator_extra == "stopped" + is_slower = data.indicator_extra == "slower" + + if is_stopped: + border_color = _fade(COLOR_LEAD_STOPPED, alpha) + elif is_slower: + border_color = _fade(_with_alpha(data.color, int(160 + 95 * _pulse(1.2))), alpha) + else: + border_color = _fade(data.color, alpha) + + # Car body: cabin and main body with high-contrast outlines + for rect_args, corner_r, fill in [ + ((cx_lead - W * 0.3, cy_lead - H, W * 0.6, H * 0.45), 0.5, _fade(rl.Color(15, 15, 15, 240), alpha)), + ((cx_lead - W / 2, cy_lead - H * 0.65, W, H * 0.55), 0.3, _fade(rl.Color(20, 20, 20, 240), alpha)), + ]: + r = rl.Rectangle(*rect_args) + rl.draw_rectangle_rounded(r, corner_r, 4, fill) + rl.draw_rectangle_rounded_lines_ex(r, corner_r, 4, 2.0, border_color) + + # Tail lights with radial bloom + tl_w = max(4.0, W * 0.16) + tl_h = max(2.5, H * 0.14) + tl_y = int(cy_lead - H * 0.55) + glow_y = int(cy_lead - H * 0.5) + for tl_x, glow_x in [(int(cx_lead - W * 0.45), int(cx_lead - W * 0.38)), + (int(cx_lead + W * 0.3), int(cx_lead + W * 0.38))]: + if is_stopped: + pulse = _pulse(2.5) + r_glow = int(W * 0.10 * (1.0 + 0.4 * pulse)) + rl.draw_circle(glow_x, glow_y, r_glow + 4, _fade(rl.Color(255, 30, 60, int(60 + 40 * pulse)), alpha)) + rl.draw_circle(glow_x, glow_y, r_glow, _fade(rl.Color(255, 30, 60, int(150 + 105 * pulse)), alpha)) + c_tl = _fade(rl.Color(255, 220, 220, 255), alpha) + elif is_slower: + pulse = _pulse(1.2) + r_glow = int(W * 0.08 * (1.0 + 0.2 * pulse)) + rl.draw_circle(glow_x, glow_y, r_glow, _fade(rl.Color(255, 140, 30, int(100 + 80 * pulse)), alpha)) + c_tl = _fade(rl.Color(255, 160, 60, int(180 + 75 * pulse)), alpha) + else: + c_tl = _fade(COLOR_FORCE_STOP, alpha) + rl.draw_rectangle(tl_x, tl_y, int(tl_w), int(tl_h), c_tl) + + # Wheels + wheel_y = int(cy_lead - H * 0.1) + wheel_w = int(W * 0.12) + wheel_h = int(H * 0.1) + wheel_c = _fade(rl.Color(10, 10, 10, 255), alpha) + rl.draw_rectangle(int(cx_lead - W * 0.4), wheel_y, wheel_w, wheel_h, wheel_c) + rl.draw_rectangle(int(cx_lead + W * 0.28), wheel_y, wheel_w, wheel_h, wheel_c) + + # Following chevrons (from lead car back toward viewer) + dt = rl.get_frame_time() + v_ego = self._speed_mps + flow_speed_factor = min(1.8, v_ego * 0.08) + self._lead_chevron_accum = (self._lead_chevron_accum + dt * flow_speed_factor * 1.8) % 1.0 + progress = self._lead_chevron_accum + for i in range(2): + t = t_lead * (1.0 - ((i + progress) / 2) % 1.0) + + cx_t = icx + _get_perspective_offset(t, data) + road_h = self._current_road_h + cy_t = bottom - t * road_h + + chevron_w = 14.0 * SCALE - t * 5.0 * SCALE + chevron_thick = max(2.0 * SCALE, 4.0 * SCALE - t * 1.5 * SCALE) + lx = cx_t - chevron_w + rx = cx_t + chevron_w + + chev_a = max(0, min(255, int(data.color.a * (1.0 - t / t_lead) * math.sin(t / t_lead * math.pi)))) + c_color = _fade(_with_alpha(data.color, chev_a), alpha) + + rl.draw_line_ex(rl.Vector2(lx, cy_t - chevron_w * 0.5), rl.Vector2(cx_t, cy_t), chevron_thick, c_color) + rl.draw_line_ex(rl.Vector2(rx, cy_t - chevron_w * 0.5), rl.Vector2(cx_t, cy_t), chevron_thick, c_color) + + def _draw_standard_chevrons(self, icx, bottom, distance, data, alpha=1.0): + is_stop = data.indicator_type == IndicatorType.STOP_LIGHT + dt = rl.get_frame_time() + v_ego = self._speed_mps + flow_speed_factor = min(1.5, v_ego * 0.08) + self._chevron_accum = (self._chevron_accum + dt * flow_speed_factor * 1.5) % 1.0 + progress = self._chevron_accum + t_stop = max(0.05, min(1.0, distance / STOP_DISTANCE_MAX)) if is_stop else 1.0 + max_t = t_stop if is_stop else 1.0 + + for i in range(CHEVRON_COUNT): + t = max_t - (i + progress) * CHEVRON_SPACING + if t < 0.0 or t > max_t: + continue + + t_next = min(max_t, t + CHEVRON_STEP) + cx_t = icx + _get_perspective_offset(t, data) + cx_next = icx + _get_perspective_offset(t_next, data) + + road_h = self._current_road_h + cy_t = bottom - t * road_h + cy_next = bottom - t_next * road_h + + dx = cx_next - cx_t + dy = cy_next - cy_t + len_v = math.hypot(dx, dy) + dir_up_x, dir_up_y = (dx / len_v, dy / len_v) if len_v > 0.001 else (0.0, -1.0) + + dir_right_x = -dir_up_y + dir_right_y = dir_up_x + + chevron_w = max(3.0 * SCALE, 18.0 * SCALE - t * 7.0 * SCALE) + chevron_h = chevron_w * 0.6 + chevron_thick = max(2.5 * SCALE, 4.5 * SCALE - t * 2.0 * SCALE) + + lx = cx_t - dir_right_x * chevron_w + dir_up_x * chevron_h + ly = cy_t - dir_right_y * chevron_w + dir_up_y * chevron_h + rx = cx_t + dir_right_x * chevron_w + dir_up_x * chevron_h + ry = cy_t + dir_right_y * chevron_w + dir_up_y * chevron_h + + alpha_factor = math.sin((t / max_t) * math.pi) if max_t > 0.01 else 0.0 + chev_a = max(0, min(255, int(data.color.a * alpha_factor))) + chev_color = _fade(_with_alpha(data.color, chev_a), alpha) + chev_shadow = _fade(rl.Color(0, 0, 0, int(chev_a * 0.5)), alpha) + + rl.draw_line_ex(rl.Vector2(lx, ly + 1.5), rl.Vector2(cx_t, cy_t + 1.5), chevron_thick, chev_shadow) + rl.draw_line_ex(rl.Vector2(rx, ry + 1.5), rl.Vector2(cx_t, cy_t + 1.5), chevron_thick, chev_shadow) + rl.draw_line_ex(rl.Vector2(lx, ly), rl.Vector2(cx_t, cy_t), chevron_thick, chev_color) + rl.draw_line_ex(rl.Vector2(rx, ry), rl.Vector2(cx_t, cy_t), chevron_thick, chev_color) + + def _draw_traffic_light(self, icx, icy, distance, data, alpha=1.0): + t_light, cx_light, cy_road = _road_xy(distance, icx, icy + ROAD_HALF_SIZE, data, self._current_road_h) + s_light = 1.0 - t_light + + scale_light = 0.65 + (s_light ** 2.0) * 1.35 + cy_light = cy_road - 38.0 * SCALE * s_light + width = 15.0 * SCALE * scale_light + height = 36.0 * SCALE * scale_light + + rl.draw_rectangle_rounded(rl.Rectangle(cx_light - width/2, cy_light - height/2 + 1.5, width, height), 0.25, 4, _fade(rl.Color(0, 0, 0, 120), alpha)) + rect_housing = rl.Rectangle(cx_light - width/2, cy_light - height/2, width, height) + rl.draw_rectangle_rounded(rect_housing, 0.25, 4, _fade(rl.Color(22, 22, 22, 255), alpha)) + rl.draw_rectangle_rounded_lines_ex(rect_housing, 0.25, 4, 1.5, _fade(rl.Color(100, 100, 100, 255), alpha)) + + r_bulb = 3.2 * SCALE * scale_light + active_light = data.indicator_extra if data.indicator_extra in ("red", "yellow", "green") else "red" + + bulbs = [ + (cy_light - 10.0 * SCALE * scale_light, "red", rl.Color(255, 30, 60, 255), rl.Color(50, 10, 15, 255)), + (cy_light, "yellow", rl.Color(255, 200, 0, 255), rl.Color(50, 40, 0, 255)), + (cy_light + 10.0 * SCALE * scale_light, "green", rl.Color(0, 255, 100, 255), rl.Color(0, 40, 15, 255)), + ] + for y, name, active_c, inactive_c in bulbs: + if name == "red" and active_light == "red": + glow_pulse = _pulse(1.5) + rl.draw_circle_v(rl.Vector2(cx_light, y), r_bulb + 8.0 * SCALE * glow_pulse, _fade(rl.Color(255, 30, 60, 25), alpha)) + rl.draw_circle_v(rl.Vector2(cx_light, y), r_bulb + 4.0 * SCALE * glow_pulse, _fade(rl.Color(255, 30, 60, 60), alpha)) + c = _fade(active_c if active_light == name else inactive_c, alpha) + rl.draw_circle_v(rl.Vector2(cx_light, y), r_bulb, c) + + def _draw_mini_cradle(self, cx, bottom, data, alpha=1.0): + if not data.text: + return + + if data.is_numeric: + val_size = self.fonts.measure(data.text, FontRole.BOLD, int(50 * SCALE)) + val_pos = rl.Vector2(int(cx - val_size.width / 2), int(bottom + 6 * SCALE)) + _draw_text_with_shadow(self.fonts, FontRole.BOLD, data.text, val_pos, int(50 * SCALE), data.color, alpha) + + accent_y = int(val_pos.y + val_size.height + 2 * SCALE) + accent_w = int(val_size.width + 16 * SCALE) + accent_x = int(cx - accent_w / 2) + rl.draw_line_ex( + rl.Vector2(accent_x, accent_y), + rl.Vector2(accent_x + accent_w, accent_y), + 2.0 * SCALE, + _fade(_with_alpha(data.color, 160), alpha), + ) + + if data.reduction_text: + red_size = self.fonts.measure(data.reduction_text, FontRole.MEDIUM, int(22 * SCALE)) + red_pos = rl.Vector2(int(cx + val_size.width / 2 + 6 * SCALE), int(val_pos.y + val_size.height / 2 - red_size.height / 2)) + _draw_text_with_shadow(self.fonts, FontRole.MEDIUM, data.reduction_text, red_pos, int(22 * SCALE), COLOR_REDUCTION, alpha) + + if data.unit: + unit_size = self.fonts.measure(data.unit, FontRole.MEDIUM, int(20 * SCALE)) + unit_pos = rl.Vector2(int(cx - unit_size.width / 2), int(accent_y + 3 * SCALE)) + _draw_text_with_shadow(self.fonts, FontRole.MEDIUM, data.unit, unit_pos, int(20 * SCALE), rl.Color(255, 255, 255, 180), alpha) + else: + val_size = self.fonts.measure(data.text, FontRole.BOLD, int(32 * SCALE)) + val_pos = rl.Vector2(int(cx - val_size.width / 2), int(bottom + 10 * SCALE)) + _draw_text_with_shadow(self.fonts, FontRole.BOLD, data.text, val_pos, int(32 * SCALE), data.color, alpha) diff --git a/openpilot/starpilot/ui/onroad_compact_widgets.py b/openpilot/starpilot/ui/onroad_compact_widgets.py index d408ade11b..57a9008348 100644 --- a/openpilot/starpilot/ui/onroad_compact_widgets.py +++ b/openpilot/starpilot/ui/onroad_compact_widgets.py @@ -143,7 +143,10 @@ class CompactHudRenderer: rl.draw_circle_gradient(int(81 + mx), int(81 + my), 81, shadow, rl.BLANK) red, green, blue, alpha = rgba(state.customization, "text", "compact", "max_speed") color = rl.Color(red, green, blue, int(alpha * 0.9 * set_speed_alpha)) - self.fonts.draw(cruise, FontRole.DISPLAY, 112, 17 + mx, -4 + my, color) + speed_size = 112 + while speed_size > 72 and self.fonts.measure(cruise, FontRole.SET_SPEED, speed_size).width > 128: + speed_size -= 1 + self.fonts.draw(cruise, FontRole.SET_SPEED, speed_size, 17 + mx, -4 + my, color) self.fonts.draw("MAX", FontRole.SEMI_BOLD, 36, 25 + mx, 109 + my, color) def render_steering_wheel(self, state: OnroadState) -> None: if state.lateral_active: diff --git a/openpilot/starpilot/ui/onroad_conditional.py b/openpilot/starpilot/ui/onroad_conditional.py index 7bba298a50..a77673fe5e 100644 --- a/openpilot/starpilot/ui/onroad_conditional.py +++ b/openpilot/starpilot/ui/onroad_conditional.py @@ -14,7 +14,7 @@ _CCM_HOLD_REASONS = {4: "ccm_lead", 6: "ccm_speed"} _REASONS = {reason.value: (reason.name[4:] if reason.name.startswith(('CEM_', 'CCM_')) else reason.name).replace('_', ' ') for reason in Reason} -_REASONS[Reason.SCENE_UNAVAILABLE.value] = 'WAITING FOR ROAD DATA' +_HIDDEN_REASONS = frozenset((Reason.SCENE_UNAVAILABLE.value, Reason.UNAVAILABLE.value)) def reason(state: OnroadState) -> str | None: @@ -23,7 +23,7 @@ def reason(state: OnroadState) -> str | None: if preview is not None and preview.cem_reason in ('CURVE', 'LEAD', 'STOP LIGHT', 'SPEED'): return {'CURVE': 'cem_curve', 'LEAD': 'cem_lead', 'STOP LIGHT': 'cem_stop', 'SPEED': 'cem_speed'}[preview.cem_reason] value = (state.conditional_effective if state.longitudinal_active else None) or getattr(state, 'conditional_perception', None) - if value is None or value.choice not in (ModeChoice.CEM, ModeChoice.CCM): + if value is None or value.choice not in (ModeChoice.CEM, ModeChoice.CCM) or value.reason in _HIDDEN_REASONS: return None if value.reason == 'cem_hold' and value.choice is ModeChoice.CEM: return _CEM_HOLD_REASONS.get(getattr(value, 'status_code', 0)) @@ -52,7 +52,7 @@ def status(state: OnroadState) -> tuple[str, str, rl.Color] | None: value = (state.conditional_effective if state.longitudinal_active else None) or getattr(state, 'conditional_perception', None) if value is None or value.choice not in (ModeChoice.CEM, ModeChoice.CCM): return None - if value.reason in ('no_trigger', 'manual_chill', 'manual_experimental', 'ccm_veto'): + if value.reason in _HIDDEN_REASONS or value.reason in ('no_trigger', 'manual_chill', 'manual_experimental', 'ccm_veto'): return None # The text is selected from known policy reasons, never rendered from wire text. detail = _REASONS.get(reason(state)) diff --git a/openpilot/starpilot/ui/onroad_customization.py b/openpilot/starpilot/ui/onroad_customization.py index 16b1ec9264..3b286ca683 100644 --- a/openpilot/starpilot/ui/onroad_customization.py +++ b/openpilot/starpilot/ui/onroad_customization.py @@ -139,8 +139,12 @@ _timer_widget = _widget("Stopped timer", STOPPED_TIMER_WIDGET, 440, 112, 18, 48) _timer_widget["default"]["enabled"] = False _timer_widget["note"] = "Counts how long you have been stopped" PROFILES["compact"]["widgets"][STOPPED_TIMER_WIDGET] = _timer_widget -# Widgets an older saved layout may lack; they start off and never block placement. -OPTIONAL_WIDGETS = frozenset({MODE_WIDGET, CLOCK_WIDGET, STOPPED_TIMER_WIDGET}) +# Restore the Dom road graphic in the gap between the speed card and main speed. +# It has no background and appears only with a current selected source. +CEM_GAUGE_WIDGET = 'cem_gauge' +PROFILES['large']['widgets'][CEM_GAUGE_WIDGET] = _widget('CEM road gauge', CEM_GAUGE_WIDGET, 344, 300, 436, 75) +# Older documents retain every saved placement and gain this independent widget. +OPTIONAL_WIDGETS = frozenset({MODE_WIDGET, CLOCK_WIDGET, STOPPED_TIMER_WIDGET, CEM_GAUGE_WIDGET}) _FRAME_COLORS = {"cardFill": "#00000000", "cardBorder": "#00000000"} _ACTION_COLORS = {"cardFill": "#0C1820EB", "cardBorder": "#A6DFBEFF", "text": "#FFFFFFFF"} @@ -148,6 +152,7 @@ WIDGET_COLORS = { "large": { MODE_WIDGET: {"text": "#FFFFFFFF"}, CLOCK_WIDGET: {"text": "#FFFFFFFF"}, + CEM_GAUGE_WIDGET: {}, "current_speed": {"text": None}, "cruise_limits": {"cardFill": "#10191FFF", "cardBorder": "#35434DFF", "text": None}, "speed_limit_actions": {"cardFill": "#10191FFF", "cardBorder": "#35434DFF", "text": "#FFFFFFFF"}, @@ -169,7 +174,7 @@ WIDGET_COLORS = { # Back-to-front order used when a layout first opts into custom layers. DEFAULT_WIDGET_ORDER = { - "large": ("pip_left", "pip_right", "cruise_limits", "speed_limit_actions", "current_speed", + "large": ("pip_left", "pip_right", "cruise_limits", "speed_limit_actions", CEM_GAUGE_WIDGET, "current_speed", "steering_wheel", "torque_bar", "driver_monitor", MODE_WIDGET, CLOCK_WIDGET), "compact": ("pip_left", "pip_right", "speed_limit", "max_speed", "steering_wheel", "torque_bar", "speed_limit_actions", "driver_monitor", *RAIL_WIDGETS, MODE_WIDGET, CLOCK_WIDGET, STOPPED_TIMER_WIDGET), diff --git a/openpilot/starpilot/ui/onroad_state.py b/openpilot/starpilot/ui/onroad_state.py index 430e5f278e..de9f8cf394 100644 --- a/openpilot/starpilot/ui/onroad_state.py +++ b/openpilot/starpilot/ui/onroad_state.py @@ -143,6 +143,28 @@ class BorderSignals: right_blindspot: bool +@dataclass(frozen=True) +class ConditionalLead: + distance_m: float + speed_mps: float + + def __post_init__(self) -> None: + if (not math.isfinite(self.distance_m) or not 0 < self.distance_m <= 500 or + not math.isfinite(self.speed_mps) or not 0 <= self.speed_mps <= 100): + raise ValueError('Conditional lead metrics must be finite and physically bounded') + + +def conditional_lead_from_message(radar) -> ConditionalLead | None: + """The caller supplies only a current display radar observation.""" + try: + lead = radar.leadOne + if lead.status is not True: + return None + return ConditionalLead(float(lead.dRel), float(lead.vLead)) + except (AttributeError, TypeError, ValueError, OverflowError): + return None + + @dataclass(frozen=True) class OnroadState: engaged: bool @@ -172,6 +194,7 @@ class OnroadState: navigation: NavigationDisplay | None = None conditional_perception: ConditionalDisplay | None = None conditional_effective: ConditionalDisplay | None = None + conditional_lead: ConditionalLead | None = None visual_preview: OnroadVisualPreview | None = None border_signals: BorderSignals | None = None stopped_duration_s: int | None = None diff --git a/openpilot/starpilot/ui/presentation.py b/openpilot/starpilot/ui/presentation.py index 5d42734d55..1f6f631a01 100644 --- a/openpilot/starpilot/ui/presentation.py +++ b/openpilot/starpilot/ui/presentation.py @@ -31,6 +31,7 @@ class FontRole(StrEnum): ROMAN = "roman" DISPLAY = "display" SPEED = "speed" + SET_SPEED = "set_speed" FALLBACK = "fallback" @@ -50,6 +51,7 @@ def font_filename(profile: Profile, role: FontRole) -> str: FontRole.BRAND: "Sora-800.fnt", FontRole.ROMAN: "Inter-Regular.fnt", FontRole.DISPLAY: "Inter-Bold.fnt", + FontRole.SET_SPEED: "DINish-Speed.fnt", FontRole.SPEED: "DINish-Speed.fnt" if profile == Profile.LARGE else "Inter-Bold.fnt", FontRole.FALLBACK: "unifont.fnt", }[role] @@ -132,15 +134,25 @@ class BitmapFonts: role = FontRole.FALLBACK return self._fonts[font_filename(self.profile, role)] + def _optical_style(self, role: FontRole, fallback: bool = False) -> tuple[float, float]: + if fallback: + return 1., 0. + if self.profile == Profile.LARGE and role == FontRole.SPEED: + return 1.04, .6 + if self.profile == Profile.COMPACT and role == FontRole.SET_SPEED: + return 1., .45 + return 1., 0. + def measure(self, text: str, role: FontRole, size: float, *, spacing: float = 0, fallback: bool = False) -> TextSize: font = self.font(role, fallback=fallback) spacing = round(spacing, 4) - key = font.texture.id, text, size, spacing + width_scale, weight = self._optical_style(role, fallback) + key = font.texture.id, text, size, spacing, width_scale, weight if key not in self._measurements: measured = rl.measure_text_ex(font, text, size * self.profile.font_scale, spacing) # noqa: TID251 if len(self._measurements) >= 1024: self._measurements.clear() - self._measurements[key] = TextSize(measured.x, measured.y) + self._measurements[key] = TextSize(measured.x * width_scale + (weight if text else 0), measured.y) return self._measurements[key] def vertical_ink(self, text: str, role: FontRole, size: float) -> tuple[float, float]: @@ -160,7 +172,20 @@ class BitmapFonts: def draw(self, text: str, role: FontRole, size: float, x: float, y: float, color: rl.Color = rl.WHITE, *, spacing: float = 0, fallback: bool = False) -> None: draw = getattr(rl, "_orig_draw_text_ex", rl.draw_text_ex) - draw(self.font(role, fallback=fallback), text, rl.Vector2(x, y), size * self.profile.font_scale, spacing, color) + width_scale, weight = self._optical_style(role, fallback) + if width_scale == 1. and weight == 0.: + draw(self.font(role, fallback=fallback), text, rl.Vector2(x, y), size * self.profile.font_scale, spacing, color) + return + rl.rl_push_matrix() + try: + rl.rl_translatef(x, y, 0) + rl.rl_scalef(width_scale, 1., 1.) + draw(self.font(role, fallback=fallback), text, rl.Vector2(0, 0), size * self.profile.font_scale, spacing, color) + if weight: + draw(self.font(role, fallback=fallback), text, rl.Vector2(weight / width_scale, 0), + size * self.profile.font_scale, spacing, color) + finally: + rl.rl_pop_matrix() def close(self) -> None: if rl.is_window_ready() or self._headless_context: diff --git a/openpilot/starpilot/ui/runtime_snapshot.py b/openpilot/starpilot/ui/runtime_snapshot.py index 90ea5bf766..96da017b36 100644 --- a/openpilot/starpilot/ui/runtime_snapshot.py +++ b/openpilot/starpilot/ui/runtime_snapshot.py @@ -37,6 +37,7 @@ from openpilot.cereal.services import SERVICE_LIST from opendbc.car.structs import car as car_schema from openpilot.starpilot.ui.device_state import DeviceRequest, DeviceState from openpilot.starpilot.ui.home_state import HomeMode, HomeState +from openpilot.starpilot.ui.onroad_state import conditional_lead_from_message from openpilot.starpilot.ui.onroad_state import (AlertSize, BorderSignals, ObservationKind, OnroadAlert, OnroadState, SpeedLimitObservation, is_personality_notice, speed_limit_from_message) from openpilot.starpilot.ui.onroad_stopped_timer import StoppedTimer @@ -620,6 +621,7 @@ class RuntimeSnapshotAdapter: curve=curve, show_curve_status=_flag(params, "ShowCSCStatus"), conditional_configured=self._conditional_choice, conditional_effective=conditional, conditional_perception=perception, + conditional_lead=conditional_lead_from_message(display_radar) if display_car_valid else None, navigation=navigation_display(navigation, now_ns=now_ns, drive_id=drive_id), border_signals=border_signals, stopped_duration_s=stopped_duration, reverse_driver_camera=reverse_driver_camera, diff --git a/openpilot/starpilot/ui/tests/test_conditional_perception.py b/openpilot/starpilot/ui/tests/test_conditional_perception.py index b535458481..a5734b252e 100644 --- a/openpilot/starpilot/ui/tests/test_conditional_perception.py +++ b/openpilot/starpilot/ui/tests/test_conditional_perception.py @@ -86,7 +86,7 @@ def test_independently_stamped_scene_survives_transport_without_control_authorit # A fresh envelope must not revive a stale or malformed nested scene. for altered in ({'observedMonoTime': envelope_ns - 200_000_001}, {'modelMonoTime': scene_ns + 1}, {'carStateMonoTime': drive_id - 1}, - {'validUntilMonoTime': scene_ns + 100_000_001}): + {'validUntilMonoTime': scene_ns + 150_000_001}): changed = event.as_reader().as_builder() for key, value in altered.items(): setattr(changed.slcState.conditionalMode, key, value) diff --git a/openpilot/starpilot/ui/tests/test_conditional_reason_semantics.py b/openpilot/starpilot/ui/tests/test_conditional_reason_semantics.py index 32d99ba799..d75f0a850e 100644 --- a/openpilot/starpilot/ui/tests/test_conditional_reason_semantics.py +++ b/openpilot/starpilot/ui/tests/test_conditional_reason_semantics.py @@ -1,4 +1,4 @@ -"""Wire receipt regression: scene readiness must not become generic unavailability.""" +"""Keep readiness receipts intact while missing road data stays visually neutral.""" from typing import Any from unittest.mock import Mock from dataclasses import replace @@ -20,7 +20,7 @@ def changed_arguments(arguments: dict[str, Any], changes: dict[str, Any]) -> dic return arguments | changes -def check_fresh_validated_receipt_preserves_unavailable_reason(choice, policy_reason, detail): +def check_fresh_validated_receipt_preserves_unavailable_reason(choice, policy_reason): now = 100_000_000_000 drive_id = now - 1_000_000_000 authority = Authority(True, True, False, False, False, True) @@ -43,8 +43,7 @@ def check_fresh_validated_receipt_preserves_unavailable_reason(choice, policy_re assert not received.slcState.conditionalMode.hasOverride ui = OnroadState(False, True, 0.0, 0.0, SpeedLimitObservation(), conditional_perception=value) display = status(ui) - assert display is not None - assert display[:2] == (choice.name, detail) + assert display is None assert not stop_active(ui) for changed in ({'now_ns': now + 200_000_001}, {'drive_id': drive_id + 1}, {'car_valid': False}, {'system_long': False}, {'lateral_active': False}, {'choice': ModeChoice.STOCK}): @@ -55,17 +54,17 @@ def check_fresh_validated_receipt_preserves_unavailable_reason(choice, policy_re class TestConditionalReasonSemantics(unittest.TestCase): - def test_cem_scene_waits_for_road_data(self): - check_fresh_validated_receipt_preserves_unavailable_reason(ModeChoice.CEM, Reason.SCENE_UNAVAILABLE, 'WAITING FOR ROAD DATA') + def test_cem_missing_scene_is_neutral(self): + check_fresh_validated_receipt_preserves_unavailable_reason(ModeChoice.CEM, Reason.SCENE_UNAVAILABLE) - def test_cem_unavailable_remains_visible(self): - check_fresh_validated_receipt_preserves_unavailable_reason(ModeChoice.CEM, Reason.UNAVAILABLE, 'UNAVAILABLE') + def test_cem_unavailable_is_neutral(self): + check_fresh_validated_receipt_preserves_unavailable_reason(ModeChoice.CEM, Reason.UNAVAILABLE) - def test_ccm_scene_waits_for_road_data(self): - check_fresh_validated_receipt_preserves_unavailable_reason(ModeChoice.CCM, Reason.SCENE_UNAVAILABLE, 'WAITING FOR ROAD DATA') + def test_ccm_missing_scene_is_neutral(self): + check_fresh_validated_receipt_preserves_unavailable_reason(ModeChoice.CCM, Reason.SCENE_UNAVAILABLE) - def test_ccm_unavailable_remains_visible(self): - check_fresh_validated_receipt_preserves_unavailable_reason(ModeChoice.CCM, Reason.UNAVAILABLE, 'UNAVAILABLE') + def test_ccm_unavailable_is_neutral(self): + check_fresh_validated_receipt_preserves_unavailable_reason(ModeChoice.CCM, Reason.UNAVAILABLE) if __name__ == '__main__': diff --git a/openpilot/starpilot/ui/tests/test_covered_camera.py b/openpilot/starpilot/ui/tests/test_covered_camera.py index a1ed8883ff..5bf6e7b632 100644 --- a/openpilot/starpilot/ui/tests/test_covered_camera.py +++ b/openpilot/starpilot/ui/tests/test_covered_camera.py @@ -174,7 +174,7 @@ def test_model_paint_flag_cannot_leak_after_render_exception(monkeypatch): assert renderer._render_lateral_active is False -def make_session(monkeypatch, profile, camera): +def make_session(monkeypatch, request, profile, camera): for name in ('BitmapFonts', 'PiPRenderer', 'PiPWarningSource', 'BluetoothStatusSource', 'RuntimeSnapshotAdapter', 'GalaxyAccessFlow', 'FeatureSettingsOwner', 'SoundsOwner', 'AppearanceOwner', 'PiPOwner', 'DisplayOwner', 'PowerOwner', 'ShellInput', 'FavoritesOwner', 'OnroadFavorites', 'DriveStateOwner', 'PhysicalSource'): @@ -186,10 +186,13 @@ def make_session(monkeypatch, profile, camera): def view(*_, camera_layer, **__): return NS(camera_layer=camera_layer, onroad=NS(unified_speed=NS(source_bounds=Mock()), compact_sidebar=NS())) monkeypatch.setattr(runtime_app, 'ShellView', view) - return runtime_app.StarShellSession(profile, camera) + session = runtime_app.StarShellSession(profile, camera) + if receiver := session._projection_bookmarks: + request.addfinalizer(receiver.close) + return session -def test_compact_layout_propagates_paint_and_resets_after_first_touch_and_exception(lifecycle, monkeypatch): +def test_compact_layout_propagates_paint_and_resets_after_first_touch_and_exception(lifecycle, monkeypatch, request): settings, _, _, _ = lifecycle monkeypatch.setattr(runtime_app.gui_app, '_nav_stack', [settings]) monkeypatch.setattr(runtime_app.gui_app, '_width', 536) @@ -210,7 +213,7 @@ def test_compact_layout_propagates_paint_and_resets_after_first_touch_and_except monkeypatch.setattr(native, 'ui_state', NS(started=True, sm={})) camera_paint = Mock() monkeypatch.setattr(native.CameraView, '_render', camera_paint) - session = make_session(monkeypatch, runtime_app.Profile.COMPACT, camera) + session = make_session(monkeypatch, request, runtime_app.Profile.COMPACT, camera) layout = runtime_app.StarMiciMainLayout.__new__(runtime_app.StarMiciMainLayout) Widget.__init__(layout) layout._rect = rl.Rectangle(0, 0, 536, 240) @@ -241,9 +244,9 @@ def test_compact_layout_propagates_paint_and_resets_after_first_touch_and_except assert layout._native_onroad._bookmark_icon.process_gesture.call_count == 3 -def test_large_camera_paint_is_unchanged(monkeypatch): +def test_large_camera_paint_is_unchanged(monkeypatch, request): camera = Mock() - session = make_session(monkeypatch, runtime_app.Profile.LARGE, camera) + session = make_session(monkeypatch, request, runtime_app.Profile.LARGE, camera) session.camera_paint = False state = OnroadState(True, True, 20, 80, SpeedLimitObservation()) session.view.camera_layer(rl.Rectangle(0, 0, 2160, 1080), state) diff --git a/openpilot/starpilot/ui/tests/test_onroad_cem_gauge.py b/openpilot/starpilot/ui/tests/test_onroad_cem_gauge.py new file mode 100644 index 0000000000..14d3660b56 --- /dev/null +++ b/openpilot/starpilot/ui/tests/test_onroad_cem_gauge.py @@ -0,0 +1,249 @@ +"""Read-only Dom CEM visuals with real presentation state and source boundaries.""" +from contextlib import ExitStack +from dataclasses import replace +from types import SimpleNamespace as NS +from unittest.mock import Mock, patch +import copy + +import pyray as rl +import pytest + +from openpilot.starpilot.conditional_mode.policy import ModeChoice +from openpilot.starpilot.curve_speed.status import CurveObservation +from openpilot.starpilot.ui.appearance_preferences import CameraViewChoice +from openpilot.starpilot.ui.conditional_status import ConditionalDisplay +from openpilot.starpilot.ui.onroad_cem_gauge import AetherGauge, IndicatorType, gauge_data +from openpilot.starpilot.ui.onroad_conditional import reason, status, stop_active +from openpilot.starpilot.ui.onroad_customization import CEM_GAUGE_WIDGET, DEFAULT_WIDGET_ORDER, default_document, validate_document +from openpilot.starpilot.ui.onroad_state import (AlertSize, ConditionalLead, OnroadAlert, OnroadState, + SpeedLimitObservation, conditional_lead_from_message) + + +def state(reason='cem_stop', *, effective=False, **changes): + value = ConditionalDisplay(ModeChoice.CEM, effective, reason, 8 if reason == 'cem_stop' else 4, 'planner', 1) + return replace(OnroadState(effective, True, 20., 90., SpeedLimitObservation(), lateral_active=True, + longitudinal_active=effective, slc_system_long_available=effective, + conditional_configured=ModeChoice.CEM, + conditional_effective=value if effective else None, + conditional_perception=None if effective else value), **changes) + + +@pytest.mark.parametrize('missing', ['scene_unavailable', 'unavailable']) +@pytest.mark.parametrize('effective', [False, True]) +def test_missing_scene_is_hidden_without_erasing_policy_receipt(missing, effective): + sample = state(missing, effective=effective) + raw = sample.conditional_effective or sample.conditional_perception + assert raw is not None and raw.reason == missing + assert reason(sample) is None and status(sample) is None + assert gauge_data(sample) is None and not stop_active(sample) + assert (sample.conditional_effective or sample.conditional_perception) is raw + + +@pytest.mark.parametrize('effective', [False, True]) +def test_stop_graphic_preserves_informational_and_control_distinction(effective): + sample = state(effective=effective) + data = gauge_data(sample) + assert data is not None and data.indicator_type is IndicatorType.STOP_LIGHT + assert data.text == 'STOP' and not data.is_numeric and data.unit == '' + assert stop_active(sample) is effective + assert sample.longitudinal_active is effective + + +@pytest.mark.parametrize('kind', ['no_trigger', 'manual_chill', 'manual_experimental', 'ccm_veto', 'unknown_wire_reason']) +def test_gauge_never_infers_trigger_from_raw_lead_or_speed(kind): + sample = state(kind, conditional_lead=ConditionalLead(12., 0.)) + assert gauge_data(sample) is None + + +def test_current_lead_stopped_slow_hysteresis_and_absence_are_distinct(): + sample = state('cem_lead', conditional_lead=ConditionalLead(12., .7)) + assert gauge_data(sample).text == 'STOPPED' + threshold = replace(sample, conditional_lead=ConditionalLead(12., 1.)) + assert gauge_data(threshold).text == 'SLOW' + assert gauge_data(threshold, lead_stopped=True).text == 'STOPPED' + assert gauge_data(replace(sample, conditional_lead=None)).text == 'LEAD' + + +@pytest.mark.parametrize('bad', [None, NS(leadOne=NS(status=False, dRel=12., vLead=0.)), + NS(leadOne=NS(status=True, dRel=0., vLead=0.)), + NS(leadOne=NS(status=True, dRel=float('nan'), vLead=0.)), + NS(leadOne=NS(status=True, dRel=12., vLead=float('inf')))]) +def test_missing_or_invalid_radar_metrics_are_not_defaulted_to_stopped(bad): + assert conditional_lead_from_message(bad) is None + + +def test_current_radar_metrics_use_native_units(): + assert conditional_lead_from_message(NS(leadOne=NS(status=True, dRel=12., vLead=2.))) == ConditionalLead(12., 2.) + + +def test_curve_selected_source_retains_speed_units_and_csc_reduction(): + curve = CurveObservation(True, 15., 15., True, True, False, 100., 2., 30., 'curve', 'selected', 'saved', True, False, .005) + sample = state('cem_curve', effective=True, curve=curve, metric=True) + data = gauge_data(sample) + assert data.indicator_type is IndicatorType.ROAD_CURVE and data.indicator_value == .005 + assert (data.text, data.unit, data.reduction_text) == ('54', 'km/h', '-36') + assert (gauge_data(replace(sample, metric=False)).text, gauge_data(replace(sample, metric=False)).unit) == ('34', 'mph') + perception = state('cem_curve', curve=curve) + assert gauge_data(perception).text == '45' # Actual ego speed; no CSC control authority. + assert gauge_data(perception).reduction_text == '' + + +@pytest.mark.parametrize('changes', [{'alert': OnroadAlert(size=AlertSize.FULL, critical=True)}, + {'longitudinal_overridden': True}, {'reverse_driver_camera': True}]) +def test_safety_and_driver_camera_states_suppress_gauge(changes): + assert gauge_data(state(**changes)) is None + + +@pytest.mark.parametrize('camera', [CameraViewChoice.DRIVER, CameraViewChoice.NONE]) +def test_gauge_is_hidden_for_non_road_camera(camera): + sample = state() + assert gauge_data(replace(sample, appearance=replace(sample.appearance, camera_view=camera))) is None + + +def test_custom_placement_disable_layer_migration_preserve_existing_widgets(): + older = default_document() + older['layouts']['large'].pop(CEM_GAUGE_WIDGET) + older['layouts']['large']['current_speed'].update(x=740, y=90, enabled=False) + older['widgetOrder'] = {'large': [key for key in DEFAULT_WIDGET_ORDER['large'] if key != CEM_GAUGE_WIDGET]} + frozen = copy.deepcopy(older) + migrated = validate_document(older) + assert older == frozen + assert {key: value for key, value in migrated['layouts']['large'].items() if key != CEM_GAUGE_WIDGET} == older['layouts']['large'] + assert [key for key in migrated['widgetOrder']['large'] if key != CEM_GAUGE_WIDGET] == older['widgetOrder']['large'] + migrated['layouts']['large'][CEM_GAUGE_WIDGET].update(x=960, y=600, enabled=True) + assert validate_document(migrated) == migrated + gauge = AetherGauge(Mock()) + sample = state(customization=migrated) + with patch.object(gauge, '_render_unified_road') as draw: + gauge.render(sample) + assert draw.call_args.args[1:3] == (1132., 685.) + migrated['layouts']['large'][CEM_GAUGE_WIDGET]['enabled'] = False + draw.reset_mock() + gauge.render(sample) + draw.assert_not_called() + + +def test_missing_receipt_clears_lead_memory_without_cooldown(): + gauge = AetherGauge(Mock()) + with patch.object(gauge, '_render_unified_road') as draw: + gauge.render(state('cem_lead', conditional_lead=ConditionalLead(12., .7))) + assert gauge._lead_stopped + gauge.render(state('scene_unavailable')) + assert not gauge._lead_stopped + assert draw.call_count == 1 + + +def test_dom_stop_road_has_fill_chevrons_line_and_pulsing_light(): + gauge = AetherGauge(Mock()) + with ExitStack() as stack: + draws = {name: stack.enter_context(patch.object(rl, name)) for name in dir(rl) if name.startswith('draw_')} + stack.enter_context(patch.object(rl, 'get_frame_time', return_value=1 / 60)) + stack.enter_context(patch.object(rl, 'get_time', return_value=1.)) + stack.enter_context(patch.object(gauge, '_draw_mini_cradle')) + gauge.render(state()) + assert draws['draw_triangle'].call_count == 48 + assert draws['draw_line_ex'].call_count > 48 # Road edges, stop-line glow, and flowing chevrons. + assert draws['draw_circle_v'].call_count == 5 # Three bulbs plus red-light bloom. + + +def test_projection_saved_gauge_migration_keeps_native_layout_and_exit_layer(): + from openpilot.starpilot.system.android_auto.projection_layout import (CAR_EXIT, default_layout, projection_customization, + validate_layout) + from openpilot.starpilot.system.android_auto.tests.test_projection_layout import SCREEN + document = default_layout(SCREEN) + document['widgets'].pop(CEM_GAUGE_WIDGET) + document['widgetOrder'] = [key for key in document.get('widgetOrder', []) if key != CEM_GAUGE_WIDGET] + if not document['widgetOrder']: + document.pop('widgetOrder') + frozen = copy.deepcopy(document) + migrated = validate_layout(document, SCREEN) + assert document == frozen + assert {key: value for key, value in migrated['widgets'].items() if key != CEM_GAUGE_WIDGET} == document['widgets'] + assert migrated['widgets'][CEM_GAUGE_WIDGET]['enabled'] + base = default_document() + unchanged = copy.deepcopy(base) + projected = projection_customization(migrated, base) + assert base == unchanged and projected['layouts']['compact'] == base['layouts']['compact'] + assert projected['layouts']['large'][CEM_GAUGE_WIDGET]['enabled'] + if 'widgetOrder' in migrated: + assert migrated['widgetOrder'][-1] == CAR_EXIT + + +def test_numeric_cradle_measurement_and_draw_share_explicit_bitmap_font_roles(): + from openpilot.starpilot.ui.onroad_cem_gauge import AetherGaugeData + from openpilot.starpilot.ui.presentation import BitmapFonts, FontRole + fonts = Mock(spec=BitmapFonts) + metrics = {'54': NS(width=110., height=90.), '-36': NS(width=55., height=40.), 'km/h': NS(width=66., height=38.)} + fonts.measure.side_effect = lambda text, _role, _size: metrics[text] + gauge = AetherGauge(fonts) + data = AetherGaugeData('54', unit='km/h', reduction_text='-36', is_numeric=True) + with patch.object(rl, 'draw_line_ex'): + gauge._draw_mini_cradle(500., 145., data) + assert [call.args for call in fonts.measure.call_args_list] == [ + ('54', FontRole.BOLD, 75), ('-36', FontRole.MEDIUM, 33), ('km/h', FontRole.MEDIUM, 30)] + expected = {'54': (FontRole.BOLD, 75, 445., 154.), '-36': (FontRole.MEDIUM, 33, 564., 179.), + 'km/h': (FontRole.MEDIUM, 30, 467., 251.)} + for text, layout in expected.items(): + calls = [call.args for call in fonts.draw.call_args_list if call.args[0] == text] + assert len(calls) == 5 # Four Dom shadow samples plus the measured foreground glyph. + assert calls[-1][1:5] == layout + assert [call[3:5] for call in calls[:4]] == [(layout[2] + dx, layout[3] + dy) + for dx, dy in ((-1, -1), (1, -1), (-1, 1), (1, 1))] + + +def test_text_cradle_uses_profile_measurement_and_keeps_shadow_geometry(): + from openpilot.starpilot.ui.onroad_cem_gauge import AetherGaugeData + from openpilot.starpilot.ui.presentation import BitmapFonts, FontRole + fonts = Mock(spec=BitmapFonts) + fonts.measure.return_value = NS(width=200., height=58.) + gauge = AetherGauge(fonts) + gauge._draw_mini_cradle(500., 145., AetherGaugeData('STOPPED')) + fonts.measure.assert_called_once_with('STOPPED', FontRole.BOLD, 48) + calls = [call.args for call in fonts.draw.call_args_list] + assert len(calls) == 5 + assert calls[-1][:5] == ('STOPPED', FontRole.BOLD, 48, 400., 160.) + assert [call[3:5] for call in calls[:4]] == [(399., 159.), (401., 159.), (399., 161.), (401., 161.)] + + +@pytest.mark.parametrize('projection', [False, True]) +def test_numeric_caption_declared_footprint_fits_actual_bottom_content_edge(projection): + from openpilot.starpilot.system.android_auto.projection_layout import (default_layout_for_viewport, layout_metadata_for_viewport, + projection_customization, validate_layout_for_viewport) + from openpilot.starpilot.ui.onroad_customization import customization_metadata + from openpilot.starpilot.ui.presentation import BitmapFonts, FontRole, Profile + viewport = (2160, 1080) + if projection: + document = default_layout_for_viewport(viewport) + metadata = layout_metadata_for_viewport(viewport) + placed = document['widgets'][CEM_GAUGE_WIDGET] + placed.update(x=980, y=750, enabled=True) + validated = validate_layout_for_viewport(document, viewport) + customization = projection_customization(validated, default_document()) + else: + document = default_document() + metadata = customization_metadata()['profiles']['large'] + placed = document['layouts']['large'][CEM_GAUGE_WIDGET] + placed.update(x=980, y=750, enabled=True) + customization = validate_document(document) + assert metadata['widgets'][CEM_GAUGE_WIDGET]['height'] == 300 + # Both screens clip the onroad content 30 pixels above the physical bottom. + assert metadata['bounds']['y'] + metadata['bounds']['height'] == 1050 + assert placed['y'] + 300 == 1050 + fonts = Mock(spec=BitmapFonts) + fonts.measure.side_effect = lambda _text, _role, size: NS(width=80., height=size * Profile.LARGE.font_scale) + gauge = AetherGauge(fonts) + with patch.object(rl, 'draw_line_ex'): + sample = state('cem_curve', customization=customization) + gauge._draw_mini_cradle(1152., placed['y'] + 145, gauge_data(sample)) + unit_calls = [call.args for call in fonts.draw.call_args_list if call.args[0] == 'mph'] + assert len(unit_calls) == 5 + unit_size = unit_calls[-1][2] + assert unit_calls[-1][1] is FontRole.MEDIUM + assert max(call[4] for call in unit_calls) + unit_size * Profile.LARGE.font_scale <= 1050 + for invalid_y in (751, 780): + placed['y'] = invalid_y + with pytest.raises(ValueError): + if projection: + validate_layout_for_viewport(document, viewport) + else: + validate_document(document) diff --git a/openpilot/starpilot/ui/tests/test_onroad_customization.py b/openpilot/starpilot/ui/tests/test_onroad_customization.py index a2cf63ba4b..ef282b68e6 100644 --- a/openpilot/starpilot/ui/tests/test_onroad_customization.py +++ b/openpilot/starpilot/ui/tests/test_onroad_customization.py @@ -411,7 +411,8 @@ def test_compact_drag_keeps_animation_state_and_separate_origins(): document = default_document() document["layouts"]["compact"]["max_speed"].update(x=100, y=10) document["layouts"]["compact"]["steering_wheel"].update(x=90, y=150) - fonts = NS(draw=Mock()) + fonts = NS(draw=Mock(), measure=lambda text, _role, size: NS( + width=len(text) * size * .5, height=size * Profile.COMPACT.font_scale)) hud = CompactHudRenderer(Mock(spec=BitmapFonts, **vars(fonts)), Mock()) hud._wheel = Mock() state = road(document, lateral_active=True, longitudinal_active=True) diff --git a/openpilot/starpilot/ui/tests/test_speed_font_roles.py b/openpilot/starpilot/ui/tests/test_speed_font_roles.py new file mode 100644 index 0000000000..993b8ef23d --- /dev/null +++ b/openpilot/starpilot/ui/tests/test_speed_font_roles.py @@ -0,0 +1,20 @@ +from openpilot.starpilot.ui.presentation import BitmapFonts, FontRole, Profile, font_filename + + +def test_compact_set_speed_uses_custom_face_without_changing_current_speed(): + assert font_filename(Profile.COMPACT, FontRole.SET_SPEED) == "DINish-Speed.fnt" + assert font_filename(Profile.COMPACT, FontRole.SPEED) == "Inter-Bold.fnt" + assert font_filename(Profile.COMPACT, FontRole.DISPLAY) == "Inter-Bold.fnt" + assert font_filename(Profile.COMPACT, FontRole.SEMI_BOLD) == "Inter-SemiBold.fnt" + + +def test_speed_optical_style_is_scoped_to_requested_faces(): + fonts = BitmapFonts.__new__(BitmapFonts) + fonts.profile = Profile.LARGE + assert fonts._optical_style(FontRole.SPEED) == (1.04, .6) + assert fonts._optical_style(FontRole.BRAND) == (1., 0.) + fonts.profile = Profile.COMPACT + assert fonts._optical_style(FontRole.SET_SPEED) == (1., .45) + assert fonts._optical_style(FontRole.SPEED) == (1., 0.) + assert fonts._optical_style(FontRole.DISPLAY) == (1., 0.) + assert fonts._optical_style(FontRole.SET_SPEED, fallback=True) == (1., 0.) diff --git a/openpilot/system/camerad/camerad b/openpilot/system/camerad/camerad index 33c10675e0..e58e1a05d6 100755 Binary files a/openpilot/system/camerad/camerad and b/openpilot/system/camerad/camerad differ diff --git a/openpilot/system/loggerd/bootlog b/openpilot/system/loggerd/bootlog index dc55d34dee..17ec78b069 100755 Binary files a/openpilot/system/loggerd/bootlog and b/openpilot/system/loggerd/bootlog differ diff --git a/openpilot/system/loggerd/encoderd b/openpilot/system/loggerd/encoderd index bea6bc8158..ec668b9662 100755 Binary files a/openpilot/system/loggerd/encoderd and b/openpilot/system/loggerd/encoderd differ diff --git a/openpilot/system/loggerd/loggerd b/openpilot/system/loggerd/loggerd index 68a9f12a47..f3ca8eff5f 100755 Binary files a/openpilot/system/loggerd/loggerd and b/openpilot/system/loggerd/loggerd differ diff --git a/prebuilt.json b/prebuilt.json index cc936f7a0f..68e15ef06e 100644 --- a/prebuilt.json +++ b/prebuilt.json @@ -1 +1 @@ -{"protected_inventory_sha256": "9a957da0cef8e808bb9e8d783a12dc3c8a4f358c01f420ec2300d9d4d2069f31", "schema_version": 1} +{"protected_inventory_sha256": "242635996db7ccf96fa36aefed6bfd946912fd94a718c4cb044b0b6e6f67e955", "schema_version": 1}