less TICI when not needed (#24698)

* less TICI when not needed

* fix process replay

* move reading voltages into hw abstraction layer

* Update selfdrive/hardware/tici/hardware.h

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

* Update selfdrive/hardware/hw.h

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

* Update selfdrive/hardware/base.h

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

* rename init function

* Update selfdrive/athena/athenad.py

Co-authored-by: Robbe Derks <robbe.derks@gmail.com>

* Update selfdrive/boardd/boardd.cc

* Apply suggestions from code review

* Update selfdrive/thermald/thermald.py

* update ref

* fix alert width if all cameras are bad

* add ecam to test_loggerd

* bump cereal

* bump cereal

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
Co-authored-by: Robbe Derks <robbe.derks@gmail.com>
old-commit-hash: f49a9c9fd2
taco
Willem Melching 3 years ago committed by GitHub
parent 86ec883c1a
commit 753045c73d
  1. 7
      SConstruct
  2. 2
      cereal
  3. 2
      common/modeldata.h
  4. 9
      common/realtime.py
  5. 6
      common/swaglog.cc
  6. 5
      common/tests/test_swaglog.cc
  7. 12
      common/transformations/camera.py
  8. 6
      launch_chffrplus.sh
  9. 6
      selfdrive/athena/athenad.py
  10. 21
      selfdrive/boardd/boardd.cc
  11. 2
      selfdrive/boardd/main.cc
  12. 6
      selfdrive/camerad/cameras/camera_common.cc
  13. 2
      selfdrive/camerad/main.cc
  14. 22
      selfdrive/camerad/snapshot/snapshot.py
  15. 30
      selfdrive/controls/controlsd.py
  16. 2
      selfdrive/controls/lib/events.py
  17. 10
      selfdrive/controls/lib/lane_planner.py
  18. 3
      selfdrive/controls/plannerd.py
  19. 3
      selfdrive/controls/radard.py
  20. 1
      selfdrive/hardware/__init__.py
  21. 6
      selfdrive/hardware/base.h
  22. 3
      selfdrive/hardware/hw.h
  23. 6
      selfdrive/hardware/tici/hardware.h
  24. 2
      selfdrive/loggerd/bootlog.cc
  25. 2
      selfdrive/loggerd/encoderd.cc
  26. 7
      selfdrive/loggerd/logger.cc
  27. 4
      selfdrive/loggerd/loggerd.cc
  28. 12
      selfdrive/loggerd/loggerd.h
  29. 24
      selfdrive/loggerd/tests/test_loggerd.py
  30. 4
      selfdrive/manager/build.py
  31. 6
      selfdrive/manager/process_config.py
  32. 4
      selfdrive/manager/test/test_manager.py
  33. 2
      selfdrive/modeld/modeld.cc
  34. 58
      selfdrive/modeld/models/dmonitoring.cc
  35. 17
      selfdrive/monitoring/driver_monitor.py
  36. 7
      selfdrive/test/process_replay/process_replay.py
  37. 2
      selfdrive/test/process_replay/ref_commit
  38. 6
      selfdrive/thermald/thermald.py
  39. 4
      selfdrive/timezoned.py
  40. 5
      selfdrive/tombstoned.py
  41. 6
      selfdrive/ui/qt/offroad/driverview.cc
  42. 2
      selfdrive/ui/qt/offroad/settings.cc
  43. 28
      selfdrive/ui/qt/widgets/cameraview.cc
  44. 2
      selfdrive/ui/ui.cc
  45. 4
      selfdrive/updated.py

@ -1,5 +1,4 @@
import os
import shutil
import subprocess
import sys
import sysconfig
@ -7,6 +6,8 @@ import platform
import numpy as np
TICI = os.path.isfile('/TICI')
AGNOS = TICI
Decider('MD5-timestamp')
AddOption('--test',
@ -56,7 +57,7 @@ real_arch = arch = subprocess.check_output(["uname", "-m"], encoding='utf8').rst
if platform.system() == "Darwin":
arch = "Darwin"
if arch == "aarch64" and TICI:
if arch == "aarch64" and AGNOS:
arch = "larch64"
USE_WEBCAM = os.getenv("USE_WEBCAM") is not None
@ -226,7 +227,7 @@ if GetOption('compile_db'):
env.CompilationDatabase('compile_commands.json')
# Setup cache dir
cache_dir = '/data/scons_cache' if TICI else '/tmp/scons_cache'
cache_dir = '/data/scons_cache' if AGNOS else '/tmp/scons_cache'
CacheDir(cache_dir)
Clean(["."], cache_dir)

@ -1 +1 @@
Subproject commit bf4960f83ccdefc7e9d2e405878ae6c8efced83b
Subproject commit 6ad4ba689c557ed241862d10aef4235d3e96331f

@ -24,8 +24,6 @@ constexpr auto T_IDXS_FLOAT = build_idxs<float, TRAJECTORY_SIZE>(10.0);
constexpr auto X_IDXS = build_idxs<double, TRAJECTORY_SIZE>(192.0);
constexpr auto X_IDXS_FLOAT = build_idxs<float, TRAJECTORY_SIZE>(192.0);
const int TICI_CAM_WIDTH = 1928;
namespace tici_dm_crop {
const int x_offset = -72;
const int y_offset = -144;

@ -8,19 +8,14 @@ from typing import Optional, List, Union
from setproctitle import getproctitle # pylint: disable=no-name-in-module
from common.clock import sec_since_boot # pylint: disable=no-name-in-module, import-error
from selfdrive.hardware import PC, TICI
from selfdrive.hardware import PC
# time step for each process
DT_CTRL = 0.01 # controlsd
DT_MDL = 0.05 # model
DT_TRML = 0.5 # thermald and manager
# driver monitoring
if TICI:
DT_DMON = 0.05
else:
DT_DMON = 0.1
DT_DMON = 0.05 # driver monitoring
class Priority:

@ -50,11 +50,7 @@ class SwaglogState : public LogState {
ctx_j["dirty"] = !getenv("CLEAN");
// device type
if (Hardware::TICI()) {
ctx_j["device"] = "tici";
} else {
ctx_j["device"] = "pc";
}
ctx_j["device"] = Hardware::get_name();
LogState::initialize();
}
};

@ -56,10 +56,7 @@ void recv_log(int thread_cnt, int thread_msg_cnt) {
REQUIRE(ctx["version"].string_value() == COMMA_VERSION);
std::string device = "pc";
if (Hardware::TICI()) {
device = "tici";
}
std::string device = Hardware::get_name();
REQUIRE(ctx["device"].string_value() == device);
int thread_id = atoi(msg["msg"].string_value().c_str());

@ -1,7 +1,6 @@
import numpy as np
import common.transformations.orientation as orient
from selfdrive.hardware import TICI
## -- hardcoded hardware params --
eon_f_focal_length = 910.0
@ -45,14 +44,9 @@ tici_fcam_intrinsics_inv = np.linalg.inv(tici_fcam_intrinsics)
tici_ecam_intrinsics_inv = np.linalg.inv(tici_ecam_intrinsics)
if not TICI:
FULL_FRAME_SIZE = eon_f_frame_size
FOCAL = eon_f_focal_length
fcam_intrinsics = eon_fcam_intrinsics
else:
FULL_FRAME_SIZE = tici_f_frame_size
FOCAL = tici_f_focal_length
fcam_intrinsics = tici_fcam_intrinsics
FULL_FRAME_SIZE = tici_f_frame_size
FOCAL = tici_f_focal_length
fcam_intrinsics = tici_fcam_intrinsics
W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1]

@ -8,7 +8,7 @@ source "$BASEDIR/launch_env.sh"
DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null && pwd )"
function tici_init {
function agnos_init {
# wait longer for weston to come up
if [ -f "$BASEDIR/prebuilt" ]; then
sleep 3
@ -77,9 +77,7 @@ function launch {
export PYTHONPATH="$PWD:$PWD/pyextra"
# hardware specific init
if [ -f /TICI ]; then
tici_init
fi
agnos_init
# write tmux scrollback to a file
tmux capture-pane -pq -S-1000 > /tmp/launch_log

@ -32,7 +32,7 @@ from common.basedir import PERSIST
from common.file_helpers import CallbackReader
from common.params import Params
from common.realtime import sec_since_boot, set_core_affinity
from selfdrive.hardware import HARDWARE, PC, TICI
from selfdrive.hardware import HARDWARE, PC, AGNOS
from selfdrive.loggerd.config import ROOT
from selfdrive.loggerd.xattr_cache import getxattr, setxattr
from selfdrive.statsd import STATS_DIR
@ -413,8 +413,8 @@ def primeActivated(activated):
@dispatcher.add_method
def setBandwithLimit(upload_speed_kbps, download_speed_kbps):
if not TICI:
return {"success": 0, "error": "only supported on comma three"}
if not AGNOS:
return {"success": 0, "error": "only supported on AGNOS"}
try:
HARDWARE.set_bandwidth_limit(upload_speed_kbps, download_speed_kbps)

@ -405,17 +405,12 @@ void send_peripheral_state(PubMaster *pm, Panda *panda) {
auto ps = evt.initPeripheralState();
ps.setPandaType(panda->hw_type);
if (Hardware::TICI()) {
double read_time = millis_since_boot();
ps.setVoltage(std::atoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input").c_str()));
ps.setCurrent(std::atoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input").c_str()));
read_time = millis_since_boot() - read_time;
if (read_time > 50) {
LOGW("reading hwmon took %lfms", read_time);
}
} else {
ps.setVoltage(pandaState.voltage_pkt);
ps.setCurrent(pandaState.current_pkt);
double read_time = millis_since_boot();
ps.setVoltage(Hardware::get_voltage());
ps.setCurrent(Hardware::get_current());
read_time = millis_since_boot() - read_time;
if (read_time > 50) {
LOGW("reading hwmon took %lfms", read_time);
}
uint16_t fan_speed_rpm = panda->get_fan_speed();
@ -534,9 +529,7 @@ void peripheral_control_thread(Panda *panda) {
int cur_integ_lines = event.getDriverCameraState().getIntegLines();
float cur_gain = event.getDriverCameraState().getGain();
if (Hardware::TICI()) {
cur_integ_lines = integ_lines_filter.update(cur_integ_lines * cur_gain);
}
cur_integ_lines = integ_lines_filter.update(cur_integ_lines * cur_gain);
last_front_frame_t = event.getLogMonoTime();
if (cur_integ_lines <= CUTOFF_IL) {

@ -12,7 +12,7 @@ int main(int argc, char *argv[]) {
int err;
err = util::set_realtime_priority(54);
assert(err == 0);
err = util::set_core_affinity({Hardware::TICI() ? 4 : 3});
err = util::set_core_affinity({4});
assert(err == 0);
}

@ -90,12 +90,6 @@ void CameraBuf::init(cl_device_id device_id, cl_context context, CameraState *s,
rgb_width = ci->frame_width;
rgb_height = ci->frame_height;
if (!Hardware::TICI() && ci->bayer) {
// debayering does a 2x downscale
rgb_width = ci->frame_width / 2;
rgb_height = ci->frame_height / 2;
}
yuv_transform = get_model_yuv_transform(ci->bayer);
vipc_server->create_buffers(rgb_type, UI_BUF_COUNT, true, rgb_width, rgb_height);

@ -7,7 +7,7 @@
#include "selfdrive/hardware/hw.h"
int main(int argc, char *argv[]) {
if (Hardware::TICI()) {
if (!Hardware::PC()) {
int ret;
ret = util::set_realtime_priority(53);
assert(ret == 0);

@ -4,13 +4,12 @@ import time
import numpy as np
from PIL import Image
from typing import List
import cereal.messaging as messaging
from cereal.visionipc import VisionIpcClient, VisionStreamType
from common.params import Params
from common.realtime import DT_MDL
from selfdrive.hardware import TICI, PC
from selfdrive.hardware import PC
from selfdrive.controls.lib.alertmanager import set_offroad_alert
from selfdrive.manager.process_config import managed_processes
@ -52,11 +51,7 @@ def extract_image(buf, w, h, stride, uv_offset):
return yuv_to_rgb(y, u, v)
def rois_in_focus(lapres: List[float]) -> float:
return sum(1. / len(lapres) for sharpness in lapres if sharpness >= LM_THRESH)
def get_snapshots(frame="roadCameraState", front_frame="driverCameraState", focus_perc_threshold=0.):
def get_snapshots(frame="roadCameraState", front_frame="driverCameraState"):
sockets = [s for s in (frame, front_frame) if s is not None]
sm = messaging.SubMaster(sockets)
vipc_clients = {s: VisionIpcClient("camerad", VISION_STREAMS[s], True) for s in sockets}
@ -68,13 +63,6 @@ def get_snapshots(frame="roadCameraState", front_frame="driverCameraState", focu
for client in vipc_clients.values():
client.connect(True)
# wait for focus
start_t = time.monotonic()
while time.monotonic() - start_t < 10:
sm.update(100)
if min(sm.rcv_frame.values()) > 1 and rois_in_focus(sm[frame].sharpnessScore) >= focus_perc_threshold:
break
# grab images
rear, front = None, None
if frame is not None:
@ -113,11 +101,9 @@ def snapshot():
if not PC:
managed_processes['camerad'].start()
frame = "wideRoadCameraState" if TICI else "roadCameraState"
frame = "wideRoadCameraState"
front_frame = "driverCameraState" if front_camera_allowed else None
focus_perc_threshold = 0. if TICI else 10 / 12.
rear, front = get_snapshots(frame, front_frame, focus_perc_threshold)
rear, front = get_snapshots(frame, front_frame)
finally:
managed_processes['camerad'].stop()
params.put_bool("IsTakingSnapshot", False)

@ -27,7 +27,7 @@ from selfdrive.controls.lib.events import Events, ET
from selfdrive.controls.lib.alertmanager import AlertManager, set_offroad_alert
from selfdrive.controls.lib.vehicle_model import VehicleModel
from selfdrive.locationd.calibrationd import Calibration
from selfdrive.hardware import HARDWARE, TICI
from selfdrive.hardware import HARDWARE
from selfdrive.manager.process_config import managed_processes
SOFT_DISABLE_TIME = 3 # seconds
@ -68,17 +68,14 @@ class Controls:
self.pm = messaging.PubMaster(['sendcan', 'controlsState', 'carState',
'carControl', 'carEvents', 'carParams'])
self.camera_packets = ["roadCameraState", "driverCameraState"]
if TICI:
self.camera_packets.append("wideRoadCameraState")
self.camera_packets = ["roadCameraState", "driverCameraState", "wideRoadCameraState"]
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 20
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
if TICI:
self.log_sock = messaging.sub_sock('androidLog')
self.log_sock = messaging.sub_sock('androidLog')
if CI is None:
# wait for one pandaState and one CAN packet
@ -358,17 +355,16 @@ class Controls:
if planner_fcw or model_fcw:
self.events.add(EventName.fcw)
if TICI:
for m in messaging.drain_sock(self.log_sock, wait_for_one=False):
try:
msg = m.androidLog.message
if any(err in msg for err in ("ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW", "APPLY FAILED")):
csid = msg.split("CSID:")[-1].split(" ")[0]
evt = CSID_MAP.get(csid, None)
if evt is not None:
self.events.add(evt)
except UnicodeDecodeError:
pass
for m in messaging.drain_sock(self.log_sock, wait_for_one=False):
try:
msg = m.androidLog.message
if any(err in msg for err in ("ERROR_CRC", "ERROR_ECC", "ERROR_STREAM_UNDERFLOW", "APPLY FAILED")):
csid = msg.split("CSID:")[-1].split(" ")[0]
evt = CSID_MAP.get(csid, None)
if evt is not None:
self.events.add(evt)
except UnicodeDecodeError:
pass
# TODO: fix simulator
if not SIMULATION:

@ -284,7 +284,7 @@ def comm_issue_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaste
def camera_malfunction_alert(CP: car.CarParams, CS: car.CarState, sm: messaging.SubMaster, metric: bool, soft_disable_time: int) -> Alert:
all_cams = ('roadCameraState', 'driverCameraState', 'wideRoadCameraState')
bad_cams = [s for s in all_cams if s in sm.data.keys() and not sm.all_checks([s, ])]
bad_cams = [s.replace('State', '') for s in all_cams if s in sm.data.keys() and not sm.all_checks([s, ])]
return NormalPermanentAlert("Camera Malfunction", ', '.join(bad_cams))

@ -3,20 +3,14 @@ from cereal import log
from common.filter_simple import FirstOrderFilter
from common.numpy_fast import interp
from common.realtime import DT_MDL
from selfdrive.hardware import TICI
from selfdrive.swaglog import cloudlog
TRAJECTORY_SIZE = 33
# camera offset is meters from center car to camera
# model path is in the frame of the camera. Empirically
# the model knows the difference between TICI and EON
# so a path offset is not needed
# model path is in the frame of the camera
PATH_OFFSET = 0.00
if TICI:
CAMERA_OFFSET = 0.04
else:
CAMERA_OFFSET = 0.0
CAMERA_OFFSET = 0.04
class LanePlanner:

@ -5,12 +5,11 @@ from common.realtime import Priority, config_realtime_process
from selfdrive.swaglog import cloudlog
from selfdrive.controls.lib.longitudinal_planner import Planner
from selfdrive.controls.lib.lateral_planner import LateralPlanner
from selfdrive.hardware import TICI
import cereal.messaging as messaging
def plannerd_thread(sm=None, pm=None):
config_realtime_process(5 if TICI else 2, Priority.CTRL_LOW)
config_realtime_process(5, Priority.CTRL_LOW)
cloudlog.info("plannerd is waiting for CarParams")
params = Params()

@ -11,7 +11,6 @@ from common.realtime import Ratekeeper, Priority, config_realtime_process
from selfdrive.controls.lib.cluster.fastcluster_py import cluster_points_centroid
from selfdrive.controls.lib.radar_helpers import Cluster, Track, RADAR_TO_CAMERA
from selfdrive.swaglog import cloudlog
from selfdrive.hardware import TICI
class KalmanParams():
@ -180,7 +179,7 @@ class RadarD():
# fuses camera and radar data for best lead detection
def radard_thread(sm=None, pm=None, can_sock=None):
config_realtime_process(5 if TICI else 2, Priority.CTRL_LOW)
config_realtime_process(5, Priority.CTRL_LOW)
# wait for stats about the car to come in from controls
cloudlog.info("radard is waiting for CarParams")

@ -6,6 +6,7 @@ from selfdrive.hardware.tici.hardware import Tici
from selfdrive.hardware.pc.hardware import Pc
TICI = os.path.isfile('/TICI')
AGNOS = TICI
PC = not TICI

@ -2,6 +2,7 @@
#include <cstdlib>
#include <fstream>
#include "cereal/messaging/messaging.h"
// no-op base hw class
class HardwareNone {
@ -10,6 +11,10 @@ public:
static constexpr float MIN_VOLUME = 0.2;
static std::string get_os_version() { return ""; }
static std::string get_name() { return ""; };
static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::UNKNOWN; };
static int get_voltage() { return 0; };
static int get_current() { return 0; };
static void reboot() {}
static void poweroff() {}
@ -21,4 +26,5 @@ public:
static bool PC() { return false; }
static bool TICI() { return false; }
static bool AGNOS() { return false; }
};

@ -10,8 +10,11 @@
class HardwarePC : public HardwareNone {
public:
static std::string get_os_version() { return "openpilot for PC"; }
static std::string get_name() { return "pc"; };
static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::PC; };
static bool PC() { return true; }
static bool TICI() { return util::getenv("TICI", 0) == 1; }
static bool AGNOS() { return util::getenv("TICI", 0) == 1; }
};
#define Hardware HardwarePC
#endif

@ -12,9 +12,15 @@ public:
static constexpr float MAX_VOLUME = 0.9;
static constexpr float MIN_VOLUME = 0.2;
static bool TICI() { return true; }
static bool AGNOS() { return true; }
static std::string get_os_version() {
return "AGNOS " + util::read_file("/VERSION");
};
static std::string get_name() { return "tici"; };
static cereal::InitData::DeviceType get_device_type() { return cereal::InitData::DeviceType::TICI; };
static int get_voltage() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input").c_str()); };
static int get_current() { return std::atoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input").c_str()); };
static void reboot() { std::system("sudo reboot"); };
static void poweroff() { std::system("sudo poweroff"); };

@ -8,7 +8,7 @@
static kj::Array<capnp::word> build_boot_log() {
std::vector<std::string> bootlog_commands;
if (Hardware::TICI()) {
if (Hardware::AGNOS()) {
bootlog_commands.push_back("journalctl");
bootlog_commands.push_back("sudo nvme smart-log --output-format=json /dev/nvme0");
}

@ -133,7 +133,7 @@ void encoderd_thread() {
}
int main() {
if (Hardware::TICI()) {
if (!Hardware::PC()) {
int ret;
ret = util::set_realtime_priority(52);
assert(ret == 0);

@ -33,12 +33,7 @@ kj::Array<capnp::word> logger_build_init_data() {
MessageBuilder msg;
auto init = msg.initEvent().initInitData();
if (Hardware::TICI()) {
init.setDeviceType(cereal::InitData::DeviceType::TICI);
} else {
init.setDeviceType(cereal::InitData::DeviceType::PC);
}
init.setDeviceType(Hardware::get_device_type());
init.setVersion(COMMA_VERSION);
std::ifstream cmdline_stream("/proc/cmdline");

@ -267,7 +267,7 @@ void loggerd_thread() {
}
int main(int argc, char** argv) {
if (Hardware::TICI()) {
if (!Hardware::PC()) {
int ret;
ret = util::set_core_affinity({0, 1, 2, 3});
assert(ret == 0);
@ -279,4 +279,4 @@ int main(int argc, char** argv) {
loggerd_thread();
return 0;
}
}

@ -33,8 +33,8 @@
#endif
constexpr int MAIN_FPS = 20;
const int MAIN_BITRATE = Hardware::TICI() ? 10000000 : 5000000;
const int DCAM_BITRATE = Hardware::TICI() ? MAIN_BITRATE : 2500000;
const int MAIN_BITRATE = 10000000;
const int DCAM_BITRATE = MAIN_BITRATE;
#define NO_CAMERA_PATIENCE 500 // fall back to time-based rotation if all cameras are dead
@ -89,8 +89,8 @@ const LogCameraInfo cameras_logged[] = {
.bitrate = MAIN_BITRATE,
.is_h265 = true,
.has_qcamera = false,
.enable = Hardware::TICI(),
.record = Hardware::TICI(),
.enable = true,
.record = true,
.frame_width = 1928,
.frame_height = 1208,
},
@ -102,6 +102,6 @@ const LogCameraInfo qcam_info = {
.is_h265 = false,
.enable = true,
.record = true,
.frame_width = Hardware::TICI() ? 526 : 480,
.frame_height = Hardware::TICI() ? 330 : 360 // keep pixel count the same?
.frame_width = 526,
.frame_height = 330,
};

@ -15,14 +15,12 @@ from cereal.services import service_list
from common.basedir import BASEDIR
from common.params import Params
from common.timeout import Timeout
from selfdrive.hardware import TICI
from selfdrive.loggerd.config import ROOT
from selfdrive.manager.process_config import managed_processes
from selfdrive.version import get_version
from tools.lib.logreader import LogReader
from cereal.visionipc import VisionIpcServer, VisionStreamType
from common.transformations.camera import eon_f_frame_size, tici_f_frame_size, \
eon_d_frame_size, tici_d_frame_size, tici_e_frame_size
from common.transformations.camera import tici_f_frame_size, tici_d_frame_size, tici_e_frame_size
SentinelType = log.Sentinel.SentinelType
@ -108,20 +106,15 @@ class TestLoggerd(unittest.TestCase):
os.environ["LOGGERD_TEST"] = "1"
Params().put("RecordFront", "1")
expected_files = {"rlog", "qlog", "qcamera.ts", "fcamera.hevc", "dcamera.hevc"}
streams = [(VisionStreamType.VISION_STREAM_ROAD, (*tici_f_frame_size, 2048*2346, 2048, 2048*1216) if TICI else eon_f_frame_size, "roadCameraState"),
(VisionStreamType.VISION_STREAM_DRIVER, (*tici_d_frame_size, 2048*2346, 2048, 2048*1216) if TICI else eon_d_frame_size, "driverCameraState")]
if TICI:
expected_files.add("ecamera.hevc")
streams.append((VisionStreamType.VISION_STREAM_WIDE_ROAD, (*tici_e_frame_size, 2048*2346, 2048, 2048*1216), "wideRoadCameraState"))
expected_files = {"rlog", "qlog", "qcamera.ts", "fcamera.hevc", "dcamera.hevc", "ecamera.hevc"}
streams = [(VisionStreamType.VISION_STREAM_ROAD, (*tici_f_frame_size, 2048*2346, 2048, 2048*1216), "roadCameraState"),
(VisionStreamType.VISION_STREAM_DRIVER, (*tici_d_frame_size, 2048*2346, 2048, 2048*1216), "driverCameraState"),
(VisionStreamType.VISION_STREAM_WIDE_ROAD, (*tici_e_frame_size, 2048*2346, 2048, 2048*1216), "wideRoadCameraState")]
pm = messaging.PubMaster(["roadCameraState", "driverCameraState", "wideRoadCameraState"])
vipc_server = VisionIpcServer("camerad")
for stream_type, frame_spec, _ in streams:
if TICI:
vipc_server.create_buffers_with_sizes(stream_type, 40, False, *(frame_spec))
else:
vipc_server.create_buffers(stream_type, 40, False, *(frame_spec))
vipc_server.create_buffers_with_sizes(stream_type, 40, False, *(frame_spec))
vipc_server.start_listener()
for _ in range(5):
@ -134,10 +127,7 @@ class TestLoggerd(unittest.TestCase):
fps = 20.0
for n in range(1, int(num_segs*length*fps)+1):
for stream_type, frame_spec, state in streams:
if TICI:
dat = np.empty(frame_spec[2], dtype=np.uint8)
else:
dat = np.empty(int(frame_spec[0]*frame_spec[1]*3/2), dtype=np.uint8)
dat = np.empty(frame_spec[2], dtype=np.uint8)
vipc_server.send(stream_type, dat[:].flatten().tobytes(), n, n/fps, n/fps)
camera_state = messaging.new_message(state)

@ -10,12 +10,12 @@ from pathlib import Path
from common.basedir import BASEDIR
from common.spinner import Spinner
from common.text_window import TextWindow
from selfdrive.hardware import TICI
from selfdrive.hardware import AGNOS
from selfdrive.swaglog import cloudlog, add_file_handler
from selfdrive.version import is_dirty
MAX_CACHE_SIZE = 4e9 if "CI" in os.environ else 2e9
CACHE_DIR = Path("/data/scons_cache" if TICI else "/tmp/scons_cache")
CACHE_DIR = Path("/data/scons_cache" if AGNOS else "/tmp/scons_cache")
TOTAL_SCONS_NODES = 2405
MAX_BUILD_PROGRESS = 100

@ -2,7 +2,7 @@ import os
from cereal import car
from common.params import Params
from selfdrive.hardware import TICI, PC
from selfdrive.hardware import PC
from selfdrive.manager.process import PythonProcess, NativeProcess, DaemonProcess
WEBCAM = os.getenv("USE_WEBCAM") is not None
@ -30,7 +30,7 @@ procs = [
NativeProcess("proclogd", "selfdrive/proclogd", ["./proclogd"]),
NativeProcess("sensord", "selfdrive/sensord", ["./sensord"], enabled=not PC),
NativeProcess("ubloxd", "selfdrive/locationd", ["./ubloxd"], enabled=(not PC or WEBCAM)),
NativeProcess("ui", "selfdrive/ui", ["./ui"], offroad=True, watchdog_max_dt=(5 if TICI else None)),
NativeProcess("ui", "selfdrive/ui", ["./ui"], offroad=True, watchdog_max_dt=(5 if not PC else None)),
NativeProcess("soundd", "selfdrive/ui/soundd", ["./soundd"], offroad=True),
NativeProcess("locationd", "selfdrive/locationd", ["./locationd"]),
NativeProcess("boardd", "selfdrive/boardd", ["./boardd"], enabled=False),
@ -45,7 +45,7 @@ procs = [
PythonProcess("plannerd", "selfdrive.controls.plannerd"),
PythonProcess("radard", "selfdrive.controls.radard"),
PythonProcess("thermald", "selfdrive.thermald.thermald", offroad=True),
PythonProcess("timezoned", "selfdrive.timezoned", enabled=TICI, offroad=True),
PythonProcess("timezoned", "selfdrive.timezoned", enabled=not PC, offroad=True),
PythonProcess("tombstoned", "selfdrive.tombstoned", enabled=not PC, offroad=True),
PythonProcess("updated", "selfdrive.updated", enabled=not PC, onroad=False, offroad=True),
PythonProcess("uploader", "selfdrive.loggerd.uploader", offroad=True),

@ -5,7 +5,7 @@ import time
import unittest
import selfdrive.manager.manager as manager
from selfdrive.hardware import TICI, HARDWARE
from selfdrive.hardware import AGNOS, HARDWARE
from selfdrive.manager.process import DaemonProcess
from selfdrive.manager.process_config import managed_processes
@ -50,7 +50,7 @@ class TestManager(unittest.TestCase):
self.assertTrue(state.running, f"{p} not running")
exit_code = managed_processes[p].stop(retry=False)
if (TICI and p in ['ui',]):
if (AGNOS and p in ['ui',]):
# TODO: make Qt UI exit gracefully
continue

@ -163,7 +163,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcCl
}
int main(int argc, char **argv) {
if (Hardware::TICI()) {
if (!Hardware::PC()) {
int ret;
ret = util::set_realtime_priority(54);
assert(ret == 0);

@ -69,22 +69,13 @@ void crop_nv12_to_yuv(uint8_t *raw, int stride, int uv_offset, uint8_t *y, uint8
}
DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib) {
Rect crop_rect;
if (width == TICI_CAM_WIDTH) {
const int cropped_height = tici_dm_crop::width / 1.33;
crop_rect = {width / 2 - tici_dm_crop::width / 2 + tici_dm_crop::x_offset,
height / 2 - cropped_height / 2 + tici_dm_crop::y_offset,
cropped_height / 2,
cropped_height};
if (!s->is_rhd) {
crop_rect.x += tici_dm_crop::width - crop_rect.w;
}
} else {
const int adapt_width = 372;
crop_rect = {0, 0, adapt_width, height};
if (!s->is_rhd) {
crop_rect.x += width - crop_rect.w;
}
const int cropped_height = tici_dm_crop::width / 1.33;
Rect crop_rect = {width / 2 - tici_dm_crop::width / 2 + tici_dm_crop::x_offset,
height / 2 - cropped_height / 2 + tici_dm_crop::y_offset,
cropped_height / 2,
cropped_height};
if (!s->is_rhd) {
crop_rect.x += tici_dm_crop::width - crop_rect.w;
}
int resized_width = MODEL_WIDTH;
@ -108,31 +99,16 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
auto [resized_buf, resized_u, resized_v] = get_yuv_buf(s->resized_buf, resized_width, resized_height);
uint8_t *resized_y = resized_buf;
libyuv::FilterMode mode = libyuv::FilterModeEnum::kFilterBilinear;
if (Hardware::TICI()) {
libyuv::I420Scale(cropped_y, crop_rect.w,
cropped_u, crop_rect.w / 2,
cropped_v, crop_rect.w / 2,
crop_rect.w, crop_rect.h,
resized_y, resized_width,
resized_u, resized_width / 2,
resized_v, resized_width / 2,
resized_width, resized_height,
mode);
} else {
const int source_height = 0.7*resized_height;
const int extra_height = (resized_height - source_height) / 2;
const int extra_width = (resized_width - source_height / 2) / 2;
const int source_width = source_height / 2 + extra_width;
libyuv::I420Scale(cropped_y, crop_rect.w,
cropped_u, crop_rect.w / 2,
cropped_v, crop_rect.w / 2,
crop_rect.w, crop_rect.h,
resized_y + extra_height * resized_width, resized_width,
resized_u + extra_height / 2 * resized_width / 2, resized_width / 2,
resized_v + extra_height / 2 * resized_width / 2, resized_width / 2,
source_width, source_height,
mode);
}
libyuv::I420Scale(cropped_y, crop_rect.w,
cropped_u, crop_rect.w / 2,
cropped_v, crop_rect.w / 2,
crop_rect.w, crop_rect.h,
resized_y, resized_width,
resized_u, resized_width / 2,
resized_v, resized_width / 2,
resized_width, resized_height,
mode);
int yuv_buf_len = (MODEL_WIDTH/2) * (MODEL_HEIGHT/2) * 6; // Y|u|v -> y|y|y|y|u|v
float *net_input_buf = get_buffer(s->net_input_buf, yuv_buf_len);

@ -3,7 +3,6 @@ from math import atan2
from cereal import car
from common.numpy_fast import interp
from common.realtime import DT_DMON
from selfdrive.hardware import TICI
from common.filter_simple import FirstOrderFilter
from common.stat_live import RunningStatFilter
@ -16,7 +15,7 @@ EventName = car.CarEvent.EventName
# ******************************************************************************************
class DRIVER_MONITOR_SETTINGS():
def __init__(self, TICI=TICI, DT_DMON=DT_DMON):
def __init__(self):
self._DT_DMON = DT_DMON
# ref (page15-16): https://eur-lex.europa.eu/legal-content/EN/TXT/PDF/?uri=CELEX:42018X1947&rid=2
self._AWARENESS_TIME = 30. # passive wheeltouch total timeout
@ -27,15 +26,15 @@ class DRIVER_MONITOR_SETTINGS():
self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6.
self._FACE_THRESHOLD = 0.5
self._PARTIAL_FACE_THRESHOLD = 0.8 if TICI else 0.45
self._EYE_THRESHOLD = 0.65 if TICI else 0.6
self._SG_THRESHOLD = 0.925 if TICI else 0.91
self._BLINK_THRESHOLD = 0.8 if TICI else 0.55
self._BLINK_THRESHOLD_SLACK = 0.9 if TICI else 0.7
self._PARTIAL_FACE_THRESHOLD = 0.8
self._EYE_THRESHOLD = 0.65
self._SG_THRESHOLD = 0.925
self._BLINK_THRESHOLD = 0.8
self._BLINK_THRESHOLD_SLACK = 0.9
self._BLINK_THRESHOLD_STRICT = self._BLINK_THRESHOLD
self._EE_THRESH11 = 0.75 if TICI else 0.4
self._EE_THRESH12 = 3.25 if TICI else 2.45
self._EE_THRESH11 = 0.75
self._EE_THRESH12 = 3.25
self._EE_THRESH21 = 0.01
self._EE_THRESH22 = 0.35

@ -242,14 +242,17 @@ CONFIGS = [
pub_sub={
"can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"],
"deviceState": [], "pandaStates": [], "peripheralState": [], "liveCalibration": [], "driverMonitoringState": [], "longitudinalPlan": [], "lateralPlan": [], "liveLocationKalman": [], "liveParameters": [], "radarState": [],
"modelV2": [], "driverCameraState": [], "roadCameraState": [], "managerState": [], "testJoystick": [],
"modelV2": [], "driverCameraState": [], "roadCameraState": [], "wideRoadCameraState": [], "managerState": [], "testJoystick": [],
},
ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"],
init_callback=fingerprint,
should_recv_callback=controlsd_rcv_callback,
tolerance=NUMPY_TOLERANCE,
fake_pubsubmaster=True,
submaster_config={'ignore_avg_freq': ['radarState', 'longitudinalPlan']}
submaster_config={
'ignore_avg_freq': ['radarState', 'longitudinalPlan', 'driverCameraState', 'driverMonitoringState'], # dcam is expected at 20 Hz
'ignore_alive': ['wideRoadCameraState'], # TODO: Add to regen
}
),
ProcessConfig(
proc_name="radard",

@ -1 +1 @@
0956446adfa91506f0a3d88f893e041bfb2890c1
49e231ccf06ef35994a3ec907af959c28e3c0870

@ -17,7 +17,7 @@ from common.filter_simple import FirstOrderFilter
from common.params import Params
from common.realtime import DT_TRML, sec_since_boot
from selfdrive.controls.lib.alertmanager import set_offroad_alert
from selfdrive.hardware import HARDWARE, TICI
from selfdrive.hardware import HARDWARE, TICI, AGNOS
from selfdrive.loggerd.config import get_available_percent
from selfdrive.statsd import statlog
from selfdrive.swaglog import cloudlog
@ -113,7 +113,7 @@ def hw_state_thread(end_event, hw_queue):
modem_temps = prev_hw_state.modem_temps
# Log modem version once
if TICI and ((modem_version is None) or (modem_nv is None)):
if AGNOS and ((modem_version is None) or (modem_nv is None)):
modem_version = HARDWARE.get_modem_version() # pylint: disable=assignment-from-none
modem_nv = HARDWARE.get_modem_nv() # pylint: disable=assignment-from-none
@ -134,7 +134,7 @@ def hw_state_thread(end_event, hw_queue):
except queue.Full:
pass
if TICI and (hw_state.network_info is not None) and (hw_state.network_info.get('state', None) == "REGISTERED"):
if AGNOS and (hw_state.network_info is not None) and (hw_state.network_info.get('state', None) == "REGISTERED"):
registered_count += 1
else:
registered_count = 0

@ -9,7 +9,7 @@ import requests
from timezonefinder import TimezoneFinder
from common.params import Params
from selfdrive.hardware import TICI
from selfdrive.hardware import AGNOS
from selfdrive.swaglog import cloudlog
@ -20,7 +20,7 @@ def set_timezone(valid_timezones, timezone):
cloudlog.debug(f"Setting timezone to {timezone}")
try:
if TICI:
if AGNOS:
tzpath = os.path.join("/usr/share/zoneinfo/", timezone)
subprocess.check_call(f'sudo su -c "ln -snf {tzpath} /data/etc/tmptime && \
mv /data/etc/tmptime /data/etc/localtime"', shell=True)

@ -10,15 +10,12 @@ import glob
from typing import NoReturn
from common.file_helpers import mkdirs_exists_ok
from selfdrive.hardware import TICI
from selfdrive.loggerd.config import ROOT
import selfdrive.sentry as sentry
from selfdrive.swaglog import cloudlog
from selfdrive.version import get_commit
MAX_SIZE = 100000 * 10 # mal size is 40-100k, allow up to 1M
if TICI:
MAX_SIZE = MAX_SIZE * 100 # Allow larger size for tici since files contain coredump
MAX_SIZE = 1_000_000 * 100 # allow up to 100M
MAX_TOMBSTONE_FN_LEN = 62 # 85 - 23 ("<dongle id>/crash/")
TOMBSTONE_DIR = "/data/tombstones/"

@ -63,12 +63,10 @@ void DriverViewScene::paintEvent(QPaintEvent* event) {
// blackout
const QColor bg(0, 0, 0, 140);
const QRect& blackout_rect = Hardware::TICI() ? rect() : rect2;
const QRect& blackout_rect = rect();
p.fillRect(blackout_rect.adjusted(0, 0, valid_rect.left() - blackout_rect.right(), 0), bg);
p.fillRect(blackout_rect.adjusted(valid_rect.right() - blackout_rect.left(), 0, 0, 0), bg);
if (Hardware::TICI()) {
p.fillRect(blackout_rect.adjusted(valid_rect.left()-blackout_rect.left()+1, 0, valid_rect.right()-blackout_rect.right()-1, -valid_rect.height()*7/10), bg); // top dz
}
p.fillRect(blackout_rect.adjusted(valid_rect.left()-blackout_rect.left()+1, 0, valid_rect.right()-blackout_rect.right()-1, -valid_rect.height()*7/10), bg); // top dz
// face bounding box
cereal::DriverState::Reader driver_state = sm["driverState"].getDriverState();

@ -162,7 +162,7 @@ DevicePanel::DevicePanel(SettingsWindow *parent) : ListWidget(parent) {
power_layout->addWidget(poweroff_btn);
QObject::connect(poweroff_btn, &QPushButton::clicked, this, &DevicePanel::poweroff);
if (Hardware::TICI()) {
if (!Hardware::PC()) {
connect(uiState(), &UIState::offroadTransition, poweroff_btn, &QPushButton::setVisible);
}

@ -55,25 +55,15 @@ const mat4 device_transform = {{
mat4 get_driver_view_transform(int screen_width, int screen_height, int stream_width, int stream_height) {
const float driver_view_ratio = 1.333;
mat4 transform;
if (stream_width == TICI_CAM_WIDTH) {
const float yscale = stream_height * driver_view_ratio / tici_dm_crop::width;
const float xscale = yscale*screen_height/screen_width*stream_width/stream_height;
transform = (mat4){{
xscale, 0.0, 0.0, xscale*tici_dm_crop::x_offset/stream_width*2,
0.0, yscale, 0.0, yscale*tici_dm_crop::y_offset/stream_height*2,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0,
}};
} else {
// frame from 4/3 to 16/9 display
transform = (mat4){{
driver_view_ratio * screen_height / screen_width, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0,
}};
}
const float yscale = stream_height * driver_view_ratio / tici_dm_crop::width;
const float xscale = yscale*screen_height/screen_width*stream_width/stream_height;
mat4 transform = (mat4){{
xscale, 0.0, 0.0, xscale*tici_dm_crop::x_offset/stream_width*2,
0.0, yscale, 0.0, yscale*tici_dm_crop::y_offset/stream_height*2,
0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 0.0, 1.0,
}};
return transform;
}

@ -182,7 +182,7 @@ static void update_state(UIState *s) {
}
}
}
if (Hardware::TICI() && sm.updated("wideRoadCameraState")) {
if (sm.updated("wideRoadCameraState")) {
auto camera_state = sm["wideRoadCameraState"].getWideRoadCameraState();
float max_lines = 1618;

@ -37,7 +37,7 @@ from markdown_it import MarkdownIt
from common.basedir import BASEDIR
from common.params import Params
from selfdrive.hardware import TICI, HARDWARE
from selfdrive.hardware import AGNOS, HARDWARE
from selfdrive.swaglog import cloudlog
from selfdrive.controls.lib.alertmanager import set_offroad_alert
from selfdrive.version import is_tested_branch
@ -328,7 +328,7 @@ def fetch_update(wait_helper: WaitTimeHelper) -> bool:
]
cloudlog.info("git reset success: %s", '\n'.join(r))
if TICI:
if AGNOS:
handle_agnos_update(wait_helper)
# Create the finalized, ready-to-swap update

Loading…
Cancel
Save