"
+ exit 1
+fi
+
+BASE="https://github.com/commaai/openpilot/pull/"
+PR_NUM="$(echo $1 | grep -o -E '[0-9]+')"
+
+curl -L $BASE/$PR_NUM.patch | git apply
diff --git a/selfdrive/assets/offroad/fcc.html b/selfdrive/assets/offroad/fcc.html
index 7f03a72046..793bea533c 100644
--- a/selfdrive/assets/offroad/fcc.html
+++ b/selfdrive/assets/offroad/fcc.html
@@ -11,7 +11,7 @@
FCC ID: 2AOHHTURBOXSOMD845
Quectel/EG25-G
- FCC ID: XMR201903EG25GM
+ FCC ID: XMR201903EG25G
This device complies with Part 15 of the FCC Rules.
Operation is subject to the following two conditions:
diff --git a/selfdrive/assets/sounds/disengage.wav b/selfdrive/assets/sounds/disengage.wav
new file mode 100644
index 0000000000..9f4a16e627
Binary files /dev/null and b/selfdrive/assets/sounds/disengage.wav differ
diff --git a/selfdrive/assets/sounds/disengaged.wav b/selfdrive/assets/sounds/disengaged.wav
deleted file mode 100644
index 3aa8e51e69..0000000000
Binary files a/selfdrive/assets/sounds/disengaged.wav and /dev/null differ
diff --git a/selfdrive/assets/sounds/engage.wav b/selfdrive/assets/sounds/engage.wav
new file mode 100644
index 0000000000..ff094bfb01
Binary files /dev/null and b/selfdrive/assets/sounds/engage.wav differ
diff --git a/selfdrive/assets/sounds/engaged.wav b/selfdrive/assets/sounds/engaged.wav
deleted file mode 100644
index 1451f937f3..0000000000
Binary files a/selfdrive/assets/sounds/engaged.wav and /dev/null differ
diff --git a/selfdrive/assets/sounds/error.wav b/selfdrive/assets/sounds/error.wav
deleted file mode 100644
index e805181aeb..0000000000
Binary files a/selfdrive/assets/sounds/error.wav and /dev/null differ
diff --git a/selfdrive/assets/sounds/prompt.wav b/selfdrive/assets/sounds/prompt.wav
new file mode 100644
index 0000000000..f35e66e144
Binary files /dev/null and b/selfdrive/assets/sounds/prompt.wav differ
diff --git a/selfdrive/assets/sounds/prompt_distracted.wav b/selfdrive/assets/sounds/prompt_distracted.wav
new file mode 100644
index 0000000000..557be172aa
Binary files /dev/null and b/selfdrive/assets/sounds/prompt_distracted.wav differ
diff --git a/selfdrive/assets/sounds/refuse.wav b/selfdrive/assets/sounds/refuse.wav
new file mode 100644
index 0000000000..a21bb1a36e
Binary files /dev/null and b/selfdrive/assets/sounds/refuse.wav differ
diff --git a/selfdrive/assets/sounds/warning_1.wav b/selfdrive/assets/sounds/warning_1.wav
deleted file mode 100644
index 43ca74cc5c..0000000000
Binary files a/selfdrive/assets/sounds/warning_1.wav and /dev/null differ
diff --git a/selfdrive/assets/sounds/warning_2.wav b/selfdrive/assets/sounds/warning_2.wav
deleted file mode 100644
index 4909f11982..0000000000
Binary files a/selfdrive/assets/sounds/warning_2.wav and /dev/null differ
diff --git a/selfdrive/assets/sounds/warning_immediate.wav b/selfdrive/assets/sounds/warning_immediate.wav
new file mode 100644
index 0000000000..b93f2cda05
Binary files /dev/null and b/selfdrive/assets/sounds/warning_immediate.wav differ
diff --git a/selfdrive/assets/sounds/warning_repeat.wav b/selfdrive/assets/sounds/warning_repeat.wav
deleted file mode 100644
index 9f6f672e28..0000000000
Binary files a/selfdrive/assets/sounds/warning_repeat.wav and /dev/null differ
diff --git a/selfdrive/assets/sounds/warning_soft.wav b/selfdrive/assets/sounds/warning_soft.wav
new file mode 100644
index 0000000000..3651366bb1
Binary files /dev/null and b/selfdrive/assets/sounds/warning_soft.wav differ
diff --git a/selfdrive/assets/sounds_tici/disengaged.wav b/selfdrive/assets/sounds_tici/disengaged.wav
deleted file mode 100644
index 182adffa2c..0000000000
Binary files a/selfdrive/assets/sounds_tici/disengaged.wav and /dev/null differ
diff --git a/selfdrive/assets/sounds_tici/engaged.wav b/selfdrive/assets/sounds_tici/engaged.wav
deleted file mode 100644
index 5c4c8b4d1d..0000000000
Binary files a/selfdrive/assets/sounds_tici/engaged.wav and /dev/null differ
diff --git a/selfdrive/assets/sounds_tici/error.wav b/selfdrive/assets/sounds_tici/error.wav
deleted file mode 100644
index c4e280e9a6..0000000000
Binary files a/selfdrive/assets/sounds_tici/error.wav and /dev/null differ
diff --git a/selfdrive/assets/sounds_tici/warning_1.wav b/selfdrive/assets/sounds_tici/warning_1.wav
deleted file mode 100644
index c937b3be06..0000000000
Binary files a/selfdrive/assets/sounds_tici/warning_1.wav and /dev/null differ
diff --git a/selfdrive/assets/sounds_tici/warning_2.wav b/selfdrive/assets/sounds_tici/warning_2.wav
deleted file mode 100644
index 49188db881..0000000000
Binary files a/selfdrive/assets/sounds_tici/warning_2.wav and /dev/null differ
diff --git a/selfdrive/assets/sounds_tici/warning_repeat.wav b/selfdrive/assets/sounds_tici/warning_repeat.wav
deleted file mode 100644
index bb3bfd0d54..0000000000
Binary files a/selfdrive/assets/sounds_tici/warning_repeat.wav and /dev/null differ
diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py
index 779045bf97..5e462a42f3 100755
--- a/selfdrive/athena/athenad.py
+++ b/selfdrive/athena/athenad.py
@@ -30,7 +30,7 @@ from selfdrive.hardware import HARDWARE, PC
from selfdrive.loggerd.config import ROOT
from selfdrive.loggerd.xattr_cache import getxattr, setxattr
from selfdrive.swaglog import cloudlog, SWAGLOG_DIR
-from selfdrive.version import version, get_version, get_git_remote, get_git_branch, get_git_commit
+from selfdrive.version import get_version, get_origin, get_short_branch, get_commit
ATHENA_HOST = os.getenv('ATHENA_HOST', 'wss://athena.comma.ai')
HANDLER_THREADS = int(os.getenv('HANDLER_THREADS', "4"))
@@ -176,9 +176,9 @@ def getMessage(service=None, timeout=1000):
def getVersion():
return {
"version": get_version(),
- "remote": get_git_remote(),
- "branch": get_git_branch(),
- "commit": get_git_commit(),
+ "remote": get_origin(),
+ "branch": get_short_branch(),
+ "commit": get_commit(),
}
@@ -551,7 +551,7 @@ def main():
except socket.timeout:
try:
r = requests.get("http://api.commadotai.com/v1/me", allow_redirects=False,
- headers={"User-Agent": f"openpilot-{version}"}, timeout=15.0)
+ headers={"User-Agent": f"openpilot-{get_version()}"}, timeout=15.0)
if r.status_code == 302 and r.headers['Location'].startswith("http://u.web2go.com"):
params.put_bool("PrimeRedirected", True)
except Exception:
diff --git a/selfdrive/athena/manage_athenad.py b/selfdrive/athena/manage_athenad.py
index 7bb717e078..fa95eacd8e 100755
--- a/selfdrive/athena/manage_athenad.py
+++ b/selfdrive/athena/manage_athenad.py
@@ -6,7 +6,7 @@ from multiprocessing import Process
from common.params import Params
from selfdrive.manager.process import launcher
from selfdrive.swaglog import cloudlog
-from selfdrive.version import version, dirty
+from selfdrive.version import get_version, get_dirty
ATHENA_MGR_PID_PARAM = "AthenadPid"
@@ -14,7 +14,7 @@ ATHENA_MGR_PID_PARAM = "AthenadPid"
def main():
params = Params()
dongle_id = params.get("DongleId").decode('utf-8')
- cloudlog.bind_global(dongle_id=dongle_id, version=version, dirty=dirty)
+ cloudlog.bind_global(dongle_id=dongle_id, version=get_version(), dirty=get_dirty())
try:
while 1:
diff --git a/selfdrive/boardd/.gitignore b/selfdrive/boardd/.gitignore
index 1f653bde8f..e8daa2ef27 100644
--- a/selfdrive/boardd/.gitignore
+++ b/selfdrive/boardd/.gitignore
@@ -1,2 +1,3 @@
boardd
boardd_api_impl.cpp
+tests/test_boardd_usbprotocol
diff --git a/selfdrive/boardd/SConscript b/selfdrive/boardd/SConscript
index f2a1f3f7bd..07ded56e1b 100644
--- a/selfdrive/boardd/SConscript
+++ b/selfdrive/boardd/SConscript
@@ -1,6 +1,9 @@
Import('env', 'envCython', 'common', 'cereal', 'messaging')
-env.Program('boardd', ['boardd.cc', 'panda.cc', 'pigeon.cc'], LIBS=['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj'])
+libs = ['usb-1.0', common, cereal, messaging, 'pthread', 'zmq', 'capnp', 'kj']
+env.Program('boardd', ['boardd.cc', 'panda.cc', 'pigeon.cc'], LIBS=libs)
env.Library('libcan_list_to_can_capnp', ['can_list_to_can_capnp.cc'])
envCython.Program('boardd_api_impl.so', 'boardd_api_impl.pyx', LIBS=["can_list_to_can_capnp", 'capnp', 'kj'] + envCython["LIBS"])
+if GetOption('test'):
+ env.Program('tests/test_boardd_usbprotocol', ['tests/test_boardd_usbprotocol.cc', 'panda.cc'], LIBS=libs)
diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc
index f9c4891d03..3f66efc9fe 100644
--- a/selfdrive/boardd/boardd.cc
+++ b/selfdrive/boardd/boardd.cc
@@ -62,7 +62,7 @@ std::atomic pigeon_active(false);
ExitHandler do_exit;
-std::string get_time_str(const struct tm &time) {
+static std::string get_time_str(const struct tm &time) {
char s[30] = {'\0'};
std::strftime(s, std::size(s), "%Y-%m-%d %H:%M:%S", &time);
return s;
@@ -70,11 +70,43 @@ std::string get_time_str(const struct tm &time) {
bool check_all_connected(const std::vector &pandas) {
for (const auto& panda : pandas) {
- if (!panda->connected) return false;
+ if (!panda->connected) {
+ do_exit = true;
+ return false;
+ }
}
return true;
}
+enum class SyncTimeDir { TO_PANDA, FROM_PANDA };
+
+void sync_time(Panda *panda, SyncTimeDir dir) {
+ if (!panda->has_rtc) return;
+
+ setenv("TZ", "UTC", 1);
+ struct tm sys_time = util::get_time();
+ struct tm rtc_time = panda->get_rtc();
+
+ if (dir == SyncTimeDir::TO_PANDA) {
+ if (util::time_valid(sys_time)) {
+ // Write time to RTC if it looks reasonable
+ double seconds = difftime(mktime(&rtc_time), mktime(&sys_time));
+ if (std::abs(seconds) > 1.1) {
+ panda->set_rtc(sys_time);
+ LOGW("Updating panda RTC. dt = %.2f System: %s RTC: %s",
+ seconds, get_time_str(sys_time).c_str(), get_time_str(rtc_time).c_str());
+ }
+ }
+ } else if (dir == SyncTimeDir::FROM_PANDA) {
+ if (!util::time_valid(sys_time) && util::time_valid(rtc_time)) {
+ const struct timeval tv = {mktime(&rtc_time), 0};
+ settimeofday(&tv, 0);
+ LOGE("System time wrong, setting from RTC. System: %s RTC: %s",
+ get_time_str(sys_time).c_str(), get_time_str(rtc_time).c_str());
+ }
+ }
+}
+
bool safety_setter_thread(std::vector pandas) {
LOGD("Starting safety setter thread");
@@ -168,19 +200,7 @@ Panda *usb_connect(std::string serial="", uint32_t index=0) {
std::call_once(connected_once, &Panda::set_usb_power_mode, panda, cereal::PeripheralState::UsbPowerMode::CDP);
#endif
- if (panda->has_rtc) {
- setenv("TZ","UTC",1);
- struct tm sys_time = util::get_time();
- struct tm rtc_time = panda->get_rtc();
-
- if (!util::time_valid(sys_time) && util::time_valid(rtc_time)) {
- LOGE("System time wrong, setting from RTC. System: %s RTC: %s",
- get_time_str(sys_time).c_str(), get_time_str(rtc_time).c_str());
- const struct timeval tv = {mktime(&rtc_time), 0};
- settimeofday(&tv, 0);
- }
- }
-
+ sync_time(panda.get(), SyncTimeDir::FROM_PANDA);
return panda.release();
}
@@ -194,14 +214,8 @@ void can_send_thread(std::vector pandas, bool fake_send) {
subscriber->setTimeout(100);
// run as fast as messages come in
- while (!do_exit) {
- if (!check_all_connected(pandas)) {
- do_exit = true;
- break;
- }
-
+ while (!do_exit && check_all_connected(pandas)) {
Message * msg = subscriber->receive();
-
if (!msg) {
if (errno == EINTR) {
do_exit = true;
@@ -239,12 +253,7 @@ void can_recv_thread(std::vector pandas) {
uint64_t next_frame_time = nanos_since_boot() + dt;
std::vector raw_can_data;
- while (!do_exit) {
- if (!check_all_connected(pandas)){
- do_exit = true;
- break;
- }
-
+ while (!do_exit && check_all_connected(pandas)) {
bool comms_healthy = true;
raw_can_data.clear();
for (const auto& panda : pandas) {
@@ -315,7 +324,7 @@ bool send_panda_states(PubMaster *pm, const std::vector &pandas, bool s
for (uint32_t i=0; i pandas, bool spoofin
LOGD("start panda state thread");
// run at 2hz
- while (!do_exit) {
- if(!check_all_connected(pandas)) {
- do_exit = true;
- break;
- }
-
+ while (!do_exit && check_all_connected(pandas)) {
+ // send out peripheralState
send_peripheral_state(pm, peripheral_panda);
ignition = send_panda_states(pm, pandas, spoofing_started);
@@ -433,7 +438,7 @@ void panda_state_thread(PubMaster *pm, std::vector pandas, bool spoofin
break;
}
- // clear VIN, CarParams, and set new safety on car start
+ // clear ignition-based params and set new safety on car start
if (ignition && !ignition_last) {
params.clearAll(CLEAR_ON_IGNITION_ON);
if (!safety_future.valid() || safety_future.wait_for(0ms) == std::future_status::ready) {
@@ -528,21 +533,8 @@ void peripheral_control_thread(Panda *panda) {
}
// Write to rtc once per minute when no ignition present
- if ((panda->has_rtc) && !ignition && (cnt % 120 == 1)) {
- // Write time to RTC if it looks reasonable
- setenv("TZ","UTC",1);
- struct tm sys_time = util::get_time();
-
- if (util::time_valid(sys_time)) {
- struct tm rtc_time = panda->get_rtc();
- double seconds = difftime(mktime(&rtc_time), mktime(&sys_time));
-
- if (std::abs(seconds) > 1.1) {
- panda->set_rtc(sys_time);
- LOGW("Updating panda RTC. dt = %.2f System: %s RTC: %s",
- seconds, get_time_str(sys_time).c_str(), get_time_str(rtc_time).c_str());
- }
- }
+ if (!ignition && (cnt % 120 == 1)) {
+ sync_time(panda, SyncTimeDir::TO_PANDA);
}
}
}
diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc
index 78b123f698..9c59bba6fc 100644
--- a/selfdrive/boardd/panda.cc
+++ b/selfdrive/boardd/panda.cc
@@ -363,7 +363,8 @@ uint8_t Panda::len_to_dlc(uint8_t len) {
}
}
-void Panda::can_send(capnp::List::Reader can_data_list) {
+void Panda::pack_can_buffer(const capnp::List::Reader &can_data_list,
+ std::function write_func) {
if (send.size() < (can_data_list.size() * CANPACKET_MAX_SIZE)) {
send.resize(can_data_list.size() * CANPACKET_MAX_SIZE);
}
@@ -383,7 +384,7 @@ void Panda::can_send(capnp::List::Reader can_data_list) {
}
auto can_data = cmsg.getDat();
uint8_t data_len_code = len_to_dlc(can_data.size());
- assert(can_data.size() <= (hw_type == cereal::PandaState::PandaType::RED_PANDA) ? 64 : 8);
+ assert(can_data.size() <= ((hw_type == cereal::PandaState::PandaType::RED_PANDA) ? 64 : 8));
assert(can_data.size() == dlc_to_len[data_len_code]);
can_header header;
@@ -410,67 +411,58 @@ void Panda::can_send(capnp::List::Reader can_data_list) {
ptr += copy_size + 1;
counter++;
}
- usb_bulk_write(3, to_write, ptr, 5);
+ write_func(to_write, ptr);
}
}
}
+void Panda::can_send(capnp::List::Reader can_data_list) {
+ pack_can_buffer(can_data_list, [=](uint8_t* data, size_t size) {
+ usb_bulk_write(3, data, size, 5);
+ });
+}
+
bool Panda::can_receive(std::vector& out_vec) {
uint8_t data[RECV_SIZE];
int recv = usb_bulk_read(0x81, (uint8_t*)data, RECV_SIZE);
-
- // Not sure if this can happen
- if (recv < 0) recv = 0;
-
- if (recv == RECV_SIZE) {
- LOGW("Receive buffer full");
- }
-
if (!comms_healthy) {
return false;
}
+ if (recv == RECV_SIZE) {
+ LOGW("Panda receive buffer full");
+ }
- static uint8_t tail[CANPACKET_MAX_SIZE];
- uint8_t tail_size = 0;
- uint8_t counter = 0;
- for (int i = 0; i < recv; i += USBPACKET_MAX_SIZE) {
- // Check for counter every 64 bytes (length of USB packet)
- if (counter != data[i]) {
+ return (recv <= 0) ? true : unpack_can_buffer(data, recv, out_vec);
+}
+
+bool Panda::unpack_can_buffer(uint8_t *data, int size, std::vector &out_vec) {
+ recv_buf.clear();
+ for (int i = 0; i < size; i += USBPACKET_MAX_SIZE) {
+ if (data[i] != i / USBPACKET_MAX_SIZE) {
LOGE("CAN: MALFORMED USB RECV PACKET");
- break;
- }
- counter++;
- uint8_t chunk_len = ((recv - i) > USBPACKET_MAX_SIZE) ? 63 : (recv - i - 1); // as 1 is always reserved for counter
- uint8_t chunk[USBPACKET_MAX_SIZE + CANPACKET_MAX_SIZE];
- memcpy(chunk, tail, tail_size);
- memcpy(&chunk[tail_size], &data[i+1], chunk_len);
- chunk_len += tail_size;
- tail_size = 0;
- uint8_t pos = 0;
- while (pos < chunk_len) {
- uint8_t data_len = dlc_to_len[(chunk[pos] >> 4)];
- uint8_t pckt_len = CANPACKET_HEAD_SIZE + data_len;
- if (pckt_len <= (chunk_len - pos)) {
- can_header header;
- memcpy(&header, &chunk[pos], CANPACKET_HEAD_SIZE);
-
- can_frame &canData = out_vec.emplace_back();
- canData.busTime = 0;
- canData.address = header.addr;
- canData.src = header.bus + bus_offset;
-
- if (header.rejected) { canData.src += CANPACKET_REJECTED; }
- if (header.returned) { canData.src += CANPACKET_RETURNED; }
- canData.dat.assign((char*)&chunk[pos+CANPACKET_HEAD_SIZE], data_len);
-
- pos += pckt_len;
- } else {
- // Keep partial CAN packet until next USB packet
- tail_size = (chunk_len - pos);
- memcpy(tail, &chunk[pos], tail_size);
- break;
- }
+ comms_healthy = false;
+ return false;
}
+ int chunk_len = std::min(USBPACKET_MAX_SIZE, (size - i));
+ recv_buf.insert(recv_buf.end(), &data[i + 1], &data[i + chunk_len]);
+ }
+
+ int pos = 0;
+ while (pos < recv_buf.size()) {
+ can_header header;
+ memcpy(&header, &recv_buf[pos], CANPACKET_HEAD_SIZE);
+
+ can_frame &canData = out_vec.emplace_back();
+ canData.busTime = 0;
+ canData.address = header.addr;
+ canData.src = header.bus + bus_offset;
+ if (header.rejected) { canData.src += CANPACKET_REJECTED; }
+ if (header.returned) { canData.src += CANPACKET_RETURNED; }
+
+ const uint8_t data_len = dlc_to_len[header.data_len_code];
+ canData.dat.assign((char *)&recv_buf[pos + CANPACKET_HEAD_SIZE], data_len);
+
+ pos += CANPACKET_HEAD_SIZE + data_len;
}
return true;
}
diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h
index 4be9454cf5..9ac75c968a 100644
--- a/selfdrive/boardd/panda.h
+++ b/selfdrive/boardd/panda.h
@@ -3,6 +3,7 @@
#include
#include
#include
+#include
#include
#include
#include
@@ -17,7 +18,7 @@
#define PANDA_BUS_CNT 4
#define RECV_SIZE (0x4000U)
#define USB_TX_SOFT_LIMIT (0x100U)
-#define USBPACKET_MAX_SIZE (0x40U)
+#define USBPACKET_MAX_SIZE (0x40)
#define CANPACKET_HEAD_SIZE 5U
#define CANPACKET_MAX_SIZE 72U
#define CANPACKET_REJECTED (0xC0U)
@@ -69,6 +70,7 @@ class Panda {
libusb_device_handle *dev_handle = NULL;
std::mutex usb_lock;
std::vector send;
+ std::vector recv_buf;
void handle_usb_issue(int err, const char func[]);
void cleanup();
@@ -113,4 +115,11 @@ class Panda {
uint8_t len_to_dlc(uint8_t len);
void can_send(capnp::List::Reader can_data_list);
bool can_receive(std::vector& out_vec);
+
+protected:
+ // for unit tests
+ Panda(uint32_t bus_offset) : bus_offset(bus_offset) {}
+ void pack_can_buffer(const capnp::List::Reader &can_data_list,
+ std::function write_func);
+ bool unpack_can_buffer(uint8_t *data, int size, std::vector &out_vec);
};
diff --git a/selfdrive/boardd/tests/test_boardd_usbprotocol.cc b/selfdrive/boardd/tests/test_boardd_usbprotocol.cc
new file mode 100644
index 0000000000..6a13cbd71f
--- /dev/null
+++ b/selfdrive/boardd/tests/test_boardd_usbprotocol.cc
@@ -0,0 +1,130 @@
+#define CATCH_CONFIG_MAIN
+#define CATCH_CONFIG_ENABLE_BENCHMARKING
+#include
+
+#include "catch2/catch.hpp"
+#include "cereal/messaging/messaging.h"
+#include "selfdrive/boardd/panda.h"
+
+const unsigned char dlc_to_len[] = {0U, 1U, 2U, 3U, 4U, 5U, 6U, 7U, 8U, 12U, 16U, 20U, 24U, 32U, 48U, 64U};
+
+int random_int(int min, int max) {
+ std::random_device dev;
+ std::mt19937 rng(dev());
+ std::uniform_int_distribution dist(min, max);
+ return dist(rng);
+}
+
+struct PandaTest : public Panda {
+ PandaTest(uint32_t bus_offset, int can_list_size, cereal::PandaState::PandaType hw_type);
+ void test_can_send();
+ void test_can_recv();
+
+ std::map test_data;
+ int can_list_size = 0;
+ int total_pakets_size = 0;
+ MessageBuilder msg;
+ capnp::List::Reader can_data_list;
+};
+
+PandaTest::PandaTest(uint32_t bus_offset_, int can_list_size, cereal::PandaState::PandaType hw_type) : can_list_size(can_list_size), Panda(bus_offset_) {
+ this->hw_type = hw_type;
+ int data_limit = ((hw_type == cereal::PandaState::PandaType::RED_PANDA) ? std::size(dlc_to_len) : 8);
+ // prepare test data
+ for (int i = 0; i < data_limit; ++i) {
+ std::random_device rd;
+ std::independent_bits_engine rbe(rd());
+
+ int data_len = dlc_to_len[i];
+ std::string bytes(data_len, '\0');
+ std::generate(bytes.begin(), bytes.end(), std::ref(rbe));
+ test_data[data_len] = bytes;
+ }
+
+ // generate can messages for this panda
+ auto can_list = msg.initEvent().initSendcan(can_list_size);
+ for (uint8_t i = 0; i < can_list_size; ++i) {
+ auto can = can_list[i];
+ uint32_t id = random_int(0, std::size(dlc_to_len) - 1);
+ const std::string &dat = test_data[dlc_to_len[id]];
+ can.setAddress(i);
+ can.setSrc(random_int(0, 3) + bus_offset);
+ can.setDat(kj::ArrayPtr((uint8_t *)dat.data(), dat.size()));
+ total_pakets_size += CANPACKET_HEAD_SIZE + dat.size();
+ }
+
+ can_data_list = can_list.asReader();
+ INFO("test " << can_list_size << " packets, total size " << total_pakets_size);
+}
+
+void PandaTest::test_can_send() {
+ std::vector unpacked_data;
+ this->pack_can_buffer(can_data_list, [&](uint8_t *chunk, size_t size) {
+ int size_left = size;
+ for (int i = 0, counter = 0; i < size; i += USBPACKET_MAX_SIZE, counter++) {
+ REQUIRE(chunk[i] == counter);
+
+ const int len = std::min(USBPACKET_MAX_SIZE, size_left);
+ unpacked_data.insert(unpacked_data.end(), &chunk[i + 1], &chunk[i + len]);
+ size_left -= len;
+ }
+ });
+ REQUIRE(unpacked_data.size() == total_pakets_size);
+
+ int cnt = 0;
+ INFO("test can message integrity");
+ for (int pos = 0, pckt_len = 0; pos < unpacked_data.size(); pos += pckt_len) {
+ can_header header;
+ memcpy(&header, &unpacked_data[pos], CANPACKET_HEAD_SIZE);
+ const uint8_t data_len = dlc_to_len[header.data_len_code];
+ pckt_len = CANPACKET_HEAD_SIZE + data_len;
+
+ REQUIRE(header.addr == cnt);
+ REQUIRE(test_data.find(data_len) != test_data.end());
+ const std::string &dat = test_data[data_len];
+ REQUIRE(memcmp(dat.data(), &unpacked_data[pos + 5], dat.size()) == 0);
+ ++cnt;
+ }
+ REQUIRE(cnt == can_list_size);
+}
+
+void PandaTest::test_can_recv() {
+ std::vector frames;
+ this->pack_can_buffer(can_data_list, [&](uint8_t *data, size_t size) {
+ this->unpack_can_buffer(data, size, frames);
+ });
+
+ REQUIRE(frames.size() == can_list_size);
+ for (int i = 0; i < frames.size(); ++i) {
+ REQUIRE(frames[i].address == i);
+ REQUIRE(test_data.find(frames[i].dat.size()) != test_data.end());
+ const std::string &dat = test_data[frames[i].dat.size()];
+ REQUIRE(memcmp(dat.data(), frames[i].dat.data(), dat.size()) == 0);
+ }
+}
+
+TEST_CASE("send/recv CAN 2.0 packets") {
+ auto bus_offset = GENERATE(0, 4);
+ auto can_list_size = GENERATE(1, 3, 5, 10, 30, 60, 100, 200);
+ PandaTest test(bus_offset, can_list_size, cereal::PandaState::PandaType::DOS);
+
+ SECTION("can_send") {
+ test.test_can_send();
+ }
+ SECTION("can_receive") {
+ test.test_can_recv();
+ }
+}
+
+TEST_CASE("send/recv CAN FD packets") {
+ auto bus_offset = GENERATE(0, 4);
+ auto can_list_size = GENERATE(1, 3, 5, 10, 30, 60, 100, 200);
+ PandaTest test(bus_offset, can_list_size, cereal::PandaState::PandaType::RED_PANDA);
+
+ SECTION("can_send") {
+ test.test_can_send();
+ }
+ SECTION("can_receive") {
+ test.test_can_recv();
+ }
+}
diff --git a/selfdrive/camerad/SConscript b/selfdrive/camerad/SConscript
index d018704d60..85bc756bc1 100644
--- a/selfdrive/camerad/SConscript
+++ b/selfdrive/camerad/SConscript
@@ -1,4 +1,4 @@
-Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc', 'USE_WEBCAM', 'USE_FRAME_STREAM')
+Import('env', 'arch', 'cereal', 'messaging', 'common', 'gpucommon', 'visionipc', 'USE_WEBCAM')
libs = ['m', 'pthread', common, 'jpeg', 'OpenCL', 'yuv', cereal, messaging, 'zmq', 'capnp', 'kj', visionipc, gpucommon]
@@ -18,15 +18,12 @@ else:
env.Append(CFLAGS = '-DWEBCAM')
env.Append(CPPPATH = ['/usr/include/opencv4', '/usr/local/include/opencv4'])
else:
- if USE_FRAME_STREAM:
- cameras = ['cameras/camera_frame_stream.cc']
- else:
- libs += ['avutil', 'avcodec', 'avformat', 'swscale', 'bz2', 'ssl', 'curl', 'crypto']
- # TODO: import replay_lib from root SConstruct
- cameras = ['cameras/camera_replay.cc',
- env.Object('camera-util', '#/selfdrive/ui/replay/util.cc'),
- env.Object('camera-framereader', '#/selfdrive/ui/replay/framereader.cc'),
- env.Object('camera-filereader', '#/selfdrive/ui/replay/filereader.cc')]
+ libs += ['avutil', 'avcodec', 'avformat', 'bz2', 'ssl', 'curl', 'crypto']
+ # TODO: import replay_lib from root SConstruct
+ cameras = ['cameras/camera_replay.cc',
+ env.Object('camera-util', '#/selfdrive/ui/replay/util.cc'),
+ env.Object('camera-framereader', '#/selfdrive/ui/replay/framereader.cc'),
+ env.Object('camera-filereader', '#/selfdrive/ui/replay/filereader.cc')]
if arch == "Darwin":
del libs[libs.index('OpenCL')]
diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc
index 4db6eaa121..b901dc39d3 100644
--- a/selfdrive/camerad/cameras/camera_common.cc
+++ b/selfdrive/camerad/cameras/camera_common.cc
@@ -28,8 +28,6 @@
#include "selfdrive/camerad/cameras/camera_replay.h"
#endif
-const int YUV_COUNT = 100;
-
class Debayer {
public:
Debayer(cl_device_id device_id, cl_context context, const CameraBuf *b, const CameraState *s) {
@@ -109,7 +107,7 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s,
vipc_server->create_buffers(rgb_type, UI_BUF_COUNT, true, rgb_width, rgb_height);
rgb_stride = vipc_server->get_buffer(rgb_type)->stride;
- vipc_server->create_buffers(yuv_type, YUV_COUNT, false, rgb_width, rgb_height);
+ vipc_server->create_buffers(yuv_type, YUV_BUFFER_COUNT, false, rgb_width, rgb_height);
if (ci->bayer) {
debayer = new Debayer(device_id, context, this, s);
diff --git a/selfdrive/camerad/cameras/camera_common.h b/selfdrive/camerad/cameras/camera_common.h
index e994fc2208..f55bbd8401 100644
--- a/selfdrive/camerad/cameras/camera_common.h
+++ b/selfdrive/camerad/cameras/camera_common.h
@@ -14,6 +14,7 @@
#include "selfdrive/common/queue.h"
#include "selfdrive/common/swaglog.h"
#include "selfdrive/common/visionimg.h"
+#include "selfdrive/hardware/hw.h"
#define CAMERA_ID_IMX298 0
#define CAMERA_ID_IMX179 1
@@ -26,7 +27,9 @@
#define CAMERA_ID_AR0231 8
#define CAMERA_ID_MAX 9
-#define UI_BUF_COUNT 4
+const int UI_BUF_COUNT = 4;
+const int YUV_BUFFER_COUNT = Hardware::EON() ? 100 : 40;
+
enum CameraType {
RoadCam = 0,
diff --git a/selfdrive/camerad/cameras/camera_frame_stream.cc b/selfdrive/camerad/cameras/camera_frame_stream.cc
deleted file mode 100644
index 8e6c69e361..0000000000
--- a/selfdrive/camerad/cameras/camera_frame_stream.cc
+++ /dev/null
@@ -1,92 +0,0 @@
-#include "selfdrive/camerad/cameras/camera_frame_stream.h"
-
-#include
-#include
-
-#include
-
-#include "cereal/messaging/messaging.h"
-#include "selfdrive/common/util.h"
-
-#define FRAME_WIDTH 1164
-#define FRAME_HEIGHT 874
-
-extern ExitHandler do_exit;
-
-namespace {
-
-// TODO: make this more generic
-CameraInfo cameras_supported[CAMERA_ID_MAX] = {
- [CAMERA_ID_IMX298] = {
- .frame_width = FRAME_WIDTH,
- .frame_height = FRAME_HEIGHT,
- .frame_stride = FRAME_WIDTH*3,
- .bayer = false,
- .bayer_flip = false,
- },
- [CAMERA_ID_OV8865] = {
- .frame_width = 1632,
- .frame_height = 1224,
- .frame_stride = 2040, // seems right
- .bayer = false,
- .bayer_flip = 3,
- .hdr = false
- },
-};
-
-void camera_init(VisionIpcServer * v, CameraState *s, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type) {
- assert(camera_id < std::size(cameras_supported));
- s->ci = cameras_supported[camera_id];
- assert(s->ci.frame_width != 0);
-
- s->camera_num = camera_id;
- s->fps = fps;
- s->buf.init(device_id, ctx, s, v, FRAME_BUF_COUNT, rgb_type, yuv_type);
-}
-
-void run_frame_stream(CameraState &camera, const char* frame_pkt) {
- SubMaster sm({frame_pkt});
-
- size_t buf_idx = 0;
- while (!do_exit) {
- sm.update(1000);
- if(sm.updated(frame_pkt)) {
- auto msg = static_cast(sm[frame_pkt]);
- auto frame = msg.get(frame_pkt).as();
- camera.buf.camera_bufs_metadata[buf_idx] = {
- .frame_id = frame.get("frameId").as(),
- .timestamp_eof = frame.get("timestampEof").as(),
- .timestamp_sof = frame.get("timestampSof").as(),
- };
-
- cl_command_queue q = camera.buf.camera_bufs[buf_idx].copy_q;
- cl_mem yuv_cl = camera.buf.camera_bufs[buf_idx].buf_cl;
-
- auto image = frame.get("image").as();
- clEnqueueWriteBuffer(q, yuv_cl, CL_TRUE, 0, image.size(), image.begin(), 0, NULL, NULL);
- camera.buf.queue(buf_idx);
- buf_idx = (buf_idx + 1) % FRAME_BUF_COUNT;
- }
- }
-}
-
-} // namespace
-
-void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
- camera_init(v, &s->road_cam, CAMERA_ID_IMX298, 20, device_id, ctx,
- VISION_STREAM_RGB_BACK, VISION_STREAM_YUV_BACK);
- camera_init(v, &s->driver_cam, CAMERA_ID_OV8865, 10, device_id, ctx,
- VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT);
-}
-
-void cameras_open(MultiCameraState *s) {}
-void cameras_close(MultiCameraState *s) {}
-void camera_autoexposure(CameraState *s, float grey_frac) {}
-void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {}
-
-void cameras_run(MultiCameraState *s) {
- std::thread t = start_process_thread(s, &s->road_cam, process_road_camera);
- set_thread_name("frame_streaming");
- run_frame_stream(s->road_cam, "roadCameraState");
- t.join();
-}
diff --git a/selfdrive/camerad/cameras/camera_frame_stream.h b/selfdrive/camerad/cameras/camera_frame_stream.h
deleted file mode 100644
index 0d4d8cfb42..0000000000
--- a/selfdrive/camerad/cameras/camera_frame_stream.h
+++ /dev/null
@@ -1,30 +0,0 @@
-#pragma once
-
-#define CL_USE_DEPRECATED_OPENCL_1_2_APIS
-#ifdef __APPLE__
-#include
-#else
-#include
-#endif
-
-#include "camera_common.h"
-
-#define FRAME_BUF_COUNT 16
-
-typedef struct CameraState {
- int camera_num;
- CameraInfo ci;
-
- int fps;
- float digital_gain;
-
- CameraBuf buf;
-} CameraState;
-
-typedef struct MultiCameraState {
- CameraState road_cam;
- CameraState driver_cam;
-
- SubMaster *sm;
- PubMaster *pm;
-} MultiCameraState;
diff --git a/selfdrive/camerad/cameras/camera_qcom.cc b/selfdrive/camerad/cameras/camera_qcom.cc
index 33b5d00c95..11383de668 100644
--- a/selfdrive/camerad/cameras/camera_qcom.cc
+++ b/selfdrive/camerad/cameras/camera_qcom.cc
@@ -211,12 +211,12 @@ void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_i
/*fps*/ 20,
#endif
device_id, ctx,
- VISION_STREAM_RGB_BACK, VISION_STREAM_YUV_BACK);
+ VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD);
camera_init(v, &s->driver_cam, CAMERA_ID_OV8865, 1,
/*pixel_clock=*/72000000, /*line_length_pclk=*/1602,
/*max_gain=*/510, 10, device_id, ctx,
- VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT);
+ VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER);
s->sm = new SubMaster({"driverState"});
s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"});
diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc
index 03138e353a..3ea065ae73 100644
--- a/selfdrive/camerad/cameras/camera_qcom2.cc
+++ b/selfdrive/camerad/cameras/camera_qcom2.cc
@@ -709,13 +709,13 @@ static void camera_open(CameraState *s) {
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
camera_init(s, v, &s->road_cam, CAMERA_ID_AR0231, 1, 20, device_id, ctx,
- VISION_STREAM_RGB_BACK, VISION_STREAM_YUV_BACK); // swap left/right
+ VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD); // swap left/right
printf("road camera initted \n");
camera_init(s, v, &s->wide_road_cam, CAMERA_ID_AR0231, 0, 20, device_id, ctx,
- VISION_STREAM_RGB_WIDE, VISION_STREAM_YUV_WIDE);
+ VISION_STREAM_RGB_WIDE, VISION_STREAM_WIDE_ROAD);
printf("wide road camera initted \n");
camera_init(s, v, &s->driver_cam, CAMERA_ID_AR0231, 2, 20, device_id, ctx,
- VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT);
+ VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER);
printf("driver camera initted \n");
s->sm = new SubMaster({"driverState"});
diff --git a/selfdrive/camerad/cameras/camera_replay.cc b/selfdrive/camerad/cameras/camera_replay.cc
index 6bc53d8c94..8ff1b6aacc 100644
--- a/selfdrive/camerad/cameras/camera_replay.cc
+++ b/selfdrive/camerad/cameras/camera_replay.cc
@@ -23,7 +23,7 @@ std::string get_url(std::string route_name, const std::string &camera, int segme
}
void camera_init(VisionIpcServer *v, CameraState *s, int camera_id, unsigned int fps, cl_device_id device_id, cl_context ctx, VisionStreamType rgb_type, VisionStreamType yuv_type, const std::string &url) {
- s->frame = new FrameReader(true);
+ s->frame = new FrameReader();
if (!s->frame->load(url)) {
printf("failed to load stream from %s", url.c_str());
assert(0);
@@ -98,9 +98,9 @@ void process_road_camera(MultiCameraState *s, CameraState *c, int cnt) {
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
camera_init(v, &s->road_cam, CAMERA_ID_LGC920, 20, device_id, ctx,
- VISION_STREAM_RGB_BACK, VISION_STREAM_YUV_BACK, get_url(road_camera_route, "fcamera", 0));
+ VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD, get_url(road_camera_route, "fcamera", 0));
// camera_init(v, &s->driver_cam, CAMERA_ID_LGC615, 10, device_id, ctx,
- // VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT, get_url(driver_camera_route, "dcamera", 0));
+ // VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER, get_url(driver_camera_route, "dcamera", 0));
s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"});
}
diff --git a/selfdrive/camerad/cameras/camera_webcam.cc b/selfdrive/camerad/cameras/camera_webcam.cc
index ff63257bd3..47da56042c 100644
--- a/selfdrive/camerad/cameras/camera_webcam.cc
+++ b/selfdrive/camerad/cameras/camera_webcam.cc
@@ -141,9 +141,9 @@ void driver_camera_thread(CameraState *s) {
void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_id, cl_context ctx) {
camera_init(v, &s->road_cam, CAMERA_ID_LGC920, 20, device_id, ctx,
- VISION_STREAM_RGB_BACK, VISION_STREAM_YUV_BACK);
+ VISION_STREAM_RGB_BACK, VISION_STREAM_ROAD);
camera_init(v, &s->driver_cam, CAMERA_ID_LGC615, 10, device_id, ctx,
- VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT);
+ VISION_STREAM_RGB_FRONT, VISION_STREAM_DRIVER);
s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"});
}
diff --git a/selfdrive/camerad/test/get_thumbnails_for_segment.py b/selfdrive/camerad/test/get_thumbnails_for_segment.py
index 2ffb4bf650..ab9a7b308d 100755
--- a/selfdrive/camerad/test/get_thumbnails_for_segment.py
+++ b/selfdrive/camerad/test/get_thumbnails_for_segment.py
@@ -20,9 +20,13 @@ if __name__ == "__main__":
mkdirs_exists_ok(out_path)
r = Route(args.route)
- lr = list(LogReader(r.qlog_paths()[args.segment]))
+ path = r.log_paths()[args.segment] or r.qlog_paths()[args.segment]
+ lr = list(LogReader(path))
for msg in tqdm(lr):
if msg.which() == 'thumbnail':
with open(os.path.join(out_path, f"{msg.thumbnail.frameId}.jpg"), 'wb') as f:
f.write(msg.thumbnail.thumbnail)
+ elif msg.which() == 'navThumbnail':
+ with open(os.path.join(out_path, f"nav_{msg.navThumbnail.frameId}.jpg"), 'wb') as f:
+ f.write(msg.navThumbnail.thumbnail)
diff --git a/selfdrive/car/car_helpers.py b/selfdrive/car/car_helpers.py
index 980b207fa7..786b00fa2b 100644
--- a/selfdrive/car/car_helpers.py
+++ b/selfdrive/car/car_helpers.py
@@ -1,7 +1,7 @@
import os
from common.params import Params
from common.basedir import BASEDIR
-from selfdrive.version import comma_remote, tested_branch
+from selfdrive.version import get_comma_remote, get_tested_branch
from selfdrive.car.fingerprints import eliminate_incompatible_cars, all_legacy_fingerprint_cars
from selfdrive.car.vin import get_vin, VIN_UNKNOWN
from selfdrive.car.fw_versions import get_fw_versions, match_fw_to_car
@@ -14,7 +14,7 @@ EventName = car.CarEvent.EventName
def get_startup_event(car_recognized, controller_available, fw_seen):
- if comma_remote and tested_branch:
+ if get_comma_remote() and get_tested_branch():
event = EventName.startup
else:
event = EventName.startupMaster
diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py
index ba85a89c3e..ab3125b011 100644
--- a/selfdrive/car/chrysler/carstate.py
+++ b/selfdrive/car/chrysler/carstate.py
@@ -31,10 +31,13 @@ class CarState(CarStateBase):
ret.espDisabled = (cp.vl["TRACTION_BUTTON"]["TRACTION_OFF"] == 1)
- ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"]
- ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"]
- ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"]
- ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"]
+ ret.wheelSpeeds = self.get_wheel_speeds(
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"],
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"],
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"],
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"],
+ unit=1,
+ )
ret.vEgoRaw = (cp.vl["SPEED_1"]["SPEED_LEFT"] + cp.vl["SPEED_1"]["SPEED_RIGHT"]) / 2.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = not ret.vEgoRaw > 0.001
diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py
index 100d472409..a621ab16ab 100644
--- a/selfdrive/car/ford/carstate.py
+++ b/selfdrive/car/ford/carstate.py
@@ -10,10 +10,14 @@ WHEEL_RADIUS = 0.33
class CarState(CarStateBase):
def update(self, cp):
ret = car.CarState.new_message()
- ret.wheelSpeeds.rr = cp.vl["WheelSpeed_CG1"]["WhlRr_W_Meas"] * WHEEL_RADIUS
- ret.wheelSpeeds.rl = cp.vl["WheelSpeed_CG1"]["WhlRl_W_Meas"] * WHEEL_RADIUS
- ret.wheelSpeeds.fr = cp.vl["WheelSpeed_CG1"]["WhlFr_W_Meas"] * WHEEL_RADIUS
- ret.wheelSpeeds.fl = cp.vl["WheelSpeed_CG1"]["WhlFl_W_Meas"] * WHEEL_RADIUS
+
+ ret.wheelSpeeds = self.get_wheel_speeds(
+ cp.vl["WheelSpeed_CG1"]["WhlFl_W_Meas"],
+ cp.vl["WheelSpeed_CG1"]["WhlFr_W_Meas"],
+ cp.vl["WheelSpeed_CG1"]["WhlRl_W_Meas"],
+ cp.vl["WheelSpeed_CG1"]["WhlRr_W_Meas"],
+ unit=WHEEL_RADIUS,
+ )
ret.vEgoRaw = mean([ret.wheelSpeeds.rr, ret.wheelSpeeds.rl, ret.wheelSpeeds.fr, ret.wheelSpeeds.fl])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = not ret.vEgoRaw > 0.001
diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py
index 00f25c5fdb..e4d6448348 100644
--- a/selfdrive/car/gm/carstate.py
+++ b/selfdrive/car/gm/carstate.py
@@ -1,6 +1,5 @@
from cereal import car
from common.numpy_fast import mean
-from selfdrive.config import Conversions as CV
from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
from selfdrive.car.interfaces import CarStateBase
@@ -21,10 +20,12 @@ class CarState(CarStateBase):
self.prev_cruise_buttons = self.cruise_buttons
self.cruise_buttons = pt_cp.vl["ASCMSteeringButton"]["ACCButtons"]
- ret.wheelSpeeds.fl = pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"] * CV.KPH_TO_MS
- ret.wheelSpeeds.fr = pt_cp.vl["EBCMWheelSpdFront"]["FRWheelSpd"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rl = pt_cp.vl["EBCMWheelSpdRear"]["RLWheelSpd"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rr = pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"] * CV.KPH_TO_MS
+ ret.wheelSpeeds = self.get_wheel_speeds(
+ pt_cp.vl["EBCMWheelSpdFront"]["FLWheelSpd"],
+ pt_cp.vl["EBCMWheelSpdFront"]["FRWheelSpd"],
+ pt_cp.vl["EBCMWheelSpdRear"]["RLWheelSpd"],
+ pt_cp.vl["EBCMWheelSpdRear"]["RRWheelSpd"],
+ )
ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.01
diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py
index 8bf3ac58f6..65a9d8fd44 100644
--- a/selfdrive/car/honda/carstate.py
+++ b/selfdrive/car/honda/carstate.py
@@ -5,7 +5,7 @@ from opendbc.can.can_define import CANDefine
from opendbc.can.parser import CANParser
from selfdrive.config import Conversions as CV
from selfdrive.car.interfaces import CarStateBase
-from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, SPEED_FACTOR, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL
+from selfdrive.car.honda.values import CAR, DBC, STEER_THRESHOLD, HONDA_BOSCH, HONDA_NIDEC_ALT_SCM_MESSAGES, HONDA_BOSCH_ALT_BRAKE_SIGNAL
TransmissionType = car.CarParams.TransmissionType
@@ -107,7 +107,7 @@ def get_can_signals(CP, gearbox_msg, main_on_sig_msg):
else:
checks += [("CRUISE_PARAMS", 50)]
- if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E):
+ if CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_2021, CAR.ACCORDH, CAR.ACCORDH_2021, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E):
signals += [("DRIVERS_DOOR_OPEN", "SCM_FEEDBACK", 1)]
elif CP.carFingerprint == CAR.ODYSSEY_CHN:
signals += [("DRIVERS_DOOR_OPEN", "SCM_BUTTONS", 1)]
@@ -185,7 +185,7 @@ class CarState(CarStateBase):
# ******************* parse out can *******************
# TODO: find wheels moving bit in dbc
- if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E):
+ if self.CP.carFingerprint in (CAR.ACCORD, CAR.ACCORD_2021, CAR.ACCORDH, CAR.ACCORDH_2021, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E):
ret.standstill = cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] < 0.1
ret.doorOpen = bool(cp.vl["SCM_FEEDBACK"]["DRIVERS_DOOR_OPEN"])
elif self.CP.carFingerprint == CAR.ODYSSEY_CHN:
@@ -213,16 +213,17 @@ class CarState(CarStateBase):
self.brake_error = cp.vl["STANDSTILL"]["BRAKE_ERROR_1"] or cp.vl["STANDSTILL"]["BRAKE_ERROR_2"]
ret.espDisabled = cp.vl["VSA_STATUS"]["ESP_DISABLED"] != 0
- speed_factor = SPEED_FACTOR.get(self.CP.carFingerprint, 1.)
- ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"] * CV.KPH_TO_MS * speed_factor
- ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"] * CV.KPH_TO_MS * speed_factor
- ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"] * CV.KPH_TO_MS * speed_factor
- ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"] * CV.KPH_TO_MS * speed_factor
- v_wheel = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr)/4.
+ ret.wheelSpeeds = self.get_wheel_speeds(
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"],
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"],
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"],
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"],
+ )
+ v_wheel = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.0
# blend in transmission speed at low speed, since it has more low speed accuracy
v_weight = interp(v_wheel, v_weight_bp, v_weight_v)
- ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] * CV.KPH_TO_MS * speed_factor + v_weight * v_wheel
+ ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]["XMISSION_SPEED"] * CV.KPH_TO_MS * self.CP.wheelSpeedFactor + v_weight * v_wheel
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]["STEER_ANGLE"]
@@ -235,7 +236,7 @@ class CarState(CarStateBase):
250, cp.vl["SCM_FEEDBACK"]["LEFT_BLINKER"], cp.vl["SCM_FEEDBACK"]["RIGHT_BLINKER"])
ret.brakeHoldActive = cp.vl["VSA_STATUS"]["BRAKE_HOLD_ACTIVE"] == 1
- if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH,
+ if self.CP.carFingerprint in (CAR.CIVIC, CAR.ODYSSEY, CAR.ODYSSEY_CHN, CAR.CRV_5G, CAR.ACCORD, CAR.ACCORD_2021, CAR.ACCORDH, CAR.ACCORDH_2021, CAR.CIVIC_BOSCH,
CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E):
self.park_brake = cp.vl["EPB_STATUS"]["EPB_STATE"] != 0
else:
diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py
index 802ce980ab..5a5aea9f92 100755
--- a/selfdrive/car/honda/interface.py
+++ b/selfdrive/car/honda/interface.py
@@ -110,7 +110,7 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 1.
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
- elif candidate in (CAR.ACCORD, CAR.ACCORDH):
+ elif candidate in (CAR.ACCORD, CAR.ACCORD_2021, CAR.ACCORDH, CAR.ACCORDH_2021):
stop_and_go = True
ret.mass = 3279. * CV.LB_TO_KG + STD_CARGO_KG
ret.wheelbase = 2.83
@@ -143,6 +143,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 1000], [0, 1000]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.444
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.8], [0.24]]
+ ret.wheelSpeedFactor = 1.025
elif candidate == CAR.CRV_5G:
stop_and_go = True
@@ -160,6 +161,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 3840], [0, 3840]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.64], [0.192]]
tire_stiffness_factor = 0.677
+ ret.wheelSpeedFactor = 1.025
elif candidate == CAR.CRV_HYBRID:
stop_and_go = True
@@ -170,6 +172,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]] # TODO: determine if there is a dead zone at the top end
tire_stiffness_factor = 0.677
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.6], [0.18]]
+ ret.wheelSpeedFactor = 1.025
elif candidate == CAR.FIT:
stop_and_go = False
@@ -201,6 +204,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralParams.torqueBP, ret.lateralParams.torqueV = [[0, 4096], [0, 4096]]
tire_stiffness_factor = 0.5
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.16], [0.025]]
+ ret.wheelSpeedFactor = 1.025
elif candidate == CAR.ACURA_RDX:
stop_and_go = False
diff --git a/selfdrive/car/honda/values.py b/selfdrive/car/honda/values.py
index 97e0ded663..340509c563 100644
--- a/selfdrive/car/honda/values.py
+++ b/selfdrive/car/honda/values.py
@@ -59,6 +59,8 @@ VISUAL_HUD = {
class CAR:
ACCORD = "HONDA ACCORD 2018"
ACCORDH = "HONDA ACCORD HYBRID 2018"
+ ACCORD_2021 = "HONDA ACCORD 2021"
+ ACCORDH_2021 = "HONDA ACCORD HYBRID 2021"
CIVIC = "HONDA CIVIC 2016"
CIVIC_BOSCH = "HONDA CIVIC (BOSCH) 2019"
CIVIC_BOSCH_DIESEL = "HONDA CIVIC SEDAN 1.6 DIESEL 2019"
@@ -268,12 +270,10 @@ FW_VERSIONS = {
],
(Ecu.fwdCamera, 0x18dab5f1, None): [
b'36161-TWA-A070\x00\x00',
- b'36161-TWA-A330\x00\x00',
],
(Ecu.fwdRadar, 0x18dab0f1, None): [
b'36802-TWA-A080\x00\x00',
b'36802-TWA-A070\x00\x00',
- b'36802-TWA-A330\x00\x00',
],
(Ecu.eps, 0x18da30f1, None): [
b'39990-TVA-A160\x00\x00',
@@ -281,6 +281,78 @@ FW_VERSIONS = {
b'39990-TVA-A340\x00\x00',
],
},
+ CAR.ACCORD_2021: {
+ (Ecu.programmedFuelInjection, 0x18da10f1, None): [
+ b'37805-6B2-C520\x00\x00',
+ ],
+ (Ecu.transmission, 0x18da1ef1, None): [
+ b'28102-6B8-A700\x00\x00',
+ ],
+ (Ecu.electricBrakeBooster, 0x18da2bf1, None): [
+ b'46114-TVA-A320\x00\x00',
+ ],
+ (Ecu.gateway, 0x18daeff1, None): [
+ b'38897-TVA-A020\x00\x00',
+ ],
+ (Ecu.vsa, 0x18da28f1, None): [
+ b'57114-TVA-E520\x00\x00',
+ ],
+ (Ecu.srs, 0x18da53f1, None): [
+ b'77959-TVA-L420\x00\x00',
+ ],
+ (Ecu.combinationMeter, 0x18da60f1, None): [
+ b'78109-TVC-A230\x00\x00',
+ ],
+ (Ecu.hud, 0x18da61f1, None): [
+ b'78209-TVA-A110\x00\x00',
+ ],
+ (Ecu.shiftByWire, 0x18da0bf1, None): [
+ b'54008-TVC-A910\x00\x00',
+ ],
+ (Ecu.fwdCamera, 0x18dab5f1, None): [
+ b'36161-TVC-A330\x00\x00',
+ ],
+ (Ecu.fwdRadar, 0x18dab0f1, None): [
+ b'36802-TVC-A330\x00\x00',
+ ],
+ (Ecu.eps, 0x18da30f1, None): [
+ b'39990-TVA-A340\x00\x00',
+ ],
+ (Ecu.unknown, 0x18da3af1, None): [
+ b'39390-TVA-A120\x00\x00',
+ ],
+ },
+ CAR.ACCORDH_2021: {
+ (Ecu.gateway, 0x18daeff1, None): [
+ b'38897-TWD-J020\x00\x00',
+ ],
+ (Ecu.vsa, 0x18da28f1, None): [
+ b'57114-TWA-A530\x00\x00',
+ b'57114-TWA-B520\x00\x00',
+ ],
+ (Ecu.srs, 0x18da53f1, None): [
+ b'77959-TWA-L420\x00\x00',
+ ],
+ (Ecu.combinationMeter, 0x18da60f1, None): [
+ b'78109-TWA-A030\x00\x00',
+ b'78109-TWA-A230\x00\x00',
+ ],
+ (Ecu.shiftByWire, 0x18da0bf1, None): [
+ b'54008-TWA-A910\x00\x00',
+ ],
+ (Ecu.fwdCamera, 0x18dab5f1, None): [
+ b'36161-TWA-A330\x00\x00',
+ ],
+ (Ecu.fwdRadar, 0x18dab0f1, None): [
+ b'36802-TWA-A330\x00\x00',
+ ],
+ (Ecu.eps, 0x18da30f1, None): [
+ b'39990-TVA-A340\x00\x00',
+ ],
+ (Ecu.unknown, 0x18da3af1, None): [
+ b'39390-TVA-A120\x00\x00',
+ ],
+ },
CAR.CIVIC: {
(Ecu.programmedFuelInjection, 0x18da10f1, None): [
b'37805-5AA-A640\x00\x00',
@@ -1324,7 +1396,9 @@ FW_VERSIONS = {
DBC = {
CAR.ACCORD: dbc_dict('honda_accord_2018_can_generated', None),
+ CAR.ACCORD_2021: dbc_dict('honda_accord_2018_can_generated', None),
CAR.ACCORDH: dbc_dict('honda_accord_2018_can_generated', None),
+ CAR.ACCORDH_2021: dbc_dict('honda_accord_2018_can_generated', None),
CAR.ACURA_ILX: dbc_dict('acura_ilx_2016_can_generated', 'acura_ilx_2016_nidec'),
CAR.ACURA_RDX: dbc_dict('acura_rdx_2018_can_generated', 'acura_ilx_2016_nidec'),
CAR.ACURA_RDX_3G: dbc_dict('acura_rdx_2020_can_generated', None),
@@ -1354,18 +1428,9 @@ STEER_THRESHOLD = {
CAR.CRV_EU: 400,
}
-# TODO: is this real?
-SPEED_FACTOR = {
- # default is 1, overrides go here
- CAR.CRV: 1.025,
- CAR.CRV_5G: 1.025,
- CAR.CRV_EU: 1.025,
- CAR.CRV_HYBRID: 1.025,
- CAR.HRV: 1.025,
-}
-
HONDA_NIDEC_ALT_PCM_ACCEL = set([CAR.ODYSSEY])
HONDA_NIDEC_ALT_SCM_MESSAGES = set([CAR.ACURA_ILX, CAR.ACURA_RDX, CAR.CRV, CAR.CRV_EU, CAR.FIT, CAR.FREED, CAR.HRV, CAR.ODYSSEY_CHN,
CAR.PILOT, CAR.PILOT_2019, CAR.PASSPORT, CAR.RIDGELINE])
-HONDA_BOSCH = set([CAR.ACCORD, CAR.ACCORDH, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G, CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E])
-HONDA_BOSCH_ALT_BRAKE_SIGNAL = set([CAR.ACCORD, CAR.CRV_5G, CAR.ACURA_RDX_3G])
+HONDA_BOSCH = set([CAR.ACCORD, CAR.ACCORD_2021, CAR.ACCORDH, CAR.ACCORDH_2021, CAR.CIVIC_BOSCH, CAR.CIVIC_BOSCH_DIESEL, CAR.CRV_5G,
+ CAR.CRV_HYBRID, CAR.INSIGHT, CAR.ACURA_RDX_3G, CAR.HONDA_E])
+HONDA_BOSCH_ALT_BRAKE_SIGNAL = set([CAR.ACCORD, CAR.ACCORD_2021, CAR.CRV_5G, CAR.ACURA_RDX_3G])
diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py
index 4db775bea5..e889f24fc9 100644
--- a/selfdrive/car/hyundai/carstate.py
+++ b/selfdrive/car/hyundai/carstate.py
@@ -28,10 +28,12 @@ class CarState(CarStateBase):
ret.seatbeltUnlatched = cp.vl["CGW1"]["CF_Gway_DrvSeatBeltSw"] == 0
- ret.wheelSpeeds.fl = cp.vl["WHL_SPD11"]["WHL_SPD_FL"] * CV.KPH_TO_MS
- ret.wheelSpeeds.fr = cp.vl["WHL_SPD11"]["WHL_SPD_FR"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rl = cp.vl["WHL_SPD11"]["WHL_SPD_RL"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rr = cp.vl["WHL_SPD11"]["WHL_SPD_RR"] * CV.KPH_TO_MS
+ ret.wheelSpeeds = self.get_wheel_speeds(
+ cp.vl["WHL_SPD11"]["WHL_SPD_FL"],
+ cp.vl["WHL_SPD11"]["WHL_SPD_FR"],
+ cp.vl["WHL_SPD11"]["WHL_SPD_RL"],
+ cp.vl["WHL_SPD11"]["WHL_SPD_RR"],
+ )
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
diff --git a/selfdrive/car/interfaces.py b/selfdrive/car/interfaces.py
index 336c1ee6c2..487fcab8c7 100644
--- a/selfdrive/car/interfaces.py
+++ b/selfdrive/car/interfaces.py
@@ -78,6 +78,7 @@ class CarInterfaceBase():
ret.steerMaxBP = [0.]
ret.steerMaxV = [1.]
ret.minSteerSpeed = 0.
+ ret.wheelSpeedFactor = 1.0
ret.pcmCruise = True # openpilot's state is tied to the PCM's cruise state on most cars
ret.minEnableSpeed = -1. # enable is done by stock ACC, so ignore this
@@ -210,6 +211,16 @@ class CarStateBase:
v_ego_x = self.v_ego_kf.update(v_ego_raw)
return float(v_ego_x[0]), float(v_ego_x[1])
+ def get_wheel_speeds(self, fl, fr, rl, rr, unit=CV.KPH_TO_MS):
+ factor = unit * self.CP.wheelSpeedFactor
+
+ wheelSpeeds = car.CarState.WheelSpeeds.new_message()
+ wheelSpeeds.fl = fl * factor
+ wheelSpeeds.fr = fr * factor
+ wheelSpeeds.rl = rl * factor
+ wheelSpeeds.rr = rr * factor
+ return wheelSpeeds
+
def update_blinker_from_lamp(self, blinker_time: int, left_blinker_lamp: bool, right_blinker_lamp: bool):
"""Update blinkers from lights. Enable output when light was seen within the last `blinker_time`
iterations"""
diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py
index 4140a4e079..7ebf60c96b 100644
--- a/selfdrive/car/mazda/carstate.py
+++ b/selfdrive/car/mazda/carstate.py
@@ -20,10 +20,12 @@ class CarState(CarStateBase):
def update(self, cp, cp_cam):
ret = car.CarState.new_message()
- ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]["FL"] * CV.KPH_TO_MS
- ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]["FR"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]["RL"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]["RR"] * CV.KPH_TO_MS
+ ret.wheelSpeeds = self.get_wheel_speeds(
+ cp.vl["WHEEL_SPEEDS"]["FL"],
+ cp.vl["WHEEL_SPEEDS"]["FR"],
+ cp.vl["WHEEL_SPEEDS"]["RL"],
+ cp.vl["WHEEL_SPEEDS"]["RR"],
+ )
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
diff --git a/selfdrive/car/mazda/values.py b/selfdrive/car/mazda/values.py
index ada3ea1ddb..58e1dd7033 100644
--- a/selfdrive/car/mazda/values.py
+++ b/selfdrive/car/mazda/values.py
@@ -21,7 +21,7 @@ class CAR:
CX9 = "MAZDA CX-9"
MAZDA3 = "MAZDA 3"
MAZDA6 = "MAZDA 6"
- CX9_2021 = "Mazda CX-9 2021" # No Steer Lockout
+ CX9_2021 = "MAZDA CX-9 2021" # No Steer Lockout
class LKAS_LIMITS:
STEER_THRESHOLD = 15
diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py
index a4019b538c..05191feedf 100644
--- a/selfdrive/car/nissan/carstate.py
+++ b/selfdrive/car/nissan/carstate.py
@@ -35,11 +35,12 @@ class CarState(CarStateBase):
elif self.CP.carFingerprint in [CAR.LEAF, CAR.LEAF_IC]:
ret.brakePressed = bool(cp.vl["CRUISE_THROTTLE"]["USER_BRAKE_PRESSED"])
- ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FL"] * CV.KPH_TO_MS
- ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FR"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RL"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RR"] * CV.KPH_TO_MS
-
+ ret.wheelSpeeds = self.get_wheel_speeds(
+ cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FL"],
+ cp.vl["WHEEL_SPEEDS_FRONT"]["WHEEL_SPEED_FR"],
+ cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RL"],
+ cp.vl["WHEEL_SPEEDS_REAR"]["WHEEL_SPEED_RR"],
+ )
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py
index e324037766..1f56f09ff7 100644
--- a/selfdrive/car/subaru/carstate.py
+++ b/selfdrive/car/subaru/carstate.py
@@ -23,10 +23,12 @@ class CarState(CarStateBase):
else:
ret.brakePressed = cp.vl["Brake_Status"]["Brake"] == 1
- ret.wheelSpeeds.fl = cp.vl["Wheel_Speeds"]["FL"] * CV.KPH_TO_MS
- ret.wheelSpeeds.fr = cp.vl["Wheel_Speeds"]["FR"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rl = cp.vl["Wheel_Speeds"]["RL"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rr = cp.vl["Wheel_Speeds"]["RR"] * CV.KPH_TO_MS
+ ret.wheelSpeeds = self.get_wheel_speeds(
+ cp.vl["Wheel_Speeds"]["FL"],
+ cp.vl["Wheel_Speeds"]["FR"],
+ cp.vl["Wheel_Speeds"]["RL"],
+ cp.vl["Wheel_Speeds"]["RR"],
+ )
ret.vEgoRaw = (ret.wheelSpeeds.fl + ret.wheelSpeeds.fr + ret.wheelSpeeds.rl + ret.wheelSpeeds.rr) / 4.
# Kalman filter, even though Subaru raw wheel speed is heaviliy filtered by default
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py
index 72cde0e537..c445528a83 100644
--- a/selfdrive/car/toyota/carcontroller.py
+++ b/selfdrive/car/toyota/carcontroller.py
@@ -5,35 +5,18 @@ from selfdrive.car.toyota.toyotacan import create_steer_command, create_ui_comma
create_accel_command, create_acc_cancel_command, \
create_fcw_command, create_lta_steer_command
from selfdrive.car.toyota.values import CAR, STATIC_DSU_MSGS, NO_STOP_TIMER_CAR, TSS2_CAR, \
- MIN_ACC_SPEED, PEDAL_HYST_GAP, PEDAL_SCALE, CarControllerParams
+ MIN_ACC_SPEED, PEDAL_TRANSITION, CarControllerParams
from opendbc.can.packer import CANPacker
VisualAlert = car.CarControl.HUDControl.VisualAlert
-def accel_hysteresis(accel, accel_steady, enabled):
-
- # for small accel oscillations within ACCEL_HYST_GAP, don't change the accel command
- if not enabled:
- # send 0 when disabled, otherwise acc faults
- accel_steady = 0.
- elif accel > accel_steady + CarControllerParams.ACCEL_HYST_GAP:
- accel_steady = accel - CarControllerParams.ACCEL_HYST_GAP
- elif accel < accel_steady - CarControllerParams.ACCEL_HYST_GAP:
- accel_steady = accel + CarControllerParams.ACCEL_HYST_GAP
- accel = accel_steady
-
- return accel, accel_steady
-
-
class CarController():
def __init__(self, dbc_name, CP, VM):
self.last_steer = 0
- self.accel_steady = 0.
self.alert_active = False
self.last_standstill = False
self.standstill_req = False
self.steer_rate_limited = False
- self.use_interceptor = False
self.packer = CANPacker(dbc_name)
@@ -43,25 +26,22 @@ class CarController():
# *** compute control surfaces ***
# gas and brake
- interceptor_gas_cmd = 0.
- pcm_accel_cmd = actuators.accel
-
- if CS.CP.enableGasInterceptor:
- # handle hysteresis when around the minimum acc speed
- if CS.out.vEgo < MIN_ACC_SPEED:
- self.use_interceptor = True
- elif CS.out.vEgo > MIN_ACC_SPEED + PEDAL_HYST_GAP:
- self.use_interceptor = False
-
- if self.use_interceptor and active:
- # only send negative accel when using interceptor. gas handles acceleration
- # +0.18 m/s^2 offset to reduce ABS pump usage when OP is engaged
- MAX_INTERCEPTOR_GAS = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED], [0.2, 0.5])
- interceptor_gas_cmd = clip(actuators.accel / PEDAL_SCALE, 0., MAX_INTERCEPTOR_GAS)
- pcm_accel_cmd = 0.18 - max(0, -actuators.accel)
-
- pcm_accel_cmd, self.accel_steady = accel_hysteresis(pcm_accel_cmd, self.accel_steady, enabled)
- pcm_accel_cmd = clip(pcm_accel_cmd, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
+ if CS.CP.enableGasInterceptor and enabled:
+ MAX_INTERCEPTOR_GAS = 0.5
+ # RAV4 has very sensitive has pedal
+ if CS.CP.carFingerprint in [CAR.RAV4, CAR.RAV4H, CAR.HIGHLANDER, CAR.HIGHLANDERH]:
+ PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.15, 0.3, 0.0])
+ elif CS.CP.carFingerprint in [CAR.COROLLA]:
+ PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.3, 0.4, 0.0])
+ else:
+ PEDAL_SCALE = interp(CS.out.vEgo, [0.0, MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_TRANSITION], [0.4, 0.5, 0.0])
+ # offset for creep and windbrake
+ pedal_offset = interp(CS.out.vEgo, [0.0, 2.3, MIN_ACC_SPEED + PEDAL_TRANSITION], [-.4, 0.0, 0.2])
+ pedal_command = PEDAL_SCALE * (actuators.accel + pedal_offset)
+ interceptor_gas_cmd = clip(pedal_command, 0., MAX_INTERCEPTOR_GAS)
+ else:
+ interceptor_gas_cmd = 0.
+ pcm_accel_cmd = clip(actuators.accel, CarControllerParams.ACCEL_MIN, CarControllerParams.ACCEL_MAX)
# steer torque
new_steer = int(round(actuators.steer * CarControllerParams.STEER_MAX))
@@ -88,7 +68,6 @@ class CarController():
self.standstill_req = False
self.last_steer = apply_steer
- self.last_accel = pcm_accel_cmd
self.last_standstill = CS.out.standstill
can_sends = []
@@ -113,7 +92,7 @@ class CarController():
lead = lead or CS.out.vEgo < 12. # at low speed we always assume the lead is present do ACC can be engaged
# Lexus IS uses a different cancellation message
- if pcm_cancel_cmd and CS.CP.carFingerprint == CAR.LEXUS_IS:
+ if pcm_cancel_cmd and CS.CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_RC]:
can_sends.append(create_acc_cancel_command(self.packer))
elif CS.CP.openpilotLongitudinalControl:
can_sends.append(create_accel_command(self.packer, pcm_accel_cmd, pcm_cancel_cmd, self.standstill_req, lead, CS.acc_type))
diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py
index cacd5b7bed..7ce5907b9b 100644
--- a/selfdrive/car/toyota/carstate.py
+++ b/selfdrive/car/toyota/carstate.py
@@ -41,10 +41,12 @@ class CarState(CarStateBase):
ret.gas = cp.vl["GAS_PEDAL"]["GAS_PEDAL"]
ret.gasPressed = cp.vl["PCM_CRUISE"]["GAS_RELEASED"] == 0
- ret.wheelSpeeds.fl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"] * CV.KPH_TO_MS
- ret.wheelSpeeds.fr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rl = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rr = cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"] * CV.KPH_TO_MS
+ ret.wheelSpeeds = self.get_wheel_speeds(
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FL"],
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_FR"],
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RL"],
+ cp.vl["WHEEL_SPEEDS"]["WHEEL_SPEED_RR"],
+ )
ret.vEgoRaw = mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr])
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
@@ -79,7 +81,7 @@ class CarState(CarStateBase):
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD
ret.steerWarning = cp.vl["EPS_STATUS"]["LKA_STATE"] not in [1, 5]
- if self.CP.carFingerprint == CAR.LEXUS_IS:
+ if self.CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_RC]:
ret.cruiseState.available = cp.vl["DSU_CRUISE"]["MAIN_ON"] != 0
ret.cruiseState.speed = cp.vl["DSU_CRUISE"]["SET_SPEED"] * CV.KPH_TO_MS
else:
@@ -93,7 +95,7 @@ class CarState(CarStateBase):
# these cars are identified by an ACC_TYPE value of 2.
# TODO: it is possible to avoid the lockout and gain stop and go if you
# send your own ACC_CONTROL msg on startup with ACC_TYPE set to 1
- if (self.CP.carFingerprint not in TSS2_CAR and self.CP.carFingerprint != CAR.LEXUS_IS) or \
+ if (self.CP.carFingerprint not in TSS2_CAR and self.CP.carFingerprint not in [CAR.LEXUS_IS, CAR.LEXUS_RC]) or \
(self.CP.carFingerprint in TSS2_CAR and self.acc_type == 1):
self.low_speed_lockout = cp.vl["PCM_CRUISE_2"]["LOW_SPEED_LOCKOUT"] == 2
@@ -168,7 +170,7 @@ class CarState(CarStateBase):
("STEER_TORQUE_SENSOR", 50),
]
- if CP.carFingerprint == CAR.LEXUS_IS:
+ if CP.carFingerprint in [CAR.LEXUS_IS, CAR.LEXUS_RC]:
signals.append(("MAIN_ON", "DSU_CRUISE", 0))
signals.append(("SET_SPEED", "DSU_CRUISE", 0))
checks.append(("DSU_CRUISE", 5))
diff --git a/selfdrive/car/toyota/interface.py b/selfdrive/car/toyota/interface.py
index a1221ae8a4..97a20f9df4 100755
--- a/selfdrive/car/toyota/interface.py
+++ b/selfdrive/car/toyota/interface.py
@@ -65,7 +65,6 @@ class CarInterface(CarInterfaceBase):
ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG
set_lat_tune(ret.lateralTuning, LatTunes.PID_B)
-
elif candidate == CAR.LEXUS_RXH:
stop_and_go = True
ret.wheelbase = 2.79
@@ -81,6 +80,7 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.5533 # not optimized yet
ret.mass = 4387. * CV.LB_TO_KG + STD_CARGO_KG
set_lat_tune(ret.lateralTuning, LatTunes.PID_D)
+ ret.wheelSpeedFactor = 1.035
elif candidate == CAR.LEXUS_RXH_TSS2:
stop_and_go = True
@@ -89,6 +89,7 @@ class CarInterface(CarInterfaceBase):
tire_stiffness_factor = 0.444 # not optimized yet
ret.mass = 4481.0 * CV.LB_TO_KG + STD_CARGO_KG # mean between min and max
set_lat_tune(ret.lateralTuning, LatTunes.PID_E)
+ ret.wheelSpeedFactor = 1.035
elif candidate in [CAR.CHR, CAR.CHRH]:
stop_and_go = True
@@ -186,6 +187,15 @@ class CarInterface(CarInterfaceBase):
ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG
set_lat_tune(ret.lateralTuning, LatTunes.PID_L)
+ elif candidate == CAR.LEXUS_RC:
+ ret.safetyConfigs[0].safetyParam = 77
+ stop_and_go = False
+ ret.wheelbase = 2.73050
+ ret.steerRatio = 13.3
+ tire_stiffness_factor = 0.444
+ ret.mass = 3736.8 * CV.LB_TO_KG + STD_CARGO_KG
+ set_lat_tune(ret.lateralTuning, LatTunes.PID_L)
+
elif candidate == CAR.LEXUS_CTH:
ret.safetyConfigs[0].safetyParam = 100
stop_and_go = True
@@ -206,7 +216,7 @@ class CarInterface(CarInterfaceBase):
elif candidate == CAR.PRIUS_TSS2:
stop_and_go = True
ret.wheelbase = 2.70002 # from toyota online sepc.
- ret.steerRatio = 13.4 # True steerRation from older prius
+ ret.steerRatio = 13.4 # True steerRatio from older prius
tire_stiffness_factor = 0.6371 # hand-tune
ret.mass = 3115. * CV.LB_TO_KG + STD_CARGO_KG
set_lat_tune(ret.lateralTuning, LatTunes.PID_N)
@@ -259,7 +269,8 @@ class CarInterface(CarInterfaceBase):
if ret.enableGasInterceptor:
set_long_tune(ret.longitudinalTuning, LongTunes.PEDAL)
- elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_NX_TSS2]:
+ elif candidate in [CAR.COROLLA_TSS2, CAR.COROLLAH_TSS2, CAR.RAV4_TSS2, CAR.RAV4H_TSS2, CAR.LEXUS_NX_TSS2,
+ CAR.HIGHLANDER_TSS2, CAR.HIGHLANDERH_TSS2]:
set_long_tune(ret.longitudinalTuning, LongTunes.TSS2)
ret.stoppingDecelRate = 0.3 # reach stopping target smoothly
ret.startingAccelRate = 6.0 # release brakes fast
diff --git a/selfdrive/car/toyota/tunes.py b/selfdrive/car/toyota/tunes.py
index 3f210d48a7..15c8bbfcc6 100644
--- a/selfdrive/car/toyota/tunes.py
+++ b/selfdrive/car/toyota/tunes.py
@@ -1,6 +1,5 @@
#!/usr/bin/env python3
from enum import Enum
-from selfdrive.car.toyota.values import MIN_ACC_SPEED, PEDAL_HYST_GAP
class LongTunes(Enum):
@@ -29,15 +28,8 @@ class LatTunes(Enum):
###### LONG ######
def set_long_tune(tune, name):
- if name == LongTunes.PEDAL:
- tune.deadzoneBP = [0.]
- tune.deadzoneV = [0.]
- tune.kpBP = [0., 5., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.]
- tune.kpV = [1.2, 0.8, 0.765, 2.255, 1.5]
- tune.kiBP = [0., MIN_ACC_SPEED, MIN_ACC_SPEED + PEDAL_HYST_GAP, 35.]
- tune.kiV = [0.18, 0.165, 0.489, 0.36]
# Improved longitudinal tune
- elif name == LongTunes.TSS2:
+ if name == LongTunes.TSS2 or name == LongTunes.PEDAL:
tune.deadzoneBP = [0., 8.05]
tune.deadzoneV = [.0, .14]
tune.kpBP = [0., 5., 20.]
diff --git a/selfdrive/car/toyota/values.py b/selfdrive/car/toyota/values.py
index 73198d925f..4e64a66749 100644
--- a/selfdrive/car/toyota/values.py
+++ b/selfdrive/car/toyota/values.py
@@ -6,9 +6,8 @@ from selfdrive.config import Conversions as CV
Ecu = car.CarParams.Ecu
MIN_ACC_SPEED = 19. * CV.MPH_TO_MS
+PEDAL_TRANSITION = 10. * CV.MPH_TO_MS
-PEDAL_HYST_GAP = 3. * CV.MPH_TO_MS
-PEDAL_SCALE = 3.0
class CarControllerParams:
ACCEL_HYST_GAP = 0.06 # don't change accel command for small oscilalitons within this value
@@ -58,6 +57,7 @@ class CAR:
LEXUS_NX = "LEXUS NX 2018"
LEXUS_NXH = "LEXUS NX HYBRID 2018"
LEXUS_NX_TSS2 = "LEXUS NX 2020"
+ LEXUS_RC = "LEXUS RC 2020"
LEXUS_RX = "LEXUS RX 2016"
LEXUS_RXH = "LEXUS RX HYBRID 2017"
LEXUS_RX_TSS2 = "LEXUS RX 2020"
@@ -442,6 +442,7 @@ FW_VERSIONS = {
b'\x0189663F438000\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
+ b'F152610012\x00\x00\x00\x00\x00\x00',
b'F152610013\x00\x00\x00\x00\x00\x00',
b'F152610014\x00\x00\x00\x00\x00\x00',
b'F152610040\x00\x00\x00\x00\x00\x00',
@@ -455,6 +456,7 @@ FW_VERSIONS = {
b'8821FF402400 ',
b'8821FF404000 ',
b'8821FF404100 ',
+ b'8821FF405000 ',
b'8821FF406000 ',
b'8821FF407100 ',
],
@@ -470,10 +472,12 @@ FW_VERSIONS = {
b'8821FF402400 ',
b'8821FF404000 ',
b'8821FF404100 ',
+ b'8821FF405000 ',
b'8821FF406000 ',
b'8821FF407100 ',
],
(Ecu.fwdCamera, 0x750, 0x6d): [
+ b'8646FF401700 ',
b'8646FF402100 ',
b'8646FF404000 ',
b'8646FF406000 ',
@@ -804,10 +808,10 @@ FW_VERSIONS = {
b'\x018966353M7100\x00\x00\x00\x00',
b'\x018966353Q2000\x00\x00\x00\x00',
b'\x018966353Q2300\x00\x00\x00\x00',
+ b'\x018966353Q4000\x00\x00\x00\x00',
b'\x018966353R1100\x00\x00\x00\x00',
b'\x018966353R7100\x00\x00\x00\x00',
b'\x018966353R8100\x00\x00\x00\x00',
- b'\x018966353Q4000\x00\x00\x00\x00',
],
(Ecu.engine, 0x7e0, None): [
b'\x0232480000\x00\x00\x00\x00\x00\x00\x00\x00A4701000\x00\x00\x00\x00\x00\x00\x00\x00',
@@ -1204,15 +1208,18 @@ FW_VERSIONS = {
b'\x018966333T5000\x00\x00\x00\x00',
b'\x018966333T5100\x00\x00\x00\x00',
b'\x018966333X6000\x00\x00\x00\x00',
+ b'\x01896633T07000\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
b'\x01F152606281\x00\x00\x00\x00\x00\x00',
b'\x01F152606340\x00\x00\x00\x00\x00\x00',
+ b'\x01F152606461\x00\x00\x00\x00\x00\x00',
b'\x01F15260E031\x00\x00\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'8965B33252\x00\x00\x00\x00\x00\x00',
b'8965B33590\x00\x00\x00\x00\x00\x00',
+ b'8965B33690\x00\x00\x00\x00\x00\x00',
b'8965B48271\x00\x00\x00\x00\x00\x00',
],
(Ecu.fwdRadar, 0x750, 0xf): [
@@ -1224,6 +1231,7 @@ FW_VERSIONS = {
b'\x028646F33030D0\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
b'\x028646F3303200\x00\x00\x00\x008646G26011A0\x00\x00\x00\x00',
b'\x028646F3304100\x00\x00\x00\x008646G2601200\x00\x00\x00\x00',
+ b'\x028646F3304300\x00\x00\x00\x008646G2601500\x00\x00\x00\x00',
b'\x028646F4810200\x00\x00\x00\x008646G2601400\x00\x00\x00\x00',
],
},
@@ -1288,6 +1296,7 @@ FW_VERSIONS = {
b'\x01896637851000\x00\x00\x00\x00',
b'\x01896637852000\x00\x00\x00\x00',
b'\x01896637854000\x00\x00\x00\x00',
+ b'\x01896637878000\x00\x00\x00\x00',
],
(Ecu.esp, 0x7b0, None): [
b'F152678130\x00\x00\x00\x00\x00\x00',
@@ -1295,6 +1304,7 @@ FW_VERSIONS = {
],
(Ecu.dsu, 0x791, None): [
b'881517803100\x00\x00\x00\x00',
+ b'881517803300\x00\x00\x00\x00',
],
(Ecu.eps, 0x7a1, None): [
b'8965B78060\x00\x00\x00\x00\x00\x00',
@@ -1306,6 +1316,7 @@ FW_VERSIONS = {
],
(Ecu.fwdCamera, 0x750, 0x6d): [
b'8646F7801100\x00\x00\x00\x00',
+ b'8646F7801300\x00\x00\x00\x00',
],
},
CAR.LEXUS_NX_TSS2: {
@@ -1358,6 +1369,26 @@ FW_VERSIONS = {
b'8646F7801100\x00\x00\x00\x00',
],
},
+ CAR.LEXUS_RC: {
+ (Ecu.engine, 0x7e0, None): [
+ b'\x0232484000\x00\x00\x00\x00\x00\x00\x00\x0052422000\x00\x00\x00\x00\x00\x00\x00\x00',
+ ],
+ (Ecu.esp, 0x7b0, None): [
+ b'F152624221\x00\x00\x00\x00\x00\x00',
+ ],
+ (Ecu.dsu, 0x791, None): [
+ b'881512409100\x00\x00\x00\x00',
+ ],
+ (Ecu.eps, 0x7a1, None): [
+ b'8965B24081\x00\x00\x00\x00\x00\x00',
+ ],
+ (Ecu.fwdRadar, 0x750, 0xf): [
+ b'8821F4702300\x00\x00\x00\x00',
+ ],
+ (Ecu.fwdCamera, 0x750, 0x6d): [
+ b'8646F2402200\x00\x00\x00\x00',
+ ],
+ },
CAR.LEXUS_RX: {
(Ecu.engine, 0x700, None): [
b'\x01896630E36200\x00\x00\x00\x00',
@@ -1558,6 +1589,7 @@ DBC = {
CAR.RAV4: dbc_dict('toyota_rav4_2017_pt_generated', 'toyota_adas'),
CAR.PRIUS: dbc_dict('toyota_prius_2017_pt_generated', 'toyota_adas'),
CAR.COROLLA: dbc_dict('toyota_corolla_2017_pt_generated', 'toyota_adas'),
+ CAR.LEXUS_RC: dbc_dict('lexus_is_2018_pt_generated', 'toyota_adas'),
CAR.LEXUS_RX: dbc_dict('lexus_rx_350_2016_pt_generated', 'toyota_adas'),
CAR.LEXUS_RXH: dbc_dict('lexus_rx_hybrid_2017_pt_generated', 'toyota_adas'),
CAR.LEXUS_RX_TSS2: dbc_dict('toyota_nodsu_pt_generated', 'toyota_tss2_adas'),
diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py
index f74e86c6cb..03130ec9d2 100644
--- a/selfdrive/car/volkswagen/carstate.py
+++ b/selfdrive/car/volkswagen/carstate.py
@@ -20,10 +20,12 @@ class CarState(CarStateBase):
def update(self, pt_cp, cam_cp, ext_cp, trans_type):
ret = car.CarState.new_message()
# Update vehicle speed and acceleration from ABS wheel speeds.
- ret.wheelSpeeds.fl = pt_cp.vl["ESP_19"]["ESP_VL_Radgeschw_02"] * CV.KPH_TO_MS
- ret.wheelSpeeds.fr = pt_cp.vl["ESP_19"]["ESP_VR_Radgeschw_02"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rl = pt_cp.vl["ESP_19"]["ESP_HL_Radgeschw_02"] * CV.KPH_TO_MS
- ret.wheelSpeeds.rr = pt_cp.vl["ESP_19"]["ESP_HR_Radgeschw_02"] * CV.KPH_TO_MS
+ ret.wheelSpeeds = self.get_wheel_speeds(
+ pt_cp.vl["ESP_19"]["ESP_VL_Radgeschw_02"],
+ pt_cp.vl["ESP_19"]["ESP_VR_Radgeschw_02"],
+ pt_cp.vl["ESP_19"]["ESP_HL_Radgeschw_02"],
+ pt_cp.vl["ESP_19"]["ESP_HR_Radgeschw_02"],
+ )
ret.vEgoRaw = float(np.mean([ret.wheelSpeeds.fl, ret.wheelSpeeds.fr, ret.wheelSpeeds.rl, ret.wheelSpeeds.rr]))
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
diff --git a/selfdrive/car/volkswagen/values.py b/selfdrive/car/volkswagen/values.py
index 9669e0e297..5dcdad3b5e 100755
--- a/selfdrive/car/volkswagen/values.py
+++ b/selfdrive/car/volkswagen/values.py
@@ -152,6 +152,7 @@ FW_VERSIONS = {
(Ecu.engine, 0x7e0, None): [
b'\xf1\x8704E906016A \xf1\x897697',
b'\xf1\x8704E906016AD\xf1\x895758',
+ b'\xf1\x8704E906016CE\xf1\x899096',
b'\xf1\x8704E906023AG\xf1\x891726',
b'\xf1\x8704E906023BN\xf1\x894518',
b'\xf1\x8704E906024K \xf1\x896811',
@@ -192,6 +193,7 @@ FW_VERSIONS = {
b'\xf1\x870CW300042F \xf1\x891604',
b'\xf1\x870CW300043B \xf1\x891601',
b'\xf1\x870CW300044S \xf1\x894530',
+ b'\xf1\x870CW300044T \xf1\x895245',
b'\xf1\x870CW300045 \xf1\x894531',
b'\xf1\x870CW300047D \xf1\x895261',
b'\xf1\x870CW300048J \xf1\x890611',
@@ -274,6 +276,7 @@ FW_VERSIONS = {
],
(Ecu.fwdRadar, 0x757, None): [
b'\xf1\x875Q0907567G \xf1\x890390\xf1\x82\00101',
+ b'\xf1\x875Q0907567J \xf1\x890396\xf1\x82\x0101',
b'\xf1\x875Q0907572A \xf1\x890141\xf1\x82\00101',
b'\xf1\x875Q0907572B \xf1\x890200\xf1\x82\00101',
b'\xf1\x875Q0907572C \xf1\x890210\xf1\x82\00101',
diff --git a/selfdrive/common/params.cc b/selfdrive/common/params.cc
index 01bf9a8f4f..ea09ee0705 100644
--- a/selfdrive/common/params.cc
+++ b/selfdrive/common/params.cc
@@ -130,11 +130,13 @@ std::unordered_map keys = {
{"JoystickDebugMode", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
{"LastAthenaPingTime", CLEAR_ON_MANAGER_START},
{"LastGPSPosition", PERSISTENT},
+ {"LastPowerDropDetected", CLEAR_ON_MANAGER_START},
{"LastUpdateException", PERSISTENT},
{"LastUpdateTime", PERSISTENT},
{"LiveParameters", PERSISTENT},
{"NavDestination", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
{"NavSettingTime24h", PERSISTENT},
+ {"NavdRender", PERSISTENT},
{"OpenpilotEnabledToggle", PERSISTENT},
{"PandaHeartbeatLost", CLEAR_ON_MANAGER_START | CLEAR_ON_IGNITION_OFF},
{"Passive", PERSISTENT},
diff --git a/selfdrive/common/version.h b/selfdrive/common/version.h
index 9f00fac4f9..aa786903c4 100644
--- a/selfdrive/common/version.h
+++ b/selfdrive/common/version.h
@@ -1 +1 @@
-#define COMMA_VERSION "0.8.11"
+#define COMMA_VERSION "0.8.12"
diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py
index bbd84d458f..d029650aaf 100755
--- a/selfdrive/controls/controlsd.py
+++ b/selfdrive/controls/controlsd.py
@@ -540,8 +540,9 @@ class Controls:
if len(lat_plan.dPathPoints):
# Check if we deviated from the path
- left_deviation = actuators.steer > 0 and lat_plan.dPathPoints[0] < -0.1
- right_deviation = actuators.steer < 0 and lat_plan.dPathPoints[0] > 0.1
+ # TODO use desired vs actual curvature
+ left_deviation = actuators.steer > 0 and lat_plan.dPathPoints[0] < -0.20
+ right_deviation = actuators.steer < 0 and lat_plan.dPathPoints[0] > 0.20
if left_deviation or right_deviation:
self.events.add(EventName.steerSaturated)
diff --git a/selfdrive/controls/lib/alertmanager.py b/selfdrive/controls/lib/alertmanager.py
index 42d9f457be..22d7f3eac4 100644
--- a/selfdrive/controls/lib/alertmanager.py
+++ b/selfdrive/controls/lib/alertmanager.py
@@ -41,6 +41,7 @@ class AlertManager:
self.activealerts: Dict[str, AlertEntry] = defaultdict(AlertEntry)
def reset(self) -> None:
+ self.alert: Optional[Alert] = None
self.alert_type: str = ""
self.alert_text_1: str = ""
self.alert_text_2: str = ""
@@ -74,13 +75,13 @@ class AlertManager:
# clear current alert
self.reset()
- a = current_alert.alert
- if a is not None:
- self.alert_type = a.alert_type
- self.audible_alert = a.audible_alert
- self.visual_alert = a.visual_alert
- self.alert_text_1 = a.alert_text_1
- self.alert_text_2 = a.alert_text_2
- self.alert_status = a.alert_status
- self.alert_size = a.alert_size
- self.alert_rate = a.alert_rate
+ self.alert = current_alert.alert
+ if self.alert is not None:
+ self.alert_type = self.alert.alert_type
+ self.audible_alert = self.alert.audible_alert
+ self.visual_alert = self.alert.visual_alert
+ self.alert_text_1 = self.alert.alert_text_1
+ self.alert_text_2 = self.alert.alert_text_2
+ self.alert_status = self.alert.alert_status
+ self.alert_size = self.alert.alert_size
+ self.alert_rate = self.alert.alert_rate
diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py
index bd728159b7..d858dbfcbc 100644
--- a/selfdrive/controls/lib/events.py
+++ b/selfdrive/controls/lib/events.py
@@ -139,11 +139,10 @@ class Alert:
class NoEntryAlert(Alert):
- def __init__(self, alert_text_2, audible_alert=AudibleAlert.chimeError,
- visual_alert=VisualAlert.none):
+ def __init__(self, alert_text_2, visual_alert=VisualAlert.none):
super().__init__("openpilot Unavailable", alert_text_2, AlertStatus.normal,
AlertSize.mid, Priority.LOW, visual_alert,
- audible_alert, 3.)
+ AudibleAlert.refuse, 3.)
class SoftDisableAlert(Alert):
@@ -151,7 +150,7 @@ class SoftDisableAlert(Alert):
super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2,
AlertStatus.userPrompt, AlertSize.full,
Priority.MID, VisualAlert.steerRequired,
- AudibleAlert.chimeWarningRepeatInfinite, 2.),
+ AudibleAlert.warningSoft, 2.),
class ImmediateDisableAlert(Alert):
@@ -159,7 +158,7 @@ class ImmediateDisableAlert(Alert):
super().__init__("TAKE CONTROL IMMEDIATELY", alert_text_2,
AlertStatus.critical, AlertSize.full,
Priority.HIGHEST, VisualAlert.steerRequired,
- AudibleAlert.chimeWarningRepeatInfinite, 4.),
+ AudibleAlert.warningImmediate, 4.),
class EngagementAlert(Alert):
@@ -201,7 +200,7 @@ def below_steer_speed_alert(CP: car.CarParams, sm: messaging.SubMaster, metric:
f"Steer Unavailable Below {get_display_speed(CP.minSteerSpeed, metric)}",
"",
AlertStatus.userPrompt, AlertSize.small,
- Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimePrompt, 0.4)
+ Priority.MID, VisualAlert.steerRequired, AudibleAlert.prompt, 0.4)
def calibration_incomplete_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert:
@@ -321,7 +320,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
"BRAKE!",
"Risk of Collision",
AlertStatus.critical, AlertSize.full,
- Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.chimeWarningRepeatInfinite, 2.),
+ Priority.HIGHEST, VisualAlert.fcw, AudibleAlert.warningSoft, 2.),
},
EventName.ldw: {
@@ -329,7 +328,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
"TAKE CONTROL",
"Lane Departure Detected",
AlertStatus.userPrompt, AlertSize.mid,
- Priority.LOW, VisualAlert.ldw, AudibleAlert.chimePrompt, 3.),
+ Priority.LOW, VisualAlert.ldw, AudibleAlert.prompt, 3.),
},
# ********** events only containing alerts that display while engaged **********
@@ -360,7 +359,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
"Steering Temporarily Unavailable",
"",
AlertStatus.userPrompt, AlertSize.small,
- Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimePrompt, 1.),
+ Priority.LOW, VisualAlert.steerRequired, AudibleAlert.prompt, 1.),
},
EventName.preDriverDistracted: {
@@ -376,7 +375,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
"KEEP EYES ON ROAD",
"Driver Distracted",
AlertStatus.userPrompt, AlertSize.mid,
- Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2RepeatInfinite, .1),
+ Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1),
},
EventName.driverDistracted: {
@@ -384,7 +383,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
"DISENGAGE IMMEDIATELY",
"Driver Distracted",
AlertStatus.critical, AlertSize.full,
- Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeatInfinite, .1),
+ Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1),
},
EventName.preDriverUnresponsive: {
@@ -400,7 +399,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
"TOUCH STEERING WHEEL",
"Driver Unresponsive",
AlertStatus.userPrompt, AlertSize.mid,
- Priority.MID, VisualAlert.steerRequired, AudibleAlert.chimeWarning2RepeatInfinite, .1),
+ Priority.MID, VisualAlert.steerRequired, AudibleAlert.promptDistracted, .1),
},
EventName.driverUnresponsive: {
@@ -408,7 +407,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
"DISENGAGE IMMEDIATELY",
"Driver Unresponsive",
AlertStatus.critical, AlertSize.full,
- Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarningRepeatInfinite, .1),
+ Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.warningImmediate, .1),
},
EventName.manualRestart: {
@@ -452,7 +451,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
"Car Detected in Blindspot",
"",
AlertStatus.userPrompt, AlertSize.small,
- Priority.LOW, VisualAlert.none, AudibleAlert.chimePrompt, .1),
+ Priority.LOW, VisualAlert.none, AudibleAlert.prompt, .1),
},
EventName.laneChange: {
@@ -468,7 +467,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
"TAKE CONTROL",
"Turn Exceeds Steering Limit",
AlertStatus.userPrompt, AlertSize.mid,
- Priority.LOW, VisualAlert.steerRequired, AudibleAlert.chimeWarning2RepeatInfinite, 1.),
+ Priority.LOW, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 1.),
},
# Thrown when the fan is driven at >50% but is not rotating
@@ -496,44 +495,44 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
# ********** events that affect controls state transitions **********
EventName.pcmEnable: {
- ET.ENABLE: EngagementAlert(AudibleAlert.chimeEngage),
+ ET.ENABLE: EngagementAlert(AudibleAlert.engage),
},
EventName.buttonEnable: {
- ET.ENABLE: EngagementAlert(AudibleAlert.chimeEngage),
+ ET.ENABLE: EngagementAlert(AudibleAlert.engage),
},
EventName.pcmDisable: {
- ET.USER_DISABLE: EngagementAlert(AudibleAlert.chimeDisengage),
+ ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
},
EventName.buttonCancel: {
- ET.USER_DISABLE: EngagementAlert(AudibleAlert.chimeDisengage),
+ ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
},
EventName.brakeHold: {
- ET.USER_DISABLE: EngagementAlert(AudibleAlert.chimeDisengage),
+ ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
ET.NO_ENTRY: NoEntryAlert("Brake Hold Active"),
},
EventName.parkBrake: {
- ET.USER_DISABLE: EngagementAlert(AudibleAlert.chimeDisengage),
+ ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
ET.NO_ENTRY: NoEntryAlert("Park Brake Engaged"),
},
EventName.pedalPressed: {
- ET.USER_DISABLE: EngagementAlert(AudibleAlert.chimeDisengage),
+ ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
ET.NO_ENTRY: NoEntryAlert("Pedal Pressed During Attempt",
visual_alert=VisualAlert.brakePressed),
},
EventName.wrongCarMode: {
- ET.USER_DISABLE: EngagementAlert(AudibleAlert.chimeDisengage),
+ ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
ET.NO_ENTRY: wrong_car_mode_alert,
},
EventName.wrongCruiseMode: {
- ET.USER_DISABLE: EngagementAlert(AudibleAlert.chimeDisengage),
+ ET.USER_DISABLE: EngagementAlert(AudibleAlert.disengage),
ET.NO_ENTRY: NoEntryAlert("Enable Adaptive Cruise"),
},
@@ -627,14 +626,12 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
# ten times the regular interval, or the average interval is more than 10% too high.
EventName.commIssue: {
ET.SOFT_DISABLE: SoftDisableAlert("Communication Issue between Processes"),
- ET.NO_ENTRY: NoEntryAlert("Communication Issue between Processes",
- audible_alert=AudibleAlert.chimeDisengage),
+ ET.NO_ENTRY: NoEntryAlert("Communication Issue between Processes"),
},
# Thrown when manager detects a service exited unexpectedly while driving
EventName.processNotRunning: {
- ET.NO_ENTRY: NoEntryAlert("System Malfunction: Reboot Your Device",
- audible_alert=AudibleAlert.chimeDisengage),
+ ET.NO_ENTRY: NoEntryAlert("System Malfunction: Reboot Your Device"),
},
EventName.radarFault: {
@@ -670,15 +667,13 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
EventName.lowMemory: {
ET.SOFT_DISABLE: SoftDisableAlert("Low Memory: Reboot Your Device"),
ET.PERMANENT: NormalPermanentAlert("Low Memory", "Reboot your Device"),
- ET.NO_ENTRY: NoEntryAlert("Low Memory: Reboot Your Device",
- audible_alert=AudibleAlert.chimeDisengage),
+ ET.NO_ENTRY: NoEntryAlert("Low Memory: Reboot Your Device"),
},
EventName.highCpuUsage: {
#ET.SOFT_DISABLE: SoftDisableAlert("System Malfunction: Reboot Your Device"),
#ET.PERMANENT: NormalPermanentAlert("System Malfunction", "Reboot your Device"),
- ET.NO_ENTRY: NoEntryAlert("System Malfunction: Reboot Your Device",
- audible_alert=AudibleAlert.chimeDisengage),
+ ET.NO_ENTRY: NoEntryAlert("System Malfunction: Reboot Your Device"),
},
EventName.accFaulted: {
@@ -782,7 +777,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
"openpilot Canceled",
"No close lead car",
AlertStatus.normal, AlertSize.mid,
- Priority.HIGH, VisualAlert.none, AudibleAlert.chimeDisengage, 3.),
+ Priority.HIGH, VisualAlert.none, AudibleAlert.disengage, 3.),
ET.NO_ENTRY: NoEntryAlert("No Close Lead Car"),
},
@@ -791,7 +786,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
"openpilot Canceled",
"Speed too low",
AlertStatus.normal, AlertSize.mid,
- Priority.HIGH, VisualAlert.none, AudibleAlert.chimeDisengage, 3.),
+ Priority.HIGH, VisualAlert.none, AudibleAlert.disengage, 3.),
},
# When the car is driving faster than most cars in the training data the model outputs can be unpredictable
@@ -800,7 +795,7 @@ EVENTS: Dict[int, Dict[str, Union[Alert, Callable[[Any, messaging.SubMaster, boo
"Speed Too High",
"Model uncertain at this speed",
AlertStatus.userPrompt, AlertSize.mid,
- Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.chimeWarning2RepeatInfinite, 4.),
+ Priority.HIGH, VisualAlert.steerRequired, AudibleAlert.promptRepeat, 4.),
ET.NO_ENTRY: NoEntryAlert("Slow down to engage"),
},
diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py
index 74b27f595d..0aa2359ae7 100644
--- a/selfdrive/controls/lib/lateral_planner.py
+++ b/selfdrive/controls/lib/lateral_planner.py
@@ -38,7 +38,7 @@ DESIRES = {
}
-class LateralPlanner():
+class LateralPlanner:
def __init__(self, CP, use_lanelines=True, wide_camera=False):
self.use_lanelines = use_lanelines
self.LP = LanePlanner(wide_camera)
@@ -55,8 +55,8 @@ class LateralPlanner():
self.prev_one_blinker = False
self.desire = log.LateralPlan.Desire.none
- self.path_xyz = np.zeros((TRAJECTORY_SIZE,3))
- self.path_xyz_stds = np.ones((TRAJECTORY_SIZE,3))
+ self.path_xyz = np.zeros((TRAJECTORY_SIZE, 3))
+ self.path_xyz_stds = np.ones((TRAJECTORY_SIZE, 3))
self.plan_yaw = np.zeros((TRAJECTORY_SIZE,))
self.t_idxs = np.arange(TRAJECTORY_SIZE)
self.y_pts = np.zeros(TRAJECTORY_SIZE)
@@ -67,12 +67,8 @@ class LateralPlanner():
def reset_mpc(self, x0=np.zeros(6)):
self.x0 = x0
self.lat_mpc.reset(x0=self.x0)
- self.desired_curvature = 0.0
- self.safe_desired_curvature = 0.0
- self.desired_curvature_rate = 0.0
- self.safe_desired_curvature_rate = 0.0
- def update(self, sm, CP):
+ def update(self, sm):
v_ego = sm['carState'].vEgo
active = sm['controlsState'].active
measured_curvature = sm['controlsState'].curvature
@@ -110,7 +106,7 @@ class LateralPlanner():
self.lane_change_direction = LaneChangeDirection.none
torque_applied = sm['carState'].steeringPressed and \
- ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
+ ((sm['carState'].steeringTorque > 0 and self.lane_change_direction == LaneChangeDirection.left) or
(sm['carState'].steeringTorque < 0 and self.lane_change_direction == LaneChangeDirection.right))
blindspot_detected = ((sm['carState'].leftBlindspot and self.lane_change_direction == LaneChangeDirection.left) or
@@ -124,7 +120,7 @@ class LateralPlanner():
# LaneChangeState.laneChangeStarting
elif self.lane_change_state == LaneChangeState.laneChangeStarting:
# fade out over .5s
- self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2*DT_MDL, 0.0)
+ self.lane_change_ll_prob = max(self.lane_change_ll_prob - 2 * DT_MDL, 0.0)
# 98% certainty
lane_change_prob = self.LP.l_lane_change_prob + self.LP.r_lane_change_prob
@@ -167,14 +163,14 @@ class LateralPlanner():
self.LP.rll_prob *= self.lane_change_ll_prob
if self.use_lanelines:
d_path_xyz = self.LP.get_d_path(v_ego, self.t_idxs, self.path_xyz)
- self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, CP.steerRateCost)
+ self.lat_mpc.set_weights(MPC_COST_LAT.PATH, MPC_COST_LAT.HEADING, self.steer_rate_cost)
else:
d_path_xyz = self.path_xyz
- path_cost = np.clip(abs(self.path_xyz[0,1]/self.path_xyz_stds[0,1]), 0.5, 1.5) * MPC_COST_LAT.PATH
+ path_cost = np.clip(abs(self.path_xyz[0, 1] / self.path_xyz_stds[0, 1]), 0.5, 1.5) * MPC_COST_LAT.PATH
# Heading cost is useful at low speed, otherwise end of plan can be off-heading
heading_cost = interp(v_ego, [5.0, 10.0], [MPC_COST_LAT.HEADING, 0.0])
- self.lat_mpc.set_weights(path_cost, heading_cost, CP.steerRateCost)
- y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:,1])
+ self.lat_mpc.set_weights(path_cost, heading_cost, self.steer_rate_cost)
+ y_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(d_path_xyz, axis=1), d_path_xyz[:, 1])
heading_pts = np.interp(v_ego * self.t_idxs[:LAT_MPC_N + 1], np.linalg.norm(self.path_xyz, axis=1), self.plan_yaw)
self.y_pts = y_pts
@@ -187,11 +183,10 @@ class LateralPlanner():
y_pts,
heading_pts)
# init state for next
- self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:,3])
+ self.x0[3] = interp(DT_MDL, self.t_idxs[:LAT_MPC_N + 1], self.lat_mpc.x_sol[:, 3])
-
- # Check for infeasable MPC solution
- mpc_nans = any(math.isnan(x) for x in self.lat_mpc.x_sol[:,3])
+ # Check for infeasible MPC solution
+ mpc_nans = any(math.isnan(x) for x in self.lat_mpc.x_sol[:, 3])
t = sec_since_boot()
if mpc_nans or self.lat_mpc.solution_status != 0:
self.reset_mpc()
@@ -212,8 +207,8 @@ class LateralPlanner():
plan_send.lateralPlan.laneWidth = float(self.LP.lane_width)
plan_send.lateralPlan.dPathPoints = [float(x) for x in self.y_pts]
plan_send.lateralPlan.psis = [float(x) for x in self.lat_mpc.x_sol[0:CONTROL_N, 2]]
- plan_send.lateralPlan.curvatures = [float(x) for x in self.lat_mpc.x_sol[0:CONTROL_N,3]]
- plan_send.lateralPlan.curvatureRates = [float(x) for x in self.lat_mpc.u_sol[0:CONTROL_N-1]] +[0.0]
+ plan_send.lateralPlan.curvatures = [float(x) for x in self.lat_mpc.x_sol[0:CONTROL_N, 3]]
+ plan_send.lateralPlan.curvatureRates = [float(x) for x in self.lat_mpc.u_sol[0:CONTROL_N - 1]] + [0.0]
plan_send.lateralPlan.lProb = float(self.LP.lll_prob)
plan_send.lateralPlan.rProb = float(self.LP.rll_prob)
plan_send.lateralPlan.dProb = float(self.LP.d_prob)
diff --git a/selfdrive/controls/lib/longcontrol.py b/selfdrive/controls/lib/longcontrol.py
index f59a6cc0d6..beacc518d0 100644
--- a/selfdrive/controls/lib/longcontrol.py
+++ b/selfdrive/controls/lib/longcontrol.py
@@ -10,20 +10,20 @@ LongCtrlState = car.CarControl.Actuators.LongControlState
STOPPING_TARGET_SPEED_OFFSET = 0.01
# As per ISO 15622:2018 for all speeds
-ACCEL_MIN_ISO = -3.5 # m/s^2
-ACCEL_MAX_ISO = 2.0 # m/s^2
+ACCEL_MIN_ISO = -3.5 # m/s^2
+ACCEL_MAX_ISO = 2.0 # m/s^2
-def long_control_state_trans(CP, active, long_control_state, v_ego, v_target, v_pid,
+def long_control_state_trans(CP, active, long_control_state, v_ego, v_target_future, v_pid,
output_accel, brake_pressed, cruise_standstill, min_speed_can):
"""Update longitudinal control state machine"""
stopping_target_speed = min_speed_can + STOPPING_TARGET_SPEED_OFFSET
stopping_condition = (v_ego < 2.0 and cruise_standstill) or \
(v_ego < CP.vEgoStopping and
- ((v_pid < stopping_target_speed and v_target < stopping_target_speed) or
+ ((v_pid < stopping_target_speed and v_target_future < stopping_target_speed) or
brake_pressed))
- starting_condition = v_target > CP.vEgoStarting and not cruise_standstill
+ starting_condition = v_target_future > CP.vEgoStarting and not cruise_standstill
if not active:
long_control_state = LongCtrlState.off
@@ -75,13 +75,10 @@ class LongControl():
v_target_upper = interp(CP.longitudinalActuatorDelayUpperBound, T_IDXS[:CONTROL_N], long_plan.speeds)
a_target_upper = 2 * (v_target_upper - long_plan.speeds[0])/CP.longitudinalActuatorDelayUpperBound - long_plan.accels[0]
-
- v_target = min(v_target_lower, v_target_upper)
a_target = min(a_target_lower, a_target_upper)
v_target_future = long_plan.speeds[-1]
else:
- v_target = 0.0
v_target_future = 0.0
a_target = 0.0
@@ -103,11 +100,11 @@ class LongControl():
# tracking objects and driving
elif self.long_control_state == LongCtrlState.pid:
- self.v_pid = v_target
+ self.v_pid = long_plan.speeds[0]
# Toyota starts braking more when it thinks you want to stop
# Freeze the integrator so we don't accelerate to compensate, and don't allow positive acceleration
- prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7
+ prevent_overshoot = not CP.stoppingControl and CS.vEgo < 1.5 and v_target_future < 0.7 and v_target_future < self.v_pid
deadzone = interp(CS.vEgo, CP.longitudinalTuning.deadzoneBP, CP.longitudinalTuning.deadzoneV)
freeze_integrator = prevent_overshoot
diff --git a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
index 577651f8d4..54d4b0be31 100644
--- a/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
+++ b/selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py
@@ -49,15 +49,16 @@ T_IDXS_LST = [index_function(idx, max_val=MAX_T, max_idx=N+1) for idx in range(N
T_IDXS = np.array(T_IDXS_LST)
T_DIFFS = np.diff(T_IDXS, prepend=[0.])
MIN_ACCEL = -3.5
-T_REACT = 1.8
-MAX_BRAKE = 9.81
-
+T_REACT = 0.5
+T_FOLLOW = 1.45
+COMFORT_BRAKE = 2.0
+STOP_DISTANCE = 6.0
def get_stopped_equivalence_factor(v_lead):
- return T_REACT * v_lead + (v_lead*v_lead) / (2 * MAX_BRAKE)
+ return v_lead**2 / (2 * COMFORT_BRAKE) - (T_FOLLOW - T_REACT) * v_lead
def get_safe_obstacle_distance(v_ego):
- return 2 * T_REACT * v_ego + (v_ego*v_ego) / (2 * MAX_BRAKE) + 4.0
+ return (v_ego*v_ego) / (2 * COMFORT_BRAKE) + T_REACT * v_ego + STOP_DISTANCE
def desired_follow_distance(v_ego, v_lead):
return get_safe_obstacle_distance(v_ego) - get_stopped_equivalence_factor(v_lead)
@@ -242,7 +243,7 @@ class LongitudinalMpc():
self.solver.cost_set(i, 'Zl', Zl)
def set_weights_for_xva_policy(self):
- W = np.asfortranarray(np.diag([0., 10., 1., 10., 0.0, 1.]))
+ W = np.asfortranarray(np.diag([0., 10., 1., 10., 1.]))
for i in range(N):
self.solver.cost_set(i, 'W', W)
# Setting the slice without the copy make the array not contiguous,
@@ -298,8 +299,9 @@ class LongitudinalMpc():
self.cruise_min_a = min_a
self.cruise_max_a = max_a
- def update(self, carstate, radarstate, v_cruise):
+ def update(self, carstate, radarstate, v_cruise, prev_accel_constraint=False):
v_ego = self.x0[1]
+ a_ego = self.x0[2]
self.status = radarstate.leadOne.status or radarstate.leadTwo.status
lead_xv_0 = self.process_lead(radarstate.leadOne)
@@ -317,17 +319,20 @@ class LongitudinalMpc():
# Fake an obstacle for cruise, this ensures smooth acceleration to set speed
# when the leads are no factor.
- cruise_lower_bound = v_ego + (3/4) * self.cruise_min_a * T_IDXS
- cruise_upper_bound = v_ego + (3/4) * self.cruise_max_a * T_IDXS
+ v_lower = v_ego + (T_IDXS * self.cruise_min_a * 1.05)
+ v_upper = v_ego + (T_IDXS * self.cruise_max_a * 1.05)
v_cruise_clipped = np.clip(v_cruise * np.ones(N+1),
- cruise_lower_bound,
- cruise_upper_bound)
- cruise_obstacle = T_IDXS*v_cruise_clipped + get_safe_obstacle_distance(v_cruise_clipped)
+ v_lower,
+ v_upper)
+ cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped)
x_obstacles = np.column_stack([lead_0_obstacle, lead_1_obstacle, cruise_obstacle])
self.source = SOURCES[np.argmin(x_obstacles[0])]
self.params[:,2] = np.min(x_obstacles, axis=1)
- self.params[:,3] = np.copy(self.prev_a)
+ if prev_accel_constraint:
+ self.params[:,3] = np.copy(self.prev_a)
+ else:
+ self.params[:,3] = a_ego
self.run()
if (np.any(lead_xv_0[:,0] - self.x_sol[:,0] < CRASH_DISTANCE) and
diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py
index 051a68a741..1c1ab0e17b 100755
--- a/selfdrive/controls/lib/longitudinal_planner.py
+++ b/selfdrive/controls/lib/longitudinal_planner.py
@@ -14,7 +14,7 @@ from selfdrive.controls.lib.drive_helpers import V_CRUISE_MAX, CONTROL_N
from selfdrive.swaglog import cloudlog
LON_MPC_STEP = 0.2 # first step is 0.2s
-AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted
+AWARENESS_DECEL = -0.2 # car smoothly decel at .2m/s^2 when user is distracted
A_CRUISE_MIN = -1.2
A_CRUISE_MAX_VALS = [1.2, 1.2, 0.8, 0.6]
A_CRUISE_MAX_BP = [0., 15., 25., 40.]
@@ -35,13 +35,13 @@ def limit_accel_in_turns(v_ego, angle_steers, a_target, CP):
"""
a_total_max = interp(v_ego, _A_TOTAL_MAX_BP, _A_TOTAL_MAX_V)
- a_y = v_ego**2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
- a_x_allowed = math.sqrt(max(a_total_max**2 - a_y**2, 0.))
+ a_y = v_ego ** 2 * angle_steers * CV.DEG_TO_RAD / (CP.steerRatio * CP.wheelbase)
+ a_x_allowed = math.sqrt(max(a_total_max ** 2 - a_y ** 2, 0.))
return [a_target[0], min(a_target[1], a_x_allowed)]
-class Planner():
+class Planner:
def __init__(self, CP, init_v=0.0, init_a=0.0):
self.CP = CP
self.mpc = LongitudinalMpc()
@@ -50,14 +50,13 @@ class Planner():
self.v_desired = init_v
self.a_desired = init_a
- self.alpha = np.exp(-DT_MDL/2.0)
+ self.alpha = np.exp(-DT_MDL / 2.0)
self.v_desired_trajectory = np.zeros(CONTROL_N)
self.a_desired_trajectory = np.zeros(CONTROL_N)
self.j_desired_trajectory = np.zeros(CONTROL_N)
-
- def update(self, sm, CP):
+ def update(self, sm):
v_ego = sm['carState'].vEgo
a_ego = sm['carState'].aEgo
@@ -68,10 +67,13 @@ class Planner():
long_control_state = sm['controlsState'].longControlState
force_slow_decel = sm['controlsState'].forceDecel
+ prev_accel_constraint = True
enabled = (long_control_state == LongCtrlState.pid) or (long_control_state == LongCtrlState.stopping)
if not enabled or sm['carState'].gasPressed:
self.v_desired = v_ego
self.a_desired = a_ego
+ # Smoothly changing between accel trajectory is only relevant when OP is driving
+ prev_accel_constraint = False
# Prevent divergence, smooth in current v_ego
self.v_desired = self.alpha * self.v_desired + (1 - self.alpha) * v_ego
@@ -88,12 +90,12 @@ class Planner():
accel_limits_turns[1] = max(accel_limits_turns[1], self.a_desired - 0.05)
self.mpc.set_accel_limits(accel_limits_turns[0], accel_limits_turns[1])
self.mpc.set_cur_state(self.v_desired, self.a_desired)
- self.mpc.update(sm['carState'], sm['radarState'], v_cruise)
+ self.mpc.update(sm['carState'], sm['radarState'], v_cruise, prev_accel_constraint=prev_accel_constraint)
self.v_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.v_solution)
self.a_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC, self.mpc.a_solution)
self.j_desired_trajectory = np.interp(T_IDXS[:CONTROL_N], T_IDXS_MPC[:-1], self.mpc.j_solution)
- #TODO counter is only needed because radar is glitchy, remove once radar is gone
+ # TODO counter is only needed because radar is glitchy, remove once radar is gone
self.fcw = self.mpc.crash_cnt > 5
if self.fcw:
cloudlog.info("FCW triggered")
@@ -101,7 +103,7 @@ class Planner():
# Interpolate 0.05 seconds and save as starting point for next iteration
a_prev = self.a_desired
self.a_desired = float(interp(DT_MDL, T_IDXS[:CONTROL_N], self.a_desired_trajectory))
- self.v_desired = self.v_desired + DT_MDL * (self.a_desired + a_prev)/2.0
+ self.v_desired = self.v_desired + DT_MDL * (self.a_desired + a_prev) / 2.0
def publish(self, sm, pm):
plan_send = messaging.new_message('longitudinalPlan')
diff --git a/selfdrive/controls/plannerd.py b/selfdrive/controls/plannerd.py
index 88734b3c13..02f1c19a77 100755
--- a/selfdrive/controls/plannerd.py
+++ b/selfdrive/controls/plannerd.py
@@ -36,9 +36,9 @@ def plannerd_thread(sm=None, pm=None):
sm.update()
if sm.updated['modelV2']:
- lateral_planner.update(sm, CP)
+ lateral_planner.update(sm)
lateral_planner.publish(sm, pm)
- longitudinal_planner.update(sm, CP)
+ longitudinal_planner.update(sm)
longitudinal_planner.publish(sm, pm)
diff --git a/selfdrive/controls/tests/test_following_distance.py b/selfdrive/controls/tests/test_following_distance.py
index 5b3fa65228..6661847b7f 100644
--- a/selfdrive/controls/tests/test_following_distance.py
+++ b/selfdrive/controls/tests/test_following_distance.py
@@ -29,7 +29,7 @@ class TestFollowingDistance(unittest.TestCase):
simulation_steady_state = run_following_distance_simulation(v_lead)
correct_steady_state = desired_follow_distance(v_lead, v_lead)
- self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(correct_steady_state*.1 + .3))
+ self.assertAlmostEqual(simulation_steady_state, correct_steady_state, delta=(correct_steady_state*.1 + 1.0))
if __name__ == "__main__":
diff --git a/selfdrive/crash.py b/selfdrive/crash.py
index f85d7c0e6d..e42b7532b0 100644
--- a/selfdrive/crash.py
+++ b/selfdrive/crash.py
@@ -1,6 +1,6 @@
"""Install exception handler for process crash."""
from selfdrive.swaglog import cloudlog
-from selfdrive.version import version
+from selfdrive.version import get_version
import sentry_sdk
from sentry_sdk.integrations.threading import ThreadingIntegration
@@ -24,4 +24,4 @@ def bind_extra(**kwargs) -> None:
def init() -> None:
sentry_sdk.init("https://a8dc76b5bfb34908a601d67e2aa8bcf9@o33823.ingest.sentry.io/77924",
default_integrations=False, integrations=[ThreadingIntegration(propagate_hub=True)],
- release=version)
+ release=get_version())
diff --git a/selfdrive/debug/cpu_usage_stat.py b/selfdrive/debug/cpu_usage_stat.py
index 5931fda640..f9b57130b1 100755
--- a/selfdrive/debug/cpu_usage_stat.py
+++ b/selfdrive/debug/cpu_usage_stat.py
@@ -66,7 +66,7 @@ if __name__ == "__main__":
for p in psutil.process_iter():
if p == psutil.Process():
continue
- matched = any([l for l in p.cmdline() if any([pn for pn in monitored_proc_names if re.match(r'.*{}.*'.format(pn), l, re.M | re.I)])])
+ matched = any(l for l in p.cmdline() if any(pn for pn in monitored_proc_names if re.match(r'.*{}.*'.format(pn), l, re.M | re.I)))
if matched:
k = ' '.join(p.cmdline())
print('Add monitored proc:', k)
diff --git a/selfdrive/debug/cycle_alerts.py b/selfdrive/debug/cycle_alerts.py
index 8b01a73b0d..ca238b4293 100755
--- a/selfdrive/debug/cycle_alerts.py
+++ b/selfdrive/debug/cycle_alerts.py
@@ -7,18 +7,31 @@ import time
from cereal import car, log
import cereal.messaging as messaging
+from common.realtime import DT_CTRL
from selfdrive.car.honda.interface import CarInterface
from selfdrive.controls.lib.events import ET, EVENTS, Events
from selfdrive.controls.lib.alertmanager import AlertManager
EventName = car.CarEvent.EventName
-def cycle_alerts(duration=2000, is_metric=False):
- alerts = list(EVENTS.keys())
- print(alerts)
+def cycle_alerts(duration=200, is_metric=False):
+ # all alerts
+ #alerts = list(EVENTS.keys())
- alerts = [EventName.preDriverDistracted, EventName.promptDriverDistracted, EventName.driverDistracted]
- #alerts = [EventName.preLaneChangeLeft, EventName.preLaneChangeRight]
+ # this plays each type of audible alert
+ alerts = [
+ (EventName.buttonEnable, ET.ENABLE),
+ (EventName.buttonCancel, ET.USER_DISABLE),
+ (EventName.wrongGear, ET.NO_ENTRY),
+
+ (EventName.vehicleModelInvalid, ET.SOFT_DISABLE),
+ (EventName.accFaulted, ET.IMMEDIATE_DISABLE),
+
+ # DM sequence
+ (EventName.preDriverDistracted, ET.WARNING),
+ (EventName.promptDriverDistracted, ET.WARNING),
+ (EventName.driverDistracted, ET.WARNING),
+ ]
CP = CarInterface.get_params("HONDA CIVIC 2016")
sm = messaging.SubMaster(['deviceState', 'pandaStates', 'roadCameraState', 'modelV2', 'liveCalibration',
@@ -30,43 +43,45 @@ def cycle_alerts(duration=2000, is_metric=False):
AM = AlertManager()
frame = 0
- idx, last_alert_millis = 0, 0
- while 1:
- if frame % duration == 0:
- idx = (idx + 1) % len(alerts)
- events.clear()
- events.add(alerts[idx])
-
-
+ while True:
current_alert_types = [ET.PERMANENT, ET.USER_DISABLE, ET.IMMEDIATE_DISABLE,
ET.SOFT_DISABLE, ET.PRE_ENABLE, ET.NO_ENTRY,
ET.ENABLE, ET.WARNING]
- a = events.create_alerts(current_alert_types, [CP, sm, is_metric])
- AM.add_many(frame, a)
- AM.process_alerts(frame)
-
- dat = messaging.new_message()
- dat.init('controlsState')
- dat.controlsState.alertText1 = AM.alert_text_1
- dat.controlsState.alertText2 = AM.alert_text_2
- dat.controlsState.alertSize = AM.alert_size
- dat.controlsState.alertStatus = AM.alert_status
- dat.controlsState.alertBlinkingRate = AM.alert_rate
- dat.controlsState.alertType = AM.alert_type
- dat.controlsState.alertSound = AM.audible_alert
- pm.send('controlsState', dat)
-
- dat = messaging.new_message()
- dat.init('deviceState')
- dat.deviceState.started = True
- pm.send('deviceState', dat)
-
- dat = messaging.new_message('pandaStates', 1)
- dat.pandaStates[0].ignitionLine = True
- dat.pandaStates[0].pandaType = log.PandaState.PandaType.uno
- pm.send('pandaStates', dat)
-
- time.sleep(0.01)
+
+ for alert, et in alerts:
+ events.clear()
+ events.add(alert)
+
+ a = events.create_alerts([et, ], [CP, sm, is_metric])
+ AM.add_many(frame, a)
+ AM.process_alerts(frame)
+ print(AM.alert)
+ for _ in range(duration):
+ dat = messaging.new_message()
+ dat.init('controlsState')
+ dat.controlsState.enabled = True
+
+ dat.controlsState.alertText1 = AM.alert_text_1
+ dat.controlsState.alertText2 = AM.alert_text_2
+ dat.controlsState.alertSize = AM.alert_size
+ dat.controlsState.alertStatus = AM.alert_status
+ dat.controlsState.alertBlinkingRate = AM.alert_rate
+ dat.controlsState.alertType = AM.alert_type
+ dat.controlsState.alertSound = AM.audible_alert
+ pm.send('controlsState', dat)
+
+ dat = messaging.new_message()
+ dat.init('deviceState')
+ dat.deviceState.started = True
+ pm.send('deviceState', dat)
+
+ dat = messaging.new_message('pandaStates', 1)
+ dat.pandaStates[0].ignitionLine = True
+ dat.pandaStates[0].pandaType = log.PandaState.PandaType.uno
+ pm.send('pandaStates', dat)
+
+ frame += 1
+ time.sleep(DT_CTRL)
if __name__ == '__main__':
cycle_alerts()
diff --git a/selfdrive/hardware/base.py b/selfdrive/hardware/base.py
index a1b67cb38b..06c86f0c26 100644
--- a/selfdrive/hardware/base.py
+++ b/selfdrive/hardware/base.py
@@ -1,7 +1,7 @@
from abc import abstractmethod
from collections import namedtuple
-ThermalConfig = namedtuple('ThermalConfig', ['cpu', 'gpu', 'mem', 'bat', 'ambient'])
+ThermalConfig = namedtuple('ThermalConfig', ['cpu', 'gpu', 'mem', 'bat', 'ambient', 'pmic'])
class HardwareBase:
@staticmethod
diff --git a/selfdrive/hardware/eon/hardware.py b/selfdrive/hardware/eon/hardware.py
index 2296c26e6d..fa275c5a9c 100644
--- a/selfdrive/hardware/eon/hardware.py
+++ b/selfdrive/hardware/eon/hardware.py
@@ -369,7 +369,7 @@ class Android(HardwareBase):
os.system('LD_LIBRARY_PATH="" svc power shutdown')
def get_thermal_config(self):
- return ThermalConfig(cpu=((5, 7, 10, 12), 10), gpu=((16,), 10), mem=(2, 10), bat=(29, 1000), ambient=(25, 1))
+ return ThermalConfig(cpu=((5, 7, 10, 12), 10), gpu=((16,), 10), mem=(2, 10), bat=(29, 1000), ambient=(25, 1), pmic=((22,), 1000))
def set_screen_brightness(self, percentage):
with open("/sys/class/leds/lcd-backlight/brightness", "w") as f:
diff --git a/selfdrive/hardware/pc/hardware.py b/selfdrive/hardware/pc/hardware.py
index b2be6ca91c..8231bbc2f9 100644
--- a/selfdrive/hardware/pc/hardware.py
+++ b/selfdrive/hardware/pc/hardware.py
@@ -78,7 +78,7 @@ class Pc(HardwareBase):
print("SHUTDOWN!")
def get_thermal_config(self):
- return ThermalConfig(cpu=((None,), 1), gpu=((None,), 1), mem=(None, 1), bat=(None, 1), ambient=(None, 1))
+ return ThermalConfig(cpu=((None,), 1), gpu=((None,), 1), mem=(None, 1), bat=(None, 1), ambient=(None, 1), pmic=((None,), 1))
def set_screen_brightness(self, percentage):
pass
diff --git a/selfdrive/hardware/tici/amplifier.py b/selfdrive/hardware/tici/amplifier.py
index 99b6d0598a..a8b2798630 100755
--- a/selfdrive/hardware/tici/amplifier.py
+++ b/selfdrive/hardware/tici/amplifier.py
@@ -31,17 +31,18 @@ BASE_CONFIG = [
AmpConfig("Enable PLL2", 0b1, 0x1A, 7, 0b10000000),
AmpConfig("DAI1: I2S mode", 0b00100, 0x14, 2, 0b01111100),
AmpConfig("DAI2: I2S mode", 0b00100, 0x1C, 2, 0b01111100),
- AmpConfig("Right speaker output volume", 0x1a, 0x3E, 0, 0b00011111),
+ AmpConfig("Right speaker output volume", 0x1c, 0x3E, 0, 0b00011111),
AmpConfig("DAI1 Passband filtering: music mode", 0b1, 0x18, 7, 0b10000000),
AmpConfig("DAI1 voice mode gain (DV1G)", 0b00, 0x2F, 4, 0b00110000),
AmpConfig("DAI1 attenuation (DV1)", 0x0, 0x2F, 0, 0b00001111),
AmpConfig("DAI2 attenuation (DV2)", 0x0, 0x31, 0, 0b00001111),
AmpConfig("DAI2: DC blocking", 0b1, 0x20, 0, 0b00000001),
AmpConfig("DAI2: High sample rate", 0b0, 0x20, 3, 0b00001000),
- AmpConfig("ALC enable", 0b0, 0x43, 7, 0b10000000),
+ AmpConfig("ALC enable", 0b1, 0x43, 7, 0b10000000),
AmpConfig("ALC/excursion limiter release time", 0b101, 0x43, 4, 0b01110000),
+ AmpConfig("ALC multiband enable", 0b1, 0x43, 3, 0b00001000),
AmpConfig("DAI1 EQ enable", 0b0, 0x49, 0, 0b00000001),
- AmpConfig("DAI2 EQ enable", 0b0, 0x49, 1, 0b00000010),
+ AmpConfig("DAI2 EQ enable", 0b1, 0x49, 1, 0b00000010),
AmpConfig("DAI2 EQ clip detection disabled", 0b1, 0x32, 4, 0b00010000),
AmpConfig("DAI2 EQ attenuation", 0x5, 0x32, 0, 0b00001111),
AmpConfig("Excursion limiter upper corner freq", 0b100, 0x41, 4, 0b01110000),
@@ -62,11 +63,11 @@ BASE_CONFIG = [
AmpConfig("Zero-crossing detection disabled", 0b0, 0x49, 5, 0b00100000),
]
-BASE_CONFIG += configs_from_eq_params(0x84, EQParams(0x65C4, 0xC07C, 0x3D66, 0x07D9, 0x120F))
+BASE_CONFIG += configs_from_eq_params(0x84, EQParams(0x274F, 0xC0FF, 0x3BF9, 0x0B3C, 0x1656))
BASE_CONFIG += configs_from_eq_params(0x8E, EQParams(0x1009, 0xC6BF, 0x2952, 0x1C97, 0x30DF))
-BASE_CONFIG += configs_from_eq_params(0x98, EQParams(0x2822, 0xC1C7, 0x3B50, 0x0EF8, 0x180A))
-BASE_CONFIG += configs_from_eq_params(0xA2, EQParams(0x1009, 0xC5C2, 0x271F, 0x1A87, 0x32A6))
-BASE_CONFIG += configs_from_eq_params(0xAC, EQParams(0x2000, 0xCA1E, 0x4000, 0x2287, 0x0000))
+BASE_CONFIG += configs_from_eq_params(0x98, EQParams(0x0F75, 0xCBE5, 0x0ED2, 0x2528, 0x3E42))
+BASE_CONFIG += configs_from_eq_params(0xA2, EQParams(0x091F, 0x3D4C, 0xCE11, 0x1266, 0x2807))
+BASE_CONFIG += configs_from_eq_params(0xAC, EQParams(0x0A9E, 0x3F20, 0xE573, 0x0A8B, 0x3A3B))
class Amplifier:
AMP_I2C_BUS = 0
diff --git a/selfdrive/hardware/tici/hardware.h b/selfdrive/hardware/tici/hardware.h
index abd7e9297a..549170412d 100644
--- a/selfdrive/hardware/tici/hardware.h
+++ b/selfdrive/hardware/tici/hardware.h
@@ -9,8 +9,8 @@
class HardwareTici : public HardwareNone {
public:
- static constexpr float MAX_VOLUME = 1.0;
- static constexpr float MIN_VOLUME = 0.4;
+ static constexpr float MAX_VOLUME = 0.9;
+ static constexpr float MIN_VOLUME = 0.15;
static bool TICI() { return true; }
static std::string get_os_version() {
return "AGNOS " + util::read_file("/VERSION");
diff --git a/selfdrive/hardware/tici/hardware.py b/selfdrive/hardware/tici/hardware.py
index ca2705c801..5b7596df26 100644
--- a/selfdrive/hardware/tici/hardware.py
+++ b/selfdrive/hardware/tici/hardware.py
@@ -275,7 +275,7 @@ class Tici(HardwareBase):
os.system("sudo poweroff")
def get_thermal_config(self):
- return ThermalConfig(cpu=((1, 2, 3, 4, 5, 6, 7, 8), 1000), gpu=((48,49), 1000), mem=(15, 1000), bat=(None, 1), ambient=(65, 1000))
+ return ThermalConfig(cpu=((1, 2, 3, 4, 5, 6, 7, 8), 1000), gpu=((48,49), 1000), mem=(15, 1000), bat=(None, 1), ambient=(65, 1000), pmic=((35, 36), 1000))
def set_screen_brightness(self, percentage):
try:
diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc
index 8bdc30a8d2..8e8641e7d5 100755
--- a/selfdrive/locationd/locationd.cc
+++ b/selfdrive/locationd/locationd.cc
@@ -18,6 +18,7 @@ const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad
const double VALID_TIME_SINCE_RESET = 1.0; // s
const double VALID_POS_STD = 50.0; // m
const double MAX_RESET_TRACKER = 5.0;
+const double SANE_GPS_UNCERTAINTY = 1500.0; // m
static VectorXd floatlist2vector(const capnp::List::Reader& floatlist) {
VectorXd res(floatlist.size());
@@ -130,16 +131,16 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
Vector3d nans = Vector3d(NAN, NAN, NAN);
// write measurements to msg
- init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->last_gps_fix > 0);
- init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->last_gps_fix > 0);
- init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->last_gps_fix > 0);
- init_measurement(fix.initVelocityNED(), ned_vel, nans, this->last_gps_fix > 0);
+ init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->gps_mode);
+ init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->gps_mode);
+ init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->gps_mode);
+ init_measurement(fix.initVelocityNED(), ned_vel, nans, this->gps_mode);
init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true);
init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true);
- init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->last_gps_fix > 0);
- init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->last_gps_fix > 0);
- init_measurement(fix.initOrientationNED(), orientation_ned, nans, this->last_gps_fix > 0);
- init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->last_gps_fix > 0);
+ init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->gps_mode);
+ init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->gps_mode);
+ init_measurement(fix.initOrientationNED(), orientation_ned, nans, this->gps_mode);
+ init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->gps_mode);
init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true);
init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated);
init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated);
@@ -243,27 +244,39 @@ void Localizer::handle_sensors(double current_time, const capnp::List observe current obs with reasonable STD
+ this->kf->predict(current_time);
+
+ VectorXd current_x = this->kf->get_x();
+ VectorXd ecef_pos = current_x.segment(STATE_ECEF_POS_START);
+ VectorXd ecef_vel = current_x.segment(STATE_ECEF_VELOCITY_START);
+ MatrixXdr ecef_pos_R = this->kf->get_fake_gps_pos_cov();
+ MatrixXdr ecef_vel_R = this->kf->get_fake_gps_vel_cov();
+
+ this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
+ this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R });
+}
+
void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log) {
// ignore the message if the fix is invalid
- if (log.getFlags() % 2 == 0) {
- return;
- }
-
- // Sanity checks
- if ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0)) {
- return;
- }
- if ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK)) {
- return;
- }
+ bool gps_invalid_flag = (log.getFlags() % 2 == 0);
+ bool gps_unreasonable = (Vector2d(log.getAccuracy(), log.getVerticalAccuracy()).norm() >= SANE_GPS_UNCERTAINTY);
+ bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0));
+ bool gps_lat_lng_alt_insane = ((std::abs(log.getLatitude()) > 90) || (std::abs(log.getLongitude()) > 180) || (std::abs(log.getAltitude()) > ALTITUDE_SANITY_CHECK));
+ bool gps_vel_insane = (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK);
- if (floatlist2vector(log.getVNED()).norm() > TRANS_SANITY_CHECK) {
+ if (gps_invalid_flag || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane){
+ this->determine_gps_mode(current_time);
return;
}
// Process message
this->last_gps_fix = current_time;
+ this->gps_mode = true;
Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() };
this->converter = std::make_unique(geodetic);
@@ -273,7 +286,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(log.getSpeedAccuracy() * 10.0, 2)).asDiagonal();
this->unix_timestamp_millis = log.getTimestamp();
- double gps_est_error = (this->kf->get_x().head(3) - ecef_pos).norm();
+ double gps_est_error = (this->kf->get_x().segment(STATE_ECEF_POS_START) - ecef_pos).norm();
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef);
@@ -290,11 +303,11 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
if (ecef_vel.norm() > 5.0 && orientation_error.norm() > 1.0) {
LOGE("Locationd vs ubloxLocation orientation difference too large, kalman reset");
- this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos);
+ this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS, { initial_pose_ecef_quat });
} else if (gps_est_error > 100.0) {
LOGE("Locationd vs ubloxLocation position difference too large, kalman reset");
- this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos);
+ this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
}
this->kf->predict_and_observe(current_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R });
@@ -358,7 +371,8 @@ void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibra
void Localizer::reset_kalman(double current_time) {
VectorXd init_x = this->kf->get_initial_x();
- this->reset_kalman(current_time, init_x.segment<4>(3), init_x.head(3));
+ MatrixXdr init_P = this->kf->get_initial_P();
+ this->reset_kalman(current_time, init_x, init_P);
}
void Localizer::finite_check(double current_time) {
@@ -390,13 +404,27 @@ void Localizer::update_reset_tracker() {
}
}
-void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd init_pos) {
+void Localizer::reset_kalman(double current_time, VectorXd init_orient, VectorXd init_pos, VectorXd init_vel, MatrixXdr init_pos_R, MatrixXdr init_vel_R) {
// too nonlinear to init on completely wrong
- VectorXd init_x = this->kf->get_initial_x();
+ VectorXd current_x = this->kf->get_x();
+ MatrixXdr current_P = this->kf->get_P();
MatrixXdr init_P = this->kf->get_initial_P();
- init_x.segment<4>(3) = init_orient;
- init_x.head(3) = init_pos;
+ MatrixXdr reset_orientation_P = this->kf->get_reset_orientation_P();
+ int non_ecef_state_err_len = init_P.rows() - (STATE_ECEF_POS_ERR_LEN + STATE_ECEF_ORIENTATION_ERR_LEN + STATE_ECEF_VELOCITY_ERR_LEN);
+
+ current_x.segment(STATE_ECEF_ORIENTATION_START) = init_orient;
+ current_x.segment(STATE_ECEF_VELOCITY_START) = init_vel;
+ current_x.segment(STATE_ECEF_POS_START) = init_pos;
+
+ init_P.block(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal() = init_pos_R.diagonal();
+ init_P.block(STATE_ECEF_ORIENTATION_ERR_START, STATE_ECEF_ORIENTATION_ERR_START).diagonal() = reset_orientation_P.diagonal();
+ init_P.block(STATE_ECEF_VELOCITY_ERR_START, STATE_ECEF_VELOCITY_ERR_START).diagonal() = init_vel_R.diagonal();
+ init_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal() = current_P.block(STATE_ANGULAR_VELOCITY_ERR_START, STATE_ANGULAR_VELOCITY_ERR_START, non_ecef_state_err_len, non_ecef_state_err_len).diagonal();
+
+ this->reset_kalman(current_time, current_x, init_P);
+}
+void Localizer::reset_kalman(double current_time, VectorXd init_x, MatrixXdr init_P) {
this->kf->init_state(init_x, init_P, current_time);
this->last_reset_time = current_time;
this->reset_tracker += 1.0;
@@ -447,6 +475,22 @@ bool Localizer::isGpsOK() {
return this->kf->get_filter_time() - this->last_gps_fix < 1.0;
}
+void Localizer::determine_gps_mode(double current_time) {
+ // 1. If the pos_std is greater than what's not acceptible and localizer is in gps-mode, reset to no-gps-mode
+ // 2. If the pos_std is greater than what's not acceptible and localizer is in no-gps-mode, fake obs
+ // 3. If the pos_std is smaller than what's not acceptible, let gps-mode be whatever it is
+ VectorXd current_pos_std = this->kf->get_P().block(STATE_ECEF_POS_ERR_START, STATE_ECEF_POS_ERR_START).diagonal().array().sqrt();
+ if (current_pos_std.norm() > SANE_GPS_UNCERTAINTY){
+ if (this->gps_mode){
+ this->gps_mode = false;
+ this->reset_kalman(current_time);
+ }
+ else{
+ this->input_fake_gps_observations(current_time);
+ }
+ }
+}
+
int Localizer::locationd_thread() {
const std::initializer_list service_list =
{ "gpsLocationExternal", "sensorEvents", "cameraOdometry", "liveCalibration", "carState" };
diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h
index 60fed112cf..9bc864bf6d 100755
--- a/selfdrive/locationd/locationd.h
+++ b/selfdrive/locationd/locationd.h
@@ -27,11 +27,13 @@ public:
int locationd_thread();
void reset_kalman(double current_time = NAN);
- void reset_kalman(double current_time, Eigen::VectorXd init_orient, Eigen::VectorXd init_pos);
+ void reset_kalman(double current_time, Eigen::VectorXd init_orient, Eigen::VectorXd init_pos, Eigen::VectorXd init_vel, MatrixXdr init_pos_R, MatrixXdr init_vel_R);
+ void reset_kalman(double current_time, Eigen::VectorXd init_x, MatrixXdr init_P);
void finite_check(double current_time = NAN);
void time_check(double current_time = NAN);
void update_reset_tracker();
bool isGpsOK();
+ void determine_gps_mode(double current_time);
kj::ArrayPtr get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,
bool inputsOK, bool sensorsOK, bool gpsOK);
@@ -49,6 +51,8 @@ public:
void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log);
void handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log);
+ void input_fake_gps_observations(double current_time);
+
private:
std::unique_ptr kf;
@@ -67,4 +71,5 @@ private:
double last_gps_fix = 0;
double reset_tracker = 0.0;
bool device_fell = false;
+ bool gps_mode = false;
};
diff --git a/selfdrive/locationd/models/live_kf.cc b/selfdrive/locationd/models/live_kf.cc
index 541c0f7730..5ff0f26995 100755
--- a/selfdrive/locationd/models/live_kf.cc
+++ b/selfdrive/locationd/models/live_kf.cc
@@ -28,11 +28,14 @@ std::vector> get_vec_mapmat(std::vector& mat_ve
}
LiveKalman::LiveKalman() {
- this->dim_state = 26;
- this->dim_state_err = 25;
+ this->dim_state = live_initial_x.rows();
+ this->dim_state_err = live_initial_P_diag.rows();
this->initial_x = live_initial_x;
this->initial_P = live_initial_P_diag.asDiagonal();
+ this->fake_gps_pos_cov = live_fake_gps_pos_cov_diag.asDiagonal();
+ this->fake_gps_vel_cov = live_fake_gps_vel_cov_diag.asDiagonal();
+ this->reset_orientation_P = live_reset_orientation_diag.asDiagonal();
this->Q = live_Q_diag.asDiagonal();
for (auto& pair : live_obs_noise_diag) {
this->obs_noise[pair.first] = pair.second.asDiagonal();
@@ -87,6 +90,10 @@ std::optional LiveKalman::predict_and_observe(double t, int kind, std:
return r;
}
+void LiveKalman::predict(double t) {
+ this->filter->predict(t);
+}
+
Eigen::VectorXd LiveKalman::get_initial_x() {
return this->initial_x;
}
@@ -95,6 +102,18 @@ MatrixXdr LiveKalman::get_initial_P() {
return this->initial_P;
}
+MatrixXdr LiveKalman::get_fake_gps_pos_cov() {
+ return this->fake_gps_pos_cov;
+}
+
+MatrixXdr LiveKalman::get_fake_gps_vel_cov() {
+ return this->fake_gps_vel_cov;
+}
+
+MatrixXdr LiveKalman::get_reset_orientation_P() {
+ return this->reset_orientation_P;
+}
+
MatrixXdr LiveKalman::H(VectorXd in) {
assert(in.size() == 6);
Matrix res;
diff --git a/selfdrive/locationd/models/live_kf.h b/selfdrive/locationd/models/live_kf.h
index 4cd4756c9f..06ec3854cb 100755
--- a/selfdrive/locationd/models/live_kf.h
+++ b/selfdrive/locationd/models/live_kf.h
@@ -36,9 +36,13 @@ public:
std::optional predict_and_update_odo_speed(std::vector speed, double t, int kind);
std::optional predict_and_update_odo_trans(std::vector trans, double t, int kind);
std::optional predict_and_update_odo_rot(std::vector rot, double t, int kind);
+ void predict(double t);
Eigen::VectorXd get_initial_x();
MatrixXdr get_initial_P();
+ MatrixXdr get_fake_gps_pos_cov();
+ MatrixXdr get_fake_gps_vel_cov();
+ MatrixXdr get_reset_orientation_P();
MatrixXdr H(Eigen::VectorXd in);
@@ -52,6 +56,9 @@ private:
Eigen::VectorXd initial_x;
MatrixXdr initial_P;
+ MatrixXdr fake_gps_pos_cov;
+ MatrixXdr fake_gps_vel_cov;
+ MatrixXdr reset_orientation_P;
MatrixXdr Q; // process noise
std::unordered_map obs_noise;
};
diff --git a/selfdrive/locationd/models/live_kf.py b/selfdrive/locationd/models/live_kf.py
index 75415a2845..fa52945932 100755
--- a/selfdrive/locationd/models/live_kf.py
+++ b/selfdrive/locationd/models/live_kf.py
@@ -26,10 +26,8 @@ class States():
ECEF_VELOCITY = slice(7, 10) # ecef velocity in m/s
ANGULAR_VELOCITY = slice(10, 13) # roll, pitch and yaw rates in device frame in radians/s
GYRO_BIAS = slice(13, 16) # roll, pitch and yaw biases
- ODO_SCALE = slice(16, 17) # odometer scale
- ACCELERATION = slice(17, 20) # Acceleration in device frame in m/s**2
- IMU_OFFSET = slice(20, 23) # imu offset angles in radians
- ACC_BIAS = slice(23, 26)
+ ACCELERATION = slice(16, 19) # Acceleration in device frame in m/s**2
+ ACC_BIAS = slice(19, 22) # Acceletometer bias in m/s**2
# Error-state has different slices because it is an ESKF
ECEF_POS_ERR = slice(0, 3)
@@ -37,10 +35,8 @@ class States():
ECEF_VELOCITY_ERR = slice(6, 9)
ANGULAR_VELOCITY_ERR = slice(9, 12)
GYRO_BIAS_ERR = slice(12, 15)
- ODO_SCALE_ERR = slice(15, 16)
- ACCELERATION_ERR = slice(16, 19)
- IMU_OFFSET_ERR = slice(19, 22)
- ACC_BIAS_ERR = slice(22, 25)
+ ACCELERATION_ERR = slice(15, 18)
+ ACC_BIAS_ERR = slice(18, 21)
class LiveKalman():
@@ -51,38 +47,37 @@ class LiveKalman():
0, 0, 0,
0, 0, 0,
0, 0, 0,
- 1,
- 0, 0, 0,
0, 0, 0,
0, 0, 0])
# state covariance
- initial_P_diag = np.array([1e3**2, 1e3**2, 1e3**2,
- 0.5**2, 0.5**2, 0.5**2,
+ initial_P_diag = np.array([10**2, 10**2, 10**2,
+ 0.01**2, 0.01**2, 0.01**2,
10**2, 10**2, 10**2,
1**2, 1**2, 1**2,
1**2, 1**2, 1**2,
- 0.02**2,
100**2, 100**2, 100**2,
- 0.01**2, 0.01**2, 0.01**2,
0.01**2, 0.01**2, 0.01**2])
+ # state covariance when resetting midway in a segment
+ reset_orientation_diag = np.array([1**2, 1**2, 1**2])
+
+ # fake observation covariance, to ensure the uncertainty estimate of the filter is under control
+ fake_gps_pos_cov_diag = np.array([1000**2, 1000**2, 1000**2])
+ fake_gps_vel_cov_diag = np.array([10**2, 10**2, 10**2])
+
# process noise
Q_diag = np.array([0.03**2, 0.03**2, 0.03**2,
0.001**2, 0.001**2, 0.001**2,
0.01**2, 0.01**2, 0.01**2,
0.1**2, 0.1**2, 0.1**2,
(0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2,
- (0.02 / 100)**2,
3**2, 3**2, 3**2,
- (0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2,
0.005**2, 0.005**2, 0.005**2])
- obs_noise_diag = {ObservationKind.ODOMETRIC_SPEED: np.array([0.2**2]),
- ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]),
+ obs_noise_diag = {ObservationKind.PHONE_GYRO: np.array([0.025**2, 0.025**2, 0.025**2]),
ObservationKind.PHONE_ACCEL: np.array([.5**2, .5**2, .5**2]),
ObservationKind.CAMERA_ODO_ROTATION: np.array([0.05**2, 0.05**2, 0.05**2]),
- ObservationKind.IMU_FRAME: np.array([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.NO_ROT: np.array([0.005**2, 0.005**2, 0.005**2]),
ObservationKind.NO_ACCEL: np.array([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.ECEF_POS: np.array([5**2, 5**2, 5**2]),
@@ -105,7 +100,6 @@ class LiveKalman():
vroll, vpitch, vyaw = omega
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
acceleration = state[States.ACCELERATION, :]
- imu_angles = state[States.IMU_OFFSET, :]
acc_bias = state[States.ACC_BIAS, :]
dt = sp.Symbol('dt')
@@ -140,7 +134,6 @@ class LiveKalman():
omega_err = state_err[States.ANGULAR_VELOCITY_ERR, :]
acceleration_err = state_err[States.ACCELERATION_ERR, :]
-
# Time derivative of the state error as a function of state error and state
quat_err_matrix = euler_rotate(quat_err[0], quat_err[1], quat_err[2])
q_err_dot = quat_err_matrix * quat_rot * (omega + omega_err)
@@ -183,7 +176,6 @@ class LiveKalman():
#
# Observation functions
#
- # imu_rot = euler_rotate(*imu_angles)
h_gyro_sym = sp.Matrix([
vroll + roll_bias,
vpitch + pitch_bias,
@@ -194,19 +186,12 @@ class LiveKalman():
h_acc_sym = (gravity + acceleration + acc_bias)
h_acc_stationary_sym = acceleration
h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
-
- speed = sp.sqrt(vx**2 + vy**2 + vz**2 + 1e-6)
- h_speed_sym = sp.Matrix([speed])
-
h_pos_sym = sp.Matrix([x, y, z])
h_vel_sym = sp.Matrix([vx, vy, vz])
h_orientation_sym = q
- h_imu_frame_sym = sp.Matrix(imu_angles)
-
h_relative_motion = sp.Matrix(quat_rot.T * v)
- obs_eqs = [[h_speed_sym, ObservationKind.ODOMETRIC_SPEED, None],
- [h_gyro_sym, ObservationKind.PHONE_GYRO, None],
+ obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
[h_pos_sym, ObservationKind.ECEF_POS, None],
@@ -214,12 +199,11 @@ class LiveKalman():
[h_orientation_sym, ObservationKind.ECEF_ORIENTATION_FROM_GPS, None],
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
- [h_imu_frame_sym, ObservationKind.IMU_FRAME, None],
[h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]]
# this returns a sympy routine for the jacobian of the observation function of the local vel
in_vec = sp.MatrixSymbol('in_vec', 6, 1) # roll, pitch, yaw, vx, vy, vz
- h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T*(sp.Matrix([in_vec[3], in_vec[4], in_vec[5]]))
+ h = euler_rotate(in_vec[0], in_vec[1], in_vec[2]).T * (sp.Matrix([in_vec[3], in_vec[4], in_vec[5]]))
extra_routines = [('H', h.jacobian(in_vec), [in_vec])]
gen_code(generated_dir, name, f_sym, dt, state_sym, obs_eqs, dim_state, dim_state_err, eskf_params, extra_routines=extra_routines)
@@ -241,6 +225,9 @@ class LiveKalman():
live_kf_header += f"static const Eigen::VectorXd live_initial_x = {numpy2eigenstring(LiveKalman.initial_x)};\n"
live_kf_header += f"static const Eigen::VectorXd live_initial_P_diag = {numpy2eigenstring(LiveKalman.initial_P_diag)};\n"
+ live_kf_header += f"static const Eigen::VectorXd live_fake_gps_pos_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_pos_cov_diag)};\n"
+ live_kf_header += f"static const Eigen::VectorXd live_fake_gps_vel_cov_diag = {numpy2eigenstring(LiveKalman.fake_gps_vel_cov_diag)};\n"
+ live_kf_header += f"static const Eigen::VectorXd live_reset_orientation_diag = {numpy2eigenstring(LiveKalman.reset_orientation_diag)};\n"
live_kf_header += f"static const Eigen::VectorXd live_Q_diag = {numpy2eigenstring(LiveKalman.Q_diag)};\n"
live_kf_header += "static const std::unordered_map> live_obs_noise_diag = {\n"
for kind, noise in LiveKalman.obs_noise_diag.items():
diff --git a/selfdrive/locationd/models/loc_kf.py b/selfdrive/locationd/models/loc_kf.py
index 2739494225..adb9098d4d 100755
--- a/selfdrive/locationd/models/loc_kf.py
+++ b/selfdrive/locationd/models/loc_kf.py
@@ -41,14 +41,17 @@ class States():
CLOCK_BIAS = slice(13, 14) # clock bias in light-meters,
CLOCK_DRIFT = slice(14, 15) # clock drift in light-meters/s,
GYRO_BIAS = slice(15, 18) # roll, pitch and yaw biases
- ODO_SCALE = slice(18, 19) # odometer scale
+ ODO_SCALE_UNUSED = slice(18, 19) # odometer scale
ACCELERATION = slice(19, 22) # Acceleration in device frame in m/s**2
- FOCAL_SCALE = slice(22, 23) # focal length scale
+ FOCAL_SCALE_UNUSED = slice(22, 23) # focal length scale
IMU_OFFSET = slice(23, 26) # imu offset angles in radians
GLONASS_BIAS = slice(26, 27) # GLONASS bias in m expressed as bias + freq_num*freq_slope
GLONASS_FREQ_SLOPE = slice(27, 28) # GLONASS bias in m expressed as bias + freq_num*freq_slope
CLOCK_ACCELERATION = slice(28, 29) # clock acceleration in light-meters/s**2,
- ACCELEROMETER_SCALE = slice(29, 30) # scale of mems accelerometer
+ ACCELEROMETER_SCALE_UNUSED = slice(29, 30) # scale of mems accelerometer
+ ACCELEROMETER_BIAS = slice(30, 33) # bias of mems accelerometer
+ # We curently do not use ACCELEROMETER_SCALE to avoid instability due to too many free variables (ACCELEROMETER_SCALE, ACCELEROMETER_BIAS, IMU_OFFSET).
+ # From experiments we see that ACCELEROMETER_BIAS is more correct than ACCELEROMETER_SCALE
# Error-state has different slices because it is an ESKF
ECEF_POS_ERR = slice(0, 3)
@@ -58,14 +61,15 @@ class States():
CLOCK_BIAS_ERR = slice(12, 13)
CLOCK_DRIFT_ERR = slice(13, 14)
GYRO_BIAS_ERR = slice(14, 17)
- ODO_SCALE_ERR = slice(17, 18)
+ ODO_SCALE_ERR_UNUSED = slice(17, 18)
ACCELERATION_ERR = slice(18, 21)
- FOCAL_SCALE_ERR = slice(21, 22)
+ FOCAL_SCALE_ERR_UNUSED = slice(21, 22)
IMU_OFFSET_ERR = slice(22, 25)
GLONASS_BIAS_ERR = slice(25, 26)
GLONASS_FREQ_SLOPE_ERR = slice(26, 27)
CLOCK_ACCELERATION_ERR = slice(27, 28)
- ACCELEROMETER_SCALE_ERR = slice(28, 29)
+ ACCELEROMETER_SCALE_ERR_UNUSED = slice(28, 29)
+ ACCELEROMETER_BIAS_ERR = slice(29, 32)
class LocKalman():
@@ -82,7 +86,8 @@ class LocKalman():
0, 0, 0,
0, 0,
0,
- 1], dtype=np.float64)
+ 1,
+ 0, 0, 0], dtype=np.float64)
# state covariance
P_initial = np.diag([1e16, 1e16, 1e16,
@@ -97,7 +102,8 @@ class LocKalman():
(0.01)**2, (0.01)**2, (0.01)**2,
10**2, 1**2,
0.2**2,
- 0.05**2])
+ 0.05**2,
+ 0.05**2, 0.05**2, 0.05**2])
# process noise
Q = np.diag([0.03**2, 0.03**2, 0.03**2,
@@ -112,7 +118,8 @@ class LocKalman():
(0.05 / 60)**2, (0.05 / 60)**2, (0.05 / 60)**2,
(.1)**2, (.01)**2,
0.005**2,
- (0.02 / 100)**2])
+ (0.02 / 100)**2,
+ (0.005 / 100)**2, (0.005 / 100)**2, (0.005 / 100)**2])
# measurements that need to pass mahalanobis distance outlier rejector
maha_test_kinds = [ObservationKind.ORB_FEATURES] # , ObservationKind.PSEUDORANGE, ObservationKind.PSEUDORANGE_RATE]
@@ -145,16 +152,14 @@ class LocKalman():
cb = state[States.CLOCK_BIAS, :]
cd = state[States.CLOCK_DRIFT, :]
roll_bias, pitch_bias, yaw_bias = state[States.GYRO_BIAS, :]
- odo_scale = state[States.ODO_SCALE, :]
acceleration = state[States.ACCELERATION, :]
- focal_scale = state[States.FOCAL_SCALE, :]
imu_angles = state[States.IMU_OFFSET, :]
imu_angles[0, 0] = 0
imu_angles[2, 0] = 0
glonass_bias = state[States.GLONASS_BIAS, :]
glonass_freq_slope = state[States.GLONASS_FREQ_SLOPE, :]
ca = state[States.CLOCK_ACCELERATION, :]
- accel_scale = state[States.ACCELEROMETER_SCALE, :]
+ accel_bias = state[States.ACCELEROMETER_BIAS, :]
dt = sp.Symbol('dt')
@@ -251,13 +256,10 @@ class LocKalman():
sat_pos_freq_sym = sp.MatrixSymbol('sat_pos', 4, 1)
sat_pos_vel_sym = sp.MatrixSymbol('sat_pos_vel', 6, 1)
# sat_los_sym = sp.MatrixSymbol('sat_los', 3, 1)
- orb_epos_sym = sp.MatrixSymbol('orb_epos_sym', 3, 1)
# expand extra args
sat_x, sat_y, sat_z, glonass_freq = sat_pos_freq_sym
sat_vx, sat_vy, sat_vz = sat_pos_vel_sym[3:]
- # los_x, los_y, los_z = sat_los_sym
- orb_x, orb_y, orb_z = orb_epos_sym
h_pseudorange_sym = sp.Matrix([
sp.sqrt(
@@ -290,37 +292,21 @@ class LocKalman():
pos = sp.Matrix([x, y, z])
# add 1 for stability, prevent division by 0
gravity = quat_rot.T * ((EARTH_GM / ((x**2 + y**2 + z**2 + 1)**(3.0 / 2.0))) * pos)
- h_acc_sym = imu_rot * (accel_scale[0] * (gravity + acceleration))
+ h_acc_sym = imu_rot * (gravity + acceleration + accel_bias)
+ h_acc_stationary_sym = acceleration
h_phone_rot_sym = sp.Matrix([vroll, vpitch, vyaw])
-
- speed = sp.sqrt(vx**2 + vy**2 + vz**2)
- h_speed_sym = sp.Matrix([speed * odo_scale])
-
- # orb stuff
- orb_pos_sym = sp.Matrix([orb_x - x, orb_y - y, orb_z - z])
- orb_pos_rot_sym = quat_rot.T * orb_pos_sym
- s = orb_pos_rot_sym[0]
- h_orb_point_sym = sp.Matrix([(1 / s) * (orb_pos_rot_sym[1]),
- (1 / s) * (orb_pos_rot_sym[2])])
-
- h_pos_sym = sp.Matrix([x, y, z])
- h_imu_frame_sym = sp.Matrix(imu_angles)
-
h_relative_motion = sp.Matrix(quat_rot.T * v)
- obs_eqs = [[h_speed_sym, ObservationKind.ODOMETRIC_SPEED, None],
- [h_gyro_sym, ObservationKind.PHONE_GYRO, None],
+ obs_eqs = [[h_gyro_sym, ObservationKind.PHONE_GYRO, None],
[h_phone_rot_sym, ObservationKind.NO_ROT, None],
[h_acc_sym, ObservationKind.PHONE_ACCEL, None],
[h_pseudorange_sym, ObservationKind.PSEUDORANGE_GPS, sat_pos_freq_sym],
[h_pseudorange_glonass_sym, ObservationKind.PSEUDORANGE_GLONASS, sat_pos_freq_sym],
[h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GPS, sat_pos_vel_sym],
[h_pseudorange_rate_sym, ObservationKind.PSEUDORANGE_RATE_GLONASS, sat_pos_vel_sym],
- [h_pos_sym, ObservationKind.ECEF_POS, None],
[h_relative_motion, ObservationKind.CAMERA_ODO_TRANSLATION, None],
[h_phone_rot_sym, ObservationKind.CAMERA_ODO_ROTATION, None],
- [h_imu_frame_sym, ObservationKind.IMU_FRAME, None],
- [h_orb_point_sym, ObservationKind.ORB_POINT, orb_epos_sym]]
+ [h_acc_stationary_sym, ObservationKind.NO_ACCEL, None]]
# MSCKF configuration
if N > 0:
@@ -368,7 +354,8 @@ class LocKalman():
ObservationKind.CAMERA_ODO_ROTATION: np.diag([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.IMU_FRAME: np.diag([0.05**2, 0.05**2, 0.05**2]),
ObservationKind.NO_ROT: np.diag([0.0025**2, 0.0025**2, 0.0025**2]),
- ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2])}
+ ObservationKind.ECEF_POS: np.diag([5**2, 5**2, 5**2]),
+ ObservationKind.NO_ACCEL: np.diag([0.0025**2, 0.0025**2, 0.0025**2])}
# MSCKF stuff
self.N = N
@@ -382,7 +369,7 @@ class LocKalman():
self.computer = LstSqComputer(generated_dir, N)
self.max_tracks = max_tracks
- self.quaternion_idxs = [3,] + [(self.dim_main + i*self.dim_augment + 3)for i in range(self.N)]
+ self.quaternion_idxs = [3, ] + [(self.dim_main + i * self.dim_augment + 3)for i in range(self.N)]
# init filter
self.filter = EKF_sym(generated_dir, name, Q, x_initial, P_initial, self.dim_main, self.dim_main_err,
@@ -440,14 +427,10 @@ class LocKalman():
r = self.predict_and_update_pseudorange(data, t, kind)
elif kind == ObservationKind.PSEUDORANGE_RATE_GPS or kind == ObservationKind.PSEUDORANGE_RATE_GLONASS:
r = self.predict_and_update_pseudorange_rate(data, t, kind)
- elif kind == ObservationKind.ORB_POINT:
- r = self.predict_and_update_orb(data, t, kind)
elif kind == ObservationKind.ORB_FEATURES:
r = self.predict_and_update_orb_features(data, t, kind)
elif kind == ObservationKind.MSCKF_TEST:
r = self.predict_and_update_msckf_test(data, t, kind)
- elif kind == ObservationKind.ODOMETRIC_SPEED:
- r = self.predict_and_update_odo_speed(data, t, kind)
else:
r = self.filter.predict_and_update_batch(t, kind, data, self.get_R(kind, len(data)))
# Normalize quats
@@ -487,21 +470,6 @@ class LocKalman():
z[i, :] = z_i
return self.filter.predict_and_update_batch(t, kind, z, R, sat_pos_vel)
- def predict_and_update_orb(self, orb, t, kind):
- true_pos = orb[:, 2:]
- z = orb[:, :2]
- R = np.zeros((len(orb), 2, 2))
- for i, _ in enumerate(z):
- R[i, :, :] = np.diag([10**2, 10**2])
- return self.filter.predict_and_update_batch(t, kind, z, R, true_pos)
-
- def predict_and_update_odo_speed(self, speed, t, kind):
- z = np.array(speed)
- R = np.zeros((len(speed), 1, 1))
- for i, _ in enumerate(z):
- R[i, :, :] = np.diag([0.2**2])
- return self.filter.predict_and_update_batch(t, kind, z, R)
-
def predict_and_update_odo_trans(self, trans, t, kind):
z = trans[:, :3]
R = np.zeros((len(trans), 3, 3))
diff --git a/selfdrive/loggerd/SConscript b/selfdrive/loggerd/SConscript
index 7e41c9d3ee..2adcfb846c 100644
--- a/selfdrive/loggerd/SConscript
+++ b/selfdrive/loggerd/SConscript
@@ -24,8 +24,8 @@ if arch == "Darwin":
del libs[libs.index('OpenCL')]
env['FRAMEWORKS'] = ['OpenCL']
-env.Program(src, LIBS=libs)
+env.Program('loggerd', ['main.cc'] + src, LIBS=libs)
env.Program('bootlog.cc', LIBS=libs)
if GetOption('test'):
- env.Program('tests/test_logger', ['tests/test_runner.cc', 'tests/test_logger.cc', env.Object('logger_util', '#/selfdrive/ui/replay/util.cc')], LIBS=[libs] + ['curl', 'crypto'])
+ env.Program('tests/test_logger', ['tests/test_runner.cc', 'tests/test_loggerd.cc', 'tests/test_logger.cc', env.Object('logger_util', '#/selfdrive/ui/replay/util.cc')] + src, LIBS=[libs] + ['curl', 'crypto', 'bz2'])
diff --git a/selfdrive/loggerd/bootlog.cc b/selfdrive/loggerd/bootlog.cc
index 481b3ee47c..5209958377 100644
--- a/selfdrive/loggerd/bootlog.cc
+++ b/selfdrive/loggerd/bootlog.cc
@@ -56,6 +56,7 @@ static kj::Array build_boot_log() {
}
int main(int argc, char** argv) {
+ clear_locks(LOG_ROOT);
const std::string path = LOG_ROOT + "/boot/" + logger_get_route_name() + ".bz2";
LOGW("bootlog to %s", path.c_str());
diff --git a/selfdrive/loggerd/logger.cc b/selfdrive/loggerd/logger.cc
index a73fefb8c9..81cfd131f6 100644
--- a/selfdrive/loggerd/logger.cc
+++ b/selfdrive/loggerd/logger.cc
@@ -2,6 +2,7 @@
#include
#include
+#include
#include
#include
@@ -266,3 +267,15 @@ void lh_close(LoggerHandle* h) {
}
pthread_mutex_unlock(&h->lock);
}
+
+int clear_locks_fn(const char* fpath, const struct stat *sb, int tyupeflag) {
+ const char* dot = strrchr(fpath, '.');
+ if (dot && strcmp(dot, ".lock") == 0) {
+ unlink(fpath);
+ }
+ return 0;
+}
+
+void clear_locks(const std::string log_root) {
+ ftw(log_root.c_str(), clear_locks_fn, 16);
+}
diff --git a/selfdrive/loggerd/logger.h b/selfdrive/loggerd/logger.h
index bdda9d6917..e85d7810e0 100644
--- a/selfdrive/loggerd/logger.h
+++ b/selfdrive/loggerd/logger.h
@@ -96,3 +96,4 @@ void logger_log(LoggerState *s, uint8_t* data, size_t data_size, bool in_qlog);
void lh_log(LoggerHandle* h, uint8_t* data, size_t data_size, bool in_qlog);
void lh_close(LoggerHandle* h);
+void clear_locks(const std::string log_root);
diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc
index f6d89b8091..e3444f3315 100644
--- a/selfdrive/loggerd/loggerd.cc
+++ b/selfdrive/loggerd/loggerd.cc
@@ -1,152 +1,43 @@
-#include
-#include
-#include
-#include
-
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-
-#include "cereal/messaging/messaging.h"
-#include "cereal/services.h"
-#include "cereal/visionipc/visionipc.h"
-#include "cereal/visionipc/visionipc_client.h"
-#include "selfdrive/camerad/cameras/camera_common.h"
-#include "selfdrive/common/params.h"
-#include "selfdrive/common/swaglog.h"
-#include "selfdrive/common/timing.h"
-#include "selfdrive/common/util.h"
-#include "selfdrive/hardware/hw.h"
-
-#include "selfdrive/loggerd/encoder.h"
-#include "selfdrive/loggerd/logger.h"
-#if defined(QCOM) || defined(QCOM2)
-#include "selfdrive/loggerd/omx_encoder.h"
-#define Encoder OmxEncoder
-#else
-#include "selfdrive/loggerd/raw_logger.h"
-#define Encoder RawLogger
-#endif
-
-namespace {
-
-constexpr int MAIN_FPS = 20;
-const int MAIN_BITRATE = Hardware::TICI() ? 10000000 : 5000000;
-const int DCAM_BITRATE = Hardware::TICI() ? MAIN_BITRATE : 2500000;
-
-#define NO_CAMERA_PATIENCE 500 // fall back to time-based rotation if all cameras are dead
-
-const bool LOGGERD_TEST = getenv("LOGGERD_TEST");
-const int SEGMENT_LENGTH = LOGGERD_TEST ? atoi(getenv("LOGGERD_SEGMENT_LENGTH")) : 60;
+#include "selfdrive/loggerd/loggerd.h"
ExitHandler do_exit;
-const LogCameraInfo cameras_logged[] = {
- {
- .type = RoadCam,
- .stream_type = VISION_STREAM_YUV_BACK,
- .filename = "fcamera.hevc",
- .frame_packet_name = "roadCameraState",
- .fps = MAIN_FPS,
- .bitrate = MAIN_BITRATE,
- .is_h265 = true,
- .downscale = false,
- .has_qcamera = true,
- .trigger_rotate = true,
- .enable = true,
- .record = true,
- },
- {
- .type = DriverCam,
- .stream_type = VISION_STREAM_YUV_FRONT,
- .filename = "dcamera.hevc",
- .frame_packet_name = "driverCameraState",
- .fps = MAIN_FPS, // on EONs, more compressed this way
- .bitrate = DCAM_BITRATE,
- .is_h265 = true,
- .downscale = false,
- .has_qcamera = false,
- .trigger_rotate = Hardware::TICI(),
- .enable = !Hardware::PC(),
- .record = Params().getBool("RecordFront"),
- },
- {
- .type = WideRoadCam,
- .stream_type = VISION_STREAM_YUV_WIDE,
- .filename = "ecamera.hevc",
- .frame_packet_name = "wideRoadCameraState",
- .fps = MAIN_FPS,
- .bitrate = MAIN_BITRATE,
- .is_h265 = true,
- .downscale = false,
- .has_qcamera = false,
- .trigger_rotate = true,
- .enable = Hardware::TICI(),
- .record = Hardware::TICI(),
- },
-};
-const LogCameraInfo qcam_info = {
- .filename = "qcamera.ts",
- .fps = MAIN_FPS,
- .bitrate = 256000,
- .is_h265 = false,
- .downscale = true,
- .frame_width = Hardware::TICI() ? 526 : 480,
- .frame_height = Hardware::TICI() ? 330 : 360 // keep pixel count the same?
-};
-
-struct LoggerdState {
- Context *ctx;
- LoggerState logger = {};
- char segment_path[4096];
- std::mutex rotate_lock;
- std::condition_variable rotate_cv;
- std::atomic rotate_segment;
- std::atomic last_camera_seen_tms;
- std::atomic ready_to_rotate; // count of encoders ready to rotate
- int max_waiting = 0;
- double last_rotate_tms = 0.; // last rotate time in ms
-
- // Sync logic for startup
- std::atomic encoders_ready = 0;
- std::atomic start_frame_id = 0;
- bool camera_ready[WideRoadCam + 1] = {};
- bool camera_synced[WideRoadCam + 1] = {};
-};
-LoggerdState s;
-
// Handle initial encoder syncing by waiting for all encoders to reach the same frame id
-bool sync_encoders(LoggerdState *state, CameraType cam_type, uint32_t frame_id) {
- if (state->camera_synced[cam_type]) return true;
+bool sync_encoders(LoggerdState *s, CameraType cam_type, uint32_t frame_id) {
+ if (s->camera_synced[cam_type]) return true;
- if (state->max_waiting > 1 && state->encoders_ready != state->max_waiting) {
+ if (s->max_waiting > 1 && s->encoders_ready != s->max_waiting) {
// add a small margin to the start frame id in case one of the encoders already dropped the next frame
- update_max_atomic(state->start_frame_id, frame_id + 2);
- if (std::exchange(state->camera_ready[cam_type], true) == false) {
- ++state->encoders_ready;
+ update_max_atomic(s->start_frame_id, frame_id + 2);
+ if (std::exchange(s->camera_ready[cam_type], true) == false) {
+ ++s->encoders_ready;
LOGE("camera %d encoder ready", cam_type);
}
return false;
} else {
- if (state->max_waiting == 1) update_max_atomic(state->start_frame_id, frame_id);
- bool synced = frame_id >= state->start_frame_id;
- state->camera_synced[cam_type] = synced;
- if (!synced) LOGE("camera %d waiting for frame %d, cur %d", cam_type, (int)state->start_frame_id, frame_id);
+ if (s->max_waiting == 1) update_max_atomic(s->start_frame_id, frame_id);
+ bool synced = frame_id >= s->start_frame_id;
+ s->camera_synced[cam_type] = synced;
+ if (!synced) LOGE("camera %d waiting for frame %d, cur %d", cam_type, (int)s->start_frame_id, frame_id);
return synced;
}
}
-void encoder_thread(const LogCameraInfo &cam_info) {
+bool trigger_rotate_if_needed(LoggerdState *s, int cur_seg, uint32_t frame_id) {
+ const int frames_per_seg = SEGMENT_LENGTH * MAIN_FPS;
+ if (cur_seg >= 0 && frame_id >= ((cur_seg + 1) * frames_per_seg) + s->start_frame_id) {
+ // trigger rotate and wait until the main logger has rotated to the new segment
+ ++s->ready_to_rotate;
+ std::unique_lock lk(s->rotate_lock);
+ s->rotate_cv.wait(lk, [&] {
+ return s->rotate_segment > cur_seg || do_exit;
+ });
+ return !do_exit;
+ }
+ return false;
+}
+
+void encoder_thread(LoggerdState *s, const LogCameraInfo &cam_info) {
set_thread_name(cam_info.filename);
int cur_seg = -1;
@@ -183,37 +74,29 @@ void encoder_thread(const LogCameraInfo &cam_info) {
if (buf == nullptr) continue;
if (cam_info.trigger_rotate) {
- s.last_camera_seen_tms = millis_since_boot();
- if (!sync_encoders(&s, cam_info.type, extra.frame_id)) {
+ s->last_camera_seen_tms = millis_since_boot();
+ if (!sync_encoders(s, cam_info.type, extra.frame_id)) {
continue;
}
// check if we're ready to rotate
- const int frames_per_seg = SEGMENT_LENGTH * MAIN_FPS;
- if (cur_seg >= 0 && extra.frame_id >= ((cur_seg+1) * frames_per_seg) + s.start_frame_id) {
- // trigger rotate and wait until the main logger has rotated to the new segment
- ++s.ready_to_rotate;
- std::unique_lock lk(s.rotate_lock);
- s.rotate_cv.wait(lk, [&] {
- return s.rotate_segment > cur_seg || do_exit;
- });
- if (do_exit) break;
- }
+ trigger_rotate_if_needed(s, cur_seg, extra.frame_id);
+ if (do_exit) break;
}
// rotate the encoder if the logger is on a newer segment
- if (s.rotate_segment > cur_seg) {
- cur_seg = s.rotate_segment;
+ if (s->rotate_segment > cur_seg) {
+ cur_seg = s->rotate_segment;
- LOGW("camera %d rotate encoder to %s", cam_info.type, s.segment_path);
+ LOGW("camera %d rotate encoder to %s", cam_info.type, s->segment_path);
for (auto &e : encoders) {
e->encoder_close();
- e->encoder_open(s.segment_path);
+ e->encoder_open(s->segment_path);
}
if (lh) {
lh_close(lh);
}
- lh = logger_get_handle(&s.logger);
+ lh = logger_get_handle(&s->logger);
}
// encode a frame
@@ -266,68 +149,42 @@ void encoder_thread(const LogCameraInfo &cam_info) {
}
}
-int clear_locks_fn(const char* fpath, const struct stat *sb, int tyupeflag) {
- const char* dot = strrchr(fpath, '.');
- if (dot && strcmp(dot, ".lock") == 0) {
- unlink(fpath);
- }
- return 0;
-}
-
-void clear_locks() {
- ftw(LOG_ROOT.c_str(), clear_locks_fn, 16);
-}
-
-void logger_rotate() {
+void logger_rotate(LoggerdState *s) {
{
- std::unique_lock lk(s.rotate_lock);
+ std::unique_lock lk(s->rotate_lock);
int segment = -1;
- int err = logger_next(&s.logger, LOG_ROOT.c_str(), s.segment_path, sizeof(s.segment_path), &segment);
+ int err = logger_next(&s->logger, LOG_ROOT.c_str(), s->segment_path, sizeof(s->segment_path), &segment);
assert(err == 0);
- s.rotate_segment = segment;
- s.ready_to_rotate = 0;
- s.last_rotate_tms = millis_since_boot();
+ s->rotate_segment = segment;
+ s->ready_to_rotate = 0;
+ s->last_rotate_tms = millis_since_boot();
}
- s.rotate_cv.notify_all();
- LOGW((s.logger.part == 0) ? "logging to %s" : "rotated to %s", s.segment_path);
+ s->rotate_cv.notify_all();
+ LOGW((s->logger.part == 0) ? "logging to %s" : "rotated to %s", s->segment_path);
}
-void rotate_if_needed() {
- if (s.ready_to_rotate == s.max_waiting) {
- logger_rotate();
+void rotate_if_needed(LoggerdState *s) {
+ if (s->ready_to_rotate == s->max_waiting) {
+ logger_rotate(s);
}
double tms = millis_since_boot();
- if ((tms - s.last_rotate_tms) > SEGMENT_LENGTH * 1000 &&
- (tms - s.last_camera_seen_tms) > NO_CAMERA_PATIENCE &&
+ if ((tms - s->last_rotate_tms) > SEGMENT_LENGTH * 1000 &&
+ (tms - s->last_camera_seen_tms) > NO_CAMERA_PATIENCE &&
!LOGGERD_TEST) {
LOGW("no camera packet seen. auto rotating");
- logger_rotate();
+ logger_rotate(s);
}
}
-} // namespace
-
-int main(int argc, char** argv) {
- if (Hardware::EON()) {
- setpriority(PRIO_PROCESS, 0, -20);
- } else if (Hardware::TICI()) {
- int ret;
- ret = set_core_affinity({0, 1, 2, 3});
- assert(ret == 0);
- // TODO: why does this impact camerad timings?
- //ret = set_realtime_priority(1);
- //assert(ret == 0);
- }
-
- clear_locks();
-
+void loggerd_thread() {
// setup messaging
typedef struct QlogState {
int counter, freq;
} QlogState;
std::unordered_map qlog_states;
+ LoggerdState s;
s.ctx = Context::create();
Poller * poller = Poller::create();
@@ -343,7 +200,7 @@ int main(int argc, char** argv) {
// init logger
logger_init(&s.logger, "rlog", true);
- logger_rotate();
+ logger_rotate(&s);
Params().put("CurrentRoute", s.logger.route_name);
// init encoders
@@ -351,7 +208,7 @@ int main(int argc, char** argv) {
std::vector encoder_threads;
for (const auto &cam : cameras_logged) {
if (cam.enable) {
- encoder_threads.push_back(std::thread(encoder_thread, cam));
+ encoder_threads.push_back(std::thread(encoder_thread, &s, cam));
if (cam.trigger_rotate) s.max_waiting++;
}
}
@@ -370,7 +227,7 @@ int main(int argc, char** argv) {
bytes_count += msg->getSize();
delete msg;
- rotate_if_needed();
+ rotate_if_needed(&s);
if ((++msg_count % 1000) == 0) {
double seconds = (millis_since_boot() - start_ts) / 1000.0;
@@ -397,6 +254,4 @@ int main(int argc, char** argv) {
for (auto &[sock, qs] : qlog_states) delete sock;
delete poller;
delete s.ctx;
-
- return 0;
}
diff --git a/selfdrive/loggerd/loggerd.h b/selfdrive/loggerd/loggerd.h
new file mode 100644
index 0000000000..977e03b8d3
--- /dev/null
+++ b/selfdrive/loggerd/loggerd.h
@@ -0,0 +1,120 @@
+#pragma once
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "cereal/messaging/messaging.h"
+#include "cereal/services.h"
+#include "cereal/visionipc/visionipc.h"
+#include "cereal/visionipc/visionipc_client.h"
+#include "selfdrive/camerad/cameras/camera_common.h"
+#include "selfdrive/common/params.h"
+#include "selfdrive/common/swaglog.h"
+#include "selfdrive/common/timing.h"
+#include "selfdrive/common/util.h"
+#include "selfdrive/hardware/hw.h"
+
+#include "selfdrive/loggerd/encoder.h"
+#include "selfdrive/loggerd/logger.h"
+#if defined(QCOM) || defined(QCOM2)
+#include "selfdrive/loggerd/omx_encoder.h"
+#define Encoder OmxEncoder
+#else
+#include "selfdrive/loggerd/raw_logger.h"
+#define Encoder RawLogger
+#endif
+
+constexpr int MAIN_FPS = 20;
+const int MAIN_BITRATE = Hardware::TICI() ? 10000000 : 5000000;
+const int DCAM_BITRATE = Hardware::TICI() ? MAIN_BITRATE : 2500000;
+
+#define NO_CAMERA_PATIENCE 500 // fall back to time-based rotation if all cameras are dead
+
+const bool LOGGERD_TEST = getenv("LOGGERD_TEST");
+const int SEGMENT_LENGTH = LOGGERD_TEST ? atoi(getenv("LOGGERD_SEGMENT_LENGTH")) : 60;
+
+const LogCameraInfo cameras_logged[] = {
+ {
+ .type = RoadCam,
+ .stream_type = VISION_STREAM_ROAD,
+ .filename = "fcamera.hevc",
+ .frame_packet_name = "roadCameraState",
+ .fps = MAIN_FPS,
+ .bitrate = MAIN_BITRATE,
+ .is_h265 = true,
+ .downscale = false,
+ .has_qcamera = true,
+ .trigger_rotate = true,
+ .enable = true,
+ .record = true,
+ },
+ {
+ .type = DriverCam,
+ .stream_type = VISION_STREAM_DRIVER,
+ .filename = "dcamera.hevc",
+ .frame_packet_name = "driverCameraState",
+ .fps = MAIN_FPS, // on EONs, more compressed this way
+ .bitrate = DCAM_BITRATE,
+ .is_h265 = true,
+ .downscale = false,
+ .has_qcamera = false,
+ .trigger_rotate = Hardware::TICI(),
+ .enable = !Hardware::PC(),
+ .record = Params().getBool("RecordFront"),
+ },
+ {
+ .type = WideRoadCam,
+ .stream_type = VISION_STREAM_WIDE_ROAD,
+ .filename = "ecamera.hevc",
+ .frame_packet_name = "wideRoadCameraState",
+ .fps = MAIN_FPS,
+ .bitrate = MAIN_BITRATE,
+ .is_h265 = true,
+ .downscale = false,
+ .has_qcamera = false,
+ .trigger_rotate = true,
+ .enable = Hardware::TICI(),
+ .record = Hardware::TICI(),
+ },
+};
+const LogCameraInfo qcam_info = {
+ .filename = "qcamera.ts",
+ .fps = MAIN_FPS,
+ .bitrate = 256000,
+ .is_h265 = false,
+ .downscale = true,
+ .frame_width = Hardware::TICI() ? 526 : 480,
+ .frame_height = Hardware::TICI() ? 330 : 360 // keep pixel count the same?
+};
+
+struct LoggerdState {
+ Context *ctx;
+ LoggerState logger = {};
+ char segment_path[4096];
+ std::mutex rotate_lock;
+ std::condition_variable rotate_cv;
+ std::atomic rotate_segment;
+ std::atomic last_camera_seen_tms;
+ std::atomic ready_to_rotate; // count of encoders ready to rotate
+ int max_waiting = 0;
+ double last_rotate_tms = 0.; // last rotate time in ms
+
+ // Sync logic for startup
+ std::atomic encoders_ready = 0;
+ std::atomic start_frame_id = 0;
+ bool camera_ready[WideRoadCam + 1] = {};
+ bool camera_synced[WideRoadCam + 1] = {};
+};
+
+bool sync_encoders(LoggerdState *s, CameraType cam_type, uint32_t frame_id);
+bool trigger_rotate_if_needed(LoggerdState *s, int cur_seg, uint32_t frame_id);
+void rotate_if_needed(LoggerdState *s);
+void loggerd_thread();
diff --git a/selfdrive/loggerd/main.cc b/selfdrive/loggerd/main.cc
new file mode 100644
index 0000000000..04da0f67e8
--- /dev/null
+++ b/selfdrive/loggerd/main.cc
@@ -0,0 +1,20 @@
+#include "selfdrive/loggerd/loggerd.h"
+
+#include
+
+int main(int argc, char** argv) {
+ if (Hardware::EON()) {
+ setpriority(PRIO_PROCESS, 0, -20);
+ } else if (Hardware::TICI()) {
+ int ret;
+ ret = set_core_affinity({0, 1, 2, 3});
+ assert(ret == 0);
+ // TODO: why does this impact camerad timings?
+ //ret = set_realtime_priority(1);
+ //assert(ret == 0);
+ }
+
+ loggerd_thread();
+
+ return 0;
+}
diff --git a/selfdrive/loggerd/tests/test_loggerd.cc b/selfdrive/loggerd/tests/test_loggerd.cc
new file mode 100644
index 0000000000..849b350ee4
--- /dev/null
+++ b/selfdrive/loggerd/tests/test_loggerd.cc
@@ -0,0 +1,111 @@
+#include
+
+#include "catch2/catch.hpp"
+#include "selfdrive/loggerd/loggerd.h"
+
+int random_int(int min, int max) {
+ std::random_device dev;
+ std::mt19937 rng(dev());
+ std::uniform_int_distribution dist(min, max);
+ return dist(rng);
+}
+
+int get_synced_frame_id(LoggerdState *s, CameraType cam_type, int start_frame_id) {
+ int frame_id = start_frame_id;
+ while (!sync_encoders(s, cam_type, frame_id)) {
+ ++frame_id;
+ usleep(0);
+ }
+ return frame_id;
+};
+
+TEST_CASE("sync_encoders") {
+ const int max_waiting = GENERATE(1, 2, 3);
+
+ for (int test_cnt = 0; test_cnt < 10; ++test_cnt) {
+ LoggerdState s{.max_waiting = max_waiting};
+ std::vector start_frames;
+ std::vector> futures;
+
+ for (int i = 0; i < max_waiting; ++i) {
+ int start_frame_id = random_int(0, 20);
+ start_frames.push_back(start_frame_id);
+ futures.emplace_back(std::async(std::launch::async, get_synced_frame_id, &s, (CameraType)i, start_frame_id));
+ }
+
+ // get results
+ int synced_frame_id = 0;
+ for (int i = 0; i < max_waiting; ++i) {
+ if (i == 0) {
+ synced_frame_id = futures[i].get();
+ // require synced_frame_id equal start_frame_id if max_waiting == 1
+ if (max_waiting == 1) {
+ REQUIRE(synced_frame_id == start_frames[0]);
+ }
+ } else {
+ REQUIRE(futures[i].get() == synced_frame_id);
+ }
+ }
+ }
+}
+
+const int MAX_SEGMENT_CNT = 100;
+
+std::pair encoder_thread(LoggerdState *s) {
+ int cur_seg = 0;
+ uint32_t frame_id = s->start_frame_id;
+
+ while (cur_seg < MAX_SEGMENT_CNT) {
+ ++frame_id;
+ if (trigger_rotate_if_needed(s, cur_seg, frame_id)) {
+ cur_seg = s->rotate_segment;
+ }
+ util::sleep_for(0);
+ }
+
+ return {cur_seg, frame_id};
+}
+
+TEST_CASE("trigger_rotate") {
+ const int encoders = GENERATE(1, 2, 3);
+ const int start_frame_id = random_int(0, 20);
+
+ LoggerdState s{
+ .max_waiting = encoders,
+ .start_frame_id = start_frame_id,
+ };
+
+ std::vector>> futures;
+ for (int i = 0; i < encoders; ++i) {
+ futures.emplace_back(std::async(std::launch::async, encoder_thread, &s));
+ }
+
+ while (s.rotate_segment < MAX_SEGMENT_CNT) {
+ rotate_if_needed(&s);
+ util::sleep_for(10);
+ }
+
+ for (auto &f : futures) {
+ auto [encoder_seg, frame_id] = f.get();
+ REQUIRE(encoder_seg == MAX_SEGMENT_CNT);
+ REQUIRE(frame_id == start_frame_id + encoder_seg * (SEGMENT_LENGTH * MAIN_FPS));
+ }
+}
+
+TEST_CASE("clear_locks") {
+ std::vector dirs;
+ for (int i = 0; i < 10; ++i) {
+ std::string &path = dirs.emplace_back(LOG_ROOT + "/" + std::to_string(i));
+ REQUIRE(util::create_directories(path, 0775));
+ std::ofstream{path + "/.lock"};
+ REQUIRE(util::file_exists(path + "/.lock"));
+ }
+
+ clear_locks(LOG_ROOT);
+
+ for (const auto &dir : dirs) {
+ std::string lock_file = dir + "/.lock";
+ REQUIRE(util::file_exists(lock_file) == false);
+ rmdir(dir.c_str());
+ }
+}
diff --git a/selfdrive/loggerd/tests/test_loggerd.py b/selfdrive/loggerd/tests/test_loggerd.py
index 7ee97f5118..021eea5c83 100755
--- a/selfdrive/loggerd/tests/test_loggerd.py
+++ b/selfdrive/loggerd/tests/test_loggerd.py
@@ -18,7 +18,7 @@ from selfdrive.hardware import PC, TICI
from selfdrive.loggerd.config import ROOT
from selfdrive.manager.process_config import managed_processes
from selfdrive.test.helpers import with_processes
-from selfdrive.version import version as VERSION
+from selfdrive.version import get_version
from tools.lib.logreader import LogReader
SentinelType = log.Sentinel.SentinelType
@@ -95,7 +95,7 @@ class TestLoggerd(unittest.TestCase):
initData = lr[0].initData
self.assertTrue(initData.dirty != bool(os.environ["CLEAN"]))
- self.assertEqual(initData.version, VERSION)
+ self.assertEqual(initData.version, get_version())
if os.path.isfile("/proc/cmdline"):
with open("/proc/cmdline") as f:
diff --git a/selfdrive/manager/build.py b/selfdrive/manager/build.py
index 66f836ce6b..d6429092ea 100755
--- a/selfdrive/manager/build.py
+++ b/selfdrive/manager/build.py
@@ -12,9 +12,9 @@ from common.spinner import Spinner
from common.text_window import TextWindow
from selfdrive.hardware import TICI
from selfdrive.swaglog import cloudlog, add_file_handler
-from selfdrive.version import dirty
+from selfdrive.version import get_dirty
-MAX_CACHE_SIZE = 2e9
+MAX_CACHE_SIZE = 4e9 if "CI" in os.environ else 2e9
CACHE_DIR = Path("/data/scons_cache" if TICI else "/tmp/scons_cache")
TOTAL_SCONS_NODES = 2405
@@ -69,7 +69,7 @@ def build(spinner, dirty=False):
else:
# Build failed log errors
errors = [line.decode('utf8', 'replace') for line in compile_output
- if any([err in line for err in [b'error: ', b'not found, needed by target']])]
+ if any(err in line for err in [b'error: ', b'not found, needed by target'])]
error_s = "\n".join(errors)
add_file_handler(cloudlog)
cloudlog.error("scons build failed\n" + error_s)
@@ -98,4 +98,4 @@ def build(spinner, dirty=False):
if __name__ == "__main__" and not PREBUILT:
spinner = Spinner()
spinner.update_progress(0, 100)
- build(spinner, dirty)
+ build(spinner, get_dirty())
diff --git a/selfdrive/manager/manager.py b/selfdrive/manager/manager.py
index 6c71c2a377..d0d32874b0 100755
--- a/selfdrive/manager/manager.py
+++ b/selfdrive/manager/manager.py
@@ -18,17 +18,20 @@ from selfdrive.manager.process import ensure_running
from selfdrive.manager.process_config import managed_processes
from selfdrive.athena.registration import register, UNREGISTERED_DONGLE_ID
from selfdrive.swaglog import cloudlog, add_file_handler
-from selfdrive.version import dirty, get_git_commit, version, origin, branch, commit, \
- terms_version, training_version, comma_remote, \
- get_git_branch, get_git_remote
+from selfdrive.version import get_dirty, get_commit, get_version, get_origin, get_short_branch, \
+ terms_version, training_version, get_comma_remote
+
sys.path.append(os.path.join(BASEDIR, "pyextra"))
-def manager_init():
+def manager_init():
# update system time from panda
set_time(cloudlog)
+ # save boot log
+ subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd"))
+
params = Params()
params.clear_all(ParamKeyType.CLEAR_ON_MANAGER_START)
@@ -67,12 +70,12 @@ def manager_init():
print("WARNING: failed to make /dev/shm")
# set version params
- params.put("Version", version)
+ params.put("Version", get_version())
params.put("TermsVersion", terms_version)
params.put("TrainingVersion", training_version)
- params.put("GitCommit", get_git_commit(default=""))
- params.put("GitBranch", get_git_branch(default=""))
- params.put("GitRemote", get_git_remote(default=""))
+ params.put("GitCommit", get_commit(default=""))
+ params.put("GitBranch", get_short_branch(default=""))
+ params.put("GitRemote", get_origin(default=""))
# set dongle id
reg_res = register(show_spinner=True)
@@ -83,16 +86,16 @@ def manager_init():
raise Exception(f"Registration failed for device {serial}")
os.environ['DONGLE_ID'] = dongle_id # Needed for swaglog
- if not dirty:
+ if not get_dirty():
os.environ['CLEAN'] = '1'
- cloudlog.bind_global(dongle_id=dongle_id, version=version, dirty=dirty,
+ cloudlog.bind_global(dongle_id=dongle_id, version=get_version(), dirty=get_dirty(),
device=HARDWARE.get_device_type())
- if comma_remote and not (os.getenv("NOLOG") or os.getenv("NOCRASH") or PC):
+ if get_comma_remote() and not (os.getenv("NOLOG") or os.getenv("NOCRASH") or PC):
crash.init()
crash.bind_user(id=dongle_id)
- crash.bind_extra(dirty=dirty, origin=origin, branch=branch, commit=commit,
+ crash.bind_extra(dirty=get_dirty(), origin=get_origin(), branch=get_short_branch(), commit=get_commit(),
device=HARDWARE.get_device_type())
@@ -102,8 +105,13 @@ def manager_prepare():
def manager_cleanup():
+ # send signals to kill all procs
for p in managed_processes.values():
- p.stop()
+ p.stop(block=False)
+
+ # ensure all are killed
+ for p in managed_processes.values():
+ p.stop(block=True)
cloudlog.info("everything is dead")
@@ -112,9 +120,6 @@ def manager_thread():
cloudlog.info("manager start")
cloudlog.info({"environ": os.environ})
- # save boot log
- subprocess.call("./bootlog", cwd=os.path.join(BASEDIR, "selfdrive/loggerd"))
-
params = Params()
ignore = []
diff --git a/selfdrive/modeld/dmonitoringmodeld.cc b/selfdrive/modeld/dmonitoringmodeld.cc
index 6ae6e780c8..c85c05c9d1 100644
--- a/selfdrive/modeld/dmonitoringmodeld.cc
+++ b/selfdrive/modeld/dmonitoringmodeld.cc
@@ -39,7 +39,7 @@ int main(int argc, char **argv) {
DMonitoringModelState model;
dmonitoring_init(&model);
- VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_YUV_FRONT, true);
+ VisionIpcClient vipc_client = VisionIpcClient("camerad", VISION_STREAM_DRIVER, true);
while (!do_exit && !vipc_client.connect(false)) {
util::sleep_for(100);
}
diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc
index 3d3e435909..9013e63617 100644
--- a/selfdrive/modeld/modeld.cc
+++ b/selfdrive/modeld/modeld.cc
@@ -153,7 +153,7 @@ int main(int argc, char **argv) {
model_init(&model, device_id, context);
LOGW("models loaded, modeld starting");
- VisionIpcClient vipc_client = VisionIpcClient("camerad", wide_camera ? VISION_STREAM_YUV_WIDE : VISION_STREAM_YUV_BACK, true, device_id, context);
+ VisionIpcClient vipc_client = VisionIpcClient("camerad", wide_camera ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD, true, device_id, context);
while (!do_exit && !vipc_client.connect(false)) {
util::sleep_for(100);
}
diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py
index ac19d27540..cefc2166ec 100755
--- a/selfdrive/test/longitudinal_maneuvers/plant.py
+++ b/selfdrive/test/longitudinal_maneuvers/plant.py
@@ -42,8 +42,7 @@ class Plant():
from selfdrive.car.honda.values import CAR
from selfdrive.car.honda.interface import CarInterface
- self.CP = CarInterface.get_params(CAR.CIVIC)
- self.planner = Planner(self.CP, init_v=self.speed)
+ self.planner = Planner(CarInterface.get_params(CAR.CIVIC), init_v=self.speed)
def current_time(self):
return float(self.rk.frame) / self.rate
@@ -97,7 +96,7 @@ class Plant():
sm = {'radarState': radar.radarState,
'carState': car_state.carState,
'controlsState': control.controlsState}
- self.planner.update(sm, self.CP)
+ self.planner.update(sm)
self.speed = self.planner.v_desired
self.acceleration = self.planner.a_desired
fcw = self.planner.fcw
diff --git a/selfdrive/test/model_test.py b/selfdrive/test/model_test.py
deleted file mode 100755
index b8a4ac0a86..0000000000
--- a/selfdrive/test/model_test.py
+++ /dev/null
@@ -1,30 +0,0 @@
-#!/usr/bin/env python3
-import os
-import numpy as np
-
-from tools.lib.logreader import LogReader
-from tools.lib.framereader import FrameReader
-from tools.lib.cache import cache_path_for_file_path
-from selfdrive.test.process_replay.model_replay import model_replay
-
-
-if __name__ == "__main__":
- lr = LogReader(os.path.expanduser('~/rlog.bz2'))
- fr = FrameReader(os.path.expanduser('~/fcamera.hevc'))
- desire = np.load(os.path.expanduser('~/desire.npy'))
- calib = np.load(os.path.expanduser('~/calib.npy'))
-
- try:
- msgs = model_replay(list(lr), fr, desire=desire, calib=calib)
- finally:
- cache_path = cache_path_for_file_path(os.path.expanduser('~/fcamera.hevc'))
- if os.path.isfile(cache_path):
- os.remove(cache_path)
-
- output_size = len(np.frombuffer(msgs[0].model.rawPredictions, dtype=np.float32))
- output_data = np.zeros((len(msgs), output_size), dtype=np.float32)
- for i, msg in enumerate(msgs):
- output_data[i] = np.frombuffer(msg.model.rawPredictions, dtype=np.float32)
- np.save(os.path.expanduser('~/modeldata.npy'), output_data)
-
- print("Finished replay")
diff --git a/selfdrive/test/process_replay/model_replay.py b/selfdrive/test/process_replay/model_replay.py
index b3c52879ce..32a5717852 100755
--- a/selfdrive/test/process_replay/model_replay.py
+++ b/selfdrive/test/process_replay/model_replay.py
@@ -7,22 +7,31 @@ from typing import Any
from tqdm import tqdm
import cereal.messaging as messaging
-from cereal import log
from cereal.visionipc.visionipc_pyx import VisionIpcServer, VisionStreamType # pylint: disable=no-name-in-module, import-error
from common.spinner import Spinner
from common.timeout import Timeout
-from common.transformations.camera import get_view_frame_from_road_frame, eon_f_frame_size, tici_f_frame_size
-from selfdrive.hardware import PC
+from common.transformations.camera import get_view_frame_from_road_frame, eon_f_frame_size, tici_f_frame_size, \
+ eon_d_frame_size, tici_d_frame_size
+from selfdrive.hardware import PC, TICI
from selfdrive.manager.process_config import managed_processes
from selfdrive.test.openpilotci import BASE_URL, get_url
from selfdrive.test.process_replay.compare_logs import compare_logs, save_log
from selfdrive.test.process_replay.test_processes import format_diff
-from selfdrive.version import get_git_commit
+from selfdrive.version import get_commit
from tools.lib.framereader import FrameReader
from tools.lib.logreader import LogReader
-TEST_ROUTE = "99c94dc769b5d96e|2019-08-03--14-19-59"
+if TICI:
+ TEST_ROUTE = "4cf7a6ad03080c90|2021-09-29--13-46-36"
+else:
+ TEST_ROUTE = "303055c0002aefd1|2021-11-22--18-36-32"
+CACHE_DIR = os.getenv("CACHE_DIR", None)
+
+packet_from_camera = {"roadCameraState":"modelV2", "driverCameraState":"driverState"}
+
+def get_log_fn(ref_commit):
+ return "%s_%s_%s.bz2" % (TEST_ROUTE, "model_tici" if TICI else "model", ref_commit)
def replace_calib(msg, calib):
msg = msg.as_builder()
@@ -30,67 +39,78 @@ def replace_calib(msg, calib):
msg.liveCalibration.extrinsicMatrix = get_view_frame_from_road_frame(*calib, 1.22).flatten().tolist()
return msg
-
-def model_replay(lr, fr, desire=None, calib=None):
-
+def process_frame(msg, pm, sm, log_msgs, vipc_server, spinner, frs, frame_idxs, last_desire):
+ if msg.which() == "roadCameraState" and last_desire is not None:
+ dat = messaging.new_message('lateralPlan')
+ dat.lateralPlan.desire = last_desire
+ pm.send('lateralPlan', dat)
+
+ f = msg.as_builder()
+ pm.send(msg.which(), f)
+
+ img = frs[msg.which()].get(frame_idxs[msg.which()], pix_fmt="yuv420p")[0]
+ if msg.which == "roadCameraState":
+ vipc_server.send(VisionStreamType.VISION_STREAM_ROAD, img.flatten().tobytes(), f.roadCameraState.frameId,
+ f.roadCameraState.timestampSof, f.roadCameraState.timestampEof)
+ else:
+ vipc_server.send(VisionStreamType.VISION_STREAM_DRIVER, img.flatten().tobytes(), f.driverCameraState.frameId,
+ f.driverCameraState.timestampSof, f.driverCameraState.timestampEof)
+ with Timeout(seconds=15):
+ log_msgs.append(messaging.recv_one(sm.sock[packet_from_camera[msg.which()]]))
+
+ frame_idxs[msg.which()] += 1
+ if frame_idxs[msg.which()] >= frs[msg.which()].frame_count:
+ return None
+ update_spinner(spinner, frame_idxs['roadCameraState'], frs['roadCameraState'].frame_count,
+ frame_idxs['driverCameraState'], frs['driverCameraState'].frame_count)
+ return 0
+
+def update_spinner(s, fidx, fcnt, didx, dcnt):
+ s.update("replaying models: road %d/%d, driver %d/%d" % (fidx, fcnt, didx, dcnt))
+
+def model_replay(lr_list, frs):
spinner = Spinner()
spinner.update("starting model replay")
- vipc_server = None
- pm = messaging.PubMaster(['roadCameraState', 'liveCalibration', 'lateralPlan'])
- sm = messaging.SubMaster(['modelV2'])
+ vipc_server = VisionIpcServer("camerad")
+ vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, *(tici_f_frame_size if TICI else eon_f_frame_size))
+ vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, *(tici_d_frame_size if TICI else eon_d_frame_size))
+ vipc_server.start_listener()
+
+ pm = messaging.PubMaster(['roadCameraState', 'driverCameraState', 'liveCalibration', 'lateralPlan'])
+ sm = messaging.SubMaster(['modelV2', 'driverState'])
- # TODO: add dmonitoringmodeld
try:
managed_processes['modeld'].start()
+ managed_processes['dmonitoringmodeld'].start()
time.sleep(5)
sm.update(1000)
- desires_by_index = {v:k for k,v in log.LateralPlan.Desire.schema.enumerants.items()}
+ last_desire = None
+ log_msgs = []
+ frame_idxs = dict.fromkeys(['roadCameraState','driverCameraState'], 0)
cal = [msg for msg in lr if msg.which() == "liveCalibration"]
for msg in cal[:5]:
- pm.send(msg.which(), replace_calib(msg, calib))
+ pm.send(msg.which(), replace_calib(msg, None))
- log_msgs = []
- frame_idx = 0
- for msg in tqdm(lr):
+ for msg in tqdm(lr_list):
if msg.which() == "liveCalibration":
- pm.send(msg.which(), replace_calib(msg, calib))
- elif msg.which() == "roadCameraState":
- if desire is not None:
- for i in desire[frame_idx].nonzero()[0]:
- dat = messaging.new_message('lateralPlan')
- dat.lateralPlan.desire = desires_by_index[i]
- pm.send('lateralPlan', dat)
-
- f = msg.as_builder()
- pm.send(msg.which(), f)
-
- img = fr.get(frame_idx, pix_fmt="yuv420p")[0]
- if vipc_server is None:
- w, h = {int(3*w*h/2): (w, h) for (w, h) in [tici_f_frame_size, eon_f_frame_size]}[len(img)]
- vipc_server = VisionIpcServer("camerad")
- vipc_server.create_buffers(VisionStreamType.VISION_STREAM_YUV_BACK, 40, False, w, h)
- vipc_server.start_listener()
- time.sleep(1) # wait for modeld to connect
-
- vipc_server.send(VisionStreamType.VISION_STREAM_YUV_BACK, img.flatten().tobytes(), f.roadCameraState.frameId,
- f.roadCameraState.timestampSof, f.roadCameraState.timestampEof)
-
- with Timeout(seconds=15):
- log_msgs.append(messaging.recv_one(sm.sock['modelV2']))
-
- spinner.update("modeld replay %d/%d" % (frame_idx, fr.frame_count))
-
- frame_idx += 1
- if frame_idx >= fr.frame_count:
+ last_calib = list(msg.liveCalibration.rpyCalib)
+ pm.send(msg.which(), replace_calib(msg, last_calib))
+ elif msg.which() == "lateralPlan":
+ last_desire = msg.lateralPlan.desire
+ elif msg.which() in ["roadCameraState", "driverCameraState"]:
+ ret = process_frame(msg, pm, sm, log_msgs, vipc_server, spinner, frs, frame_idxs, last_desire)
+ if ret is None:
break
+
except KeyboardInterrupt:
pass
finally:
spinner.close()
managed_processes['modeld'].stop()
+ managed_processes['dmonitoringmodeld'].stop()
return log_msgs
@@ -98,26 +118,40 @@ if __name__ == "__main__":
update = "--update" in sys.argv
+ if TICI:
+ os.system('sudo mount -o remount,size=200M /tmp') # c3 hevcs are 75M each
+
replay_dir = os.path.dirname(os.path.abspath(__file__))
ref_commit_fn = os.path.join(replay_dir, "model_replay_ref_commit")
- lr = LogReader(get_url(TEST_ROUTE, 0))
- fr = FrameReader(get_url(TEST_ROUTE, 0, log_type="fcamera"))
+ segnum = 0
+ frs = {}
+ if CACHE_DIR:
+ lr = LogReader(os.path.join(CACHE_DIR, '%s--%d--rlog.bz2' % (TEST_ROUTE.replace('|', '_'), segnum)))
+ frs['roadCameraState'] = FrameReader(os.path.join(CACHE_DIR, '%s--%d--fcamera.hevc' % (TEST_ROUTE.replace('|', '_'), segnum)))
+ frs['driverCameraState'] = FrameReader(os.path.join(CACHE_DIR, '%s--%d--dcamera.hevc' % (TEST_ROUTE.replace('|', '_'), segnum)))
+ else:
+ lr = LogReader(get_url(TEST_ROUTE, segnum))
+ frs['roadCameraState'] = FrameReader(get_url(TEST_ROUTE, segnum, log_type="fcamera"))
+ frs['driverCameraState'] = FrameReader(get_url(TEST_ROUTE, segnum, log_type="dcamera"))
- log_msgs = model_replay(list(lr), fr)
+ lr_list = list(lr)
+ log_msgs = model_replay(lr_list, frs)
failed = False
if not update:
ref_commit = open(ref_commit_fn).read().strip()
- log_fn = "%s_%s_%s.bz2" % (TEST_ROUTE, "model", ref_commit)
+ log_fn = get_log_fn(ref_commit)
cmp_log = LogReader(BASE_URL + log_fn)
ignore = ['logMonoTime', 'valid',
'modelV2.frameDropPerc',
- 'modelV2.modelExecutionTime']
+ 'modelV2.modelExecutionTime',
+ 'driverState.modelExecutionTime',
+ 'driverState.dspExecutionTime']
tolerance = None if not PC else 1e-3
results: Any = {TEST_ROUTE: {}}
- results[TEST_ROUTE]["modeld"] = compare_logs(cmp_log, log_msgs, tolerance=tolerance, ignore_fields=ignore)
+ results[TEST_ROUTE]["models"] = compare_logs(cmp_log, log_msgs, tolerance=tolerance, ignore_fields=ignore)
diff1, diff2, failed = format_diff(results, ref_commit)
print(diff2)
@@ -135,8 +169,8 @@ if __name__ == "__main__":
print("Uploading new refs")
- new_commit = get_git_commit()
- log_fn = "%s_%s_%s.bz2" % (TEST_ROUTE, "model", new_commit)
+ new_commit = get_commit()
+ log_fn = get_log_fn(new_commit)
save_log(log_fn, log_msgs)
try:
upload_file(log_fn, os.path.basename(log_fn))
diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit
index c006ea47a4..93b61856de 100644
--- a/selfdrive/test/process_replay/model_replay_ref_commit
+++ b/selfdrive/test/process_replay/model_replay_ref_commit
@@ -1 +1 @@
-46abfc14629d16f84ee45255ac876dceea2e7a11
+ed6d7e8f5e7e2c141b35814bb1c9b965f5e7dde8
diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py
index 451a7cfc50..ccdbc2754f 100755
--- a/selfdrive/test/process_replay/process_replay.py
+++ b/selfdrive/test/process_replay/process_replay.py
@@ -341,6 +341,15 @@ def replay_process(cfg, lr, fingerprint=None):
else:
return cpp_replay_process(cfg, lr, fingerprint)
+def setup_env():
+ params = Params()
+ params.clear_all()
+ params.put_bool("OpenpilotEnabledToggle", True)
+ params.put_bool("Passive", False)
+ params.put_bool("CommunityFeaturesToggle", True)
+
+ os.environ['NO_RADAR_SLEEP'] = "1"
+ os.environ["SIMULATION"] = "1"
def python_replay_process(cfg, lr, fingerprint=None):
sub_sockets = [s for _, sub in cfg.pub_sub.items() for s in sub]
@@ -356,21 +365,14 @@ def python_replay_process(cfg, lr, fingerprint=None):
all_msgs = sorted(lr, key=lambda msg: msg.logMonoTime)
pub_msgs = [msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())]
- params = Params()
- params.clear_all()
- params.put_bool("OpenpilotEnabledToggle", True)
- params.put_bool("Passive", False)
- params.put_bool("CommunityFeaturesToggle", True)
-
- os.environ['NO_RADAR_SLEEP'] = "1"
- os.environ["SIMULATION"] = "1"
-
+ setup_env()
# TODO: remove after getting new route for civic & accord
migration = {
"HONDA CIVIC 2016 TOURING": "HONDA CIVIC 2016",
"HONDA ACCORD 2018 SPORT 2T": "HONDA ACCORD 2018",
"HONDA ACCORD 2T 2018": "HONDA ACCORD 2018",
+ "Mazda CX-9 2021": "MAZDA CX-9 2021",
}
if fingerprint is not None:
@@ -383,7 +385,7 @@ def python_replay_process(cfg, lr, fingerprint=None):
if msg.which() == 'carParams':
car_fingerprint = migration.get(msg.carParams.carFingerprint, msg.carParams.carFingerprint)
if len(msg.carParams.carFw) and (car_fingerprint in FW_VERSIONS):
- params.put("CarParamsCache", msg.carParams.as_builder().to_bytes())
+ Params().put("CarParamsCache", msg.carParams.as_builder().to_bytes())
else:
os.environ['SKIP_FW_QUERY'] = "1"
os.environ['FINGERPRINT'] = car_fingerprint
@@ -401,7 +403,7 @@ def python_replay_process(cfg, lr, fingerprint=None):
can_sock = None
cfg.init_callback(all_msgs, fsm, can_sock, fingerprint)
- CP = car.CarParams.from_bytes(params.get("CarParams", block=True))
+ CP = car.CarParams.from_bytes(Params().get("CarParams", block=True))
# wait for started process to be ready
if 'can' in list(cfg.pub_sub.keys()):
@@ -446,6 +448,8 @@ def cpp_replay_process(cfg, lr, fingerprint=None):
pub_msgs = [msg for msg in all_msgs if msg.which() in list(cfg.pub_sub.keys())]
log_msgs = []
+ setup_env()
+
managed_processes[cfg.proc_name].prepare()
managed_processes[cfg.proc_name].start()
diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit
index 9021aaec7a..640e6864da 100644
--- a/selfdrive/test/process_replay/ref_commit
+++ b/selfdrive/test/process_replay/ref_commit
@@ -1 +1 @@
-567866c06976f8f95fa7d1670b51d8723e663ea1
\ No newline at end of file
+4d55b3974e099f2bd6e77ea12e0e0830af051041
\ No newline at end of file
diff --git a/selfdrive/test/process_replay/regen.py b/selfdrive/test/process_replay/regen.py
index 0e8a0eaf11..5326075240 100755
--- a/selfdrive/test/process_replay/regen.py
+++ b/selfdrive/test/process_replay/regen.py
@@ -39,8 +39,8 @@ def replay_service(s, msgs):
vs = None
def replay_cameras(lr, frs):
cameras = [
- ("roadCameraState", DT_MDL, eon_f_frame_size, VisionStreamType.VISION_STREAM_YUV_BACK),
- ("driverCameraState", DT_DMON, eon_d_frame_size, VisionStreamType.VISION_STREAM_YUV_FRONT),
+ ("roadCameraState", DT_MDL, eon_f_frame_size, VisionStreamType.VISION_STREAM_ROAD),
+ ("driverCameraState", DT_DMON, eon_d_frame_size, VisionStreamType.VISION_STREAM_DRIVER),
]
def replay_camera(s, stream, dt, vipc_server, fr, size):
diff --git a/selfdrive/test/process_replay/update_refs.py b/selfdrive/test/process_replay/update_refs.py
index fe5212d2c6..96feb0d534 100755
--- a/selfdrive/test/process_replay/update_refs.py
+++ b/selfdrive/test/process_replay/update_refs.py
@@ -6,7 +6,7 @@ from selfdrive.test.openpilotci import upload_file, get_url
from selfdrive.test.process_replay.compare_logs import save_log
from selfdrive.test.process_replay.process_replay import replay_process, CONFIGS
from selfdrive.test.process_replay.test_processes import segments
-from selfdrive.version import get_git_commit
+from selfdrive.version import get_commit
from tools.lib.logreader import LogReader
if __name__ == "__main__":
@@ -16,7 +16,7 @@ if __name__ == "__main__":
process_replay_dir = os.path.dirname(os.path.abspath(__file__))
ref_commit_fn = os.path.join(process_replay_dir, "ref_commit")
- ref_commit = get_git_commit()
+ ref_commit = get_commit()
if ref_commit is None:
raise Exception("couldn't get ref commit")
with open(ref_commit_fn, "w") as f:
diff --git a/selfdrive/test/test_models.py b/selfdrive/test/test_models.py
index a55ad35a12..fc1f919b83 100755
--- a/selfdrive/test/test_models.py
+++ b/selfdrive/test/test_models.py
@@ -10,6 +10,7 @@ from cereal import log, car
from common.params import Params
from selfdrive.car.fingerprints import all_known_cars
from selfdrive.car.car_helpers import interfaces
+from selfdrive.car.gm.values import CAR as GM
from selfdrive.car.honda.values import HONDA_BOSCH, CAR as HONDA
from selfdrive.car.chrysler.values import CAR as CHRYSLER
from selfdrive.car.hyundai.values import CAR as HYUNDAI
@@ -37,6 +38,11 @@ ignore_carstate_check = [
CHRYSLER.PACIFICA_2017_HYBRID,
]
+ignore_addr_checks_valid = [
+ GM.BUICK_REGAL,
+ HYUNDAI.GENESIS_G70_2020,
+]
+
@parameterized_class(('car_model'), [(car,) for i, car in enumerate(sorted(all_known_cars())) if i % NUM_JOBS == JOB_ID])
class TestCarModel(unittest.TestCase):
@@ -50,35 +56,35 @@ class TestCarModel(unittest.TestCase):
else:
raise Exception(f"missing test route for car {cls.car_model}")
+ params = Params()
+ params.clear_all()
+
for seg in [2, 1, 0]:
try:
lr = LogReader(get_url(ROUTES[cls.car_model], seg))
- break
except Exception:
- lr = None
-
- if lr is None:
- raise Exception("Route not found. Is it uploaded?")
-
- params = Params()
- params.clear_all()
-
- can_msgs = []
- fingerprint = {i: dict() for i in range(3)}
- for msg in lr:
- if msg.which() == "can":
- for m in msg.can:
- if m.src < 128:
- fingerprint[m.src][m.address] = len(m.dat)
- can_msgs.append(msg)
- elif msg.which() == "carParams":
- if msg.carParams.openpilotLongitudinalControl:
- params.put_bool("DisableRadar", True)
+ continue
+
+ can_msgs = []
+ fingerprint = {i: dict() for i in range(3)}
+ for msg in lr:
+ if msg.which() == "can":
+ for m in msg.can:
+ if m.src < 64:
+ fingerprint[m.src][m.address] = len(m.dat)
+ can_msgs.append(msg)
+ elif msg.which() == "carParams":
+ if msg.carParams.openpilotLongitudinalControl:
+ params.put_bool("DisableRadar", True)
+
+ if len(can_msgs):
+ break
+ else:
+ raise Exception("Route not found or no CAN msgs found. Is it uploaded?")
cls.can_msgs = sorted(can_msgs, key=lambda msg: msg.logMonoTime)
cls.CarInterface, cls.CarController, cls.CarState = interfaces[cls.car_model]
-
cls.CP = cls.CarInterface.get_params(cls.car_model, fingerprint, [])
assert cls.CP
@@ -108,6 +114,8 @@ class TestCarModel(unittest.TestCase):
self.assertTrue(len(self.CP.lateralTuning.lqr.a))
elif tuning == 'indi':
self.assertTrue(len(self.CP.lateralTuning.indi.outerLoopGainV))
+ else:
+ raise Exception("unkown tuning")
def test_car_interface(self):
# TODO: also check for checkusm and counter violations from can parser
@@ -141,14 +149,28 @@ class TestCarModel(unittest.TestCase):
if self.CP.dashcamOnly:
self.skipTest("no need to check panda safety for dashcamOnly")
+ start_ts = self.can_msgs[0].logMonoTime
+
failed_addrs = Counter()
for can in self.can_msgs:
+ # update panda timer
+ t = (can.logMonoTime - start_ts) / 1e3
+ self.safety.set_timer(int(t))
+
+ # run all msgs through the safety RX hook
for msg in can.can:
- if msg.src >= 128:
+ if msg.src >= 64:
continue
+
to_send = package_can_msg([msg.address, 0, msg.dat, msg.src])
if self.safety.safety_rx_hook(to_send) != 1:
failed_addrs[hex(msg.address)] += 1
+
+ # ensure all msgs defined in the addr checks are valid
+ if self.car_model not in ignore_addr_checks_valid:
+ self.safety.safety_tick_current_rx_checks()
+ if t > 1e6:
+ self.assertTrue(self.safety.addr_checks_valid())
self.assertFalse(len(failed_addrs), f"panda safety RX check failed: {failed_addrs}")
def test_panda_safety_carstate(self):
@@ -164,7 +186,7 @@ class TestCarModel(unittest.TestCase):
CC = car.CarControl.new_message()
for can in self.can_msgs:
for msg in can.can:
- if msg.src >= 128:
+ if msg.src >= 64:
continue
to_send = package_can_msg([msg.address, 0, msg.dat, msg.src])
ret = self.safety.safety_rx_hook(to_send)
@@ -197,6 +219,12 @@ class TestCarModel(unittest.TestCase):
if failed_checks['brakePressed'] < 150:
del failed_checks['brakePressed']
+ # TODO: use the same signal in panda and carState
+ # tolerate a small delay between the button press and PCM entering a cruise state
+ if self.car_model == HONDA.ACCORD_2021:
+ if failed_checks['controlsAllowed'] < 500:
+ del failed_checks['controlsAllowed']
+
self.assertFalse(len(failed_checks), f"panda safety doesn't agree with CarState: {failed_checks}")
if __name__ == "__main__":
diff --git a/selfdrive/test/test_routes.py b/selfdrive/test/test_routes.py
index 5344f8fd75..696fe0de04 100755
--- a/selfdrive/test/test_routes.py
+++ b/selfdrive/test/test_routes.py
@@ -53,7 +53,9 @@ routes = [
TestRoute("81722949a62ea724|2019-04-06--15-19-25", HONDA.ODYSSEY_CHN),
TestRoute("08a3deb07573f157|2020-03-06--16-11-19", HONDA.ACCORD), # 1.5T
TestRoute("1da5847ac2488106|2021-05-24--19-31-50", HONDA.ACCORD), # 2.0T
+ TestRoute("085ac1d942c35910|2021-03-25--20-11-15", HONDA.ACCORD_2021), # 2.0T
TestRoute("07585b0da3c88459|2021-05-26--18-52-04", HONDA.ACCORDH),
+ TestRoute("f29e2b57a55e7ad5|2021-03-24--20-52-38", HONDA.ACCORDH_2021),
TestRoute("1ad763dd22ef1a0e|2020-02-29--18-37-03", HONDA.CRV_5G),
TestRoute("0a96f86fcfe35964|2020-02-05--07-25-51", HONDA.ODYSSEY),
TestRoute("d83f36766f8012a5|2020-02-05--18-42-21", HONDA.CIVIC_BOSCH_DIESEL),
@@ -122,6 +124,7 @@ routes = [
TestRoute("25057fa6a5a63dfb|2020-03-04--08-44-23", TOYOTA.LEXUS_CTH),
TestRoute("f49e8041283f2939|2019-05-30--11-51-51", TOYOTA.LEXUS_ESH_TSS2),
TestRoute("37041c500fd30100|2020-12-30--12-17-24", TOYOTA.LEXUS_ESH),
+ TestRoute("32696cea52831b02|2021-11-19--18-13-30", TOYOTA.LEXUS_RC),
TestRoute("886fcd8408d570e9|2020-01-29--05-11-22", TOYOTA.LEXUS_RX),
TestRoute("886fcd8408d570e9|2020-01-29--02-18-55", TOYOTA.LEXUS_RX),
TestRoute("d27ad752e9b08d4f|2021-05-26--19-39-51", TOYOTA.LEXUS_RXH),
diff --git a/selfdrive/test/test_valgrind_replay.py b/selfdrive/test/test_valgrind_replay.py
old mode 100644
new mode 100755
index 2c7870a337..238b822ec9
--- a/selfdrive/test/test_valgrind_replay.py
+++ b/selfdrive/test/test_valgrind_replay.py
@@ -1,3 +1,4 @@
+#!/usr/bin/env python3
import os
import threading
import time
@@ -53,9 +54,11 @@ class TestValgrind(unittest.TestCase):
# Run valgrind on a process
command = "valgrind --leak-check=full " + arg
p = subprocess.Popen(command, stderr=subprocess.PIPE, shell=True, preexec_fn=os.setsid) # pylint: disable=W1509
- while not self.done:
+
+ while not self.replay_done:
time.sleep(0.1)
+ # Kill valgrind and extract leak output
os.killpg(os.getpgid(p.pid), signal.SIGINT)
_, err = p.communicate()
error_msg = str(err, encoding='utf-8')
@@ -91,18 +94,24 @@ class TestValgrind(unittest.TestCase):
if config.wait_for_response:
sm.update(100)
- self.done = True
+ self.replay_done = True
def test_config(self):
open(os.path.join(BASEDIR, "selfdrive/test/valgrind_logs.txt"), "w").close()
for cfg in CONFIGS:
- self.done = False
+ self.leak = None
+ self.replay_done = False
+
r, n = cfg.segment.rsplit("--", 1)
lr = LogReader(get_url(r, n))
self.replay_process(cfg, lr)
- time.sleep(1) # Wait for the logs to get written
+
+ while self.leak is None:
+ time.sleep(0.1) # Wait for the valgrind to finish
+
self.assertFalse(self.leak)
+
if __name__ == "__main__":
unittest.main()
diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py
index f68a0accb8..eb3e594269 100755
--- a/selfdrive/thermald/thermald.py
+++ b/selfdrive/thermald/thermald.py
@@ -22,7 +22,7 @@ from selfdrive.hardware import EON, TICI, PC, HARDWARE
from selfdrive.loggerd.config import get_available_percent
from selfdrive.swaglog import cloudlog
from selfdrive.thermald.power_monitoring import PowerMonitoring
-from selfdrive.version import tested_branch, terms_version, training_version
+from selfdrive.version import get_tested_branch, terms_version, training_version
ThermalStatus = log.DeviceState.ThermalStatus
NetworkType = log.DeviceState.NetworkType
@@ -66,6 +66,7 @@ def read_thermal(thermal_config):
dat.deviceState.gpuTempC = [read_tz(z) / thermal_config.gpu[1] for z in thermal_config.gpu[0]]
dat.deviceState.memoryTempC = read_tz(thermal_config.mem[0]) / thermal_config.mem[1]
dat.deviceState.ambientTempC = read_tz(thermal_config.ambient[0]) / thermal_config.ambient[1]
+ dat.deviceState.pmicTempC = [read_tz(z) / thermal_config.pmic[1] for z in thermal_config.pmic[0]]
return dat
@@ -342,7 +343,7 @@ def thermald_thread():
last_update_exception = params.get("LastUpdateException", encoding='utf8')
if update_failed_count > 15 and last_update_exception is not None:
- if tested_branch:
+ if get_tested_branch():
extra_text = "Ensure the software is correctly installed"
else:
extra_text = last_update_exception
@@ -406,6 +407,8 @@ def thermald_thread():
power_monitor.calculate(peripheralState, startup_conditions["ignition"])
msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used()
msg.deviceState.carBatteryCapacityUwh = max(0, power_monitor.get_car_battery_capacity())
+ current_power_draw = HARDWARE.get_current_power_draw() # pylint: disable=assignment-from-none
+ msg.deviceState.powerDrawW = current_power_draw if current_power_draw is not None else 0
# Check if we need to disable charging (handled by boardd)
msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(startup_conditions["ignition"], in_car, off_ts)
diff --git a/selfdrive/tombstoned.py b/selfdrive/tombstoned.py
index fb7a1a2c99..a301725bae 100755
--- a/selfdrive/tombstoned.py
+++ b/selfdrive/tombstoned.py
@@ -15,7 +15,7 @@ from common.file_helpers import mkdirs_exists_ok
from selfdrive.hardware import TICI, HARDWARE
from selfdrive.loggerd.config import ROOT
from selfdrive.swaglog import cloudlog
-from selfdrive.version import branch, commit, dirty, origin, version
+from selfdrive.version import get_branch, get_commit, get_dirty, get_origin, get_version
MAX_SIZE = 100000 * 10 # mal size is 40-100k, allow up to 1M
if TICI:
@@ -109,7 +109,7 @@ def report_tombstone_android(fn):
clean_path = executable.replace('./', '').replace('/', '_')
date = datetime.datetime.now().strftime("%Y-%m-%d--%H-%M-%S")
- new_fn = f"{date}_{commit[:8]}_{safe_fn(clean_path)}"[:MAX_TOMBSTONE_FN_LEN]
+ new_fn = f"{date}_{get_commit(default='nocommit')[:8]}_{safe_fn(clean_path)}"[:MAX_TOMBSTONE_FN_LEN]
crashlog_dir = os.path.join(ROOT, "crash")
mkdirs_exists_ok(crashlog_dir)
@@ -183,7 +183,7 @@ def report_tombstone_apport(fn):
clean_path = path.replace('/', '_')
date = datetime.datetime.now().strftime("%Y-%m-%d--%H-%M-%S")
- new_fn = f"{date}_{commit[:8]}_{safe_fn(clean_path)}"[:MAX_TOMBSTONE_FN_LEN]
+ new_fn = f"{date}_{get_commit(default='nocommit')[:8]}_{safe_fn(clean_path)}"[:MAX_TOMBSTONE_FN_LEN]
crashlog_dir = os.path.join(ROOT, "crash")
mkdirs_exists_ok(crashlog_dir)
@@ -203,14 +203,14 @@ def main():
sentry_sdk.utils.MAX_STRING_LENGTH = 8192
sentry_sdk.init("https://a40f22e13cbc4261873333c125fc9d38@o33823.ingest.sentry.io/157615",
- default_integrations=False, release=version)
+ default_integrations=False, release=get_version())
dongle_id = Params().get("DongleId", encoding='utf-8')
sentry_sdk.set_user({"id": dongle_id})
- sentry_sdk.set_tag("dirty", dirty)
- sentry_sdk.set_tag("origin", origin)
- sentry_sdk.set_tag("branch", branch)
- sentry_sdk.set_tag("commit", commit)
+ sentry_sdk.set_tag("dirty", get_dirty())
+ sentry_sdk.set_tag("origin", get_origin())
+ sentry_sdk.set_tag("branch", get_branch())
+ sentry_sdk.set_tag("commit", get_commit())
sentry_sdk.set_tag("device", HARDWARE.get_device_type())
while True:
diff --git a/selfdrive/ui/SConscript b/selfdrive/ui/SConscript
index bc21022dd9..da16c12d14 100644
--- a/selfdrive/ui/SConscript
+++ b/selfdrive/ui/SConscript
@@ -116,7 +116,7 @@ if arch in ['x86_64', 'Darwin'] or GetOption('extras'):
replay_lib_src = ["replay/replay.cc", "replay/camera.cc", "replay/filereader.cc", "replay/logreader.cc", "replay/framereader.cc", "replay/route.cc", "replay/util.cc"]
replay_lib = qt_env.Library("qt_replay", replay_lib_src, LIBS=base_libs)
- replay_libs = [replay_lib, 'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'swscale', 'yuv'] + qt_libs
+ replay_libs = [replay_lib, 'avutil', 'avcodec', 'avformat', 'bz2', 'curl', 'yuv'] + qt_libs
qt_env.Program("replay/replay", ["replay/main.cc"], LIBS=replay_libs)
qt_env.Program("watch3", ["watch3.cc"], LIBS=qt_libs + ['common', 'json11'])
@@ -126,5 +126,8 @@ if arch in ['x86_64', 'Darwin'] or GetOption('extras'):
# navd
if maps:
- navd_src = ["navd/main.cc", "navd/route_engine.cc"]
+ navd_src = ["navd/main.cc", "navd/route_engine.cc", "navd/map_renderer.cc"]
qt_env.Program("navd/_navd", navd_src, LIBS=qt_libs + ['common', 'json11'])
+
+ if GetOption('extras'):
+ qt_env.SharedLibrary("navd/map_renderer", ["navd/map_renderer.cc"], LIBS=qt_libs + ['common', 'messaging'])
diff --git a/selfdrive/ui/navd/main.cc b/selfdrive/ui/navd/main.cc
index cc8d9c339b..7ae0393a47 100644
--- a/selfdrive/ui/navd/main.cc
+++ b/selfdrive/ui/navd/main.cc
@@ -5,9 +5,11 @@
#include
#include "selfdrive/ui/qt/util.h"
+#include "selfdrive/ui/qt/maps/map_helpers.h"
#include "selfdrive/ui/navd/route_engine.h"
-
-RouteEngine* route_engine = nullptr;
+#include "selfdrive/ui/navd/map_renderer.h"
+#include "selfdrive/hardware/hw.h"
+#include "selfdrive/common/params.h"
void sigHandler(int s) {
qInfo() << "Shutting down";
@@ -30,7 +32,14 @@ int main(int argc, char *argv[]) {
parser.process(app);
const QStringList args = parser.positionalArguments();
- route_engine = new RouteEngine();
+
+ RouteEngine* route_engine = new RouteEngine();
+
+ if (Params().getBool("NavdRender")) {
+ MapRenderer * m = new MapRenderer(get_mapbox_settings());
+ QObject::connect(route_engine, &RouteEngine::positionUpdated, m, &MapRenderer::updatePosition);
+ QObject::connect(route_engine, &RouteEngine::routeUpdated, m, &MapRenderer::updateRoute);
+ }
return app.exec();
}
diff --git a/selfdrive/ui/navd/map_renderer.cc b/selfdrive/ui/navd/map_renderer.cc
new file mode 100644
index 0000000000..1d78d68dfe
--- /dev/null
+++ b/selfdrive/ui/navd/map_renderer.cc
@@ -0,0 +1,200 @@
+#include "selfdrive/ui/navd/map_renderer.h"
+
+#include
+#include
+#include
+
+#include "selfdrive/ui/qt/maps/map_helpers.h"
+#include "selfdrive/common/timing.h"
+
+const float ZOOM = 13.5; // Don't go below 13 or features will start to disappear
+const int WIDTH = 256;
+const int HEIGHT = WIDTH;
+
+const int NUM_VIPC_BUFFERS = 4;
+
+MapRenderer::MapRenderer(const QMapboxGLSettings &settings, bool enable_vipc) : m_settings(settings) {
+ QSurfaceFormat fmt;
+ fmt.setRenderableType(QSurfaceFormat::OpenGLES);
+
+ ctx = std::make_unique();
+ ctx->setFormat(fmt);
+ ctx->create();
+ assert(ctx->isValid());
+
+ surface = std::make_unique();
+ surface->setFormat(ctx->format());
+ surface->create();
+
+ ctx->makeCurrent(surface.get());
+ assert(QOpenGLContext::currentContext() == ctx.get());
+
+ gl_functions.reset(ctx->functions());
+ gl_functions->initializeOpenGLFunctions();
+
+ QOpenGLFramebufferObjectFormat fbo_format;
+ fbo.reset(new QOpenGLFramebufferObject(WIDTH, HEIGHT, fbo_format));
+
+ m_map.reset(new QMapboxGL(nullptr, m_settings, fbo->size(), 1));
+ m_map->setCoordinateZoom(QMapbox::Coordinate(0, 0), ZOOM);
+ m_map->setStyleUrl("mapbox://styles/commaai/ckvmksrpd4n0a14pfdo5heqzr");
+ m_map->createRenderer();
+
+ m_map->resize(fbo->size());
+ m_map->setFramebufferObject(fbo->handle(), fbo->size());
+ gl_functions->glViewport(0, 0, WIDTH, HEIGHT);
+
+ if (enable_vipc) {
+ qWarning() << "Enabling navd map rendering";
+ vipc_server.reset(new VisionIpcServer("navd"));
+ vipc_server->create_buffers(VisionStreamType::VISION_STREAM_RGB_MAP, NUM_VIPC_BUFFERS, true, WIDTH, HEIGHT);
+ vipc_server->start_listener();
+
+ pm.reset(new PubMaster({"navThumbnail"}));
+ }
+}
+
+void MapRenderer::updatePosition(QMapbox::Coordinate position, float bearing) {
+ if (m_map.isNull()) {
+ return;
+ }
+
+ m_map->setCoordinate(position);
+ m_map->setBearing(bearing);
+ update();
+}
+
+bool MapRenderer::loaded() {
+ return m_map->isFullyLoaded();
+}
+
+void MapRenderer::update() {
+ gl_functions->glClear(GL_COLOR_BUFFER_BIT);
+ m_map->render();
+ gl_functions->glFlush();
+
+ sendVipc();
+}
+
+void MapRenderer::sendVipc() {
+ if (!vipc_server || !loaded()) {
+ return;
+ }
+
+ QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor);
+ uint64_t ts = nanos_since_boot();
+ VisionBuf* buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_RGB_MAP);
+ VisionIpcBufExtra extra = {
+ .frame_id = frame_id,
+ .timestamp_sof = ts,
+ .timestamp_eof = ts,
+ };
+
+ assert(cap.sizeInBytes() == buf->len);
+ memcpy(buf->addr, cap.bits(), buf->len);
+ vipc_server->send(buf, &extra);
+
+ if (frame_id % 100 == 0) {
+ // Write jpeg into buffer
+ QByteArray buffer_bytes;
+ QBuffer buffer(&buffer_bytes);
+ buffer.open(QIODevice::WriteOnly);
+ cap.save(&buffer, "JPG", 50);
+
+ kj::Array buffer_kj = kj::heapArray((const capnp::byte*)buffer_bytes.constData(), buffer_bytes.size());
+
+ // Send thumbnail
+ MessageBuilder msg;
+ auto thumbnaild = msg.initEvent().initNavThumbnail();
+ thumbnaild.setFrameId(frame_id);
+ thumbnaild.setTimestampEof(ts);
+ thumbnaild.setThumbnail(buffer_kj);
+ pm->send("navThumbnail", msg);
+ }
+
+ frame_id++;
+}
+
+uint8_t* MapRenderer::getImage() {
+ QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor);
+ uint8_t* buf = new uint8_t[cap.sizeInBytes()];
+ memcpy(buf, cap.bits(), cap.sizeInBytes());
+
+ return buf;
+}
+
+void MapRenderer::updateRoute(QList coordinates) {
+ if (m_map.isNull()) return;
+ initLayers();
+
+ auto route_points = coordinate_list_to_collection(coordinates);
+ QMapbox::Feature feature(QMapbox::Feature::LineStringType, route_points, {}, {});
+ QVariantMap navSource;
+ navSource["type"] = "geojson";
+ navSource["data"] = QVariant::fromValue(feature);
+ m_map->updateSource("navSource", navSource);
+ m_map->setLayoutProperty("navLayer", "visibility", "visible");
+}
+
+void MapRenderer::initLayers() {
+ if (!m_map->layerExists("navLayer")) {
+ QVariantMap nav;
+ nav["id"] = "navLayer";
+ nav["type"] = "line";
+ nav["source"] = "navSource";
+ m_map->addLayer(nav, "road-intersection");
+ m_map->setPaintProperty("navLayer", "line-color", QColor("blue"));
+ m_map->setPaintProperty("navLayer", "line-width", 3);
+ m_map->setLayoutProperty("navLayer", "line-cap", "round");
+ }
+}
+
+MapRenderer::~MapRenderer() {
+}
+
+extern "C" {
+ MapRenderer* map_renderer_init() {
+ char *argv[] = {
+ (char*)"navd",
+ nullptr
+ };
+ int argc = 0;
+ QApplication *app = new QApplication(argc, argv);
+ assert(app);
+
+ QMapboxGLSettings settings;
+ settings.setApiBaseUrl(MAPS_HOST);
+ settings.setAccessToken(get_mapbox_token());
+
+ return new MapRenderer(settings, false);
+ }
+
+ void map_renderer_update_position(MapRenderer *inst, float lat, float lon, float bearing) {
+ inst->updatePosition({lat, lon}, bearing);
+ QApplication::processEvents();
+ }
+
+ void map_renderer_update_route(MapRenderer *inst, char* polyline) {
+ inst->updateRoute(polyline_to_coordinate_list(QString::fromUtf8(polyline)));
+ }
+
+ void map_renderer_update(MapRenderer *inst) {
+ inst->update();
+ }
+
+ void map_renderer_process(MapRenderer *inst) {
+ QApplication::processEvents();
+ }
+
+ bool map_renderer_loaded(MapRenderer *inst) {
+ return inst->loaded();
+ }
+
+ uint8_t * map_renderer_get_image(MapRenderer *inst) {
+ return inst->getImage();
+ }
+
+ void map_renderer_free_image(MapRenderer *inst, uint8_t * buf) {
+ delete[] buf;
+ }
+}
diff --git a/selfdrive/ui/navd/map_renderer.h b/selfdrive/ui/navd/map_renderer.h
new file mode 100644
index 0000000000..1746e76695
--- /dev/null
+++ b/selfdrive/ui/navd/map_renderer.h
@@ -0,0 +1,49 @@
+#pragma once
+
+#include
+
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+#include
+
+#include "cereal/visionipc/visionipc_server.h"
+#include "cereal/messaging/messaging.h"
+
+
+class MapRenderer : public QObject {
+ Q_OBJECT
+
+public:
+ MapRenderer(const QMapboxGLSettings &, bool enable_vipc=true);
+ uint8_t* getImage();
+ void update();
+ bool loaded();
+ ~MapRenderer();
+
+
+private:
+ std::unique_ptr ctx;
+ std::unique_ptr surface;
+ std::unique_ptr gl_functions;
+ std::unique_ptr fbo;
+
+ std::unique_ptr vipc_server;
+ std::unique_ptr pm;
+ void sendVipc();
+
+ QMapboxGLSettings m_settings;
+ QScopedPointer m_map;
+
+ void initLayers();
+
+ uint32_t frame_id = 0;
+
+public slots:
+ void updatePosition(QMapbox::Coordinate position, float bearing);
+ void updateRoute(QList coordinates);
+};
diff --git a/selfdrive/ui/navd/map_renderer.py b/selfdrive/ui/navd/map_renderer.py
new file mode 100755
index 0000000000..28d006841b
--- /dev/null
+++ b/selfdrive/ui/navd/map_renderer.py
@@ -0,0 +1,78 @@
+#!/usr/bin/env python3
+# You might need to uninstall the PyQt5 pip package to avoid conflicts
+
+import os
+import time
+from cffi import FFI
+
+from common.ffi_wrapper import suffix
+from common.basedir import BASEDIR
+
+HEIGHT = WIDTH = 256
+
+
+def get_ffi():
+ lib = os.path.join(BASEDIR, "selfdrive", "ui", "navd", "libmap_renderer" + suffix())
+
+ ffi = FFI()
+ ffi.cdef("""
+void* map_renderer_init();
+void map_renderer_update_position(void *inst, float lat, float lon, float bearing);
+void map_renderer_update_route(void *inst, char *polyline);
+void map_renderer_update(void *inst);
+void map_renderer_process(void *inst);
+bool map_renderer_loaded(void *inst);
+uint8_t* map_renderer_get_image(void *inst);
+void map_renderer_free_image(void *inst, uint8_t *buf);
+""")
+ return ffi, ffi.dlopen(lib)
+
+
+def wait_ready(lib, renderer):
+ while not lib.map_renderer_loaded(renderer):
+ lib.map_renderer_update(renderer)
+
+ # The main qt app is not execed, so we need to periodically process events for e.g. network requests
+ lib.map_renderer_process(renderer)
+
+ time.sleep(0.01)
+
+
+def get_image(lib, renderer):
+ buf = lib.map_renderer_get_image(renderer)
+ r = list(buf[0:3 * WIDTH * HEIGHT])
+ lib.map_renderer_free_image(renderer, buf)
+
+ # Convert to numpy
+ r = np.asarray(r)
+ return r.reshape((WIDTH, HEIGHT, 3))
+
+
+if __name__ == "__main__":
+ import matplotlib.pyplot as plt
+ import numpy as np
+
+ ffi, lib = get_ffi()
+ renderer = lib.map_renderer_init()
+ wait_ready(lib, renderer)
+
+ geometry = r"{yxk}@|obn~Eg@@eCFqc@J{RFw@?kA@gA?q|@Riu@NuJBgi@ZqVNcRBaPBkG@iSD{I@_H@cH?gG@mG@gG?aD@{LDgDDkVVyQLiGDgX@q_@@qI@qKhS{R~[}NtYaDbGoIvLwNfP_b@|f@oFnF_JxHel@bf@{JlIuxAlpAkNnLmZrWqFhFoh@jd@kX|TkJxH_RnPy^|[uKtHoZ~Um`DlkCorC``CuShQogCtwB_ThQcr@fk@sVrWgRhVmSb\\oj@jxA{Qvg@u]tbAyHzSos@xjBeKbWszAbgEc~@~jCuTrl@cYfo@mRn\\_m@v}@ij@jp@om@lk@y|A`pAiXbVmWzUod@xj@wNlTw}@|uAwSn\\kRfYqOdS_IdJuK`KmKvJoOhLuLbHaMzGwO~GoOzFiSrEsOhD}PhCqw@vJmnAxSczA`Vyb@bHk[fFgl@pJeoDdl@}}@zIyr@hG}X`BmUdBcM^aRR}Oe@iZc@mR_@{FScHxAn_@vz@zCzH~GjPxAhDlB~DhEdJlIbMhFfG|F~GlHrGjNjItLnGvQ~EhLnBfOn@p`@AzAAvn@CfC?fc@`@lUrArStCfSxEtSzGxM|ElFlBrOzJlEbDnC~BfDtCnHjHlLvMdTnZzHpObOf^pKla@~G|a@dErg@rCbj@zArYlj@ttJ~AfZh@r]LzYg@`TkDbj@gIdv@oE|i@kKzhA{CdNsEfOiGlPsEvMiDpLgBpHyB`MkB|MmArPg@|N?|P^rUvFz~AWpOCdAkB|PuB`KeFfHkCfGy@tAqC~AsBPkDs@uAiAcJwMe@s@eKkPMoXQux@EuuCoH?eI?Kas@}Dy@wAUkMOgDL"
+ lib.map_renderer_update_route(renderer, geometry.encode())
+
+ POSITIONS = [
+ (32.71569271952601, -117.16384270868463, 0), (32.71569271952601, -117.16384270868463, 45), # San Diego
+ (52.378641991483136, 4.902623379456488, 0), (52.378641991483136, 4.902623379456488, 45), # Amsterdam
+ ]
+ plt.figure()
+
+ for i, pos in enumerate(POSITIONS):
+ t = time.time()
+ lib.map_renderer_update_position(renderer, *pos)
+ wait_ready(lib, renderer)
+
+ print(f"{pos} took {time.time() - t:.2f} s")
+
+ plt.subplot(2, 2, i + 1)
+ plt.imshow(get_image(lib, renderer))
+
+ plt.show()
diff --git a/selfdrive/ui/navd/route_engine.cc b/selfdrive/ui/navd/route_engine.cc
index 30d070dc73..f3cb7df3d9 100644
--- a/selfdrive/ui/navd/route_engine.cc
+++ b/selfdrive/ui/navd/route_engine.cc
@@ -91,7 +91,6 @@ static void parse_banner(cereal::NavInstruction::Builder &instruction, const QMa
}
}
}
-
}
RouteEngine::RouteEngine() {
@@ -99,14 +98,17 @@ RouteEngine::RouteEngine() {
pm = new PubMaster({"navInstruction", "navRoute"});
// Timers
- timer = new QTimer(this);
- QObject::connect(timer, SIGNAL(timeout()), this, SLOT(timerUpdate()));
- timer->start(1000);
+ route_timer = new QTimer(this);
+ QObject::connect(route_timer, SIGNAL(timeout()), this, SLOT(routeUpdate()));
+ route_timer->start(1000);
+
+ msg_timer = new QTimer(this);
+ QObject::connect(msg_timer, SIGNAL(timeout()), this, SLOT(msgUpdate()));
+ msg_timer->start(50);
// Build routing engine
QVariantMap parameters;
- QString token = MAPBOX_TOKEN.isEmpty() ? CommaApi::create_jwt({}, 4 * 7 * 24 * 3600) : MAPBOX_TOKEN;
- parameters["mapbox.access_token"] = token;
+ parameters["mapbox.access_token"] = get_mapbox_token();
parameters["mapbox.directions_api_url"] = MAPS_HOST + "/directions/v5/mapbox/";
geoservice_provider = new QGeoServiceProvider("mapbox", parameters);
@@ -124,12 +126,14 @@ RouteEngine::RouteEngine() {
}
}
-void RouteEngine::timerUpdate() {
- sm->update(0);
+void RouteEngine::msgUpdate() {
+ sm->update(1000);
if (!sm->updated("liveLocationKalman")) {
+ active = false;
return;
}
+
if (sm->updated("managerState")) {
for (auto const &p : (*sm)["managerState"].getManagerState().getProcesses()) {
if (p.getName() == "ui" && p.getRunning()) {
@@ -153,6 +157,15 @@ void RouteEngine::timerUpdate() {
if (localizer_valid) {
last_bearing = RAD2DEG(orientation.getValue()[2]);
last_position = QMapbox::Coordinate(pos.getValue()[0], pos.getValue()[1]);
+ emit positionUpdated(*last_position, *last_bearing);
+ }
+
+ active = true;
+}
+
+void RouteEngine::routeUpdate() {
+ if (!active) {
+ return;
}
recomputeRoute();
@@ -261,6 +274,10 @@ bool RouteEngine::shouldRecompute() {
}
void RouteEngine::recomputeRoute() {
+ if (!last_position) {
+ return;
+ }
+
auto new_destination = coordinate_from_param("NavDestination");
if (!new_destination) {
clearRoute();
@@ -308,6 +325,9 @@ void RouteEngine::routeCalculated(QGeoRouteReply *reply) {
route = reply->routes().at(0);
segment = route.firstRouteSegment();
+
+ auto path = route.path();
+ emit routeUpdated(path);
} else {
qWarning() << "Got empty route response";
}
diff --git a/selfdrive/ui/navd/route_engine.h b/selfdrive/ui/navd/route_engine.h
index 9a6c990417..33cbc79107 100644
--- a/selfdrive/ui/navd/route_engine.h
+++ b/selfdrive/ui/navd/route_engine.h
@@ -23,7 +23,8 @@ public:
SubMaster *sm;
PubMaster *pm;
- QTimer* timer;
+ QTimer* msg_timer;
+ QTimer* route_timer;
std::optional ui_pid;
@@ -41,6 +42,7 @@ public:
bool localizer_valid = false;
// Route recompute
+ bool active = false;
int recompute_backoff = 0;
int recompute_countdown = 0;
void calculateRoute(QMapbox::Coordinate destination);
@@ -48,8 +50,13 @@ public:
bool shouldRecompute();
private slots:
- void timerUpdate();
+ void routeUpdate();
+ void msgUpdate();
void routeCalculated(QGeoRouteReply *reply);
void recomputeRoute();
void sendRoute();
+
+signals:
+ void positionUpdated(QMapbox::Coordinate position, float bearing);
+ void routeUpdated(QList coordinates);
};
diff --git a/selfdrive/ui/qt/api.cc b/selfdrive/ui/qt/api.cc
index 6339884aec..8ac9b41313 100644
--- a/selfdrive/ui/qt/api.cc
+++ b/selfdrive/ui/qt/api.cc
@@ -4,6 +4,7 @@
#include
#include
+#include
#include
#include
#include
@@ -62,18 +63,20 @@ QString create_jwt(const QJsonObject &payloads, int expiry) {
} // namespace CommaApi
HttpRequest::HttpRequest(QObject *parent, bool create_jwt, int timeout) : create_jwt(create_jwt), QObject(parent) {
- networkAccessManager = new QNetworkAccessManager(this);
-
networkTimer = new QTimer(this);
networkTimer->setSingleShot(true);
networkTimer->setInterval(timeout);
connect(networkTimer, &QTimer::timeout, this, &HttpRequest::requestTimeout);
}
-bool HttpRequest::active() {
+bool HttpRequest::active() const {
return reply != nullptr;
}
+bool HttpRequest::timeout() const {
+ return reply && reply->error() == QNetworkReply::OperationCanceledError;
+}
+
void HttpRequest::sendRequest(const QString &requestURL, const HttpRequest::Method method) {
if (active()) {
qDebug() << "HttpRequest is active";
@@ -97,9 +100,9 @@ void HttpRequest::sendRequest(const QString &requestURL, const HttpRequest::Meth
}
if (method == HttpRequest::Method::GET) {
- reply = networkAccessManager->get(request);
+ reply = nam()->get(request);
} else if (method == HttpRequest::Method::DELETE) {
- reply = networkAccessManager->deleteResource(request);
+ reply = nam()->deleteResource(request);
}
networkTimer->start();
@@ -110,29 +113,31 @@ void HttpRequest::requestTimeout() {
reply->abort();
}
-// This function should always emit something
void HttpRequest::requestFinished() {
- bool success = false;
- if (reply->error() != QNetworkReply::OperationCanceledError) {
- networkTimer->stop();
- QString response = reply->readAll();
-
- if (reply->error() == QNetworkReply::NoError) {
- success = true;
- emit receivedResponse(response);
- } else {
- emit failedResponse(reply->errorString());
+ networkTimer->stop();
+ if (reply->error() == QNetworkReply::NoError) {
+ emit requestDone(reply->readAll(), true);
+ } else {
+ QString error;
+ if (reply->error() == QNetworkReply::OperationCanceledError) {
+ nam()->clearAccessCache();
+ nam()->clearConnectionCache();
+ error = "Request timed out";
+ } else {
if (reply->error() == QNetworkReply::ContentAccessDenied || reply->error() == QNetworkReply::AuthenticationRequiredError) {
qWarning() << ">> Unauthorized. Authenticate with tools/lib/auth.py <<";
}
+ error = reply->errorString();
}
- } else {
- networkAccessManager->clearAccessCache();
- networkAccessManager->clearConnectionCache();
- emit timeoutResponse("timeout");
+ emit requestDone(error, false);
}
- emit requestDone(success);
+
reply->deleteLater();
reply = nullptr;
}
+
+QNetworkAccessManager *HttpRequest::nam() {
+ static QNetworkAccessManager *networkAccessManager = new QNetworkAccessManager(qApp);
+ return networkAccessManager;
+}
diff --git a/selfdrive/ui/qt/api.h b/selfdrive/ui/qt/api.h
index 1c7c333ea0..8f74678109 100644
--- a/selfdrive/ui/qt/api.h
+++ b/selfdrive/ui/qt/api.h
@@ -27,23 +27,21 @@ public:
explicit HttpRequest(QObject* parent, bool create_jwt = true, int timeout = 20000);
void sendRequest(const QString &requestURL, const Method method = Method::GET);
- bool active();
+ bool active() const;
+ bool timeout() const;
+
+signals:
+ void requestDone(const QString &response, bool success);
protected:
QNetworkReply *reply = nullptr;
private:
- QNetworkAccessManager *networkAccessManager = nullptr;
+ static QNetworkAccessManager *nam();
QTimer *networkTimer = nullptr;
bool create_jwt;
private slots:
void requestTimeout();
void requestFinished();
-
-signals:
- void requestDone(bool success);
- void receivedResponse(const QString &response);
- void failedResponse(const QString &errorString);
- void timeoutResponse(const QString &errorString);
};
diff --git a/selfdrive/ui/qt/home.cc b/selfdrive/ui/qt/home.cc
index 5484c5d743..1e89264952 100644
--- a/selfdrive/ui/qt/home.cc
+++ b/selfdrive/ui/qt/home.cc
@@ -178,6 +178,6 @@ void OffroadHome::refresh() {
update_notif->setVisible(updateAvailable);
alert_notif->setVisible(alerts);
if (alerts) {
- alert_notif->setText(QString::number(alerts) + " ALERT" + (alerts > 1 ? "S" : ""));
+ alert_notif->setText(QString::number(alerts) + (alerts > 1 ? " ALERTS" : " ALERT"));
}
}
diff --git a/selfdrive/ui/qt/maps/map.h b/selfdrive/ui/qt/maps/map.h
index b74f5bf29d..c62089cb25 100644
--- a/selfdrive/ui/qt/maps/map.h
+++ b/selfdrive/ui/qt/maps/map.h
@@ -22,9 +22,6 @@
#include "selfdrive/common/util.h"
#include "cereal/messaging/messaging.h"
-const QString MAPBOX_TOKEN = util::getenv("MAPBOX_TOKEN").c_str();
-const QString MAPS_HOST = util::getenv("MAPS_HOST", MAPBOX_TOKEN.isEmpty() ? "https://maps.comma.ai" : "https://api.mapbox.com").c_str();
-
class MapInstructions : public QWidget {
Q_OBJECT
diff --git a/selfdrive/ui/qt/maps/map_helpers.cc b/selfdrive/ui/qt/maps/map_helpers.cc
index 422ae99b6b..f87e80403f 100644
--- a/selfdrive/ui/qt/maps/map_helpers.cc
+++ b/selfdrive/ui/qt/maps/map_helpers.cc
@@ -4,7 +4,25 @@
#include
#include "selfdrive/common/params.h"
+#include "selfdrive/hardware/hw.h"
+#include "selfdrive/ui/qt/api.h"
+QString get_mapbox_token() {
+ // Valid for 4 weeks since we can't swap tokens on the fly
+ return MAPBOX_TOKEN.isEmpty() ? CommaApi::create_jwt({}, 4 * 7 * 24 * 3600) : MAPBOX_TOKEN;
+}
+
+QMapboxGLSettings get_mapbox_settings() {
+ QMapboxGLSettings settings;
+
+ if (!Hardware::PC()) {
+ settings.setCacheDatabasePath(MAPS_CACHE_PATH);
+ }
+ settings.setApiBaseUrl(MAPS_HOST);
+ settings.setAccessToken(get_mapbox_token());
+
+ return settings;
+}
QGeoCoordinate to_QGeoCoordinate(const QMapbox::Coordinate &in) {
return QGeoCoordinate(in.first, in.second);
@@ -83,6 +101,48 @@ QMapbox::CoordinatesCollections coordinate_list_to_collection(QList polyline_to_coordinate_list(const QString &polylineString) {
+ QList path;
+ if (polylineString.isEmpty())
+ return path;
+
+ QByteArray data = polylineString.toLatin1();
+
+ bool parsingLatitude = true;
+
+ int shift = 0;
+ int value = 0;
+
+ QGeoCoordinate coord(0, 0);
+
+ for (int i = 0; i < data.length(); ++i) {
+ unsigned char c = data.at(i) - 63;
+
+ value |= (c & 0x1f) << shift;
+ shift += 5;
+
+ // another chunk
+ if (c & 0x20)
+ continue;
+
+ int diff = (value & 1) ? ~(value >> 1) : (value >> 1);
+
+ if (parsingLatitude) {
+ coord.setLatitude(coord.latitude() + (double)diff/1e6);
+ } else {
+ coord.setLongitude(coord.longitude() + (double)diff/1e6);
+ path.append(coord);
+ }
+
+ parsingLatitude = !parsingLatitude;
+
+ value = 0;
+ shift = 0;
+ }
+
+ return path;
+}
+
static QGeoCoordinate sub(QGeoCoordinate v, QGeoCoordinate w) {
return QGeoCoordinate(v.latitude() - w.latitude(), v.longitude() - w.longitude());
}
diff --git a/selfdrive/ui/qt/maps/map_helpers.h b/selfdrive/ui/qt/maps/map_helpers.h
index 53a44f42d8..1c8cacbebc 100644
--- a/selfdrive/ui/qt/maps/map_helpers.h
+++ b/selfdrive/ui/qt/maps/map_helpers.h
@@ -5,12 +5,17 @@
#include
#include
+#include "selfdrive/common/util.h"
#include "common/transformations/coordinates.hpp"
#include "common/transformations/orientation.hpp"
#include "cereal/messaging/messaging.h"
-#define RAD2DEG(x) ((x) * 180.0 / M_PI)
+const QString MAPBOX_TOKEN = util::getenv("MAPBOX_TOKEN").c_str();
+const QString MAPS_HOST = util::getenv("MAPS_HOST", MAPBOX_TOKEN.isEmpty() ? "https://maps.comma.ai" : "https://api.mapbox.com").c_str();
+const QString MAPS_CACHE_PATH = "/data/mbgl-cache-navd.db";
+QString get_mapbox_token();
+QMapboxGLSettings get_mapbox_settings();
QGeoCoordinate to_QGeoCoordinate(const QMapbox::Coordinate &in);
QMapbox::CoordinatesCollections model_to_collection(
const cereal::LiveLocationKalman::Measurement::Reader &calibratedOrientationECEF,
@@ -19,6 +24,7 @@ QMapbox::CoordinatesCollections model_to_collection(
QMapbox::CoordinatesCollections coordinate_to_collection(QMapbox::Coordinate c);
QMapbox::CoordinatesCollections capnp_coordinate_list_to_collection(const capnp::List::Reader &coordinate_list);
QMapbox::CoordinatesCollections coordinate_list_to_collection(QList coordinate_list);
+QList polyline_to_coordinate_list(const QString &polylineString);
float minimum_distance(QGeoCoordinate a, QGeoCoordinate b, QGeoCoordinate p);
std::optional coordinate_from_param(std::string param);
diff --git a/selfdrive/ui/qt/maps/map_settings.cc b/selfdrive/ui/qt/maps/map_settings.cc
index 60a7812265..e130a9a1ec 100644
--- a/selfdrive/ui/qt/maps/map_settings.cc
+++ b/selfdrive/ui/qt/maps/map_settings.cc
@@ -127,8 +127,7 @@ MapPanel::MapPanel(QWidget* parent) : QWidget(parent) {
{
QString url = CommaApi::BASE_URL + "/v1/navigation/" + *dongle_id + "/locations";
RequestRepeater* repeater = new RequestRepeater(this, url, "ApiCache_NavDestinations", 30, true);
- QObject::connect(repeater, &RequestRepeater::receivedResponse, this, &MapPanel::parseResponse);
- QObject::connect(repeater, &RequestRepeater::failedResponse, this, &MapPanel::failedResponse);
+ QObject::connect(repeater, &RequestRepeater::requestDone, this, &MapPanel::parseResponse);
}
// Destination set while offline
@@ -137,8 +136,8 @@ MapPanel::MapPanel(QWidget* parent) : QWidget(parent) {
RequestRepeater* repeater = new RequestRepeater(this, url, "", 10, true);
HttpRequest* deleter = new HttpRequest(this);
- QObject::connect(repeater, &RequestRepeater::receivedResponse, [=](QString resp) {
- if (resp != "null") {
+ QObject::connect(repeater, &RequestRepeater::requestDone, [=](const QString &resp, bool success) {
+ if (success && resp != "null") {
if (params.get("NavDestination").empty()) {
qWarning() << "Setting NavDestination from /next" << resp;
params.put("NavDestination", resp.toStdString());
@@ -183,7 +182,12 @@ void MapPanel::updateCurrentRoute() {
current_widget->setVisible(dest.size() && !doc.isNull());
}
-void MapPanel::parseResponse(const QString &response) {
+void MapPanel::parseResponse(const QString &response, bool success) {
+ if (!success) {
+ stack->setCurrentIndex(1);
+ return;
+ }
+
QJsonDocument doc = QJsonDocument::fromJson(response.trimmed().toUtf8());
if (doc.isNull()) {
qDebug() << "JSON Parse failed on navigation locations";
@@ -283,10 +287,6 @@ void MapPanel::parseResponse(const QString &response) {
repaint();
}
-void MapPanel::failedResponse(const QString &response) {
- stack->setCurrentIndex(1);
-}
-
void MapPanel::navigateTo(const QJsonObject &place) {
QJsonDocument doc(place);
params.put("NavDestination", doc.toJson().toStdString());
diff --git a/selfdrive/ui/qt/maps/map_settings.h b/selfdrive/ui/qt/maps/map_settings.h
index ec409c4962..03720edee7 100644
--- a/selfdrive/ui/qt/maps/map_settings.h
+++ b/selfdrive/ui/qt/maps/map_settings.h
@@ -17,8 +17,7 @@ public:
explicit MapPanel(QWidget* parent = nullptr);
void navigateTo(const QJsonObject &place);
- void parseResponse(const QString &response);
- void failedResponse(const QString &response);
+ void parseResponse(const QString &response, bool success);
void updateCurrentRoute();
void clear();
diff --git a/selfdrive/ui/qt/offroad/driverview.cc b/selfdrive/ui/qt/offroad/driverview.cc
index f988d193e3..6f03e2ed4a 100644
--- a/selfdrive/ui/qt/offroad/driverview.cc
+++ b/selfdrive/ui/qt/offroad/driverview.cc
@@ -12,7 +12,7 @@ DriverViewWindow::DriverViewWindow(QWidget* parent) : QWidget(parent) {
layout = new QStackedLayout(this);
layout->setStackingMode(QStackedLayout::StackAll);
- cameraView = new CameraViewWidget(VISION_STREAM_RGB_FRONT, true, this);
+ cameraView = new CameraViewWidget("camerad", VISION_STREAM_RGB_FRONT, true, this);
layout->addWidget(cameraView);
scene = new DriverViewScene(this);
diff --git a/selfdrive/ui/qt/offroad/networking.h b/selfdrive/ui/qt/offroad/networking.h
index b5a0c2ba2e..037ef82f67 100644
--- a/selfdrive/ui/qt/offroad/networking.h
+++ b/selfdrive/ui/qt/offroad/networking.h
@@ -1,7 +1,6 @@
#pragma once
#include
-#include
#include
#include
diff --git a/selfdrive/ui/qt/offroad/onboarding.cc b/selfdrive/ui/qt/offroad/onboarding.cc
index aa03ac63db..e6593f7679 100644
--- a/selfdrive/ui/qt/offroad/onboarding.cc
+++ b/selfdrive/ui/qt/offroad/onboarding.cc
@@ -145,7 +145,7 @@ void DeclinePage::showEvent(QShowEvent *event) {
QObject::connect(back_btn, &QPushButton::clicked, this, &DeclinePage::getBack);
- QPushButton *uninstall_btn = new QPushButton("Decline, uninstall " + getBrand());
+ QPushButton *uninstall_btn = new QPushButton(QString("Decline, uninstall %1").arg(getBrand()));
uninstall_btn->setStyleSheet("background-color: #B73D3D");
buttons->addWidget(uninstall_btn);
QObject::connect(uninstall_btn, &QPushButton::clicked, [=]() {
diff --git a/selfdrive/ui/qt/offroad/settings.cc b/selfdrive/ui/qt/offroad/settings.cc
index 7caa29c428..e5438f7c85 100644
--- a/selfdrive/ui/qt/offroad/settings.cc
+++ b/selfdrive/ui/qt/offroad/settings.cc
@@ -109,74 +109,53 @@ TogglesPanel::TogglesPanel(SettingsWindow *parent) : ListWidget(parent) {
}
}
-DevicePanel::DevicePanel(QWidget* parent) : ListWidget(parent) {
- setSpacing(50);
+DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) {
Params params = Params();
- addItem(new LabelControl("Dongle ID", getDongleId().value_or("N/A")));
- QString serial = QString::fromStdString(params.get("HardwareSerial", false));
- addItem(new LabelControl("Serial", serial));
+ setSpacing(50);
+ addItem(new LabelControl("Dongle ID", getDongleId().value_or("N/A")));
+ addItem(new LabelControl("Serial", params.get("HardwareSerial").c_str()));
// offroad-only buttons
auto dcamBtn = new ButtonControl("Driver Camera", "PREVIEW",
- "Preview the driver facing camera to help optimize device mounting position for best driver monitoring experience. (vehicle must be off)");
+ "Preview the driver facing camera to help optimize device mounting position for best driver monitoring experience. (vehicle must be off)");
connect(dcamBtn, &ButtonControl::clicked, [=]() { emit showDriverView(); });
+ addItem(dcamBtn);
- QString resetCalibDesc = "openpilot requires the device to be mounted within 4° left or right and within 5° up or down. openpilot is continuously calibrating, resetting is rarely required.";
- auto resetCalibBtn = new ButtonControl("Reset Calibration", "RESET", resetCalibDesc);
- connect(resetCalibBtn, &ButtonControl::clicked, [=]() {
+ auto resetCalibBtn = new ButtonControl("Reset Calibration", "RESET", " ");
+ connect(resetCalibBtn, &ButtonControl::showDescription, this, &DevicePanel::updateCalibDescription);
+ connect(resetCalibBtn, &ButtonControl::clicked, [&]() {
if (ConfirmationDialog::confirm("Are you sure you want to reset calibration?", this)) {
- Params().remove("CalibrationParams");
+ params.remove("CalibrationParams");
}
});
- connect(resetCalibBtn, &ButtonControl::showDescription, [=]() {
- QString desc = resetCalibDesc;
- std::string calib_bytes = Params().get("CalibrationParams");
- if (!calib_bytes.empty()) {
- try {
- AlignedBuffer aligned_buf;
- capnp::FlatArrayMessageReader cmsg(aligned_buf.align(calib_bytes.data(), calib_bytes.size()));
- auto calib = cmsg.getRoot().getLiveCalibration();
- if (calib.getCalStatus() != 0) {
- double pitch = calib.getRpyCalib()[1] * (180 / M_PI);
- double yaw = calib.getRpyCalib()[2] * (180 / M_PI);
- desc += QString(" Your device is pointed %1° %2 and %3° %4.")
- .arg(QString::number(std::abs(pitch), 'g', 1), pitch > 0 ? "up" : "down",
- QString::number(std::abs(yaw), 'g', 1), yaw > 0 ? "right" : "left");
- }
- } catch (kj::Exception) {
- qInfo() << "invalid CalibrationParams";
- }
- }
- resetCalibBtn->setDescription(desc);
- });
+ addItem(resetCalibBtn);
- ButtonControl *retrainingBtn = nullptr;
if (!params.getBool("Passive")) {
- retrainingBtn = new ButtonControl("Review Training Guide", "REVIEW", "Review the rules, features, and limitations of openpilot");
+ auto retrainingBtn = new ButtonControl("Review Training Guide", "REVIEW", "Review the rules, features, and limitations of openpilot");
connect(retrainingBtn, &ButtonControl::clicked, [=]() {
if (ConfirmationDialog::confirm("Are you sure you want to review the training guide?", this)) {
emit reviewTrainingGuide();
}
});
+ addItem(retrainingBtn);
}
- ButtonControl *regulatoryBtn = nullptr;
if (Hardware::TICI()) {
- regulatoryBtn = new ButtonControl("Regulatory", "VIEW", "");
+ auto regulatoryBtn = new ButtonControl("Regulatory", "VIEW", "");
connect(regulatoryBtn, &ButtonControl::clicked, [=]() {
const std::string txt = util::read_file("../assets/offroad/fcc.html");
RichTextDialog::alert(QString::fromStdString(txt), this);
});
+ addItem(regulatoryBtn);
}
- for (auto btn : {dcamBtn, resetCalibBtn, retrainingBtn, regulatoryBtn}) {
- if (btn) {
- connect(parent, SIGNAL(offroadTransition(bool)), btn, SLOT(setEnabled(bool)));
- addItem(btn);
+ QObject::connect(parent, &SettingsWindow::offroadTransition, [=](bool offroad) {
+ for (auto btn : findChildren()) {
+ btn->setEnabled(offroad);
}
- }
+ });
// power buttons
QHBoxLayout *power_layout = new QHBoxLayout();
@@ -185,34 +164,12 @@ DevicePanel::DevicePanel(QWidget* parent) : ListWidget(parent) {
QPushButton *reboot_btn = new QPushButton("Reboot");
reboot_btn->setObjectName("reboot_btn");
power_layout->addWidget(reboot_btn);
- QObject::connect(reboot_btn, &QPushButton::clicked, [=]() {
- if (QUIState::ui_state.status == UIStatus::STATUS_DISENGAGED) {
- if (ConfirmationDialog::confirm("Are you sure you want to reboot?", this)) {
- // Check engaged again in case it changed while the dialog was open
- if (QUIState::ui_state.status == UIStatus::STATUS_DISENGAGED) {
- Params().putBool("DoReboot", true);
- }
- }
- } else {
- ConfirmationDialog::alert("Disengage to Reboot", this);
- }
- });
+ QObject::connect(reboot_btn, &QPushButton::clicked, this, &DevicePanel::reboot);
QPushButton *poweroff_btn = new QPushButton("Power Off");
poweroff_btn->setObjectName("poweroff_btn");
power_layout->addWidget(poweroff_btn);
- QObject::connect(poweroff_btn, &QPushButton::clicked, [=]() {
- if (QUIState::ui_state.status == UIStatus::STATUS_DISENGAGED) {
- if (ConfirmationDialog::confirm("Are you sure you want to power off?", this)) {
- // Check engaged again in case it changed while the dialog was open
- if (QUIState::ui_state.status == UIStatus::STATUS_DISENGAGED) {
- Params().putBool("DoShutdown", true);
- }
- }
- } else {
- ConfirmationDialog::alert("Disengage to Power Off", this);
- }
- });
+ QObject::connect(poweroff_btn, &QPushButton::clicked, this, &DevicePanel::poweroff);
setStyleSheet(R"(
QPushButton {
@@ -227,6 +184,56 @@ DevicePanel::DevicePanel(QWidget* parent) : ListWidget(parent) {
addItem(power_layout);
}
+void DevicePanel::updateCalibDescription() {
+ QString desc =
+ "openpilot requires the device to be mounted within 4° left or right and "
+ "within 5° up or down. openpilot is continuously calibrating, resetting is rarely required.";
+ std::string calib_bytes = Params().get("CalibrationParams");
+ if (!calib_bytes.empty()) {
+ try {
+ AlignedBuffer aligned_buf;
+ capnp::FlatArrayMessageReader cmsg(aligned_buf.align(calib_bytes.data(), calib_bytes.size()));
+ auto calib = cmsg.getRoot().getLiveCalibration();
+ if (calib.getCalStatus() != 0) {
+ double pitch = calib.getRpyCalib()[1] * (180 / M_PI);
+ double yaw = calib.getRpyCalib()[2] * (180 / M_PI);
+ desc += QString(" Your device is pointed %1° %2 and %3° %4.")
+ .arg(QString::number(std::abs(pitch), 'g', 1), pitch > 0 ? "up" : "down",
+ QString::number(std::abs(yaw), 'g', 1), yaw > 0 ? "right" : "left");
+ }
+ } catch (kj::Exception) {
+ qInfo() << "invalid CalibrationParams";
+ }
+ }
+ qobject_cast(sender())->setDescription(desc);
+}
+
+void DevicePanel::reboot() {
+ if (QUIState::ui_state.status == UIStatus::STATUS_DISENGAGED) {
+ if (ConfirmationDialog::confirm("Are you sure you want to reboot?", this)) {
+ // Check engaged again in case it changed while the dialog was open
+ if (QUIState::ui_state.status == UIStatus::STATUS_DISENGAGED) {
+ Params().putBool("DoReboot", true);
+ }
+ }
+ } else {
+ ConfirmationDialog::alert("Disengage to Reboot", this);
+ }
+}
+
+void DevicePanel::poweroff() {
+ if (QUIState::ui_state.status == UIStatus::STATUS_DISENGAGED) {
+ if (ConfirmationDialog::confirm("Are you sure you want to power off?", this)) {
+ // Check engaged again in case it changed while the dialog was open
+ if (QUIState::ui_state.status == UIStatus::STATUS_DISENGAGED) {
+ Params().putBool("DoShutdown", true);
+ }
+ }
+ } else {
+ ConfirmationDialog::alert("Disengage to Power Off", this);
+ }
+}
+
SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) {
gitBranchLbl = new LabelControl("Git Branch");
gitCommitLbl = new LabelControl("Git Commit");
@@ -246,9 +253,9 @@ SoftwarePanel::SoftwarePanel(QWidget* parent) : ListWidget(parent) {
auto uninstallBtn = new ButtonControl("Uninstall " + getBrand(), "UNINSTALL");
- connect(uninstallBtn, &ButtonControl::clicked, [=]() {
+ connect(uninstallBtn, &ButtonControl::clicked, [&]() {
if (ConfirmationDialog::confirm("Are you sure you want to uninstall?", this)) {
- Params().putBool("DoUninstall", true);
+ params.putBool("DoUninstall", true);
}
});
connect(parent, SIGNAL(offroadTransition(bool)), uninstallBtn, SLOT(setEnabled(bool)));
diff --git a/selfdrive/ui/qt/offroad/settings.h b/selfdrive/ui/qt/offroad/settings.h
index a2c7e01d8f..fe88180662 100644
--- a/selfdrive/ui/qt/offroad/settings.h
+++ b/selfdrive/ui/qt/offroad/settings.h
@@ -38,10 +38,15 @@ private:
class DevicePanel : public ListWidget {
Q_OBJECT
public:
- explicit DevicePanel(QWidget* parent = nullptr);
+ explicit DevicePanel(SettingsWindow *parent);
signals:
void reviewTrainingGuide();
void showDriverView();
+
+private slots:
+ void poweroff();
+ void reboot();
+ void updateCalibDescription();
};
class TogglesPanel : public ListWidget {
diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc
index e7a233000c..5abcf61047 100644
--- a/selfdrive/ui/qt/onroad.cc
+++ b/selfdrive/ui/qt/onroad.cc
@@ -5,9 +5,9 @@
#include "selfdrive/common/timing.h"
#include "selfdrive/ui/paint.h"
#include "selfdrive/ui/qt/util.h"
-#include "selfdrive/ui/qt/api.h"
#ifdef ENABLE_MAPS
#include "selfdrive/ui/qt/maps/map.h"
+#include "selfdrive/ui/qt/maps/map_helpers.h"
#endif
OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) {
@@ -68,19 +68,7 @@ void OnroadWindow::offroadTransition(bool offroad) {
#ifdef ENABLE_MAPS
if (!offroad) {
if (map == nullptr && (QUIState::ui_state.has_prime || !MAPBOX_TOKEN.isEmpty())) {
- QMapboxGLSettings settings;
-
- // Valid for 4 weeks since we can't swap tokens on the fly
- QString token = MAPBOX_TOKEN.isEmpty() ? CommaApi::create_jwt({}, 4 * 7 * 24 * 3600) : MAPBOX_TOKEN;
-
- if (!Hardware::PC()) {
- settings.setCacheDatabasePath("/data/mbgl-cache.db");
- }
- settings.setApiBaseUrl(MAPS_HOST);
- settings.setCacheDatabaseMaximumSize(20 * 1024 * 1024);
- settings.setAccessToken(token.trimmed());
-
- MapWindow * m = new MapWindow(settings);
+ MapWindow * m = new MapWindow(get_mapbox_settings());
m->setFixedWidth(topWidget(this)->width() / 2);
QObject::connect(this, &OnroadWindow::offroadTransitionSignal, m, &MapWindow::offroadTransition);
split->addWidget(m, 0, Qt::AlignRight);
diff --git a/selfdrive/ui/qt/onroad.h b/selfdrive/ui/qt/onroad.h
index ee44973cf2..1076ed01e6 100644
--- a/selfdrive/ui/qt/onroad.h
+++ b/selfdrive/ui/qt/onroad.h
@@ -29,7 +29,7 @@ class NvgWindow : public CameraViewWidget {
Q_OBJECT
public:
- explicit NvgWindow(VisionStreamType type, QWidget* parent = 0) : CameraViewWidget(type, true, parent) {}
+ explicit NvgWindow(VisionStreamType type, QWidget* parent = 0) : CameraViewWidget("camerad", type, true, parent) {}
protected:
void paintGL() override;
diff --git a/selfdrive/ui/qt/request_repeater.cc b/selfdrive/ui/qt/request_repeater.cc
index c11698d6f1..d2c0f9bc30 100644
--- a/selfdrive/ui/qt/request_repeater.cc
+++ b/selfdrive/ui/qt/request_repeater.cc
@@ -15,10 +15,10 @@ RequestRepeater::RequestRepeater(QObject *parent, const QString &requestURL, con
if (!cacheKey.isEmpty()) {
prevResp = QString::fromStdString(params.get(cacheKey.toStdString()));
if (!prevResp.isEmpty()) {
- QTimer::singleShot(500, [=]() { emit receivedResponse(prevResp); });
+ QTimer::singleShot(500, [=]() { emit requestDone(prevResp, true); });
}
- QObject::connect(this, &HttpRequest::receivedResponse, [=](const QString &resp) {
- if (resp != prevResp) {
+ QObject::connect(this, &HttpRequest::requestDone, [=](const QString &resp, bool success) {
+ if (success && resp != prevResp) {
params.put(cacheKey.toStdString(), resp.toStdString());
prevResp = resp;
}
diff --git a/selfdrive/ui/qt/setup/setup.cc b/selfdrive/ui/qt/setup/setup.cc
index a855813688..bd494327cc 100644
--- a/selfdrive/ui/qt/setup/setup.cc
+++ b/selfdrive/ui/qt/setup/setup.cc
@@ -169,7 +169,7 @@ QWidget * Setup::network_setup() {
// setup timer for testing internet connection
HttpRequest *request = new HttpRequest(this, false, 2500);
- QObject::connect(request, &HttpRequest::requestDone, [=](bool success) {
+ QObject::connect(request, &HttpRequest::requestDone, [=](const QString &, bool success) {
cont->setEnabled(success);
if (success) {
const bool cell = networking->wifi->currentNetworkType() == NetworkType::CELL;
diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc
index 1daabd7d1e..f09b1b1796 100644
--- a/selfdrive/ui/qt/widgets/cameraview.cc
+++ b/selfdrive/ui/qt/widgets/cameraview.cc
@@ -94,11 +94,10 @@ mat4 get_fit_view_transform(float widget_aspect_ratio, float frame_aspect_ratio)
} // namespace
-CameraViewWidget::CameraViewWidget(VisionStreamType type, bool zoom, QWidget* parent) :
- stream_type(type), zoomed_view(zoom), QOpenGLWidget(parent) {
+CameraViewWidget::CameraViewWidget(std::string stream_name, VisionStreamType type, bool zoom, QWidget* parent) :
+ stream_name(stream_name), stream_type(type), zoomed_view(zoom), QOpenGLWidget(parent) {
setAttribute(Qt::WA_OpaquePaintEvent);
connect(this, &CameraViewWidget::vipcThreadConnected, this, &CameraViewWidget::vipcConnected, Qt::BlockingQueuedConnection);
- connect(this, &CameraViewWidget::vipcThreadFrameReceived, this, &CameraViewWidget::vipcFrameReceived);
}
CameraViewWidget::~CameraViewWidget() {
@@ -114,7 +113,7 @@ CameraViewWidget::~CameraViewWidget() {
void CameraViewWidget::initializeGL() {
initializeOpenGLFunctions();
- program = new QOpenGLShaderProgram(context());
+ program = std::make_unique(context());
bool ret = program->addShaderFromSourceCode(QOpenGLShader::Vertex, frame_vertex_shader);
assert(ret);
ret = program->addShaderFromSourceCode(QOpenGLShader::Fragment, frame_fragment_shader);
@@ -152,17 +151,22 @@ void CameraViewWidget::initializeGL() {
}
void CameraViewWidget::showEvent(QShowEvent *event) {
- latest_frame = nullptr;
- vipc_thread = new QThread();
- connect(vipc_thread, &QThread::started, [=]() { vipcThread(); });
- connect(vipc_thread, &QThread::finished, vipc_thread, &QObject::deleteLater);
- vipc_thread->start();
+ latest_texture_id = -1;
+ if (!vipc_thread) {
+ vipc_thread = new QThread();
+ connect(vipc_thread, &QThread::started, [=]() { vipcThread(); });
+ connect(vipc_thread, &QThread::finished, vipc_thread, &QObject::deleteLater);
+ vipc_thread->start();
+ }
}
void CameraViewWidget::hideEvent(QHideEvent *event) {
- vipc_thread->requestInterruption();
- vipc_thread->quit();
- vipc_thread->wait();
+ if (vipc_thread) {
+ vipc_thread->requestInterruption();
+ vipc_thread->quit();
+ vipc_thread->wait();
+ vipc_thread = nullptr;
+ }
}
void CameraViewWidget::updateFrameMat(int w, int h) {
@@ -195,18 +199,17 @@ void CameraViewWidget::updateFrameMat(int w, int h) {
}
void CameraViewWidget::paintGL() {
- if (!latest_frame) {
+ if (latest_texture_id == -1) {
glClearColor(bg.redF(), bg.greenF(), bg.blueF(), bg.alphaF());
glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
return;
}
- std::unique_lock lk(texture_lock);
glViewport(0, 0, width(), height());
glBindVertexArray(frame_vao);
glActiveTexture(GL_TEXTURE0);
- glBindTexture(GL_TEXTURE_2D, texture[latest_frame->idx]->frame_tex);
+ glBindTexture(GL_TEXTURE_2D, texture[latest_texture_id]->frame_tex);
glUseProgram(program->programId());
glUniform1i(program->uniformLocation("uTexture"), 0);
@@ -234,17 +237,12 @@ void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) {
glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_SWIZZLE_B, GL_RED);
assert(glGetError() == GL_NO_ERROR);
}
- latest_frame = nullptr;
+ latest_texture_id = -1;
stream_width = vipc_client->buffers[0].width;
stream_height = vipc_client->buffers[0].height;
updateFrameMat(width(), height());
}
-void CameraViewWidget::vipcFrameReceived(VisionBuf *buf) {
- latest_frame = buf;
- update();
-}
-
void CameraViewWidget::vipcThread() {
VisionStreamType cur_stream_type = stream_type;
std::unique_ptr vipc_client;
@@ -271,7 +269,7 @@ void CameraViewWidget::vipcThread() {
while (!QThread::currentThread()->isInterruptionRequested()) {
if (!vipc_client || cur_stream_type != stream_type) {
cur_stream_type = stream_type;
- vipc_client.reset(new VisionIpcClient("camerad", cur_stream_type, true));
+ vipc_client.reset(new VisionIpcClient(stream_name, cur_stream_type, true));
}
if (!vipc_client->connected) {
@@ -293,8 +291,6 @@ void CameraViewWidget::vipcThread() {
if (VisionBuf *buf = vipc_client->recv(nullptr, 1000)) {
if (!Hardware::EON()) {
- std::unique_lock lk(texture_lock);
-
void *texture_buffer = gl_buffer->map(QOpenGLBuffer::WriteOnly);
memcpy(texture_buffer, buf->addr, buf->len);
gl_buffer->unmap();
@@ -304,13 +300,15 @@ void CameraViewWidget::vipcThread() {
glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, buf->width, buf->height, GL_RGB, GL_UNSIGNED_BYTE, 0);
glBindTexture(GL_TEXTURE_2D, 0);
assert(glGetError() == GL_NO_ERROR);
-
- emit vipcThreadFrameReceived(buf);
-
- glFlush();
- } else {
- emit vipcThreadFrameReceived(buf);
+ // use glFinish to ensure that the texture has been uploaded.
+ glFinish();
}
+ latest_texture_id = buf->idx;
+ // Schedule update. update() will be invoked on the gui thread.
+ QMetaObject::invokeMethod(this, "update");
+
+ // TODO: remove later, it's only connected by DriverView.
+ emit vipcThreadFrameReceived(buf);
}
}
}
diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h
index 90622c6b77..146528cd23 100644
--- a/selfdrive/ui/qt/widgets/cameraview.h
+++ b/selfdrive/ui/qt/widgets/cameraview.h
@@ -15,7 +15,7 @@ class CameraViewWidget : public QOpenGLWidget, protected QOpenGLFunctions {
public:
using QOpenGLWidget::QOpenGLWidget;
- explicit CameraViewWidget(VisionStreamType stream_type, bool zoom, QWidget* parent = nullptr);
+ explicit CameraViewWidget(std::string stream_name, VisionStreamType stream_type, bool zoom, QWidget* parent = nullptr);
~CameraViewWidget();
void setStreamType(VisionStreamType type) { stream_type = type; }
void setBackgroundColor(const QColor &color) { bg = color; }
@@ -36,21 +36,20 @@ protected:
void vipcThread();
bool zoomed_view;
- VisionBuf *latest_frame = nullptr;
+ std::atomic latest_texture_id = -1;
GLuint frame_vao, frame_vbo, frame_ibo;
mat4 frame_mat;
std::unique_ptr texture[UI_BUF_COUNT];
- QOpenGLShaderProgram *program;
+ std::unique_ptr program;
QColor bg = QColor("#000000");
+ std::string stream_name;
int stream_width = 0;
int stream_height = 0;
std::atomic stream_type;
QThread *vipc_thread = nullptr;
- std::mutex texture_lock;
protected slots:
void vipcConnected(VisionIpcClient *vipc_client);
- void vipcFrameReceived(VisionBuf *buf);
};
diff --git a/selfdrive/ui/qt/widgets/drive_stats.cc b/selfdrive/ui/qt/widgets/drive_stats.cc
index bd1dc71766..f4c8f502a3 100644
--- a/selfdrive/ui/qt/widgets/drive_stats.cc
+++ b/selfdrive/ui/qt/widgets/drive_stats.cc
@@ -48,7 +48,7 @@ DriveStats::DriveStats(QWidget* parent) : QFrame(parent) {
if (auto dongleId = getDongleId()) {
QString url = CommaApi::BASE_URL + "/v1.1/devices/" + *dongleId + "/stats";
RequestRepeater* repeater = new RequestRepeater(this, url, "ApiCache_DriveStats", 30);
- QObject::connect(repeater, &RequestRepeater::receivedResponse, this, &DriveStats::parseResponse);
+ QObject::connect(repeater, &RequestRepeater::requestDone, this, &DriveStats::parseResponse);
}
setStyleSheet(R"(
@@ -76,7 +76,9 @@ void DriveStats::updateStats() {
update(json["week"].toObject(), week_);
}
-void DriveStats::parseResponse(const QString& response) {
+void DriveStats::parseResponse(const QString& response, bool success) {
+ if (!success) return;
+
QJsonDocument doc = QJsonDocument::fromJson(response.trimmed().toUtf8());
if (doc.isNull()) {
qDebug() << "JSON Parse failed on getting past drives statistics";
diff --git a/selfdrive/ui/qt/widgets/drive_stats.h b/selfdrive/ui/qt/widgets/drive_stats.h
index 40ecbdeaf2..9446294516 100644
--- a/selfdrive/ui/qt/widgets/drive_stats.h
+++ b/selfdrive/ui/qt/widgets/drive_stats.h
@@ -21,5 +21,5 @@ private:
} all_, week_;
private slots:
- void parseResponse(const QString &response);
+ void parseResponse(const QString &response, bool success);
};
diff --git a/selfdrive/ui/qt/widgets/keyboard.cc b/selfdrive/ui/qt/widgets/keyboard.cc
index e6e60e4097..1c59686535 100644
--- a/selfdrive/ui/qt/widgets/keyboard.cc
+++ b/selfdrive/ui/qt/widgets/keyboard.cc
@@ -126,7 +126,7 @@ Keyboard::Keyboard(QWidget *parent) : QFrame(parent) {
std::vector> specials = {
{"[","]","{","}","#","%","^","*","+","="},
{"_","\\","|","~","<",">","€","£","¥","•"},
- {"123",".",",","?","!","`",BACKSPACE_KEY},
+ {"123",".",",","?","!","'",BACKSPACE_KEY},
{"ABC"," ",".",ENTER_KEY},
};
main_layout->addWidget(new KeyboardLayout(this, specials));
diff --git a/selfdrive/ui/qt/widgets/prime.cc b/selfdrive/ui/qt/widgets/prime.cc
index 665466df5e..ab6dc67d36 100644
--- a/selfdrive/ui/qt/widgets/prime.cc
+++ b/selfdrive/ui/qt/widgets/prime.cc
@@ -161,7 +161,7 @@ PrimeUserWidget::PrimeUserWidget(QWidget* parent) : QWidget(parent) {
if (auto dongleId = getDongleId()) {
QString url = CommaApi::BASE_URL + "/v1/devices/" + *dongleId + "/owner";
RequestRepeater *repeater = new RequestRepeater(this, url, "ApiCache_Owner", 6);
- QObject::connect(repeater, &RequestRepeater::receivedResponse, this, &PrimeUserWidget::replyFinished);
+ QObject::connect(repeater, &RequestRepeater::requestDone, this, &PrimeUserWidget::replyFinished);
}
}
@@ -291,14 +291,14 @@ SetupWidget::SetupWidget(QWidget* parent) : QFrame(parent) {
QString url = CommaApi::BASE_URL + "/v1.1/devices/" + *dongleId + "/";
RequestRepeater* repeater = new RequestRepeater(this, url, "ApiCache_Device", 5);
- QObject::connect(repeater, &RequestRepeater::failedResponse, this, &SetupWidget::show);
- QObject::connect(repeater, &RequestRepeater::receivedResponse, this, &SetupWidget::replyFinished);
+ QObject::connect(repeater, &RequestRepeater::requestDone, this, &SetupWidget::replyFinished);
}
hide(); // Only show when first request comes back
}
-void SetupWidget::replyFinished(const QString &response) {
+void SetupWidget::replyFinished(const QString &response, bool success) {
show();
+ if (!success) return;
QJsonDocument doc = QJsonDocument::fromJson(response.toUtf8());
if (doc.isNull()) {
diff --git a/selfdrive/ui/qt/widgets/prime.h b/selfdrive/ui/qt/widgets/prime.h
index 6044f06104..f7470fe441 100644
--- a/selfdrive/ui/qt/widgets/prime.h
+++ b/selfdrive/ui/qt/widgets/prime.h
@@ -68,5 +68,5 @@ private:
PrimeUserWidget *primeUser;
private slots:
- void replyFinished(const QString &response);
+ void replyFinished(const QString &response, bool success);
};
diff --git a/selfdrive/ui/qt/widgets/ssh_keys.cc b/selfdrive/ui/qt/widgets/ssh_keys.cc
index c6af2f5cd1..7630e65731 100644
--- a/selfdrive/ui/qt/widgets/ssh_keys.cc
+++ b/selfdrive/ui/qt/widgets/ssh_keys.cc
@@ -41,23 +41,22 @@ void SshControl::refresh() {
void SshControl::getUserKeys(const QString &username) {
HttpRequest *request = new HttpRequest(this, false);
- QObject::connect(request, &HttpRequest::receivedResponse, [=](const QString &resp) {
- if (!resp.isEmpty()) {
- params.put("GithubUsername", username.toStdString());
- params.put("GithubSshKeys", resp.toStdString());
+ QObject::connect(request, &HttpRequest::requestDone, [=](const QString &resp, bool success) {
+ if (success) {
+ if (!resp.isEmpty()) {
+ params.put("GithubUsername", username.toStdString());
+ params.put("GithubSshKeys", resp.toStdString());
+ } else {
+ ConfirmationDialog::alert(QString("Username '%1' has no keys on GitHub").arg(username), this);
+ }
} else {
- ConfirmationDialog::alert("Username '" + username + "' has no keys on GitHub", this);
+ if (request->timeout()) {
+ ConfirmationDialog::alert("Request timed out", this);
+ } else {
+ ConfirmationDialog::alert(QString("Username '%1' doesn't exist on GitHub").arg(username), this);
+ }
}
- refresh();
- request->deleteLater();
- });
- QObject::connect(request, &HttpRequest::failedResponse, [=] {
- ConfirmationDialog::alert("Username '" + username + "' doesn't exist on GitHub", this);
- refresh();
- request->deleteLater();
- });
- QObject::connect(request, &HttpRequest::timeoutResponse, [=] {
- ConfirmationDialog::alert("Request timed out", this);
+
refresh();
request->deleteLater();
});
diff --git a/selfdrive/ui/replay/camera.h b/selfdrive/ui/replay/camera.h
index 8b1fa7620b..21e02292d5 100644
--- a/selfdrive/ui/replay/camera.h
+++ b/selfdrive/ui/replay/camera.h
@@ -32,9 +32,9 @@ protected:
void cameraThread(Camera &cam);
Camera cameras_[MAX_CAMERAS] = {
- {.type = RoadCam, .rgb_type = VISION_STREAM_RGB_BACK, .yuv_type = VISION_STREAM_YUV_BACK},
- {.type = DriverCam, .rgb_type = VISION_STREAM_RGB_FRONT, .yuv_type = VISION_STREAM_YUV_FRONT},
- {.type = WideRoadCam, .rgb_type = VISION_STREAM_RGB_WIDE, .yuv_type = VISION_STREAM_YUV_WIDE},
+ {.type = RoadCam, .rgb_type = VISION_STREAM_RGB_BACK, .yuv_type = VISION_STREAM_ROAD},
+ {.type = DriverCam, .rgb_type = VISION_STREAM_RGB_FRONT, .yuv_type = VISION_STREAM_DRIVER},
+ {.type = WideRoadCam, .rgb_type = VISION_STREAM_RGB_WIDE, .yuv_type = VISION_STREAM_WIDE_ROAD},
};
std::atomic publishing_ = 0;
std::unique_ptr vipc_server_;
diff --git a/selfdrive/ui/replay/filereader.cc b/selfdrive/ui/replay/filereader.cc
index d110255497..84dc76694b 100644
--- a/selfdrive/ui/replay/filereader.cc
+++ b/selfdrive/ui/replay/filereader.cc
@@ -1,12 +1,7 @@
#include "selfdrive/ui/replay/filereader.h"
-#include
-
-#include
-#include
#include
#include
-#include
#include "selfdrive/common/util.h"
#include "selfdrive/ui/replay/util.h"
@@ -31,7 +26,7 @@ std::string FileReader::read(const std::string &file, std::atomic *abort)
} else if (is_remote) {
result = download(file, abort);
if (cache_to_local_ && !result.empty()) {
- std::ofstream fs(local_file, fs.binary | fs.out);
+ std::ofstream fs(local_file, std::ios::binary | std::ios::out);
fs.write(result.data(), result.size());
}
}
@@ -39,23 +34,13 @@ std::string FileReader::read(const std::string &file, std::atomic *abort)
}
std::string FileReader::download(const std::string &url, std::atomic *abort) {
- std::string result;
- size_t remote_file_size = 0;
for (int i = 0; i <= max_retries_ && !(abort && *abort); ++i) {
- if (i > 0) {
- std::cout << "download failed, retrying" << i << std::endl;
- }
- if (remote_file_size <= 0) {
- remote_file_size = getRemoteFileSize(url);
+ std::string result = httpGet(url, chunk_size_, abort);
+ if (!result.empty()) {
+ return result;
}
- if (remote_file_size > 0 && !(abort && *abort)) {
- std::ostringstream oss;
- result.resize(remote_file_size);
- oss.rdbuf()->pubsetbuf(result.data(), result.size());
- int chunks = chunk_size_ > 0 ? std::max(1, (int)std::nearbyint(remote_file_size / (float)chunk_size_)) : 1;
- if (httpMultiPartDownload(url, oss, chunks, remote_file_size, abort)) {
- return result;
- }
+ if (i != max_retries_) {
+ std::cout << "download failed, retrying " << i + 1 << std::endl;
}
}
return {};
diff --git a/selfdrive/ui/replay/filereader.h b/selfdrive/ui/replay/filereader.h
index 06ce14e9f2..34aa91e858 100644
--- a/selfdrive/ui/replay/filereader.h
+++ b/selfdrive/ui/replay/filereader.h
@@ -5,14 +5,14 @@
class FileReader {
public:
- FileReader(bool cache_to_local, int chunk_size = -1, int max_retries = 3)
- : cache_to_local_(cache_to_local), chunk_size_(chunk_size), max_retries_(max_retries) {}
+ FileReader(bool cache_to_local, size_t chunk_size = 0, int retries = 3)
+ : cache_to_local_(cache_to_local), chunk_size_(chunk_size), max_retries_(retries) {}
virtual ~FileReader() {}
std::string read(const std::string &file, std::atomic *abort = nullptr);
private:
std::string download(const std::string &url, std::atomic *abort);
- int chunk_size_;
+ size_t chunk_size_;
int max_retries_;
bool cache_to_local_;
};
diff --git a/selfdrive/ui/replay/framereader.cc b/selfdrive/ui/replay/framereader.cc
index fdbfc0672c..32af922f1f 100644
--- a/selfdrive/ui/replay/framereader.cc
+++ b/selfdrive/ui/replay/framereader.cc
@@ -27,43 +27,46 @@ enum AVPixelFormat get_hw_format(AVCodecContext *ctx, const enum AVPixelFormat *
if (*p == *hw_pix_fmt) return *p;
}
printf("Please run replay with the --no-cuda flag!\n");
- assert(0);
- return AV_PIX_FMT_NONE;
+ // fallback to YUV420p
+ *hw_pix_fmt = AV_PIX_FMT_NONE;
+ return AV_PIX_FMT_YUV420P;
}
} // namespace
-FrameReader::FrameReader(bool local_cache, int chunk_size, int retries) : FileReader(local_cache, chunk_size, retries) {
- input_ctx = avformat_alloc_context();
- sws_frame.reset(av_frame_alloc());
-}
+FrameReader::FrameReader() {}
FrameReader::~FrameReader() {
- for (auto &f : frames_) {
- av_packet_unref(&f.pkt);
+ for (AVPacket *pkt : packets) {
+ av_packet_free(&pkt);
}
if (decoder_ctx) avcodec_free_context(&decoder_ctx);
if (input_ctx) avformat_close_input(&input_ctx);
if (hw_device_ctx) av_buffer_unref(&hw_device_ctx);
- if (rgb_sws_ctx_) sws_freeContext(rgb_sws_ctx_);
- if (yuv_sws_ctx_) sws_freeContext(yuv_sws_ctx_);
-
if (avio_ctx_) {
av_freep(&avio_ctx_->buffer);
avio_context_free(&avio_ctx_);
}
}
-bool FrameReader::load(const std::string &url, bool no_cuda, std::atomic *abort) {
- std::string content = read(url, abort);
- if (content.empty()) return false;
+bool FrameReader::load(const std::string &url, bool no_cuda, std::atomic *abort, bool local_cache, int chunk_size, int retries) {
+ FileReader f(local_cache, chunk_size, retries);
+ std::string data = f.read(url, abort);
+ if (data.empty()) return false;
+
+ return load((std::byte *)data.data(), data.size(), no_cuda, abort);
+}
+
+bool FrameReader::load(const std::byte *data, size_t size, bool no_cuda, std::atomic *abort) {
+ input_ctx = avformat_alloc_context();
+ if (!input_ctx) return false;
struct buffer_data bd = {
- .data = (uint8_t *)content.data(),
+ .data = (const uint8_t*)data,
.offset = 0,
- .size = content.size(),
+ .size = size,
};
const int avio_ctx_buffer_size = 64 * 1024;
unsigned char *avio_ctx_buffer = (unsigned char *)av_malloc(avio_ctx_buffer_size);
@@ -71,11 +74,11 @@ bool FrameReader::load(const std::string &url, bool no_cuda, std::atomic *
input_ctx->pb = avio_ctx_;
input_ctx->probesize = 10 * 1024 * 1024; // 10MB
- int ret = avformat_open_input(&input_ctx, url.c_str(), NULL, NULL);
+ int ret = avformat_open_input(&input_ctx, nullptr, nullptr, nullptr);
if (ret != 0) {
char err_str[1024] = {0};
av_strerror(ret, err_str, std::size(err_str));
- printf("Error loading video - %s - %s\n", err_str, url.c_str());
+ printf("Error loading video - %s\n", err_str);
return false;
}
@@ -96,36 +99,31 @@ bool FrameReader::load(const std::string &url, bool no_cuda, std::atomic *
width = (decoder_ctx->width + 3) & ~3;
height = decoder_ctx->height;
- if (!no_cuda) {
+ if (has_cuda_device && !no_cuda) {
if (!initHardwareDecoder(AV_HWDEVICE_TYPE_CUDA)) {
printf("No CUDA capable device was found. fallback to CPU decoding.\n");
+ } else {
+ nv12toyuv_buffer.resize(getYUVSize());
}
}
- rgb_sws_ctx_ = sws_getContext(decoder_ctx->width, decoder_ctx->height, sws_src_format,
- width, height, AV_PIX_FMT_BGR24,
- SWS_BILINEAR, NULL, NULL, NULL);
- if (!rgb_sws_ctx_) return false;
- yuv_sws_ctx_ = sws_getContext(decoder_ctx->width, decoder_ctx->height, sws_src_format,
- width, height, AV_PIX_FMT_YUV420P,
- SWS_BILINEAR, NULL, NULL, NULL);
- if (!yuv_sws_ctx_) return false;
-
- ret = avcodec_open2(decoder_ctx, decoder, NULL);
+ ret = avcodec_open2(decoder_ctx, decoder, nullptr);
if (ret < 0) return false;
- frames_.reserve(60 * 20); // 20fps, one minute
+ packets.reserve(60 * 20); // 20fps, one minute
while (!(abort && *abort)) {
- Frame &frame = frames_.emplace_back();
- ret = av_read_frame(input_ctx, &frame.pkt);
+ AVPacket *pkt = av_packet_alloc();
+ ret = av_read_frame(input_ctx, pkt);
if (ret < 0) {
- frames_.pop_back();
+ av_packet_free(&pkt);
valid_ = (ret == AVERROR_EOF);
break;
}
+ packets.push_back(pkt);
// some stream seems to contian no keyframes
- key_frames_count_ += frame.pkt.flags & AV_PKT_FLAG_KEY;
+ key_frames_count_ += pkt->flags & AV_PKT_FLAG_KEY;
}
+ valid_ = valid_ && !packets.empty();
return valid_;
}
@@ -145,21 +143,12 @@ bool FrameReader::initHardwareDecoder(AVHWDeviceType hw_device_type) {
int ret = av_hwdevice_ctx_create(&hw_device_ctx, hw_device_type, nullptr, nullptr, 0);
if (ret < 0) {
+ hw_pix_fmt = AV_PIX_FMT_NONE;
+ has_cuda_device = false;
printf("Failed to create specified HW device %d.\n", ret);
return false;
}
- // get sws source format
- AVHWFramesConstraints *hw_frames_const = av_hwdevice_get_hwframe_constraints(hw_device_ctx, nullptr);
- assert(hw_frames_const != 0);
- for (AVPixelFormat *p = hw_frames_const->valid_sw_formats; *p != AV_PIX_FMT_NONE; p++) {
- if (sws_isSupportedInput(*p)) {
- sws_src_format = *p;
- break;
- }
- }
- av_hwframe_constraints_free(&hw_frames_const);
-
decoder_ctx->hw_device_ctx = av_buffer_ref(hw_device_ctx);
decoder_ctx->opaque = &hw_pix_fmt;
decoder_ctx->get_format = get_hw_format;
@@ -168,35 +157,29 @@ bool FrameReader::initHardwareDecoder(AVHWDeviceType hw_device_type) {
bool FrameReader::get(int idx, uint8_t *rgb, uint8_t *yuv) {
assert(rgb || yuv);
- if (!valid_ || idx < 0 || idx >= frames_.size()) {
+ if (!valid_ || idx < 0 || idx >= packets.size()) {
return false;
}
return decode(idx, rgb, yuv);
}
bool FrameReader::decode(int idx, uint8_t *rgb, uint8_t *yuv) {
- auto get_keyframe = [=](int idx) {
- for (int i = idx; i >= 0 && key_frames_count_ > 1; --i) {
- if (frames_[i].pkt.flags & AV_PKT_FLAG_KEY) return i;
- }
- return idx;
- };
-
int from_idx = idx;
- if (idx > 0 && !frames_[idx].decoded && !frames_[idx - 1].decoded) {
- // find the previous keyframe
- from_idx = get_keyframe(idx);
+ if (idx != prev_idx + 1 && key_frames_count_ > 1) {
+ // seeking to the nearest key frame
+ for (int i = idx; i >= 0; --i) {
+ if (packets[i]->flags & AV_PKT_FLAG_KEY) {
+ from_idx = i;
+ break;
+ }
+ }
}
+ prev_idx = idx;
for (int i = from_idx; i <= idx; ++i) {
- Frame &frame = frames_[i];
- if ((!frame.decoded || i == idx) && !frame.failed) {
- AVFrame *f = decodeFrame(&frame.pkt);
- frame.decoded = f != nullptr;
- frame.failed = !frame.decoded;
- if (frame.decoded && i == idx) {
- return copyBuffers(f, rgb, yuv);
- }
+ AVFrame *f = decodeFrame(packets[i]);
+ if (f && i == idx) {
+ return copyBuffers(f, rgb, yuv);
}
}
return false;
@@ -228,27 +211,26 @@ AVFrame *FrameReader::decodeFrame(AVPacket *pkt) {
}
bool FrameReader::copyBuffers(AVFrame *f, uint8_t *rgb, uint8_t *yuv) {
- if (yuv) {
- if (sws_src_format == AV_PIX_FMT_NV12) {
- // libswscale crash if height is not 16 bytes aligned for NV12->YUV420 conversion
- assert(sws_src_format == AV_PIX_FMT_NV12);
+ if (hw_pix_fmt == AV_PIX_FMT_CUDA) {
+ uint8_t *y = yuv ? yuv : nv12toyuv_buffer.data();
+ uint8_t *u = y + width * height;
+ uint8_t *v = u + (width / 2) * (height / 2);
+ libyuv::NV12ToI420(f->data[0], f->linesize[0], f->data[1], f->linesize[1],
+ y, width, u, width / 2, v, width / 2, width, height);
+ libyuv::I420ToRGB24(y, width, u, width / 2, v, width / 2,
+ rgb, width * 3, width, height);
+ } else {
+ if (yuv) {
uint8_t *u = yuv + width * height;
uint8_t *v = u + (width / 2) * (height / 2);
- libyuv::NV12ToI420(f->data[0], f->linesize[0],
- f->data[1], f->linesize[1],
- yuv, width,
- u, width / 2,
- v, width / 2,
- width, height);
- } else {
- av_image_fill_arrays(sws_frame->data, sws_frame->linesize, yuv, AV_PIX_FMT_YUV420P, width, height, 1);
- int ret = sws_scale(yuv_sws_ctx_, (const uint8_t **)f->data, f->linesize, 0, f->height, sws_frame->data, sws_frame->linesize);
- if (ret < 0) return false;
+ memcpy(yuv, f->data[0], width * height);
+ memcpy(u, f->data[1], width / 2 * height / 2);
+ memcpy(v, f->data[2], width / 2 * height / 2);
}
+ libyuv::I420ToRGB24(f->data[0], f->linesize[0],
+ f->data[1], f->linesize[1],
+ f->data[2], f->linesize[2],
+ rgb, width * 3, width, height);
}
-
- // images is going to be written to output buffers, no alignment (align = 1)
- av_image_fill_arrays(sws_frame->data, sws_frame->linesize, rgb, AV_PIX_FMT_BGR24, width, height, 1);
- int ret = sws_scale(rgb_sws_ctx_, (const uint8_t **)f->data, f->linesize, 0, f->height, sws_frame->data, sws_frame->linesize);
- return ret >= 0;
+ return true;
}
diff --git a/selfdrive/ui/replay/framereader.h b/selfdrive/ui/replay/framereader.h
index c7b04138cf..d572b727e5 100644
--- a/selfdrive/ui/replay/framereader.h
+++ b/selfdrive/ui/replay/framereader.h
@@ -9,23 +9,22 @@
extern "C" {
#include
#include
-#include
-#include
}
struct AVFrameDeleter {
void operator()(AVFrame* frame) const { av_frame_free(&frame); }
};
-class FrameReader : protected FileReader {
+class FrameReader {
public:
- FrameReader(bool local_cache = false, int chunk_size = -1, int retries = 0);
+ FrameReader();
~FrameReader();
- bool load(const std::string &url, bool no_cuda = false, std::atomic *abort = nullptr);
+ bool load(const std::string &url, bool no_cuda = false, std::atomic *abort = nullptr, bool local_cache = false, int chunk_size = -1, int retries = 0);
+ bool load(const std::byte *data, size_t size, bool no_cuda = false, std::atomic *abort = nullptr);
bool get(int idx, uint8_t *rgb, uint8_t *yuv);
int getRGBSize() const { return width * height * 3; }
int getYUVSize() const { return width * height * 3 / 2; }
- size_t getFrameCount() const { return frames_.size(); }
+ size_t getFrameCount() const { return packets.size(); }
bool valid() const { return valid_; }
int width = 0, height = 0;
@@ -36,15 +35,8 @@ private:
AVFrame * decodeFrame(AVPacket *pkt);
bool copyBuffers(AVFrame *f, uint8_t *rgb, uint8_t *yuv);
- struct Frame {
- AVPacket pkt = {};
- int decoded = false;
- bool failed = false;
- };
- std::vector frames_;
- AVPixelFormat sws_src_format = AV_PIX_FMT_YUV420P;
- SwsContext *rgb_sws_ctx_ = nullptr, *yuv_sws_ctx_ = nullptr;
- std::unique_ptrav_frame_, sws_frame, hw_frame;
+ std::vector packets;
+ std::unique_ptrav_frame_, hw_frame;
AVFormatContext *input_ctx = nullptr;
AVCodecContext *decoder_ctx = nullptr;
int key_frames_count_ = 0;
@@ -53,4 +45,7 @@ private:
AVPixelFormat hw_pix_fmt = AV_PIX_FMT_NONE;
AVBufferRef *hw_device_ctx = nullptr;
+ std::vector nv12toyuv_buffer;
+ int prev_idx = -1;
+ inline static std::atomic has_cuda_device = true;
};
diff --git a/selfdrive/ui/replay/logreader.cc b/selfdrive/ui/replay/logreader.cc
index 7e7a943f88..8e2836a4ff 100644
--- a/selfdrive/ui/replay/logreader.cc
+++ b/selfdrive/ui/replay/logreader.cc
@@ -1,6 +1,7 @@
#include "selfdrive/ui/replay/logreader.h"
#include
+#include
#include "selfdrive/ui/replay/util.h"
Event::Event(const kj::ArrayPtr &amsg, bool frame) : reader(amsg), frame(frame) {
@@ -26,7 +27,7 @@ Event::Event(const kj::ArrayPtr &amsg, bool frame) : reader(a
// class LogReader
-LogReader::LogReader(bool local_cache, int chunk_size, int retries, size_t memory_pool_block_size) : FileReader(local_cache, chunk_size, retries) {
+LogReader::LogReader(size_t memory_pool_block_size) {
#ifdef HAS_MEMORY_RESOURCE
const size_t buf_size = sizeof(Event) * memory_pool_block_size;
pool_buffer_ = ::operator new(buf_size);
@@ -39,19 +40,32 @@ LogReader::~LogReader() {
for (Event *e : events) {
delete e;
}
+
#ifdef HAS_MEMORY_RESOURCE
delete mbr_;
::operator delete(pool_buffer_);
#endif
}
-bool LogReader::load(const std::string &file, std::atomic *abort) {
- raw_ = decompressBZ2(read(file, abort));
- if (raw_.empty()) return false;
+bool LogReader::load(const std::string &url, std::atomic *abort, bool local_cache, int chunk_size, int retries) {
+ FileReader f(local_cache, chunk_size, retries);
+ std::string data = f.read(url, abort);
+ if (data.empty()) return false;
+
+ return load((std::byte*)data.data(), data.size(), abort);
+}
+
+bool LogReader::load(const std::byte *data, size_t size, std::atomic *abort) {
+ raw_ = decompressBZ2(data, size);
+ if (raw_.empty()) {
+ std::cout << "failed to decompress log" << std::endl;
+ return false;
+ }
+
+ try {
+ kj::ArrayPtr words((const capnp::word *)raw_.data(), raw_.size() / sizeof(capnp::word));
+ while (words.size() > 0) {
- kj::ArrayPtr words((const capnp::word *)raw_.data(), raw_.size() / sizeof(capnp::word));
- while (words.size() > 0) {
- try {
#ifdef HAS_MEMORY_RESOURCE
Event *evt = new (mbr_) Event(words);
#else
@@ -62,20 +76,26 @@ bool LogReader::load(const std::string &file, std::atomic *abort) {
if (evt->which == cereal::Event::ROAD_ENCODE_IDX ||
evt->which == cereal::Event::DRIVER_ENCODE_IDX ||
evt->which == cereal::Event::WIDE_ROAD_ENCODE_IDX) {
+
#ifdef HAS_MEMORY_RESOURCE
Event *frame_evt = new (mbr_) Event(words, true);
#else
Event *frame_evt = new Event(words, true);
#endif
+
events.push_back(frame_evt);
}
words = kj::arrayPtr(evt->reader.getEnd(), words.end());
events.push_back(evt);
- } catch (const kj::Exception &e) {
- return false;
}
+ } catch (const kj::Exception &e) {
+ std::cout << "failed to parse log : " << e.getDescription().cStr() << std::endl;
+ if (events.empty()) return false;
+
+ std::cout << "read " << events.size() << " events from corrupt log" << std::endl;
}
+
std::sort(events.begin(), events.end(), Event::lessThan());
return true;
}
diff --git a/selfdrive/ui/replay/logreader.h b/selfdrive/ui/replay/logreader.h
index 5bb613d9de..33d7ea82f2 100644
--- a/selfdrive/ui/replay/logreader.h
+++ b/selfdrive/ui/replay/logreader.h
@@ -46,11 +46,12 @@ public:
bool frame;
};
-class LogReader : protected FileReader {
+class LogReader {
public:
- LogReader(bool local_cache = false, int chunk_size = -1, int retries = 0, size_t memory_pool_block_size = DEFAULT_EVENT_MEMORY_POOL_BLOCK_SIZE);
+ LogReader(size_t memory_pool_block_size = DEFAULT_EVENT_MEMORY_POOL_BLOCK_SIZE);
~LogReader();
- bool load(const std::string &file, std::atomic *abort = nullptr);
+ bool load(const std::string &url, std::atomic *abort = nullptr, bool local_cache = false, int chunk_size = -1, int retries = 0);
+ bool load(const std::byte *data, size_t size, std::atomic *abort = nullptr);
std::vector events;
diff --git a/selfdrive/ui/replay/route.cc b/selfdrive/ui/replay/route.cc
index 9ee1d387c4..23c27073cb 100644
--- a/selfdrive/ui/replay/route.cc
+++ b/selfdrive/ui/replay/route.cc
@@ -35,10 +35,8 @@ bool Route::load() {
bool Route::loadFromServer() {
QEventLoop loop;
HttpRequest http(nullptr, !Hardware::PC());
- QObject::connect(&http, &HttpRequest::failedResponse, [&] { loop.exit(0); });
- QObject::connect(&http, &HttpRequest::timeoutResponse, [&] { loop.exit(0); });
- QObject::connect(&http, &HttpRequest::receivedResponse, [&](const QString &json) {
- loop.exit(loadFromJson(json));
+ QObject::connect(&http, &HttpRequest::requestDone, [&](const QString &json, bool success) {
+ loop.exit(success ? loadFromJson(json) : 0);
});
http.sendRequest("https://api.commadotai.com/v1/route/" + route_.str + "/files");
return loop.exec();
@@ -118,11 +116,11 @@ void Segment::loadFile(int id, const std::string file) {
const bool local_cache = !(flags & REPLAY_FLAG_NO_FILE_CACHE);
bool success = false;
if (id < MAX_CAMERAS) {
- frames[id] = std::make_unique(local_cache, 20 * 1024 * 1024, 3);
- success = frames[id]->load(file, flags & REPLAY_FLAG_NO_CUDA, &abort_);
+ frames[id] = std::make_unique();
+ success = frames[id]->load(file, flags & REPLAY_FLAG_NO_CUDA, &abort_, local_cache, 20 * 1024 * 1024, 3);
} else {
- log = std::make_unique(local_cache, -1, 3);
- success = log->load(file, &abort_);
+ log = std::make_unique();
+ success = log->load(file, &abort_, local_cache, 0, 3);
}
if (!success) {
diff --git a/selfdrive/ui/replay/tests/test_replay.cc b/selfdrive/ui/replay/tests/test_replay.cc
index f2a2171c82..5b9b7cdeb7 100644
--- a/selfdrive/ui/replay/tests/test_replay.cc
+++ b/selfdrive/ui/replay/tests/test_replay.cc
@@ -1,7 +1,5 @@
#include
#include
-#include
-#include
#include "catch2/catch.hpp"
#include "selfdrive/common/util.h"
@@ -16,19 +14,15 @@ TEST_CASE("httpMultiPartDownload") {
char filename[] = "/tmp/XXXXXX";
close(mkstemp(filename));
+ const size_t chunk_size = 5 * 1024 * 1024;
std::string content;
- auto file_size = getRemoteFileSize(TEST_RLOG_URL);
- REQUIRE(file_size > 0);
- SECTION("5 connections, download to file") {
- std::ofstream of(filename, of.binary | of.out);
- REQUIRE(httpMultiPartDownload(TEST_RLOG_URL, of, 5, file_size));
+ SECTION("download to file") {
+ REQUIRE(httpDownload(TEST_RLOG_URL, filename, chunk_size));
content = util::read_file(filename);
}
- SECTION("5 connection, download to buffer") {
- std::ostringstream oss;
- content.resize(file_size);
- oss.rdbuf()->pubsetbuf(content.data(), content.size());
- REQUIRE(httpMultiPartDownload(TEST_RLOG_URL, oss, 5, file_size));
+ SECTION("download to buffer") {
+ content = httpGet(TEST_RLOG_URL, chunk_size);
+ REQUIRE(!content.empty());
}
REQUIRE(content.size() == 9112651);
REQUIRE(sha256(content) == TEST_RLOG_CHECKSUM);
@@ -56,6 +50,17 @@ TEST_CASE("FileReader") {
}
}
+TEST_CASE("LogReader") {
+ SECTION("corrupt log") {
+ FileReader reader(true);
+ std::string corrupt_content = reader.read(TEST_RLOG_URL);
+ corrupt_content.resize(corrupt_content.length() / 2);
+ LogReader log;
+ REQUIRE(log.load((std::byte *)corrupt_content.data(), corrupt_content.size()));
+ REQUIRE(log.events.size() > 0);
+ }
+}
+
TEST_CASE("Segment") {
auto flags = GENERATE(REPLAY_FLAG_DCAM | REPLAY_FLAG_ECAM, REPLAY_FLAG_QCAMERA);
Route demo_route(DEMO_ROUTE);
diff --git a/selfdrive/ui/replay/util.cc b/selfdrive/ui/replay/util.cc
index b4fcbf7e6b..d6791465f2 100644
--- a/selfdrive/ui/replay/util.cc
+++ b/selfdrive/ui/replay/util.cc
@@ -4,8 +4,10 @@
#include
#include
+#include
#include
-#include
+#include
+#include
#include
#include
#include
