mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-11-09 22:13:43 +08:00
Fix startup locks and restore onroad behavior
Release inherited Params locks correctly across fork and exec. Preserve qualified lateral armed intent through temporary faults, restore the Big UI CEM gauge, improve speed typography, and hide unavailable CEM initialization text. Include matching device artifacts and focused regression coverage.
This commit is contained in:
firestar5683
@@ -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'])
|
||||
|
||||
Binary file not shown.
Binary file not shown.
@@ -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
|
||||
|
||||
@@ -0,0 +1,205 @@
|
||||
// Native regression tests exercise the actual Params/FileLock implementation.
|
||||
#include <sys/file.h>
|
||||
#include <sys/wait.h>
|
||||
#include <sys/stat.h>
|
||||
#include <poll.h>
|
||||
#include <fcntl.h>
|
||||
#include <unistd.h>
|
||||
#include <cerrno>
|
||||
#include <chrono>
|
||||
#include <atomic>
|
||||
#include <filesystem>
|
||||
#include <iostream>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
#include <thread>
|
||||
#include <vector>
|
||||
#include "common/tests/native_test.h"
|
||||
|
||||
static int fail_ex = 0;
|
||||
static std::atomic<int> 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<std::string> keys{"GitCommit", "GitBranch", "GitRemote", "Version"};
|
||||
std::vector<std::thread> writers;
|
||||
std::atomic<int> 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;
|
||||
}
|
||||
}
|
||||
@@ -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")
|
||||
@@ -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),
|
||||
|
||||
@@ -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 <eigen3/Eigen/Dense>
|
||||
#include <iostream>
|
||||
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
|
||||
Binary file not shown.
Binary file not shown.
File diff suppressed because it is too large
Load Diff
@@ -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);
|
||||
}
|
||||
Binary file not shown.
@@ -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
|
||||
@@ -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:
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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.
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
@@ -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:
|
||||
|
||||
@@ -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))
|
||||
|
||||
@@ -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),
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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:
|
||||
|
||||
@@ -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,
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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__':
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -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)
|
||||
@@ -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)
|
||||
|
||||
@@ -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.)
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
+1
-1
@@ -1 +1 @@
|
||||
{"protected_inventory_sha256": "9a957da0cef8e808bb9e8d783a12dc3c8a4f358c01f420ec2300d9d4d2069f31", "schema_version": 1}
|
||||
{"protected_inventory_sha256": "242635996db7ccf96fa36aefed6bfd946912fd94a718c4cb044b0b6e6f67e955", "schema_version": 1}
|
||||
|
||||
Reference in New Issue
Block a user