spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

cereal cleanup part 2 (#20092) 

* car stuff

* thermal

* Revert "car stuff"

This reverts commit 77fd1c65eb.

* panda state

* camera stuff

* start deg

* most is building

* builds

* planner + controls run

* fix up paramsd

* cleanup

* process replay passes

* fix webcam build

* camerad

* no more frame

* thermald

* ui

* paramsd

* camera replay

* fix long tests

* fix camerad tests

* maxSteeringAngle

* bump cereal

* more frame

* cereal master
pull/20101/head
Adeeb Shihadeh 2021-02-16 21:39:32 -08:00 committed by GitHub
parent 139470f033
commit 312b681a46
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
86 changed files with 500 additions and 515 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
cereal

@ -1 +1 @@
Subproject commit 39d80be84108820a42fc4b1b1817e8e3b73c89fb
Subproject commit a0348379249f77b85765846f0ebc5dcdb358821d

6
selfdrive/athena/athenad.py
View File

@ -125,9 +125,9 @@ def listDataDirectory():
@dispatcher.add_method
def reboot():
thermal_sock = messaging.sub_sock("thermal", timeout=1000)
ret = messaging.recv_one(thermal_sock)
if ret is None or ret.thermal.started:
sock = messaging.sub_sock("deviceState", timeout=1000)
ret = messaging.recv_one(sock)
if ret is None or ret.deviceState.started:
raise Exception("Reboot unavailable")
def do_reboot():

12
selfdrive/athena/tests/test_athenad.py
View File

@ -35,22 +35,22 @@ class TestAthenadMethods(unittest.TestCase):
with self.assertRaises(TimeoutError) as _:
dispatcher["getMessage"]("controlsState")
def send_thermal():
def send_deviceState():
messaging.context = messaging.Context()
pub_sock = messaging.pub_sock("thermal")
pub_sock = messaging.pub_sock("deviceState")
start = time.time()
while time.time() - start < 1:
msg = messaging.new_message('thermal')
msg = messaging.new_message('deviceState')
pub_sock.send(msg.to_bytes())
time.sleep(0.01)
p = Process(target=send_thermal)
p = Process(target=send_deviceState)
p.start()
time.sleep(0.1)
try:
thermal = dispatcher["getMessage"]("thermal")
assert thermal['thermal']
deviceState = dispatcher["getMessage"]("deviceState")
assert deviceState['deviceState']
finally:
p.terminate()

12
selfdrive/athena/tests/test_athenad_old.py
View File

@ -107,18 +107,18 @@ def test_athena():
assert resp.get('result'), resp
assert resp['result']['jpegBack'], resp['result']
@with_processes(["thermald"])
def test_athena_thermal():
print("ATHENA: getMessage(thermal)")
@with_processes(["deviceStated"])
def test_athena_deviceState():
print("ATHENA: getMessage(deviceState)")
resp = athena_post({
"method": "getMessage",
"params": {"service": "thermal", "timeout": 5000},
"params": {"service": "deviceState", "timeout": 5000},
"id": 0,
"jsonrpc": "2.0"
})
assert resp.get('result'), resp
assert resp['result']['thermal'], resp['result']
test_athena_thermal()
assert resp['result']['deviceState'], resp['result']
test_athena_deviceState()
finally:
try:
athenad_pid = subprocess.check_output(["pgrep", "-P", manage_athenad_pid], encoding="utf-8").strip()

108
selfdrive/boardd/boardd.cc
View File

@ -157,7 +157,7 @@ bool usb_connect() {
// power on charging, only the first time. Panda can also change mode and it causes a brief disconneciton
#ifndef __x86_64__
if (!connected_once) {
tmp_panda->set_usb_power_mode(cereal::HealthData::UsbPowerMode::CDP);
tmp_panda->set_usb_power_mode(cereal::PandaState::UsbPowerMode::CDP);
}
#endif
@ -263,38 +263,38 @@ void can_recv_thread() {
}
}
void can_health_thread(bool spoofing_started) {
LOGD("start health thread");
PubMaster pm({"health"});
void panda_state_thread(bool spoofing_started) {
LOGD("start panda state thread");
PubMaster pm({"pandaState"});
uint32_t no_ignition_cnt = 0;
bool ignition_last = false;
Params params = Params();
// Broadcast empty health message when panda is not yet connected
// Broadcast empty pandaState message when panda is not yet connected
while (!do_exit && !panda) {
MessageBuilder msg;
auto healthData = msg.initEvent().initHealth();
auto pandaState = msg.initEvent().initPandaState();
healthData.setPandaType(cereal::HealthData::PandaType::UNKNOWN);
pm.send("health", msg);
pandaState.setPandaType(cereal::PandaState::PandaType::UNKNOWN);
pm.send("pandaState", msg);
util::sleep_for(500);
}
// run at 2hz
while (!do_exit && panda->connected) {
health_t health = panda->get_health();
health_t pandaState = panda->get_state();
if (spoofing_started) {
health.ignition_line = 1;
pandaState.ignition_line = 1;
}
// Make sure CAN buses are live: safety_setter_thread does not work if Panda CAN are silent and there is only one other CAN node
if (health.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
if (pandaState.safety_model == (uint8_t)(cereal::CarParams::SafetyModel::SILENT)) {
panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
}
ignition = ((health.ignition_line != 0) || (health.ignition_can != 0));
ignition = ((pandaState.ignition_line != 0) || (pandaState.ignition_can != 0));
if (ignition) {
no_ignition_cnt = 0;
@ -304,12 +304,12 @@ void can_health_thread(bool spoofing_started) {
#ifndef __x86_64__
bool power_save_desired = !ignition;
if (health.power_save_enabled != power_save_desired){
if (pandaState.power_save_enabled != power_save_desired){
panda->set_power_saving(power_save_desired);
}
// set safety mode to NO_OUTPUT when car is off. ELM327 is an alternative if we want to leverage athenad/connect
if (!ignition && (health.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) {
if (!ignition && (pandaState.safety_model != (uint8_t)(cereal::CarParams::SafetyModel::NO_OUTPUT))) {
panda->set_safety_model(cereal::CarParams::SafetyModel::NO_OUTPUT);
}
#endif
@ -341,48 +341,48 @@ void can_health_thread(bool spoofing_started) {
ignition_last = ignition;
uint16_t fan_speed_rpm = panda->get_fan_speed();
// set fields
// build msg
MessageBuilder msg;
auto healthData = msg.initEvent().initHealth();
healthData.setUptime(health.uptime);
auto ps = msg.initEvent().initPandaState();
ps.setUptime(pandaState.uptime);
#ifdef QCOM2
healthData.setVoltage(std::stoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input")));
healthData.setCurrent(std::stoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input")));
ps.setVoltage(std::stoi(util::read_file("/sys/class/hwmon/hwmon1/in1_input")));
ps.setCurrent(std::stoi(util::read_file("/sys/class/hwmon/hwmon1/curr1_input")));
#else
healthData.setVoltage(health.voltage);
healthData.setCurrent(health.current);
ps.setVoltage(pandaState.voltage);
ps.setCurrent(pandaState.current);
#endif
healthData.setIgnitionLine(health.ignition_line);
healthData.setIgnitionCan(health.ignition_can);
healthData.setControlsAllowed(health.controls_allowed);
healthData.setGasInterceptorDetected(health.gas_interceptor_detected);
healthData.setHasGps(panda->is_pigeon);
healthData.setCanRxErrs(health.can_rx_errs);
healthData.setCanSendErrs(health.can_send_errs);
healthData.setCanFwdErrs(health.can_fwd_errs);
healthData.setGmlanSendErrs(health.gmlan_send_errs);
healthData.setPandaType(panda->hw_type);
healthData.setUsbPowerMode(cereal::HealthData::UsbPowerMode(health.usb_power_mode));
healthData.setSafetyModel(cereal::CarParams::SafetyModel(health.safety_model));
healthData.setFanSpeedRpm(fan_speed_rpm);
healthData.setFaultStatus(cereal::HealthData::FaultStatus(health.fault_status));
healthData.setPowerSaveEnabled((bool)(health.power_save_enabled));
ps.setIgnitionLine(pandaState.ignition_line);
ps.setIgnitionCan(pandaState.ignition_can);
ps.setControlsAllowed(pandaState.controls_allowed);
ps.setGasInterceptorDetected(pandaState.gas_interceptor_detected);
ps.setHasGps(panda->is_pigeon);
ps.setCanRxErrs(pandaState.can_rx_errs);
ps.setCanSendErrs(pandaState.can_send_errs);
ps.setCanFwdErrs(pandaState.can_fwd_errs);
ps.setGmlanSendErrs(pandaState.gmlan_send_errs);
ps.setPandaType(panda->hw_type);
ps.setUsbPowerMode(cereal::PandaState::UsbPowerMode(pandaState.usb_power_mode));
ps.setSafetyModel(cereal::CarParams::SafetyModel(pandaState.safety_model));
ps.setFanSpeedRpm(fan_speed_rpm);
ps.setFaultStatus(cereal::PandaState::FaultStatus(pandaState.fault_status));
ps.setPowerSaveEnabled((bool)(pandaState.power_save_enabled));
// Convert faults bitset to capnp list
std::bitset<sizeof(health.faults) * 8> fault_bits(health.faults);
auto faults = healthData.initFaults(fault_bits.count());
std::bitset<sizeof(pandaState.faults) * 8> fault_bits(pandaState.faults);
auto faults = ps.initFaults(fault_bits.count());
size_t i = 0;
for (size_t f = size_t(cereal::HealthData::FaultType::RELAY_MALFUNCTION);
f <= size_t(cereal::HealthData::FaultType::INTERRUPT_RATE_TIM9); f++){
for (size_t f = size_t(cereal::PandaState::FaultType::RELAY_MALFUNCTION);
f <= size_t(cereal::PandaState::FaultType::INTERRUPT_RATE_TIM9); f++){
if (fault_bits.test(f)) {
faults.set(i, cereal::HealthData::FaultType(f));
faults.set(i, cereal::PandaState::FaultType(f));
i++;
}
}
pm.send("health", msg);
pm.send("pandaState", msg);
panda->send_heartbeat();
util::sleep_for(500);
}
@ -390,7 +390,7 @@ void can_health_thread(bool spoofing_started) {
void hardware_control_thread() {
LOGD("start hardware control thread");
SubMaster sm({"thermal", "frontFrame"});
SubMaster sm({"deviceState", "driverCameraState"});
uint64_t last_front_frame_t = 0;
uint16_t prev_fan_speed = 999;
@ -406,15 +406,15 @@ void hardware_control_thread() {
sm.update(1000); // TODO: what happens if EINTR is sent while in sm.update?
#ifdef QCOM
if (sm.updated("thermal")){
if (sm.updated("deviceState")){
// Charging mode
bool charging_disabled = sm["thermal"].getThermal().getChargingDisabled();
bool charging_disabled = sm["deviceState"].getDeviceState().getChargingDisabled();
if (charging_disabled != prev_charging_disabled){
if (charging_disabled){
panda->set_usb_power_mode(cereal::HealthData::UsbPowerMode::CLIENT);
panda->set_usb_power_mode(cereal::PandaState::UsbPowerMode::CLIENT);
LOGW("TURN OFF CHARGING!\n");
} else {
panda->set_usb_power_mode(cereal::HealthData::UsbPowerMode::CDP);
panda->set_usb_power_mode(cereal::PandaState::UsbPowerMode::CDP);
LOGW("TURN ON CHARGING!\n");
}
prev_charging_disabled = charging_disabled;
@ -423,18 +423,18 @@ void hardware_control_thread() {
#endif
// Other pandas don't have fan/IR to control
if (panda->hw_type != cereal::HealthData::PandaType::UNO && panda->hw_type != cereal::HealthData::PandaType::DOS) continue;
if (sm.updated("thermal")){
if (panda->hw_type != cereal::PandaState::PandaType::UNO && panda->hw_type != cereal::PandaState::PandaType::DOS) continue;
if (sm.updated("deviceState")){
// Fan speed
uint16_t fan_speed = sm["thermal"].getThermal().getFanSpeedPercentDesired();
uint16_t fan_speed = sm["deviceState"].getDeviceState().getFanSpeedPercentDesired();
if (fan_speed != prev_fan_speed || cnt % 100 == 0){
panda->set_fan_speed(fan_speed);
prev_fan_speed = fan_speed;
}
}
if (sm.updated("frontFrame")){
auto event = sm["frontFrame"];
int cur_integ_lines = event.getFrontFrame().getIntegLines();
if (sm.updated("driverCameraState")){
auto event = sm["driverCameraState"];
int cur_integ_lines = event.getDriverCameraState().getIntegLines();
last_front_frame_t = event.getLogMonoTime();
if (cur_integ_lines <= CUTOFF_IL) {
@ -523,7 +523,7 @@ int main() {
while (!do_exit){
std::vector<std::thread> threads;
threads.push_back(std::thread(can_health_thread, getenv("STARTED") != nullptr));
threads.push_back(std::thread(panda_state_thread, getenv("STARTED") != nullptr));
// connect to the board
if (usb_retry_connect()) {

20
selfdrive/boardd/panda.cc
View File

@ -53,12 +53,12 @@ Panda::Panda(){
hw_type = get_hw_type();
is_pigeon =
(hw_type == cereal::HealthData::PandaType::GREY_PANDA) ||
(hw_type == cereal::HealthData::PandaType::BLACK_PANDA) ||
(hw_type == cereal::HealthData::PandaType::UNO) ||
(hw_type == cereal::HealthData::PandaType::DOS);
has_rtc = (hw_type == cereal::HealthData::PandaType::UNO) ||
(hw_type == cereal::HealthData::PandaType::DOS);
(hw_type == cereal::PandaState::PandaType::GREY_PANDA) ||
(hw_type == cereal::PandaState::PandaType::BLACK_PANDA) ||
(hw_type == cereal::PandaState::PandaType::UNO) ||
(hw_type == cereal::PandaState::PandaType::DOS);
has_rtc = (hw_type == cereal::PandaState::PandaType::UNO) ||
(hw_type == cereal::PandaState::PandaType::DOS);
return;
@ -186,11 +186,11 @@ void Panda::set_unsafe_mode(uint16_t unsafe_mode) {
usb_write(0xdf, unsafe_mode, 0);
}
cereal::HealthData::PandaType Panda::get_hw_type() {
cereal::PandaState::PandaType Panda::get_hw_type() {
unsigned char hw_query[1] = {0};
usb_read(0xc1, 0, 0, hw_query, 1);
return (cereal::HealthData::PandaType)(hw_query[0]);
return (cereal::PandaState::PandaType)(hw_query[0]);
}
void Panda::set_rtc(struct tm sys_time){
@ -242,7 +242,7 @@ void Panda::set_ir_pwr(uint16_t ir_pwr) {
usb_write(0xb0, ir_pwr, 0);
}
health_t Panda::get_health(){
health_t Panda::get_state(){
health_t health {0};
usb_read(0xd2, 0, 0, (unsigned char*)&health, sizeof(health));
return health;
@ -269,7 +269,7 @@ void Panda::set_power_saving(bool power_saving){
usb_write(0xe7, power_saving, 0);
}
void Panda::set_usb_power_mode(cereal::HealthData::UsbPowerMode power_mode){
void Panda::set_usb_power_mode(cereal::PandaState::UsbPowerMode power_mode){
usb_write(0xe6, (uint16_t)power_mode, 0);
}

8
selfdrive/boardd/panda.h
View File

@ -53,7 +53,7 @@ class Panda {
~Panda();
std::atomic<bool> connected = true;
cereal::HealthData::PandaType hw_type = cereal::HealthData::PandaType::UNKNOWN;
cereal::PandaState::PandaType hw_type = cereal::PandaState::PandaType::UNKNOWN;
bool is_pigeon = false;
bool has_rtc = false;
@ -64,7 +64,7 @@ class Panda {
int usb_bulk_read(unsigned char endpoint, unsigned char* data, int length, unsigned int timeout=TIMEOUT);
// Panda functionality
cereal::HealthData::PandaType get_hw_type();
cereal::PandaState::PandaType get_hw_type();
void set_safety_model(cereal::CarParams::SafetyModel safety_model, int safety_param=0);
void set_unsafe_mode(uint16_t unsafe_mode);
void set_rtc(struct tm sys_time);
@ -72,12 +72,12 @@ class Panda {
void set_fan_speed(uint16_t fan_speed);
uint16_t get_fan_speed();
void set_ir_pwr(uint16_t ir_pwr);
health_t get_health();
health_t get_state();
void set_loopback(bool loopback);
std::optional<std::vector<uint8_t>> get_firmware_version();
std::optional<std::string> get_serial();
void set_power_saving(bool power_saving);
void set_usb_power_mode(cereal::HealthData::UsbPowerMode power_mode);
void set_usb_power_mode(cereal::PandaState::UsbPowerMode power_mode);
void send_heartbeat();
void can_send(capnp::List<cereal::CanData>::Reader can_data_list);
int can_receive(kj::Array<capnp::word>& out_buf);

14
selfdrive/boardd/tests/boardd_old.py
View File

@ -156,7 +156,7 @@ def boardd_loop(rate=100):
# *** publishes can and health
logcan = messaging.pub_sock('can')
health_sock = messaging.pub_sock('health')
health_sock = messaging.pub_sock('pandaState')
# *** subscribes to can send
sendcan = messaging.sub_sock('sendcan')
@ -168,14 +168,14 @@ def boardd_loop(rate=100):
# health packet @ 2hz
if (rk.frame % (rate // 2)) == 0:
health = can_health()
msg = messaging.new_message('health')
msg = messaging.new_message('pandaState')
# store the health to be logged
msg.health.voltage = health['voltage']
msg.health.current = health['current']
msg.health.ignitionLine = health['ignition_line']
msg.health.ignitionCan = health['ignition_can']
msg.health.controlsAllowed = True
msg.pandaState.voltage = health['voltage']
msg.pandaState.current = health['current']
msg.pandaState.ignitionLine = health['ignition_line']
msg.pandaState.ignitionCan = health['ignition_can']
msg.pandaState.controlsAllowed = True
health_sock.send(msg.to_bytes())

20
selfdrive/boardd/tests/fuzzer.py
View File

@ -1,20 +0,0 @@
#!/usr/bin/env python3
from __future__ import print_function
import time
import random
from boardd_old import can_init, can_recv, can_send_many, can_health
if __name__ == "__main__":
can_init()
while 1:
c = random.randint(0, 3)
if c == 0:
print(can_recv())
elif c == 1:
print(can_health())
elif c == 2:
many = [[0x123, 0, "abcdef", 0]] * random.randint(1, 10)
can_send_many(many)
elif c == 3:
time.sleep(random.randint(0, 100) / 1000.0)

4
selfdrive/boardd/tests/test_boardd_loopback.py
View File

@ -40,8 +40,8 @@ def test_boardd_loopback():
time.sleep(2)
with Timeout(60, "boardd didn't start"):
sm = messaging.SubMaster(['health'])
while sm.rcv_frame['health'] < 1:
sm = messaging.SubMaster(['pandaState'])
while sm.rcv_frame['pandaState'] < 1:
sm.update(1000)
# boardd blocks on CarVin and CarParams

4
selfdrive/camerad/cameras/camera_common.cc
View File

@ -407,11 +407,11 @@ void common_camera_process_front(SubMaster *sm, PubMaster *pm, CameraState *c, i
}
MessageBuilder msg;
auto framed = msg.initEvent().initFrontFrame();
auto framed = msg.initEvent().initDriverCameraState();
framed.setFrameType(cereal::FrameData::FrameType::FRONT);
fill_frame_data(framed, b->cur_frame_data);
if (env_send_front) {
framed.setImage(get_frame_image(b));
}
pm->send("frontFrame", msg);
pm->send("driverCameraState", msg);
}

2
selfdrive/camerad/cameras/camera_frame_stream.cc
View File

@ -88,6 +88,6 @@ void camera_process_rear(MultiCameraState *s, CameraState *c, int cnt) {}
void cameras_run(MultiCameraState *s) {
std::thread t = start_process_thread(s, "processing", &s->rear, camera_process_rear);
set_thread_name("frame_streaming");
run_frame_stream(s->rear, "frame");
run_frame_stream(s->rear, "roadCameraState");
t.join();
}

6
selfdrive/camerad/cameras/camera_qcom.cc
View File

@ -307,7 +307,7 @@ void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_i
s->front.device = s->device;
s->sm_front = new SubMaster({"driverState"});
s->pm = new PubMaster({"frame", "frontFrame", "thumbnail"});
s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"});
for (int i = 0; i < FRAME_BUF_COUNT; i++) {
// TODO: make lengths correct
@ -1655,7 +1655,7 @@ void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) {
setup_self_recover(c, &s->lapres[0], std::size(s->lapres));
MessageBuilder msg;
auto framed = msg.initEvent().initFrame();
auto framed = msg.initEvent().initRoadCameraState();
fill_frame_data(framed, b->cur_frame_data);
if (env_send_rear) {
framed.setImage(get_frame_image(b));
@ -1665,7 +1665,7 @@ void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) {
framed.setRecoverState(s->rear.self_recover);
framed.setSharpnessScore(s->lapres);
framed.setTransform(b->yuv_transform.v);
s->pm->send("frame", msg);
s->pm->send("roadCameraState", msg);
if (cnt % 3 == 0) {
const int x = 290, y = 322, width = 560, height = 314;

6
selfdrive/camerad/cameras/camera_qcom2.cc
View File

@ -807,7 +807,7 @@ void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_i
printf("front initted \n");
s->sm = new SubMaster({"driverState"});
s->pm = new PubMaster({"frame", "frontFrame", "wideFrame", "thumbnail"});
s->pm = new PubMaster({"roadCameraState", "driverCameraState", "wideRoadCameraState", "thumbnail"});
}
void cameras_open(MultiCameraState *s) {
@ -1101,7 +1101,7 @@ void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) {
const CameraBuf *b = &c->buf;
MessageBuilder msg;
auto framed = c == &s->rear ? msg.initEvent().initFrame() : msg.initEvent().initWideFrame();
auto framed = c == &s->rear ? msg.initEvent().initRoadCameraState() : msg.initEvent().initWideRoadCameraState();
fill_frame_data(framed, b->cur_frame_data);
if ((c == &s->rear && env_send_rear) || (c == &s->wide && env_send_wide)) {
framed.setImage(get_frame_image(b));
@ -1109,7 +1109,7 @@ void camera_process_frame(MultiCameraState *s, CameraState *c, int cnt) {
if (c == &s->rear) {
framed.setTransform(b->yuv_transform.v);
}
s->pm->send(c == &s->rear ? "frame" : "wideFrame", msg);
s->pm->send(c == &s->rear ? "roadCameraState" : "wideRoadCameraState", msg);
if (cnt % 3 == 0) {
const auto [x, y, w, h] = (c == &s->wide) ? std::tuple(96, 250, 1734, 524) : std::tuple(96, 160, 1734, 986);

10
selfdrive/camerad/cameras/camera_webcam.cc
View File

@ -226,7 +226,7 @@ void cameras_init(VisionIpcServer *v, MultiCameraState *s, cl_device_id device_i
VISION_STREAM_RGB_BACK, VISION_STREAM_YUV_BACK);
camera_init(v, &s->front, CAMERA_ID_LGC615, 10, device_id, ctx,
VISION_STREAM_RGB_FRONT, VISION_STREAM_YUV_FRONT);
s->pm = new PubMaster({"frame", "frontFrame", "thumbnail"});
s->pm = new PubMaster({"roadCameraState", "driverCameraState", "thumbnail"});
}
void camera_autoexposure(CameraState *s, float grey_frac) {}
@ -247,20 +247,20 @@ void cameras_close(MultiCameraState *s) {
void camera_process_front(MultiCameraState *s, CameraState *c, int cnt) {
MessageBuilder msg;
auto framed = msg.initEvent().initFrontFrame();
auto framed = msg.initEvent().initDriverCameraState();
framed.setFrameType(cereal::FrameData::FrameType::FRONT);
fill_frame_data(framed, c->buf.cur_frame_data);
s->pm->send("frontFrame", msg);
s->pm->send("driverCameraState", msg);
}
void camera_process_rear(MultiCameraState *s, CameraState *c, int cnt) {
const CameraBuf *b = &c->buf;
MessageBuilder msg;
auto framed = msg.initEvent().initFrame();
auto framed = msg.initEvent().initRoadCameraState();
fill_frame_data(framed, b->cur_frame_data);
framed.setImage(kj::arrayPtr((const uint8_t *)b->cur_yuv_buf->addr, b->cur_yuv_buf->len));
framed.setTransform(b->yuv_transform.v);
s->pm->send("frame", msg);
s->pm->send("roadCameraState", msg);
}
void cameras_run(MultiCameraState *s) {

4
selfdrive/camerad/snapshot/snapshot.py
View File

@ -29,7 +29,7 @@ def extract_image(dat, frame_sizes):
return np.dstack([r, g, b])
def get_snapshots(frame="frame", front_frame="frontFrame"):
def get_snapshots(frame="roadCameraState", front_frame="driverCameraState"):
frame_sizes = [eon_f_frame_size, eon_d_frame_size, leon_d_frame_size, tici_f_frame_size]
frame_sizes = {w * h: (w, h) for (w, h) in frame_sizes}
@ -73,7 +73,7 @@ def snapshot():
cwd=os.path.join(BASEDIR, "selfdrive/camerad"), env=env)
time.sleep(3.0)
frame = "wideFrame" if TICI else "frame"
frame = "wideRoadCameraState" if TICI else "roadCameraState"
rear, front = get_snapshots(frame)
proc.send_signal(signal.SIGINT)

4
selfdrive/camerad/test/check_skips.py
View File

@ -2,7 +2,7 @@
# type: ignore
import cereal.messaging as messaging
all_sockets = ['frame','frontFrame','wideFrame']
all_sockets = ['roadCameraState', 'driverCameraState', 'wideRoadCameraState']
prev_id = [None,None,None]
this_id = [None,None,None]
dt = [None,None,None]
@ -12,7 +12,7 @@ if __name__ == "__main__":
sm = messaging.SubMaster(all_sockets)
while True:
sm.update()
for i in range(len(all_sockets)):
if not sm.updated[all_sockets[i]]:
continue

6
selfdrive/camerad/test/frame_test.py
View File

@ -19,18 +19,18 @@ if __name__ == "__main__":
from common.realtime import Ratekeeper
rk = Ratekeeper(20)
pm = messaging.PubMaster(['frame'])
pm = messaging.PubMaster(['roadCameraState'])
frm = [get_frame(x) for x in range(30)]
idx = 0
while 1:
print("send %d" % idx)
dat = messaging.new_message('frame')
dat = messaging.new_message('roadCameraState')
dat.valid = True
dat.frame = {
"frameId": idx,
"image": frm[idx % len(frm)],
}
pm.send('frame', dat)
pm.send('roadCameraState', dat)
idx += 1
rk.keep_time()

8
selfdrive/camerad/test/test_camerad.py
View File

@ -17,13 +17,13 @@ LAG_FRAME_TOLERANCE = 2 # ms
FPS_BASELINE = 20
CAMERAS = {
"frame": FPS_BASELINE,
"frontFrame": FPS_BASELINE // 2,
"roadCameraState": FPS_BASELINE,
"driverCameraState": FPS_BASELINE // 2,
}
if TICI:
CAMERAS["frontFrame"] = FPS_BASELINE
CAMERAS["wideFrame"] = FPS_BASELINE
CAMERAS["driverCameraState"] = FPS_BASELINE
CAMERAS["wideRoadCameraState"] = FPS_BASELINE
class TestCamerad(unittest.TestCase):
@classmethod

4
selfdrive/car/chrysler/carstate.py
View File

@ -42,8 +42,8 @@ class CarState(CarStateBase):
ret.leftBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 1
ret.rightBlinker = cp.vl["STEERING_LEVERS"]['TURN_SIGNALS'] == 2
ret.steeringAngle = cp.vl["STEERING"]['STEER_ANGLE']
ret.steeringRate = cp.vl["STEERING"]['STEERING_RATE']
ret.steeringAngleDeg = cp.vl["STEERING"]['STEER_ANGLE']
ret.steeringRateDeg = cp.vl["STEERING"]['STEERING_RATE']
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(cp.vl['GEAR']['PRNDL'], None))
ret.cruiseState.enabled = cp.vl["ACC_2"]['ACC_STATUS_2'] == 7 # ACC is green.

4
selfdrive/car/ford/carcontroller.py
View File

@ -33,7 +33,7 @@ class CarController():
if (frame % 3) == 0:
curvature = self.vehicle_model.calc_curvature(actuators.steerAngle*3.1415/180., CS.out.vEgo)
curvature = self.vehicle_model.calc_curvature(actuators.steeringAngleDeg*3.1415/180., CS.out.vEgo)
# The use of the toggle below is handy for trying out the various LKAS modes
if TOGGLE_DEBUG:
@ -43,7 +43,7 @@ class CarController():
self.lkas_action = 5 # 4 and 5 seem the best. 8 and 9 seem to aggressive and laggy
can_sends.append(create_steer_command(self.packer, apply_steer, enabled,
CS.lkas_state, CS.out.steeringAngle, curvature, self.lkas_action))
CS.lkas_state, CS.out.steeringAngleDeg, curvature, self.lkas_action))
self.generic_toggle_last = CS.out.genericToggle
if (frame % 100) == 0:

2
selfdrive/car/ford/carstate.py
View File

@ -17,7 +17,7 @@ class CarState(CarStateBase):
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
ret.steeringAngle = cp.vl["Steering_Wheel_Data_CG1"]['SteWhlRelInit_An_Sns']
ret.steeringAngleDeg = cp.vl["Steering_Wheel_Data_CG1"]['SteWhlRelInit_An_Sns']
ret.steeringPressed = not cp.vl["Lane_Keep_Assist_Status"]['LaHandsOff_B_Actl']
ret.steerError = cp.vl["Lane_Keep_Assist_Status"]['LaActDeny_B_Actl'] == 1
ret.cruiseState.speed = cp.vl["Cruise_Status"]['Set_Speed'] * CV.MPH_TO_MS

2
selfdrive/car/gm/carstate.py
View File

@ -28,7 +28,7 @@ class CarState(CarStateBase):
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.standstill = ret.vEgoRaw < 0.01
ret.steeringAngle = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']
ret.steeringAngleDeg = pt_cp.vl["PSCMSteeringAngle"]['SteeringWheelAngle']
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(pt_cp.vl["ECMPRDNL"]['PRNDL'], None))
ret.brake = pt_cp.vl["EBCMBrakePedalPosition"]['BrakePedalPosition'] / 0xd0
# Brake pedal's potentiometer returns near-zero reading even when pedal is not pressed.

4
selfdrive/car/honda/carstate.py
View File

@ -227,8 +227,8 @@ class CarState(CarStateBase):
ret.vEgoRaw = (1. - v_weight) * cp.vl["ENGINE_DATA"]['XMISSION_SPEED'] * CV.KPH_TO_MS * speed_factor + v_weight * v_wheel
ret.vEgo, ret.aEgo = self.update_speed_kf(ret.vEgoRaw)
ret.steeringAngle = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
ret.steeringRate = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']
ret.steeringAngleDeg = cp.vl["STEERING_SENSORS"]['STEER_ANGLE']
ret.steeringRateDeg = cp.vl["STEERING_SENSORS"]['STEER_ANGLE_RATE']
self.cruise_setting = cp.vl["SCM_BUTTONS"]['CRUISE_SETTING']
self.cruise_buttons = cp.vl["SCM_BUTTONS"]['CRUISE_BUTTONS']

2
selfdrive/car/honda/interface.py
View File

@ -447,7 +447,7 @@ class CarInterface(CarInterfaceBase):
ret = self.CS.update(self.cp, self.cp_cam, self.cp_body)
ret.canValid = self.cp.can_valid and self.cp_cam.can_valid and (self.cp_body is None or self.cp_body.can_valid)
ret.yawRate = self.VM.yaw_rate(ret.steeringAngle * CV.DEG_TO_RAD, ret.vEgo)
ret.yawRate = self.VM.yaw_rate(ret.steeringAngleDeg * CV.DEG_TO_RAD, ret.vEgo)
# FIXME: read sendcan for brakelights
brakelights_threshold = 0.02 if self.CS.CP.carFingerprint == CAR.CIVIC else 0.1
ret.brakeLights = bool(self.CS.brake_switch or

2
selfdrive/car/hyundai/carcontroller.py
View File

@ -50,7 +50,7 @@ class CarController():
self.steer_rate_limited = new_steer != apply_steer
# disable if steer angle reach 90 deg, otherwise mdps fault in some models
lkas_active = enabled and abs(CS.out.steeringAngle) < CS.CP.maxSteerAngle
lkas_active = enabled and abs(CS.out.steeringAngleDeg) < CS.CP.maxSteeringAngleDeg
# fix for Genesis hard fault at low speed
if CS.out.vEgo < 16.7 and self.car_fingerprint == CAR.HYUNDAI_GENESIS:

4
selfdrive/car/hyundai/carstate.py
View File

@ -26,8 +26,8 @@ class CarState(CarStateBase):
ret.standstill = ret.vEgoRaw < 0.1
ret.steeringAngle = cp.vl["SAS11"]['SAS_Angle']
ret.steeringRate = cp.vl["SAS11"]['SAS_Speed']
ret.steeringAngleDeg = cp.vl["SAS11"]['SAS_Angle']
ret.steeringRateDeg = cp.vl["SAS11"]['SAS_Speed']
ret.yawRate = cp.vl["ESP12"]['YAW_RATE']
ret.leftBlinker, ret.rightBlinker = self.update_blinker(50, cp.vl["CGW1"]['CF_Gway_TurnSigLh'],
cp.vl["CGW1"]['CF_Gway_TurnSigRh'])

4
selfdrive/car/hyundai/interface.py
View File

@ -27,7 +27,7 @@ class CarInterface(CarInterfaceBase):
ret.steerLimitTimer = 0.4
tire_stiffness_factor = 1.
ret.maxSteerAngle = 90.
ret.maxSteeringAngleDeg = 90.
eps_modified = False
for fw in car_fw:
@ -66,7 +66,7 @@ class CarInterface(CarInterfaceBase):
ret.lateralTuning.pid.kiBP, ret.lateralTuning.pid.kpBP = [[0.], [0.]]
ret.lateralTuning.pid.kpV, ret.lateralTuning.pid.kiV = [[0.3], [0.05]]
if eps_modified:
ret.maxSteerAngle = 1000.
ret.maxSteeringAngleDeg = 1000.
elif candidate in [CAR.ELANTRA, CAR.ELANTRA_GT_I30]:
ret.lateralTuning.pid.kf = 0.00006
ret.mass = 1275. + STD_CARGO_KG

4
selfdrive/car/mazda/carstate.py
View File

@ -38,12 +38,12 @@ class CarState(CarStateBase):
ret.leftBlinker = cp.vl["BLINK_INFO"]['LEFT_BLINK'] == 1
ret.rightBlinker = cp.vl["BLINK_INFO"]['RIGHT_BLINK'] == 1
ret.steeringAngle = cp.vl["STEER"]['STEER_ANGLE']
ret.steeringAngleDeg = cp.vl["STEER"]['STEER_ANGLE']
ret.steeringTorque = cp.vl["STEER_TORQUE"]['STEER_TORQUE_SENSOR']
ret.steeringPressed = abs(ret.steeringTorque) > LKAS_LIMITS.STEER_THRESHOLD
ret.steeringTorqueEps = cp.vl["STEER_TORQUE"]['STEER_TORQUE_MOTOR']
ret.steeringRate = cp.vl["STEER_RATE"]['STEER_ANGLE_RATE']
ret.steeringRateDeg = cp.vl["STEER_RATE"]['STEER_ANGLE_RATE']
ret.brakePressed = cp.vl["PEDALS"]['BRAKE_ON'] == 1
ret.brake = cp.vl["BRAKE"]['BRAKE_PRESSURE']

2
selfdrive/car/mock/interface.py
View File

@ -81,7 +81,7 @@ class CarInterface(CarInterfaceBase):
self.yawRate = LPG * self.yaw_rate_meas + (1. - LPG) * self.yaw_rate
curvature = self.yaw_rate / max(self.speed, 1.)
ret.steeringAngle = curvature * self.CP.steerRatio * self.CP.wheelbase * CV.RAD_TO_DEG
ret.steeringAngleDeg = curvature * self.CP.steerRatio * self.CP.wheelbase * CV.RAD_TO_DEG
return ret.as_reader()

4
selfdrive/car/nissan/carcontroller.py
View File

@ -29,7 +29,7 @@ class CarController():
acc_active = bool(CS.out.cruiseState.enabled)
lkas_hud_msg = CS.lkas_hud_msg
lkas_hud_info_msg = CS.lkas_hud_info_msg
apply_angle = actuators.steerAngle
apply_angle = actuators.steeringAngleDeg
steer_hud_alert = 1 if hud_alert == VisualAlert.steerRequired else 0
@ -55,7 +55,7 @@ class CarController():
)
else:
apply_angle = CS.out.steeringAngle
apply_angle = CS.out.steeringAngleDeg
self.lkas_max_torque = 0
self.last_angle = apply_angle

2
selfdrive/car/nissan/carstate.py
View File

@ -70,7 +70,7 @@ class CarState(CarStateBase):
# Filtering driver torque to prevent steeringPressed false positives
ret.steeringPressed = bool(abs(sum(self.steeringTorqueSamples) / TORQUE_SAMPLES) > CarControllerParams.STEER_THRESHOLD)
ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"]
ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]["STEER_ANGLE"]
ret.leftBlinker = bool(cp.vl["LIGHTS"]["LEFT_BLINKER"])
ret.rightBlinker = bool(cp.vl["LIGHTS"]["RIGHT_BLINKER"])

2
selfdrive/car/subaru/carstate.py
View File

@ -43,7 +43,7 @@ class CarState(CarStateBase):
can_gear = int(cp.vl["Transmission"]['Gear'])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))
ret.steeringAngle = cp.vl["Steering_Torque"]['Steering_Angle']
ret.steeringAngleDeg = cp.vl["Steering_Torque"]['Steering_Angle']
ret.steeringTorque = cp.vl["Steering_Torque"]['Steer_Torque_Sensor']
ret.steeringPressed = abs(ret.steeringTorque) > STEER_THRESHOLD[self.car_fingerprint]

2
selfdrive/car/toyota/carcontroller.py
View File

@ -103,7 +103,7 @@ class CarController():
# LTA mode. Set ret.steerControlType = car.CarParams.SteerControlType.angle and whitelist 0x191 in the panda
# if frame % 2 == 0:
# can_sends.append(create_steer_command(self.packer, 0, 0, frame // 2))
# can_sends.append(create_lta_steer_command(self.packer, actuators.steerAngle, apply_steer_req, frame // 2))
# can_sends.append(create_lta_steer_command(self.packer, actuators.steeringAngleDeg, apply_steer_req, frame // 2))
# we can spam can to cancel the system even if we are using lat only control
if (frame % 3 == 0 and CS.CP.openpilotLongitudinalControl) or (pcm_cancel_cmd and Ecu.fwdCamera in self.fake_ecus):

10
selfdrive/car/toyota/carstate.py
View File

@ -52,17 +52,17 @@ class CarState(CarStateBase):
self.accurate_steer_angle_seen = True
if self.accurate_steer_angle_seen:
ret.steeringAngle = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
ret.steeringAngleDeg = cp.vl["STEER_TORQUE_SENSOR"]['STEER_ANGLE'] - self.angle_offset
if self.needs_angle_offset:
angle_wheel = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngle) > 1e-3:
if abs(angle_wheel) > 1e-3 and abs(ret.steeringAngleDeg) > 1e-3:
self.needs_angle_offset = False
self.angle_offset = ret.steeringAngle - angle_wheel
self.angle_offset = ret.steeringAngleDeg - angle_wheel
else:
ret.steeringAngle = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
ret.steeringAngleDeg = cp.vl["STEER_ANGLE_SENSOR"]['STEER_ANGLE'] + cp.vl["STEER_ANGLE_SENSOR"]['STEER_FRACTION']
ret.steeringRate = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
ret.steeringRateDeg = cp.vl["STEER_ANGLE_SENSOR"]['STEER_RATE']
can_gear = int(cp.vl["GEAR_PACKET"]['GEAR'])
ret.gearShifter = self.parse_gear_shifter(self.shifter_values.get(can_gear, None))

4
selfdrive/car/volkswagen/carstate.py
View File

@ -28,8 +28,8 @@ class CarState(CarStateBase):
# Update steering angle, rate, yaw rate, and driver input torque. VW send
# the sign/direction in a separate signal so they must be recombined.
ret.steeringAngle = pt_cp.vl["LWI_01"]['LWI_Lenkradwinkel'] * (1, -1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])]
ret.steeringRate = pt_cp.vl["LWI_01"]['LWI_Lenkradw_Geschw'] * (1, -1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])]
ret.steeringAngleDeg = pt_cp.vl["LWI_01"]['LWI_Lenkradwinkel'] * (1, -1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])]
ret.steeringRateDeg = pt_cp.vl["LWI_01"]['LWI_Lenkradw_Geschw'] * (1, -1)[int(pt_cp.vl["LWI_01"]['LWI_VZ_Lenkradwinkel'])]
ret.steeringTorque = pt_cp.vl["EPS_01"]['Driver_Strain'] * (1, -1)[int(pt_cp.vl["EPS_01"]['Driver_Strain_VZ'])]
ret.steeringPressed = abs(ret.steeringTorque) > CarControllerParams.STEER_DRIVER_ALLOWANCE
ret.yawRate = pt_cp.vl["ESP_02"]['ESP_Gierrate'] * (1, -1)[int(pt_cp.vl["ESP_02"]['ESP_VZ_Gierrate'])] * CV.DEG_TO_RAD

60
selfdrive/controls/controlsd.py
View File

@ -32,9 +32,9 @@ SIMULATION = "SIMULATION" in os.environ
NOSENSOR = "NOSENSOR" in os.environ
IGNORE_PROCESSES = set(["rtshield", "uploader", "deleter", "loggerd", "logmessaged", "tombstoned", "logcatd", "proclogd", "clocksd", "updated", "timezoned"])
ThermalStatus = log.ThermalData.ThermalStatus
ThermalStatus = log.DeviceState.ThermalStatus
State = log.ControlsState.OpenpilotState
PandaType = log.HealthData.PandaType
PandaType = log.PandaState.PandaType
LongitudinalPlanSource = log.LongitudinalPlan.LongitudinalPlanSource
Desire = log.LateralPlan.Desire
LaneChangeState = log.LateralPlan.LaneChangeState
@ -54,17 +54,17 @@ class Controls:
self.sm = sm
if self.sm is None:
ignore = ['ubloxRaw', 'frontFrame', 'managerState'] if SIMULATION else None
self.sm = messaging.SubMaster(['thermal', 'health', 'modelV2', 'liveCalibration', 'ubloxRaw',
ignore = ['ubloxRaw', 'driverCameraState', 'managerState'] if SIMULATION else None
self.sm = messaging.SubMaster(['deviceState', 'pandaState', 'modelV2', 'liveCalibration', 'ubloxRaw',
'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman',
'frame', 'frontFrame', 'managerState', 'liveParameters', 'radarState'], ignore_alive=ignore)
'roadCameraState', 'driverCameraState', 'managerState', 'liveParameters', 'radarState'], ignore_alive=ignore)
self.can_sock = can_sock
if can_sock is None:
can_timeout = None if os.environ.get('NO_CAN_TIMEOUT', False) else 100
self.can_sock = messaging.sub_sock('can', timeout=can_timeout)
# wait for one health and one CAN packet
# wait for one pandaState and one CAN packet
print("Waiting for CAN messages...")
get_one_can(self.can_sock)
@ -127,7 +127,7 @@ class Controls:
self.logged_comm_issue = False
self.sm['liveCalibration'].calStatus = Calibration.CALIBRATED
self.sm['thermal'].freeSpacePercent = 1.
self.sm['deviceState'].freeSpacePercent = 1.
self.sm['driverMonitoringState'].events = []
self.sm['driverMonitoringState'].awarenessStatus = 1.
self.sm['driverMonitoringState'].faceDetected = False
@ -158,20 +158,20 @@ class Controls:
self.startup_event = None
# Create events for battery, temperature, disk space, and memory
if self.sm['thermal'].batteryPercent < 1 and self.sm['thermal'].chargingError:
if self.sm['deviceState'].batteryPercent < 1 and self.sm['deviceState'].chargingError:
# at zero percent battery, while discharging, OP should not allowed
self.events.add(EventName.lowBattery)
if self.sm['thermal'].thermalStatus >= ThermalStatus.red:
if self.sm['deviceState'].thermalStatus >= ThermalStatus.red:
self.events.add(EventName.overheat)
if self.sm['thermal'].freeSpacePercent < 0.07:
if self.sm['deviceState'].freeSpacePercent < 0.07:
# under 7% of space free no enable allowed
self.events.add(EventName.outOfSpace)
if self.sm['thermal'].memoryUsagePercent > 90:
if self.sm['deviceState'].memoryUsagePercent > 90:
self.events.add(EventName.lowMemory)
# Alert if fan isn't spinning for 5 seconds
if self.sm['health'].pandaType in [PandaType.uno, PandaType.dos]:
if self.sm['health'].fanSpeedRpm == 0 and self.sm['thermal'].fanSpeedPercentDesired > 50:
if self.sm['pandaState'].pandaType in [PandaType.uno, PandaType.dos]:
if self.sm['pandaState'].fanSpeedRpm == 0 and self.sm['deviceState'].fanSpeedPercentDesired > 50:
if (self.sm.frame - self.last_functional_fan_frame) * DT_CTRL > 5.0:
self.events.add(EventName.fanMalfunction)
else:
@ -202,7 +202,7 @@ class Controls:
if self.can_rcv_error or (not CS.canValid and self.sm.frame > 5 / DT_CTRL):
self.events.add(EventName.canError)
if (self.sm['health'].safetyModel != self.CP.safetyModel and self.sm.frame > 2 / DT_CTRL) or \
if (self.sm['pandaState'].safetyModel != self.CP.safetyModel and self.sm.frame > 2 / DT_CTRL) or \
self.mismatch_counter >= 200:
self.events.add(EventName.controlsMismatch)
@ -227,7 +227,7 @@ class Controls:
self.events.add(EventName.posenetInvalid)
if not self.sm['liveLocationKalman'].deviceStable:
self.events.add(EventName.deviceFalling)
if log.HealthData.FaultType.relayMalfunction in self.sm['health'].faults:
if log.PandaState.FaultType.relayMalfunction in self.sm['pandaState'].faults:
self.events.add(EventName.relayMalfunction)
if self.sm['longitudinalPlan'].fcw:
self.events.add(EventName.fcw)
@ -240,7 +240,7 @@ class Controls:
elif not self.sm['liveLocationKalman'].gpsOK and (self.distance_traveled > 1000) and not TICI:
# Not show in first 1 km to allow for driving out of garage. This event shows after 5 minutes
self.events.add(EventName.noGps)
if not self.sm.all_alive(['frame', 'frontFrame']) and (self.sm.frame > 5 / DT_CTRL):
if not self.sm.all_alive(['roadCameraState', 'driverCameraState']) and (self.sm.frame > 5 / DT_CTRL):
self.events.add(EventName.cameraMalfunction)
if self.sm['modelV2'].frameDropPerc > 20:
self.events.add(EventName.modeldLagging)
@ -278,7 +278,7 @@ class Controls:
if not self.enabled:
self.mismatch_counter = 0
if not self.sm['health'].controlsAllowed and self.enabled:
if not self.sm['pandaState'].controlsAllowed and self.enabled:
self.mismatch_counter += 1
self.distance_traveled += CS.vEgo * DT_CTRL
@ -365,8 +365,8 @@ class Controls:
def state_control(self, CS):
"""Given the state, this function returns an actuators packet"""
plan = self.sm['longitudinalPlan']
path_plan = self.sm['lateralPlan']
lat_plan = self.sm['lateralPlan']
long_plan = self.sm['longitudinalPlan']
actuators = car.CarControl.Actuators.new_message()
@ -379,21 +379,21 @@ class Controls:
self.LaC.reset()
self.LoC.reset(v_pid=CS.vEgo)
plan_age = DT_CTRL * (self.sm.frame - self.sm.rcv_frame['longitudinalPlan'])
long_plan_age = DT_CTRL * (self.sm.frame - self.sm.rcv_frame['longitudinalPlan'])
# no greater than dt mpc + dt, to prevent too high extraps
dt = min(plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL
dt = min(long_plan_age, LON_MPC_STEP + DT_CTRL) + DT_CTRL
a_acc_sol = plan.aStart + (dt / LON_MPC_STEP) * (plan.aTarget - plan.aStart)
v_acc_sol = plan.vStart + dt * (a_acc_sol + plan.aStart) / 2.0
a_acc_sol = long_plan.aStart + (dt / LON_MPC_STEP) * (long_plan.aTarget - long_plan.aStart)
v_acc_sol = long_plan.vStart + dt * (a_acc_sol + long_plan.aStart) / 2.0
# Gas/Brake PID loop
actuators.gas, actuators.brake = self.LoC.update(self.active, CS, v_acc_sol, plan.vTargetFuture, a_acc_sol, self.CP)
actuators.gas, actuators.brake = self.LoC.update(self.active, CS, v_acc_sol, long_plan.vTargetFuture, a_acc_sol, self.CP)
# Steering PID loop and lateral MPC
actuators.steer, actuators.steerAngle, lac_log = self.LaC.update(self.active, CS, self.CP, path_plan)
actuators.steer, actuators.steeringAngleDeg, lac_log = self.LaC.update(self.active, CS, self.CP, lat_plan)
# Check for difference between desired angle and angle for angle based control
angle_control_saturated = self.CP.steerControlType == car.CarParams.SteerControlType.angle and \
abs(actuators.steerAngle - CS.steeringAngle) > STEER_ANGLE_SATURATION_THRESHOLD
abs(actuators.steeringAngleDeg - CS.steeringAngleDeg) > STEER_ANGLE_SATURATION_THRESHOLD
if angle_control_saturated and not CS.steeringPressed and self.active:
self.saturated_count += 1
@ -404,8 +404,8 @@ class Controls:
if (lac_log.saturated and not CS.steeringPressed) or \
(self.saturated_count > STEER_ANGLE_SATURATION_TIMEOUT):
# Check if we deviated from the path
left_deviation = actuators.steer > 0 and path_plan.dPathPoints[0] < -0.1
right_deviation = actuators.steer < 0 and path_plan.dPathPoints[0] > 0.1
left_deviation = actuators.steer > 0 and lat_plan.dPathPoints[0] < -0.1
right_deviation = actuators.steer < 0 and lat_plan.dPathPoints[0] > 0.1
if left_deviation or right_deviation:
self.events.add(EventName.steerSaturated)
@ -470,7 +470,7 @@ class Controls:
force_decel = (self.sm['driverMonitoringState'].awarenessStatus < 0.) or \
(self.state == State.softDisabling)
steer_angle_rad = (CS.steeringAngle - self.sm['lateralPlan'].angleOffset) * CV.DEG_TO_RAD
steer_angle_rad = (CS.steeringAngleDeg - self.sm['lateralPlan'].angleOffsetDeg) * CV.DEG_TO_RAD
# controlsState
dat = messaging.new_message('controlsState')
@ -497,7 +497,7 @@ class Controls:
controlsState.upAccelCmd = float(self.LoC.pid.p)
controlsState.uiAccelCmd = float(self.LoC.pid.i)
controlsState.ufAccelCmd = float(self.LoC.pid.f)
controlsState.angleSteersDes = float(self.LaC.angle_steers_des)
controlsState.steeringAngleDesiredDeg = float(self.LaC.angle_steers_des)
controlsState.vTargetLead = float(v_acc)
controlsState.aTarget = float(a_acc)
controlsState.cumLagMs = -self.rk.remaining * 1000.

2
selfdrive/controls/lib/events.py
View File

@ -195,7 +195,7 @@ def calibration_incomplete_alert(CP: car.CarParams, sm: messaging.SubMaster, met
Priority.LOWEST, VisualAlert.none, AudibleAlert.none, 0., 0., .2)
def no_gps_alert(CP: car.CarParams, sm: messaging.SubMaster, metric: bool) -> Alert:
gps_integrated = sm['health'].pandaType in [log.HealthData.PandaType.uno, log.HealthData.PandaType.dos]
gps_integrated = sm['pandaState'].pandaType in [log.PandaState.PandaType.uno, log.PandaState.PandaType.dos]
return Alert(
"Poor GPS reception",
"If sky is visible, contact support" if gps_integrated else "Check GPS antenna placement",

14
selfdrive/controls/lib/latcontrol_indi.py
View File

@ -80,24 +80,24 @@ class LatControlINDI():
return self.sat_count > self.sat_limit
def update(self, active, CS, CP, path_plan):
def update(self, active, CS, CP, lat_plan):
self.speed = CS.vEgo
# Update Kalman filter
y = np.array([[math.radians(CS.steeringAngle)], [math.radians(CS.steeringRate)]])
y = np.array([[math.radians(CS.steeringAngleDeg)], [math.radians(CS.steeringRateDeg)]])
self.x = np.dot(self.A_K, self.x) + np.dot(self.K, y)
indi_log = log.ControlsState.LateralINDIState.new_message()
indi_log.steerAngle = math.degrees(self.x[0])
indi_log.steerRate = math.degrees(self.x[1])
indi_log.steerAccel = math.degrees(self.x[2])
indi_log.steeringAngleDeg = math.degrees(self.x[0])
indi_log.steeringRateDeg = math.degrees(self.x[1])
indi_log.steeringAccelDeg = math.degrees(self.x[2])
if CS.vEgo < 0.3 or not active:
indi_log.active = False
self.output_steer = 0.0
self.delayed_output = 0.0
else:
self.angle_steers_des = path_plan.angleSteers
self.rate_steers_des = path_plan.rateSteers
self.angle_steers_des = lat_plan.steeringAngleDeg
self.rate_steers_des = lat_plan.steeringRateDeg
steers_des = math.radians(self.angle_steers_des)
rate_des = math.radians(self.rate_steers_des)

10
selfdrive/controls/lib/latcontrol_lqr.py
View File

@ -43,17 +43,17 @@ class LatControlLQR():
return self.sat_count > self.sat_limit
def update(self, active, CS, CP, path_plan):
def update(self, active, CS, CP, lat_plan):
lqr_log = log.ControlsState.LateralLQRState.new_message()
steers_max = get_steer_max(CP, CS.vEgo)
torque_scale = (0.45 + CS.vEgo / 60.0)**2 # Scale actuator model with speed
steering_angle = CS.steeringAngle
steering_angle = CS.steeringAngleDeg
# Subtract offset. Zero angle should correspond to zero torque
self.angle_steers_des = path_plan.angleSteers - path_plan.angleOffset
steering_angle -= path_plan.angleOffset
self.angle_steers_des = lat_plan.steeringAngleDeg - lat_plan.angleOffsetDeg
steering_angle -= lat_plan.angleOffsetDeg
# Update Kalman filter
angle_steers_k = float(self.C.dot(self.x_hat))
@ -89,7 +89,7 @@ class LatControlLQR():
check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
saturated = self._check_saturation(self.output_steer, check_saturation, steers_max)
lqr_log.steerAngle = angle_steers_k + path_plan.angleOffset
lqr_log.steeringAngleDeg = angle_steers_k + lat_plan.angleOffsetDeg
lqr_log.i = self.i_lqr
lqr_log.output = self.output_steer
lqr_log.lqrOutput = lqr_output

14
selfdrive/controls/lib/latcontrol_pid.py
View File

@ -15,30 +15,30 @@ class LatControlPID():
def reset(self):
self.pid.reset()
def update(self, active, CS, CP, path_plan):
def update(self, active, CS, CP, lat_plan):
pid_log = log.ControlsState.LateralPIDState.new_message()
pid_log.steerAngle = float(CS.steeringAngle)
pid_log.steerRate = float(CS.steeringRate)
pid_log.steeringAngleDeg = float(CS.steeringAngleDeg)
pid_log.steeringRateDeg = float(CS.steeringRateDeg)
if CS.vEgo < 0.3 or not active:
output_steer = 0.0
pid_log.active = False
self.pid.reset()
else:
self.angle_steers_des = path_plan.angleSteers # get from MPC/LateralPlanner
self.angle_steers_des = lat_plan.steeringAngleDeg # get from MPC/LateralPlanner
steers_max = get_steer_max(CP, CS.vEgo)
self.pid.pos_limit = steers_max
self.pid.neg_limit = -steers_max
steer_feedforward = self.angle_steers_des # feedforward desired angle
if CP.steerControlType == car.CarParams.SteerControlType.torque:
# TODO: feedforward something based on path_plan.rateSteers
steer_feedforward -= path_plan.angleOffset # subtract the offset, since it does not contribute to resistive torque
# TODO: feedforward something based on lat_plan.rateSteers
steer_feedforward -= lat_plan.angleOffsetDeg # subtract the offset, since it does not contribute to resistive torque
steer_feedforward *= CS.vEgo**2 # proportional to realigning tire momentum (~ lateral accel)
deadzone = 0.0
check_saturation = (CS.vEgo > 10) and not CS.steeringRateLimited and not CS.steeringPressed
output_steer = self.pid.update(self.angle_steers_des, CS.steeringAngle, check_saturation=check_saturation, override=CS.steeringPressed,
output_steer = self.pid.update(self.angle_steers_des, CS.steeringAngleDeg, check_saturation=check_saturation, override=CS.steeringPressed,
feedforward=steer_feedforward, speed=CS.vEgo, deadzone=deadzone)
pid_log.active = True
pid_log.p = self.pid.p

10
selfdrive/controls/lib/lateral_planner.py
View File

@ -82,8 +82,8 @@ class LateralPlanner():
def update(self, sm, CP, VM):
v_ego = sm['carState'].vEgo
active = sm['controlsState'].active
steering_wheel_angle_offset_deg = sm['liveParameters'].angleOffset
steering_wheel_angle_deg = sm['carState'].steeringAngle
steering_wheel_angle_offset_deg = sm['liveParameters'].angleOffsetDeg
steering_wheel_angle_deg = sm['carState'].steeringAngleDeg
# Update vehicle model
x = max(sm['liveParameters'].stiffnessFactor, 0.1)
@ -232,9 +232,9 @@ class LateralPlanner():
plan_send.lateralPlan.rProb = float(self.LP.rll_prob)
plan_send.lateralPlan.dProb = float(self.LP.d_prob)
plan_send.lateralPlan.angleSteers = float(self.desired_steering_wheel_angle_deg)
plan_send.lateralPlan.rateSteers = float(self.desired_steering_wheel_angle_rate_deg)
plan_send.lateralPlan.angleOffset = float(sm['liveParameters'].angleOffsetAverage)
plan_send.lateralPlan.steeringAngleDeg = float(self.desired_steering_wheel_angle_deg)
plan_send.lateralPlan.steeringRateDeg = float(self.desired_steering_wheel_angle_rate_deg)
plan_send.lateralPlan.angleOffsetDeg = float(sm['liveParameters'].angleOffsetAverageDeg)
plan_send.lateralPlan.mpcSolutionValid = bool(plan_solution_valid)
plan_send.lateralPlan.desire = self.desire

2
selfdrive/controls/lib/longitudinal_planner.py
View File

@ -132,7 +132,7 @@ class Planner():
if enabled and not self.first_loop and not sm['carState'].gasPressed:
accel_limits = [float(x) for x in calc_cruise_accel_limits(v_ego, following)]
jerk_limits = [min(-0.1, accel_limits[0]), max(0.1, accel_limits[1])] # TODO: make a separate lookup for jerk tuning
accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngle, accel_limits, self.CP)
accel_limits_turns = limit_accel_in_turns(v_ego, sm['carState'].steeringAngleDeg, accel_limits, self.CP)
if force_slow_decel:
# if required so, force a smooth deceleration

8
selfdrive/controls/tests/test_startup.py
View File

@ -41,7 +41,7 @@ class TestStartup(unittest.TestCase):
# TODO: this should be done without any real sockets
controls_sock = messaging.sub_sock("controlsState")
pm = messaging.PubMaster(['can', 'health'])
pm = messaging.PubMaster(['can', 'pandaState'])
params = Params()
params.clear_all()
@ -51,9 +51,9 @@ class TestStartup(unittest.TestCase):
time.sleep(2) # wait for controlsd to be ready
health = messaging.new_message('health')
health.health.pandaType = log.HealthData.PandaType.uno
pm.send('health', health)
msg = messaging.new_message('pandaState')
msg.pandaState.pandaType = log.PandaState.PandaType.uno
pm.send('pandaState', msg)
# fingerprint
if car is None:

12
selfdrive/debug/cycle_alerts.py
View File

@ -16,11 +16,11 @@ def cycle_alerts(duration=200, is_metric=False):
print(alerts)
CP = CarInterface.get_params("HONDA CIVIC 2016 TOURING")
sm = messaging.SubMaster(['thermal', 'health', 'frame', 'model', 'liveCalibration',
'driverMonitoringState', 'plan', 'lateralPlan', 'liveLocationKalman'])
sm = messaging.SubMaster(['deviceState', 'pandaState', 'roadCameraState', 'modelV2', 'liveCalibration',
'driverMonitoringState', 'longitudinalPlan', 'lateralPlan', 'liveLocationKalman'])
controls_state = messaging.pub_sock('controlsState')
thermal = messaging.pub_sock('thermal')
deviceState = messaging.pub_sock('deviceState')
idx, last_alert_millis = 0, 0
@ -56,9 +56,9 @@ def cycle_alerts(duration=200, is_metric=False):
controls_state.send(dat.to_bytes())
dat = messaging.new_message()
dat.init('thermal')
dat.thermal.started = True
thermal.send(dat.to_bytes())
dat.init('deviceState')
dat.deviceState.started = True
deviceState.send(dat.to_bytes())
frame += 1
time.sleep(0.01)

2
selfdrive/debug/internal/check_alive_valid.py
View File

@ -4,7 +4,7 @@ import cereal.messaging as messaging
if __name__ == "__main__":
sm = messaging.SubMaster(['thermal', 'health', 'model', 'liveCalibration', 'driverMonitoringState', 'plan', 'lateralPlan'])
sm = messaging.SubMaster(['deviceState', 'pandaState', 'modelV2', 'liveCalibration', 'driverMonitoringState', 'longitudinalPlan', 'lateralPlan'])
i = 0
while True:

2
selfdrive/debug/internal/measure_steering_accuracy.py
View File

@ -47,7 +47,7 @@ if __name__ == "__main__":
if cnt >= 500:
# calculate error before rounding
actual_angle = sm['controlsState'].angleSteers
desired_angle = sm['carControl'].actuators.steerAngle
desired_angle = sm['carControl'].actuators.steeringAngleDeg
angle_error = abs(desired_angle - actual_angle)
# round numbers

4
selfdrive/debug/internal/sounds/test_sound_stability.py
View File

@ -19,7 +19,7 @@ if __name__ == "__main__":
os.environ["LD_LIBRARY_PATH"] = ""
sm = messaging.SubMaster(["thermal"])
sm = messaging.SubMaster(["deviceState"])
FNULL = open(os.devnull, "w")
start_time = time.time()
@ -42,4 +42,4 @@ if __name__ == "__main__":
s = time.time() - start_time
hhmmss = str(datetime.timedelta(seconds=s)).split(".")[0]
print("test duration:", hhmmss)
print("\tbattery percent", sm["thermal"].batteryPercent)
print("\tbattery percent", sm["deviceState"].batteryPercent)

10
selfdrive/debug/live_cpu_and_temp.py
View File

@ -33,7 +33,7 @@ if __name__ == "__main__":
parser.add_argument('--cpu', action='store_true')
args = parser.parse_args()
sm = SubMaster(['thermal', 'procLog'])
sm = SubMaster(['deviceState', 'procLog'])
last_temp = 0.0
last_mem = 0.0
@ -46,10 +46,10 @@ if __name__ == "__main__":
while True:
sm.update()
if sm.updated['thermal']:
t = sm['thermal']
last_temp = mean(t.cpu)
last_mem = t.memUsedPercent
if sm.updated['deviceState']:
t = sm['deviceState']
last_temp = mean(t.cpuTempC)
last_mem = t.memoryUsagePercent
if sm.updated['procLog']:
m = sm['procLog']

4
selfdrive/debug/test_fw_query_on_routes.py
View File

@ -46,8 +46,8 @@ if __name__ == "__main__":
lr = LogReader(qlog_path)
for msg in lr:
if msg.which() == "health":
if msg.health.pandaType not in ['uno', 'blackPanda']:
if msg.which() == "pandaState":
if msg.pandaState.pandaType not in ['uno', 'blackPanda']:
dongles.append(dongle_id)
break

10
selfdrive/debug/uiview.py
View File

@ -3,20 +3,20 @@ import time
import cereal.messaging as messaging
from selfdrive.manager import start_managed_process, kill_managed_process
services = ['controlsState', 'thermal', 'radarState'] # the services needed to be spoofed to start ui offroad
services = ['controlsState', 'deviceState', 'radarState'] # the services needed to be spoofed to start ui offroad
procs = ['camerad', 'ui', 'modeld', 'calibrationd']
[start_managed_process(p) for p in procs] # start needed processes
pm = messaging.PubMaster(services)
dat_cs, dat_thermal, dat_radar = [messaging.new_message(s) for s in services]
dat_cs, dat_deviceState, dat_radar = [messaging.new_message(s) for s in services]
dat_cs.controlsState.rearViewCam = False # ui checks for these two messages
dat_thermal.thermal.started = True
dat_deviceState.deviceState.started = True
try:
while True:
pm.send('controlsState', dat_cs)
pm.send('thermal', dat_thermal)
pm.send('deviceState', dat_deviceState)
pm.send('radarState', dat_radar)
time.sleep(1 / 100) # continually send, rate doesn't matter for thermal
time.sleep(1 / 100) # continually send, rate doesn't matter for deviceState
except KeyboardInterrupt:
[kill_managed_process(p) for p in procs]

4
selfdrive/hardware/eon/hardware.py
View File

@ -8,8 +8,8 @@ import subprocess
from cereal import log
from selfdrive.hardware.base import HardwareBase
NetworkType = log.ThermalData.NetworkType
NetworkStrength = log.ThermalData.NetworkStrength
NetworkType = log.DeviceState.NetworkType
NetworkStrength = log.DeviceState.NetworkStrength
def service_call(call):

4
selfdrive/hardware/pc/hardware.py
View File

@ -3,8 +3,8 @@ import random
from cereal import log
from selfdrive.hardware.base import HardwareBase
NetworkType = log.ThermalData.NetworkType
NetworkStrength = log.ThermalData.NetworkStrength
NetworkType = log.DeviceState.NetworkType
NetworkStrength = log.DeviceState.NetworkStrength
class Pc(HardwareBase):

4
selfdrive/hardware/tici/hardware.py
View File

@ -20,8 +20,8 @@ MM_MODEM_STATE_CONNECTED = 11
TIMEOUT = 0.1
NetworkType = log.ThermalData.NetworkType
NetworkStrength = log.ThermalData.NetworkStrength
NetworkType = log.DeviceState.NetworkType
NetworkStrength = log.DeviceState.NetworkStrength
# https://developer.gnome.org/ModemManager/unstable/ModemManager-Flags-and-Enumerations.html#MMModemAccessTechnology
MM_MODEM_ACCESS_TECHNOLOGY_UMTS = 1 << 5

2
selfdrive/locationd/locationd.py
View File

@ -196,7 +196,7 @@ class Localizer():
orientation_ecef = euler_from_quat(self.kf.x[States.ECEF_ORIENTATION])
orientation_ned = ned_euler_from_ecef(ecef_pos, orientation_ecef)
orientation_ned_gps = np.array([0, 0, np.radians(log.bearing)])
orientation_ned_gps = np.array([0, 0, np.radians(log.bearingDeg)])
orientation_error = np.mod(orientation_ned - orientation_ned_gps - np.pi, 2*np.pi) - np.pi
initial_pose_ecef_quat = quat_from_euler(ecef_euler_from_ned(ecef_pos, orientation_ned_gps))
if np.linalg.norm(ecef_vel) > 5 and np.linalg.norm(orientation_error) > 1:

31
selfdrive/locationd/paramsd.py
View File

@ -48,7 +48,7 @@ class ParamsLearner:
self.kf.predict_and_observe(t, ObservationKind.ANGLE_OFFSET_FAST, np.array([[[0]]]))
elif which == 'carState':
self.steering_angle = msg.steeringAngle
self.steering_angle = msg.steeringAngleDeg
self.steering_pressed = msg.steeringPressed
self.speed = msg.vEgo
@ -56,7 +56,7 @@ class ParamsLearner:
self.active = self.speed > 5 and in_linear_region
if self.active:
self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngle)]]]))
self.kf.predict_and_observe(t, ObservationKind.STEER_ANGLE, np.array([[[math.radians(msg.steeringAngleDeg)]]]))
self.kf.predict_and_observe(t, ObservationKind.ROAD_FRAME_X_SPEED, np.array([[[self.speed]]]))
if not self.active:
@ -88,18 +88,23 @@ def main(sm=None, pm=None):
cloudlog.info("Parameter learner found parameters for wrong car.")
params = None
if (params is not None) and not all((
abs(params['angleOffsetAverage']) < 10.0,
min_sr <= params['steerRatio'] <= max_sr)):
cloudlog.info(f"Invalid starting values found {params}")
try:
if params is not None and not all((
abs(params.get('angleOffsetAverageDeg')) < 10.0,
min_sr <= params['steerRatio'] <= max_sr)):
cloudlog.info(f"Invalid starting values found {params}")
params = None
except Exception as e:
cloudlog.info(f"Error reading params {params}: {str(e)}")
params = None
# TODO: cache the params with the capnp struct
if params is None:
params = {
'carFingerprint': CP.carFingerprint,
'steerRatio': CP.steerRatio,
'stiffnessFactor': 1.0,
'angleOffsetAverage': 0.0,
'angleOffsetAverageDeg': 0.0,
}
cloudlog.info("Parameter learner resetting to default values")
@ -107,7 +112,7 @@ def main(sm=None, pm=None):
# Without a way to detect this we have to reset the stiffness every drive
params['stiffnessFactor'] = 1.0
learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverage']))
learner = ParamsLearner(CP, params['steerRatio'], params['stiffnessFactor'], math.radians(params['angleOffsetAverageDeg']))
while True:
sm.update()
@ -127,11 +132,11 @@ def main(sm=None, pm=None):
x = learner.kf.x
msg.liveParameters.steerRatio = float(x[States.STEER_RATIO])
msg.liveParameters.stiffnessFactor = float(x[States.STIFFNESS])
msg.liveParameters.angleOffsetAverage = math.degrees(x[States.ANGLE_OFFSET])
msg.liveParameters.angleOffset = msg.liveParameters.angleOffsetAverage + math.degrees(x[States.ANGLE_OFFSET_FAST])
msg.liveParameters.angleOffsetAverageDeg = math.degrees(x[States.ANGLE_OFFSET])
msg.liveParameters.angleOffsetDeg = msg.liveParameters.angleOffsetAverageDeg + math.degrees(x[States.ANGLE_OFFSET_FAST])
msg.liveParameters.valid = all((
abs(msg.liveParameters.angleOffsetAverage) < 10.0,
abs(msg.liveParameters.angleOffset) < 10.0,
abs(msg.liveParameters.angleOffsetAverageDeg) < 10.0,
abs(msg.liveParameters.angleOffsetDeg) < 10.0,
0.2 <= msg.liveParameters.stiffnessFactor <= 5.0,
min_sr <= msg.liveParameters.steerRatio <= max_sr,
))
@ -141,7 +146,7 @@ def main(sm=None, pm=None):
'carFingerprint': CP.carFingerprint,
'steerRatio': msg.liveParameters.steerRatio,
'stiffnessFactor': msg.liveParameters.stiffnessFactor,
'angleOffsetAverage': msg.liveParameters.angleOffsetAverage,
'angleOffsetAverageDeg': msg.liveParameters.angleOffsetAverageDeg,
}
put_nonblocking("LiveParameters", json.dumps(params))

4
selfdrive/locationd/test/ubloxd.py
View File

@ -138,7 +138,7 @@ def gen_solution(msg):
msg_data['sec']) -
datetime.datetime(1970, 1, 1)).total_seconds())*1e+03 +
msg_data['nano']*1e-06)
gps_fix = {'bearing': msg_data['headMot']*1e-05, # heading of motion in degrees
gps_fix = {'bearingDeg': msg_data['headMot']*1e-05, # heading of motion in degrees
'altitude': msg_data['height']*1e-03, # altitude above ellipsoid
'latitude': msg_data['lat']*1e-07, # latitude in degrees
'longitude': msg_data['lon']*1e-07, # longitude in degrees
@ -150,7 +150,7 @@ def gen_solution(msg):
msg_data['velD']*1e-03], # velocity in NED frame in m/s
'speedAccuracy': msg_data['sAcc']*1e-03, # speed accuracy in m/s
'verticalAccuracy': msg_data['vAcc']*1e-03, # vertical accuracy in meters
'bearingAccuracy': msg_data['headAcc']*1e-05, # heading accuracy in degrees
'bearingAccuracyDeg': msg_data['headAcc']*1e-05, # heading accuracy in degrees
'source': 'ublox',
'flags': msg_data['flags'],
}

4
selfdrive/locationd/test/ubloxd_regression_test.py
View File

@ -48,8 +48,8 @@ def compare_results(dir1, dir2):
old_gps = events1[i].gpsLocationExternal
gps = events2[i].gpsLocationExternal
# print(gps, old_gps)
attrs = ['flags', 'latitude', 'longitude', 'altitude', 'speed', 'bearing',
'accuracy', 'timestamp', 'source', 'vNED', 'verticalAccuracy', 'bearingAccuracy', 'speedAccuracy']
attrs = ['flags', 'latitude', 'longitude', 'altitude', 'speed', 'bearingDeg',
'accuracy', 'timestamp', 'source', 'vNED', 'verticalAccuracy', 'bearingAccuracyDeg', 'speedAccuracy']
for attr in attrs:
o = getattr(old_gps, attr)
n = getattr(gps, attr)

4
selfdrive/locationd/ublox_msg.cc
View File

@ -192,7 +192,7 @@ kj::Array<capnp::word> UbloxMsgParser::gen_solution() {
gpsLoc.setLongitude(msg->lon * 1e-07);
gpsLoc.setAltitude(msg->height * 1e-03);
gpsLoc.setSpeed(msg->gSpeed * 1e-03);
gpsLoc.setBearing(msg->headMot * 1e-5);
gpsLoc.setBearingDeg(msg->headMot * 1e-5);
gpsLoc.setAccuracy(msg->hAcc * 1e-03);
std::tm timeinfo = std::tm();
timeinfo.tm_year = msg->year - 1900;
@ -207,7 +207,7 @@ kj::Array<capnp::word> UbloxMsgParser::gen_solution() {
gpsLoc.setVNED(f);
gpsLoc.setVerticalAccuracy(msg->vAcc * 1e-03);
gpsLoc.setSpeedAccuracy(msg->sAcc * 1e-03);
gpsLoc.setBearingAccuracy(msg->headAcc * 1e-05);
gpsLoc.setBearingAccuracyDeg(msg->headAcc * 1e-05);
return capnp::messageToFlatArray(msg_builder);
}

16
selfdrive/loggerd/loggerd.cc
View File

@ -61,7 +61,7 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = {
[LOG_CAMERA_ID_FCAMERA] = {
.stream_type = VISION_STREAM_YUV_BACK,
.filename = "fcamera.hevc",
.frame_packet_name = "frame",
.frame_packet_name = "roadCameraState",
.fps = MAIN_FPS,
.bitrate = MAIN_BITRATE,
.is_h265 = true,
@ -71,7 +71,7 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = {
[LOG_CAMERA_ID_DCAMERA] = {
.stream_type = VISION_STREAM_YUV_FRONT,
.filename = "dcamera.hevc",
.frame_packet_name = "frontFrame",
.frame_packet_name = "driverCameraState",
.fps = MAIN_FPS, // on EONs, more compressed this way
.bitrate = DCAM_BITRATE,
.is_h265 = true,
@ -81,7 +81,7 @@ LogCameraInfo cameras_logged[LOG_CAMERA_ID_MAX] = {
[LOG_CAMERA_ID_ECAMERA] = {
.stream_type = VISION_STREAM_YUV_WIDE,
.filename = "ecamera.hevc",
.frame_packet_name = "wideFrame",
.frame_packet_name = "wideRoadCameraState",
.fps = MAIN_FPS,
.bitrate = MAIN_BITRATE,
.is_h265 = true,
@ -272,8 +272,8 @@ void encoder_thread(int cam_idx) {
// publish encode index
MessageBuilder msg;
// this is really ugly
auto eidx = cam_idx == LOG_CAMERA_ID_DCAMERA ? msg.initEvent().initFrontEncodeIdx() :
(cam_idx == LOG_CAMERA_ID_ECAMERA ? msg.initEvent().initWideEncodeIdx() : msg.initEvent().initEncodeIdx());
auto eidx = cam_idx == LOG_CAMERA_ID_DCAMERA ? msg.initEvent().initDriverEncodeIdx() :
(cam_idx == LOG_CAMERA_ID_ECAMERA ? msg.initEvent().initWideRoadEncodeIdx() : msg.initEvent().initRoadEncodeIdx());
eidx.setFrameId(extra.frame_id);
eidx.setTimestampSof(extra.timestamp_sof);
eidx.setTimestampEof(extra.timestamp_eof);
@ -437,11 +437,11 @@ int main(int argc, char** argv) {
cereal::Event::Reader event = cmsg.getRoot<cereal::Event>();
if (fpkt_id == LOG_CAMERA_ID_FCAMERA) {
s.rotate_state[fpkt_id].setLogFrameId(event.getFrame().getFrameId());
s.rotate_state[fpkt_id].setLogFrameId(event.getRoadCameraState().getFrameId());
} else if (fpkt_id == LOG_CAMERA_ID_DCAMERA) {
s.rotate_state[fpkt_id].setLogFrameId(event.getFrontFrame().getFrameId());
s.rotate_state[fpkt_id].setLogFrameId(event.getDriverCameraState().getFrameId());
} else if (fpkt_id == LOG_CAMERA_ID_ECAMERA) {
s.rotate_state[fpkt_id].setLogFrameId(event.getWideFrame().getFrameId());
s.rotate_state[fpkt_id].setLogFrameId(event.getWideRoadCameraState().getFrameId());
}
last_camera_seen_tms = millis_since_boot();
}

6
selfdrive/loggerd/uploader.py
View File

@ -15,7 +15,7 @@ from selfdrive.loggerd.xattr_cache import getxattr, setxattr
from selfdrive.loggerd.config import ROOT
from selfdrive.swaglog import cloudlog
NetworkType = log.ThermalData.NetworkType
NetworkType = log.DeviceState.NetworkType
UPLOAD_ATTR_NAME = 'user.upload'
UPLOAD_ATTR_VALUE = b'1'
@ -198,13 +198,13 @@ def uploader_fn(exit_event):
cloudlog.info("uploader missing dongle_id")
raise Exception("uploader can't start without dongle id")
sm = messaging.SubMaster(['thermal'])
sm = messaging.SubMaster(['deviceState'])
uploader = Uploader(dongle_id, ROOT)
backoff = 0.1
while not exit_event.is_set():
sm.update(0)
on_wifi = force_wifi or sm['thermal'].networkType == NetworkType.wifi
on_wifi = force_wifi or sm['deviceState'].networkType == NetworkType.wifi
offroad = params.get("IsOffroad") == b'1'
allow_raw_upload = params.get("IsUploadRawEnabled") != b"0"

10
selfdrive/manager.py
View File

@ -459,16 +459,16 @@ def manager_thread():
started_prev = False
logger_dead = False
params = Params()
thermal_sock = messaging.sub_sock('thermal')
device_state_sock = messaging.sub_sock('deviceState')
pm = messaging.PubMaster(['managerState'])
while 1:
msg = messaging.recv_sock(thermal_sock, wait=True)
msg = messaging.recv_sock(device_state_sock, wait=True)
if msg.thermal.freeSpacePercent < 0.05:
if msg.deviceState.freeSpacePercent < 0.05:
logger_dead = True
if msg.thermal.started:
if msg.deviceState.started:
for p in car_started_processes:
if p == "loggerd" and logger_dead:
kill_managed_process(p)
@ -494,7 +494,7 @@ def manager_thread():
os.sync()
send_managed_process_signal("updated", signal.SIGHUP)
started_prev = msg.thermal.started
started_prev = msg.deviceState.started
# check the status of all processes, did any of them die?
running_list = ["%s%s\u001b[0m" % ("\u001b[32m" if running[p].is_alive() else "\u001b[31m", p) for p in running]

4
selfdrive/modeld/modeld.cc
View File

@ -107,7 +107,7 @@ int main(int argc, char **argv) {
// messaging
PubMaster pm({"modelV2", "cameraOdometry"});
SubMaster sm({"lateralPlan", "frame"});
SubMaster sm({"lateralPlan", "roadCameraState"});
// cl init
cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
@ -159,7 +159,7 @@ int main(int argc, char **argv) {
if (sm.update(0) > 0){
// TODO: path planner timeout?
desire = ((int)sm["lateralPlan"].getLateralPlan().getDesire());
frame_id = sm["frame"].getFrame().getFrameId();
frame_id = sm["roadCameraState"].getRoadCameraState().getFrameId();
}
double mt1 = 0, mt2 = 0;

4
selfdrive/sensord/sensors_qcom.cc
View File

@ -49,7 +49,7 @@ void sensor_loop() {
bool low_power_mode = false;
while (!do_exit) {
SubMaster sm({"thermal"});
SubMaster sm({"deviceState"});
PubMaster pm({"sensorEvents"});
struct sensors_poll_device_t* device;
@ -198,7 +198,7 @@ void sensor_loop() {
// Check whether to go into low power mode at 5Hz
if (frame % 20 == 0 && sm.update(0) > 0) {
bool offroad = !sm["thermal"].getThermal().getStarted();
bool offroad = !sm["deviceState"].getDeviceState().getStarted();
if (low_power_mode != offroad) {
for (auto &s : sensors) {
device->activate(device, s.first, 0);

22
selfdrive/test/longitudinal_maneuvers/plant.py
View File

@ -110,14 +110,14 @@ class Plant():
self.rate = rate
if not Plant.messaging_initialized:
Plant.pm = messaging.PubMaster(['frame', 'frontFrame', 'ubloxRaw'])
Plant.pm = messaging.PubMaster(['roadCameraState', 'driverCameraState', 'ubloxRaw'])
Plant.logcan = messaging.pub_sock('can')
Plant.sendcan = messaging.sub_sock('sendcan')
Plant.model = messaging.pub_sock('modelV2')
Plant.live_params = messaging.pub_sock('liveParameters')
Plant.live_location_kalman = messaging.pub_sock('liveLocationKalman')
Plant.health = messaging.pub_sock('health')
Plant.thermal = messaging.pub_sock('thermal')
Plant.pandaState = messaging.pub_sock('pandaState')
Plant.deviceState = messaging.pub_sock('deviceState')
Plant.driverMonitoringState = messaging.pub_sock('driverMonitoringState')
Plant.cal = messaging.pub_sock('liveCalibration')
Plant.controls_state = messaging.sub_sock('controlsState')
@ -373,15 +373,15 @@ class Plant():
dmon_state.driverMonitoringState.isDistracted = False
Plant.driverMonitoringState.send(dmon_state.to_bytes())
health = messaging.new_message('health')
health.health.safetyModel = car.CarParams.SafetyModel.hondaNidec
health.health.controlsAllowed = True
Plant.health.send(health.to_bytes())
pandaState = messaging.new_message('pandaState')
pandaState.pandaState.safetyModel = car.CarParams.SafetyModel.hondaNidec
pandaState.pandaState.controlsAllowed = True
Plant.pandaState.send(pandaState.to_bytes())
thermal = messaging.new_message('thermal')
thermal.thermal.freeSpacePercent = 1.
thermal.thermal.batteryPercent = 100
Plant.thermal.send(thermal.to_bytes())
deviceState = messaging.new_message('deviceState')
deviceState.deviceState.freeSpacePercent = 1.
deviceState.deviceState.batteryPercent = 100
Plant.deviceState.send(deviceState.to_bytes())
live_location_kalman = messaging.new_message('liveLocationKalman')
live_location_kalman.liveLocationKalman.inputsOK = True

6
selfdrive/test/process_replay/camera_replay.py
View File

@ -34,7 +34,7 @@ def camera_replay(lr, fr, desire=None, calib=None):
spinner = Spinner()
spinner.update("starting model replay")
pm = messaging.PubMaster(['frame', 'liveCalibration', 'lateralPlan'])
pm = messaging.PubMaster(['roadCameraState', 'liveCalibration', 'lateralPlan'])
sm = messaging.SubMaster(['modelV2'])
# TODO: add dmonitoringmodeld
@ -60,7 +60,7 @@ def camera_replay(lr, fr, desire=None, calib=None):
for msg in tqdm(lr):
if msg.which() == "liveCalibration":
pm.send(msg.which(), replace_calib(msg, calib))
elif msg.which() == "frame":
elif msg.which() == "roadCameraState":
if desire is not None:
for i in desire[frame_idx].nonzero()[0]:
dat = messaging.new_message('lateralPlan')
@ -69,7 +69,7 @@ def camera_replay(lr, fr, desire=None, calib=None):
f = msg.as_builder()
img = fr.get(frame_idx, pix_fmt="rgb24")[0][:,:,::-1]
f.frame.image = img.flatten().tobytes()
f.roadCameraState.image = img.flatten().tobytes()
frame_idx += 1
pm.send(msg.which(), f)

6
selfdrive/test/process_replay/inject_model.py
View File

@ -25,12 +25,12 @@ def regen_model(msgs, pm, frame_reader, model_sock):
for msg in tqdm(msgs):
w = msg.which()
if w == 'frame':
if w == 'roadCameraState':
msg = msg.as_builder()
img = frame_reader.get(fidx, pix_fmt="rgb24")[0][:,:,::-1]
msg.frame.image = img.flatten().tobytes()
msg.roadCameraState.image = img.flatten().tobytes()
pm.send(w, msg)
model = recv_one(model_sock)
@ -52,7 +52,7 @@ def inject_model(msgs, segment_name):
# TODO do better than just wait for modeld to boot
time.sleep(5)
pm = PubMaster(['liveCalibration', 'frame'])
pm = PubMaster(['liveCalibration', 'roadCameraState'])
model_sock = sub_sock('model')
try:
out_msgs = regen_model(msgs, pm, frame_reader, model_sock)

4
selfdrive/test/process_replay/process_replay.py
View File

@ -221,8 +221,8 @@ CONFIGS = [
proc_name="controlsd",
pub_sub={
"can": ["controlsState", "carState", "carControl", "sendcan", "carEvents", "carParams"],
"thermal": [], "health": [], "liveCalibration": [], "driverMonitoringState": [], "longitudinalPlan": [], "lateralPlan": [], "liveLocationKalman": [], "liveParameters": [], "radarState": [],
"modelV2": [], "frontFrame": [], "frame": [], "ubloxRaw": [], "managerState": [],
"deviceState": [], "pandaState": [], "liveCalibration": [], "driverMonitoringState": [], "longitudinalPlan": [], "lateralPlan": [], "liveLocationKalman": [], "liveParameters": [], "radarState": [],
"modelV2": [], "driverCameraState": [], "roadCameraState": [], "ubloxRaw": [], "managerState": [],
},
ignore=["logMonoTime", "valid", "controlsState.startMonoTime", "controlsState.cumLagMs"],
init_callback=fingerprint,

2
selfdrive/test/test_car_models.py
View File

@ -584,7 +584,7 @@ if __name__ == "__main__":
# Start unlogger
print("Start unlogger")
unlogger_cmd = [os.path.join(BASEDIR, 'tools/replay/unlogger.py'), route, '/tmp']
disable_socks = 'frame,encodeIdx,plan,lateralPlan,liveLongitudinalMpc,radarState,controlsState,liveTracks,liveMpc,sendcan,carState,carControl,carEvents,carParams'
disable_socks = 'frame,roadEncodeIdx,plan,lateralPlan,liveLongitudinalMpc,radarState,controlsState,liveTracks,liveMpc,sendcan,carState,carControl,carEvents,carParams'
unlogger = subprocess.Popen(unlogger_cmd + ['--disable', disable_socks, '--no-interactive'], preexec_fn=os.setsid) # pylint: disable=subprocess-popen-preexec-fn
print("Check sockets")

2
selfdrive/test/test_models.py
View File

@ -16,7 +16,7 @@ from tools.lib.logreader import LogReader
from panda.tests.safety import libpandasafety_py
from panda.tests.safety.common import package_can_msg
PandaType = log.HealthData.PandaType
PandaType = log.PandaState.PandaType
ROUTES = {v['carFingerprint']: k for k, v in routes.items() if v['enableCamera']}

8
selfdrive/test/test_sounds.py
View File

@ -37,7 +37,7 @@ def test_sound_card_init():
@with_processes(['ui', 'camerad'])
def test_alert_sounds():
pm = messaging.PubMaster(['thermal', 'controlsState'])
pm = messaging.PubMaster(['deviceState', 'controlsState'])
# make sure they're all defined
alert_sounds = {v: k for k, v in car.CarControl.HUDControl.AudibleAlert.schema.enumerants.items()}
@ -47,9 +47,9 @@ def test_alert_sounds():
# wait for procs to init
time.sleep(5)
msg = messaging.new_message('thermal')
msg.thermal.started = True
pm.send('thermal', msg)
msg = messaging.new_message('deviceState')
msg.deviceState.started = True
pm.send('deviceState', msg)
for sound, expected_writes in SOUNDS.items():
print(f"testing {alert_sounds[sound]}")

28
selfdrive/thermald/power_monitoring.py
View File

@ -26,7 +26,7 @@ class PowerMonitoring:
self.last_save_time = 0 # Used for saving current value in a param
self.power_used_uWh = 0 # Integrated power usage in uWh since going into offroad
self.next_pulsed_measurement_time = None
self.car_voltage_mV = 12e3 # Low-passed version of health voltage
self.car_voltage_mV = 12e3 # Low-passed version of pandaState voltage
self.integration_lock = threading.Lock()
car_battery_capacity_uWh = self.params.get("CarBatteryCapacity")
@ -38,12 +38,12 @@ class PowerMonitoring:
# Calculation tick
def calculate(self, health):
def calculate(self, pandaState):
try:
now = sec_since_boot()
# If health is None, we're probably not in a car, so we don't care
if health is None or health.health.pandaType == log.HealthData.PandaType.unknown:
# If pandaState is None, we're probably not in a car, so we don't care
if pandaState is None or pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown:
with self.integration_lock:
self.last_measurement_time = None
self.next_pulsed_measurement_time = None
@ -51,7 +51,7 @@ class PowerMonitoring:
return
# Low-pass battery voltage
self.car_voltage_mV = ((health.health.voltage * CAR_VOLTAGE_LOW_PASS_K) + (self.car_voltage_mV * (1 - CAR_VOLTAGE_LOW_PASS_K)))
self.car_voltage_mV = ((pandaState.pandaState.voltage * CAR_VOLTAGE_LOW_PASS_K) + (self.car_voltage_mV * (1 - CAR_VOLTAGE_LOW_PASS_K)))
# Cap the car battery power and save it in a param every 10-ish seconds
self.car_battery_capacity_uWh = max(self.car_battery_capacity_uWh, 0)
@ -66,7 +66,7 @@ class PowerMonitoring:
self.last_measurement_time = now
return
if (health.health.ignitionLine or health.health.ignitionCan):
if (pandaState.pandaState.ignitionLine or pandaState.pandaState.ignitionCan):
# If there is ignition, we integrate the charging rate of the car
with self.integration_lock:
self.power_used_uWh = 0
@ -77,7 +77,7 @@ class PowerMonitoring:
self.last_measurement_time = now
else:
# No ignition, we integrate the offroad power used by the device
is_uno = health.health.pandaType == log.HealthData.PandaType.uno
is_uno = pandaState.pandaState.pandaType == log.PandaState.PandaType.uno
# Get current power draw somehow
current_power = HARDWARE.get_current_power_draw()
if current_power is not None:
@ -154,8 +154,8 @@ class PowerMonitoring:
return int(self.car_battery_capacity_uWh)
# See if we need to disable charging
def should_disable_charging(self, health, offroad_timestamp):
if health is None or offroad_timestamp is None:
def should_disable_charging(self, pandaState, offroad_timestamp):
if pandaState is None or offroad_timestamp is None:
return False
now = sec_since_boot()
@ -163,22 +163,22 @@ class PowerMonitoring:
disable_charging |= (now - offroad_timestamp) > MAX_TIME_OFFROAD_S
disable_charging |= (self.car_voltage_mV < (VBATT_PAUSE_CHARGING * 1e3))
disable_charging |= (self.car_battery_capacity_uWh <= 0)
disable_charging &= (not health.health.ignitionLine and not health.health.ignitionCan)
disable_charging &= (not pandaState.pandaState.ignitionLine and not pandaState.pandaState.ignitionCan)
disable_charging &= (self.params.get("DisablePowerDown") != b"1")
return disable_charging
# See if we need to shutdown
def should_shutdown(self, health, offroad_timestamp, started_seen, LEON):
if health is None or offroad_timestamp is None:
def should_shutdown(self, pandaState, offroad_timestamp, started_seen, LEON):
if pandaState is None or offroad_timestamp is None:
return False
now = sec_since_boot()
panda_charging = (health.health.usbPowerMode != log.HealthData.UsbPowerMode.client)
panda_charging = (pandaState.pandaState.usbPowerMode != log.PandaState.UsbPowerMode.client)
BATT_PERC_OFF = 10 if LEON else 3
should_shutdown = False
# Wait until we have shut down charging before powering down
should_shutdown |= (not panda_charging and self.should_disable_charging(health, offroad_timestamp))
should_shutdown |= (not panda_charging and self.should_disable_charging(pandaState, offroad_timestamp))
should_shutdown |= ((HARDWARE.get_battery_capacity() < BATT_PERC_OFF) and (not HARDWARE.get_battery_charging()) and ((now - offroad_timestamp) > 60))
should_shutdown &= started_seen
return should_shutdown

70
selfdrive/thermald/tests/test_power_monitoring.py
View File

@ -21,10 +21,10 @@ with patch("common.realtime.sec_since_boot", new=mock_sec_since_boot):
CAR_CHARGING_RATE_W, VBATT_PAUSE_CHARGING
TEST_DURATION_S = 50
ALL_PANDA_TYPES = [(hw_type,) for hw_type in [log.HealthData.PandaType.whitePanda,
log.HealthData.PandaType.greyPanda,
log.HealthData.PandaType.blackPanda,
log.HealthData.PandaType.uno]]
ALL_PANDA_TYPES = [(hw_type,) for hw_type in [log.PandaState.PandaType.whitePanda,
log.PandaState.PandaType.greyPanda,
log.PandaState.PandaType.blackPanda,
log.PandaState.PandaType.uno]]
def pm_patch(name, value, constant=False):
if constant:
@ -37,16 +37,16 @@ class TestPowerMonitoring(unittest.TestCase):
params.delete("CarBatteryCapacity")
params.delete("DisablePowerDown")
def mock_health(self, ignition, hw_type, car_voltage=12):
health = messaging.new_message('health')
health.health.pandaType = hw_type
health.health.voltage = car_voltage * 1e3
health.health.ignitionLine = ignition
health.health.ignitionCan = False
return health
def mock_pandaState(self, ignition, hw_type, car_voltage=12):
pandaState = messaging.new_message('pandaState')
pandaState.pandaState.pandaType = hw_type
pandaState.pandaState.voltage = car_voltage * 1e3
pandaState.pandaState.ignitionLine = ignition
pandaState.pandaState.ignitionCan = False
return pandaState
# Test to see that it doesn't do anything when health is None
def test_health_present(self):
# Test to see that it doesn't do anything when pandaState is None
def test_pandaState_present(self):
pm = PowerMonitoring()
for _ in range(10):
pm.calculate(None)
@ -58,7 +58,7 @@ class TestPowerMonitoring(unittest.TestCase):
def test_offroad_ignition(self, hw_type):
pm = PowerMonitoring()
for _ in range(10):
pm.calculate(self.mock_health(True, hw_type))
pm.calculate(self.mock_pandaState(True, hw_type))
self.assertEqual(pm.get_power_used(), 0)
# Test to see that it integrates with discharging battery
@ -70,7 +70,7 @@ class TestPowerMonitoring(unittest.TestCase):
pm_patch("HARDWARE.get_battery_status", "Discharging"), pm_patch("HARDWARE.get_current_power_draw", None):
pm = PowerMonitoring()
for _ in range(TEST_DURATION_S + 1):
pm.calculate(self.mock_health(False, hw_type))
pm.calculate(self.mock_pandaState(False, hw_type))
expected_power_usage = ((TEST_DURATION_S/3600) * (BATT_VOLTAGE * BATT_CURRENT) * 1e6)
self.assertLess(abs(pm.get_power_used() - expected_power_usage), 10)
@ -84,7 +84,7 @@ class TestPowerMonitoring(unittest.TestCase):
pm = PowerMonitoring()
pm.car_battery_capacity_uWh = 0
for _ in range(TEST_DURATION_S + 1):
pm.calculate(self.mock_health(True, hw_type))
pm.calculate(self.mock_pandaState(True, hw_type))
expected_capacity = ((TEST_DURATION_S/3600) * CAR_CHARGING_RATE_W * 1e6)
self.assertLess(abs(pm.get_car_battery_capacity() - expected_capacity), 10)
@ -98,7 +98,7 @@ class TestPowerMonitoring(unittest.TestCase):
pm = PowerMonitoring()
pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh - 1000
for _ in range(TEST_DURATION_S + 1):
pm.calculate(self.mock_health(True, hw_type))
pm.calculate(self.mock_pandaState(True, hw_type))
estimated_capacity = CAR_BATTERY_CAPACITY_uWh + (CAR_CHARGING_RATE_W / 3600 * 1e6)
self.assertLess(abs(pm.get_car_battery_capacity() - estimated_capacity), 10)
@ -112,7 +112,7 @@ class TestPowerMonitoring(unittest.TestCase):
pm = PowerMonitoring()
pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh
for _ in range(TEST_DURATION_S + 1):
pm.calculate(self.mock_health(False, hw_type))
pm.calculate(self.mock_pandaState(False, hw_type))
expected_capacity = CAR_BATTERY_CAPACITY_uWh - ((TEST_DURATION_S/3600) * (BATT_VOLTAGE * BATT_CURRENT) * 1e6)
self.assertLess(abs(pm.get_car_battery_capacity() - expected_capacity), 10)
@ -126,7 +126,7 @@ class TestPowerMonitoring(unittest.TestCase):
pm = PowerMonitoring()
pm.car_battery_capacity_uWh = 1000
for _ in range(TEST_DURATION_S + 1):
pm.calculate(self.mock_health(False, hw_type))
pm.calculate(self.mock_pandaState(False, hw_type))
estimated_capacity = 0 - ((1/3600) * (BATT_VOLTAGE * BATT_CURRENT) * 1e6)
self.assertLess(abs(pm.get_car_battery_capacity() - estimated_capacity), 10)
@ -143,12 +143,12 @@ class TestPowerMonitoring(unittest.TestCase):
pm = PowerMonitoring()
pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh
start_time = ssb
health = self.mock_health(False, hw_type)
pandaState = self.mock_pandaState(False, hw_type)
while ssb <= start_time + MOCKED_MAX_OFFROAD_TIME:
pm.calculate(health)
pm.calculate(pandaState)
if (ssb - start_time) % 1000 == 0 and ssb < start_time + MOCKED_MAX_OFFROAD_TIME:
self.assertFalse(pm.should_disable_charging(health, start_time))
self.assertTrue(pm.should_disable_charging(health, start_time))
self.assertFalse(pm.should_disable_charging(pandaState, start_time))
self.assertTrue(pm.should_disable_charging(pandaState, start_time))
# Test to check policy of stopping charging when the car voltage is too low
@parameterized.expand(ALL_PANDA_TYPES)
@ -161,12 +161,12 @@ class TestPowerMonitoring(unittest.TestCase):
pm_patch("HARDWARE.get_battery_status", "Discharging"), pm_patch("HARDWARE.get_current_power_draw", None):
pm = PowerMonitoring()
pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh
health = self.mock_health(False, hw_type, car_voltage=(VBATT_PAUSE_CHARGING - 1))
pandaState = self.mock_pandaState(False, hw_type, car_voltage=(VBATT_PAUSE_CHARGING - 1))
for i in range(TEST_TIME):
pm.calculate(health)
pm.calculate(pandaState)
if i % 10 == 0:
self.assertEqual(pm.should_disable_charging(health, ssb), (pm.car_voltage_mV < VBATT_PAUSE_CHARGING*1e3))
self.assertTrue(pm.should_disable_charging(health, ssb))
self.assertEqual(pm.should_disable_charging(pandaState, ssb), (pm.car_voltage_mV < VBATT_PAUSE_CHARGING*1e3))
self.assertTrue(pm.should_disable_charging(pandaState, ssb))
# Test to check policy of not stopping charging when DisablePowerDown is set
def test_disable_power_down(self):
@ -179,12 +179,12 @@ class TestPowerMonitoring(unittest.TestCase):
pm_patch("HARDWARE.get_battery_status", "Discharging"), pm_patch("HARDWARE.get_current_power_draw", None):
pm = PowerMonitoring()
pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh
health = self.mock_health(False, log.HealthData.PandaType.uno, car_voltage=(VBATT_PAUSE_CHARGING - 1))
pandaState = self.mock_pandaState(False, log.PandaState.PandaType.uno, car_voltage=(VBATT_PAUSE_CHARGING - 1))
for i in range(TEST_TIME):
pm.calculate(health)
pm.calculate(pandaState)
if i % 10 == 0:
self.assertFalse(pm.should_disable_charging(health, ssb))
self.assertFalse(pm.should_disable_charging(health, ssb))
self.assertFalse(pm.should_disable_charging(pandaState, ssb))
self.assertFalse(pm.should_disable_charging(pandaState, ssb))
# Test to check policy of not stopping charging when ignition
def test_ignition(self):
@ -196,12 +196,12 @@ class TestPowerMonitoring(unittest.TestCase):
pm_patch("HARDWARE.get_battery_status", "Discharging"), pm_patch("HARDWARE.get_current_power_draw", None):
pm = PowerMonitoring()
pm.car_battery_capacity_uWh = CAR_BATTERY_CAPACITY_uWh
health = self.mock_health(True, log.HealthData.PandaType.uno, car_voltage=(VBATT_PAUSE_CHARGING - 1))
pandaState = self.mock_pandaState(True, log.PandaState.PandaType.uno, car_voltage=(VBATT_PAUSE_CHARGING - 1))
for i in range(TEST_TIME):
pm.calculate(health)
pm.calculate(pandaState)
if i % 10 == 0:
self.assertFalse(pm.should_disable_charging(health, ssb))
self.assertFalse(pm.should_disable_charging(health, ssb))
self.assertFalse(pm.should_disable_charging(pandaState, ssb))
self.assertFalse(pm.should_disable_charging(pandaState, ssb))
if __name__ == "__main__":
unittest.main()

116
selfdrive/thermald/thermald.py
View File

@ -26,9 +26,9 @@ ThermalConfig = namedtuple('ThermalConfig', ['cpu', 'gpu', 'mem', 'bat', 'ambien
FW_SIGNATURE = get_expected_signature()
ThermalStatus = log.ThermalData.ThermalStatus
NetworkType = log.ThermalData.NetworkType
NetworkStrength = log.ThermalData.NetworkStrength
ThermalStatus = log.DeviceState.ThermalStatus
NetworkType = log.DeviceState.NetworkType
NetworkStrength = log.DeviceState.NetworkStrength
CURRENT_TAU = 15. # 15s time constant
CPU_TEMP_TAU = 5. # 5s time constant
DAYS_NO_CONNECTIVITY_MAX = 7 # do not allow to engage after a week without internet
@ -63,12 +63,12 @@ def read_tz(x):
def read_thermal(thermal_config):
dat = messaging.new_message('thermal')
dat.thermal.cpu = [read_tz(z) / thermal_config.cpu[1] for z in thermal_config.cpu[0]]
dat.thermal.gpu = [read_tz(z) / thermal_config.gpu[1] for z in thermal_config.gpu[0]]
dat.thermal.mem = read_tz(thermal_config.mem[0]) / thermal_config.mem[1]
dat.thermal.ambient = read_tz(thermal_config.ambient[0]) / thermal_config.ambient[1]
dat.thermal.bat = read_tz(thermal_config.bat[0]) / thermal_config.bat[1]
dat = messaging.new_message('deviceState')
dat.deviceState.cpuTempC = [read_tz(z) / thermal_config.cpu[1] for z in thermal_config.cpu[0]]
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.batteryTempC = read_tz(thermal_config.bat[0]) / thermal_config.bat[1]
return dat
@ -163,10 +163,10 @@ def set_offroad_alert_if_changed(offroad_alert: str, show_alert: bool, extra_tex
def thermald_thread():
pm = messaging.PubMaster(['thermal'])
pm = messaging.PubMaster(['deviceState'])
health_timeout = int(1000 * 2.5 * DT_TRML) # 2.5x the expected health frequency
health_sock = messaging.sub_sock('health', timeout=health_timeout)
pandaState_timeout = int(1000 * 2.5 * DT_TRML) # 2.5x the expected pandaState frequency
pandaState_sock = messaging.sub_sock('pandaState', timeout=pandaState_timeout)
location_sock = messaging.sub_sock('gpsLocationExternal')
fan_speed = 0
@ -189,7 +189,7 @@ def thermald_thread():
current_filter = FirstOrderFilter(0., CURRENT_TAU, DT_TRML)
cpu_temp_filter = FirstOrderFilter(0., CPU_TEMP_TAU, DT_TRML)
health_prev = None
pandaState_prev = None
should_start_prev = False
handle_fan = None
is_uno = False
@ -201,14 +201,14 @@ def thermald_thread():
thermal_config = get_thermal_config()
while 1:
health = messaging.recv_sock(health_sock, wait=True)
pandaState = messaging.recv_sock(pandaState_sock, wait=True)
msg = read_thermal(thermal_config)
if health is not None:
usb_power = health.health.usbPowerMode != log.HealthData.UsbPowerMode.client
if pandaState is not None:
usb_power = pandaState.pandaState.usbPowerMode != log.PandaState.UsbPowerMode.client
# If we lose connection to the panda, wait 5 seconds before going offroad
if health.health.pandaType == log.HealthData.PandaType.unknown:
if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown:
no_panda_cnt += 1
if no_panda_cnt > DISCONNECT_TIMEOUT / DT_TRML:
if startup_conditions["ignition"]:
@ -216,11 +216,11 @@ def thermald_thread():
startup_conditions["ignition"] = False
else:
no_panda_cnt = 0
startup_conditions["ignition"] = health.health.ignitionLine or health.health.ignitionCan
startup_conditions["ignition"] = pandaState.pandaState.ignitionLine or pandaState.pandaState.ignitionCan
# Setup fan handler on first connect to panda
if handle_fan is None and health.health.pandaType != log.HealthData.PandaType.unknown:
is_uno = health.health.pandaType == log.HealthData.PandaType.uno
if handle_fan is None and pandaState.pandaState.pandaType != log.PandaState.PandaType.unknown:
is_uno = pandaState.pandaState.pandaType == log.PandaState.PandaType.uno
if (not EON) or is_uno:
cloudlog.info("Setting up UNO fan handler")
@ -231,11 +231,11 @@ def thermald_thread():
handle_fan = handle_fan_eon
# Handle disconnect
if health_prev is not None:
if health.health.pandaType == log.HealthData.PandaType.unknown and \
health_prev.health.pandaType != log.HealthData.PandaType.unknown:
if pandaState_prev is not None:
if pandaState.pandaState.pandaType == log.PandaState.PandaType.unknown and \
pandaState_prev.pandaState.pandaType != log.PandaState.PandaType.unknown:
params.panda_disconnect()
health_prev = health
pandaState_prev = pandaState
# get_network_type is an expensive call. update every 10s
if (count % int(10. / DT_TRML)) == 0:
@ -245,33 +245,33 @@ def thermald_thread():
except Exception:
cloudlog.exception("Error getting network status")
msg.thermal.freeSpacePercent = get_available_percent(default=100.0) / 100.0
msg.thermal.memoryUsagePercent = int(round(psutil.virtual_memory().percent))
msg.thermal.cpuUsagePercent = int(round(psutil.cpu_percent()))
msg.thermal.networkType = network_type
msg.thermal.networkStrength = network_strength
msg.thermal.batteryPercent = HARDWARE.get_battery_capacity()
msg.thermal.batteryStatus = HARDWARE.get_battery_status()
msg.thermal.batteryCurrent = HARDWARE.get_battery_current()
msg.thermal.batteryVoltage = HARDWARE.get_battery_voltage()
msg.thermal.usbOnline = HARDWARE.get_usb_present()
msg.deviceState.freeSpacePercent = get_available_percent(default=100.0) / 100.0
msg.deviceState.memoryUsagePercent = int(round(psutil.virtual_memory().percent))
msg.deviceState.cpuUsagePercent = int(round(psutil.cpu_percent()))
msg.deviceState.networkType = network_type
msg.deviceState.networkStrength = network_strength
msg.deviceState.batteryPercent = HARDWARE.get_battery_capacity()
msg.deviceState.batteryStatus = HARDWARE.get_battery_status()
msg.deviceState.batteryCurrent = HARDWARE.get_battery_current()
msg.deviceState.batteryVoltage = HARDWARE.get_battery_voltage()
msg.deviceState.usbOnline = HARDWARE.get_usb_present()
# Fake battery levels on uno for frame
if (not EON) or is_uno:
msg.thermal.batteryPercent = 100
msg.thermal.batteryStatus = "Charging"
msg.thermal.bat = 0
msg.deviceState.batteryPercent = 100
msg.deviceState.batteryStatus = "Charging"
msg.deviceState.batteryTempC = 0
current_filter.update(msg.thermal.batteryCurrent / 1e6)
current_filter.update(msg.deviceState.batteryCurrent / 1e6)
# TODO: add car battery voltage check
max_cpu_temp = cpu_temp_filter.update(max(msg.thermal.cpu))
max_comp_temp = max(max_cpu_temp, msg.thermal.mem, max(msg.thermal.gpu))
bat_temp = msg.thermal.bat
max_cpu_temp = cpu_temp_filter.update(max(msg.deviceState.cpuTempC))
max_comp_temp = max(max_cpu_temp, msg.deviceState.memoryTempC, max(msg.deviceState.gpuTempC))
bat_temp = msg.deviceState.batteryTempC
if handle_fan is not None:
fan_speed = handle_fan(max_cpu_temp, bat_temp, fan_speed, startup_conditions["ignition"])
msg.thermal.fanSpeedPercentDesired = fan_speed
msg.deviceState.fanSpeedPercentDesired = fan_speed
# If device is offroad we want to cool down before going onroad
# since going onroad increases load and can make temps go over 107
@ -347,7 +347,7 @@ def thermald_thread():
set_offroad_alert_if_changed("Offroad_PandaFirmwareMismatch", (not startup_conditions["fw_version_match"]))
# with 2% left, we killall, otherwise the phone will take a long time to boot
startup_conditions["free_space"] = msg.thermal.freeSpacePercent > 0.02
startup_conditions["free_space"] = msg.deviceState.freeSpacePercent > 0.02
startup_conditions["completed_training"] = params.get("CompletedTrainingVersion") == training_version or \
(current_branch in ['dashcam', 'dashcam-staging'])
startup_conditions["not_driver_view"] = not params.get("IsDriverViewEnabled") == b"1"
@ -357,9 +357,9 @@ def thermald_thread():
startup_conditions["device_temp_good"] = thermal_status < ThermalStatus.danger
set_offroad_alert_if_changed("Offroad_TemperatureTooHigh", (not startup_conditions["device_temp_good"]))
startup_conditions["hardware_supported"] = health is not None and health.health.pandaType not in [log.HealthData.PandaType.whitePanda,
log.HealthData.PandaType.greyPanda]
set_offroad_alert_if_changed("Offroad_HardwareUnsupported", health is not None and not startup_conditions["hardware_supported"])
startup_conditions["hardware_supported"] = pandaState is not None and pandaState.pandaState.pandaType not in [log.PandaState.PandaType.whitePanda,
log.PandaState.PandaType.greyPanda]
set_offroad_alert_if_changed("Offroad_HardwareUnsupported", pandaState is not None and not startup_conditions["hardware_supported"])
# Handle offroad/onroad transition
should_start = all(startup_conditions.values())
@ -383,26 +383,26 @@ def thermald_thread():
off_ts = sec_since_boot()
# Offroad power monitoring
power_monitor.calculate(health)
msg.thermal.offroadPowerUsage = power_monitor.get_power_used()
msg.thermal.carBatteryCapacity = max(0, power_monitor.get_car_battery_capacity())
power_monitor.calculate(pandaState)
msg.deviceState.offroadPowerUsageUwh = power_monitor.get_power_used()
msg.deviceState.carBatteryCapacityUwh = max(0, power_monitor.get_car_battery_capacity())
# Check if we need to disable charging (handled by boardd)
msg.thermal.chargingDisabled = power_monitor.should_disable_charging(health, off_ts)
msg.deviceState.chargingDisabled = power_monitor.should_disable_charging(pandaState, off_ts)
# Check if we need to shut down
if power_monitor.should_shutdown(health, off_ts, started_seen, LEON):
if power_monitor.should_shutdown(pandaState, off_ts, started_seen, LEON):
cloudlog.info(f"shutting device down, offroad since {off_ts}")
# TODO: add function for blocking cloudlog instead of sleep
time.sleep(10)
os.system('LD_LIBRARY_PATH="" svc power shutdown')
msg.thermal.chargingError = current_filter.x > 0. and msg.thermal.batteryPercent < 90 # if current is positive, then battery is being discharged
msg.thermal.started = started_ts is not None
msg.thermal.startedMonoTime = int(1e9*(started_ts or 0))
msg.deviceState.chargingError = current_filter.x > 0. and msg.deviceState.batteryPercent < 90 # if current is positive, then battery is being discharged
msg.deviceState.started = started_ts is not None
msg.deviceState.startedMonoTime = int(1e9*(started_ts or 0))
msg.thermal.thermalStatus = thermal_status
pm.send("thermal", msg)
msg.deviceState.thermalStatus = thermal_status
pm.send("deviceState", msg)
set_offroad_alert_if_changed("Offroad_ChargeDisabled", (not usb_power))
@ -414,9 +414,9 @@ def thermald_thread():
location = messaging.recv_sock(location_sock)
cloudlog.event("STATUS_PACKET",
count=count,
health=(health.to_dict() if health else None),
pandaState=(pandaState.to_dict() if pandaState else None),
location=(location.gpsLocationExternal.to_dict() if location else None),
thermal=msg.to_dict())
deviceState=msg.to_dict())
count += 1

50
selfdrive/ui/sidebar.cc
View File

@ -26,36 +26,36 @@ static void draw_home_button(UIState *s) {
}
static void draw_network_strength(UIState *s) {
static std::map<cereal::ThermalData::NetworkStrength, int> network_strength_map = {
{cereal::ThermalData::NetworkStrength::UNKNOWN, 1},
{cereal::ThermalData::NetworkStrength::POOR, 2},
{cereal::ThermalData::NetworkStrength::MODERATE, 3},
{cereal::ThermalData::NetworkStrength::GOOD, 4},
{cereal::ThermalData::NetworkStrength::GREAT, 5}};
const int img_idx = s->scene.thermal.getNetworkType() == cereal::ThermalData::NetworkType::NONE ? 0 : network_strength_map[s->scene.thermal.getNetworkStrength()];
static std::map<cereal::DeviceState::NetworkStrength, int> network_strength_map = {
{cereal::DeviceState::NetworkStrength::UNKNOWN, 1},
{cereal::DeviceState::NetworkStrength::POOR, 2},
{cereal::DeviceState::NetworkStrength::MODERATE, 3},
{cereal::DeviceState::NetworkStrength::GOOD, 4},
{cereal::DeviceState::NetworkStrength::GREAT, 5}};
const int img_idx = s->scene.deviceState.getNetworkType() == cereal::DeviceState::NetworkType::NONE ? 0 : network_strength_map[s->scene.deviceState.getNetworkStrength()];
ui_draw_image(s, {58, 196, 176, 27}, util::string_format("network_%d", img_idx).c_str(), 1.0f);
}
static void draw_battery_icon(UIState *s) {
const char *battery_img = s->scene.thermal.getBatteryStatus() == "Charging" ? "battery_charging" : "battery";
const char *battery_img = s->scene.deviceState.getBatteryStatus() == "Charging" ? "battery_charging" : "battery";
const Rect rect = {160, 255, 76, 36};
ui_fill_rect(s->vg, {rect.x + 6, rect.y + 5,
int((rect.w - 19) * s->scene.thermal.getBatteryPercent() * 0.01), rect.h - 11}, COLOR_WHITE);
int((rect.w - 19) * s->scene.deviceState.getBatteryPercent() * 0.01), rect.h - 11}, COLOR_WHITE);
ui_draw_image(s, rect, battery_img, 1.0f);
}
static void draw_network_type(UIState *s) {
static std::map<cereal::ThermalData::NetworkType, const char *> network_type_map = {
{cereal::ThermalData::NetworkType::NONE, "--"},
{cereal::ThermalData::NetworkType::WIFI, "WiFi"},
{cereal::ThermalData::NetworkType::CELL2_G, "2G"},
{cereal::ThermalData::NetworkType::CELL3_G, "3G"},
{cereal::ThermalData::NetworkType::CELL4_G, "4G"},
{cereal::ThermalData::NetworkType::CELL5_G, "5G"}};
static std::map<cereal::DeviceState::NetworkType, const char *> network_type_map = {
{cereal::DeviceState::NetworkType::NONE, "--"},
{cereal::DeviceState::NetworkType::WIFI, "WiFi"},
{cereal::DeviceState::NetworkType::CELL2_G, "2G"},
{cereal::DeviceState::NetworkType::CELL3_G, "3G"},
{cereal::DeviceState::NetworkType::CELL4_G, "4G"},
{cereal::DeviceState::NetworkType::CELL5_G, "5G"}};
const int network_x = 50;
const int network_y = 273;
const int network_w = 100;
const char *network_type = network_type_map[s->scene.thermal.getNetworkType()];
const char *network_type = network_type_map[s->scene.deviceState.getNetworkType()];
nvgFillColor(s->vg, COLOR_WHITE);
nvgFontSize(s->vg, 48);
nvgFontFace(s->vg, "sans-regular");
@ -104,13 +104,13 @@ static void draw_metric(UIState *s, const char *label_str, const char *value_str
}
static void draw_temp_metric(UIState *s) {
static std::map<cereal::ThermalData::ThermalStatus, const int> temp_severity_map = {
{cereal::ThermalData::ThermalStatus::GREEN, 0},
{cereal::ThermalData::ThermalStatus::YELLOW, 1},
{cereal::ThermalData::ThermalStatus::RED, 2},
{cereal::ThermalData::ThermalStatus::DANGER, 3}};
std::string temp_val = std::to_string((int)s->scene.thermal.getAmbient()) + "°C";
draw_metric(s, "TEMP", temp_val.c_str(), temp_severity_map[s->scene.thermal.getThermalStatus()], 0, NULL);
static std::map<cereal::DeviceState::ThermalStatus, const int> temp_severity_map = {
{cereal::DeviceState::ThermalStatus::GREEN, 0},
{cereal::DeviceState::ThermalStatus::YELLOW, 1},
{cereal::DeviceState::ThermalStatus::RED, 2},
{cereal::DeviceState::ThermalStatus::DANGER, 3}};
std::string temp_val = std::to_string((int)s->scene.deviceState.getAmbientTempC()) + "°C";
draw_metric(s, "TEMP", temp_val.c_str(), temp_severity_map[s->scene.deviceState.getThermalStatus()], 0, NULL);
}
static void draw_panda_metric(UIState *s) {
@ -118,7 +118,7 @@ static void draw_panda_metric(UIState *s) {
int panda_severity = 0;
std::string panda_message = "VEHICLE\nONLINE";
if (s->scene.pandaType == cereal::HealthData::PandaType::UNKNOWN) {
if (s->scene.pandaType == cereal::PandaState::PandaType::UNKNOWN) {
panda_severity = 2;
panda_message = "NO\nPANDA";
}

24
selfdrive/ui/ui.cc
View File

@ -41,8 +41,8 @@ static void ui_init_vision(UIState *s) {
void ui_init(UIState *s) {
s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "thermal", "frame", "liveLocationKalman",
"health", "carParams", "driverState", "driverMonitoringState", "sensorEvents", "carState", "ubloxGnss"});
s->sm = new SubMaster({"modelV2", "controlsState", "uiLayoutState", "liveCalibration", "radarState", "deviceState", "roadCameraState", "liveLocationKalman",
"pandaState", "carParams", "driverState", "driverMonitoringState", "sensorEvents", "carState", "ubloxGnss"});
s->started = false;
s->status = STATUS_OFFROAD;
@ -161,15 +161,15 @@ static void update_sockets(UIState *s) {
s->active_app = data.getActiveApp();
s->sidebar_collapsed = data.getSidebarCollapsed();
}
if (sm.updated("thermal")) {
scene.thermal = sm["thermal"].getThermal();
if (sm.updated("deviceState")) {
scene.deviceState = sm["deviceState"].getDeviceState();
}
if (sm.updated("health")) {
auto health = sm["health"].getHealth();
scene.pandaType = health.getPandaType();
s->ignition = health.getIgnitionLine() || health.getIgnitionCan();
} else if ((s->sm->frame - s->sm->rcv_frame("health")) > 5*UI_FREQ) {
scene.pandaType = cereal::HealthData::PandaType::UNKNOWN;
if (sm.updated("pandaState")) {
auto pandaState = sm["pandaState"].getPandaState();
scene.pandaType = pandaState.getPandaType();
s->ignition = pandaState.getIgnitionLine() || pandaState.getIgnitionCan();
} else if ((s->sm->frame - s->sm->rcv_frame("pandaState")) > 5*UI_FREQ) {
scene.pandaType = cereal::PandaState::PandaType::UNKNOWN;
}
if (sm.updated("ubloxGnss")) {
auto data = sm["ubloxGnss"].getUbloxGnss();
@ -205,7 +205,7 @@ static void update_sockets(UIState *s) {
}
}
}
s->started = scene.thermal.getStarted() || scene.frontview;
s->started = scene.deviceState.getStarted() || scene.frontview;
}
static void update_alert(UIState *s) {
@ -227,7 +227,7 @@ static void update_alert(UIState *s) {
}
// Handle controls timeout
if (scene.thermal.getStarted() && (s->sm->frame - s->started_frame) > 10 * UI_FREQ) {
if (scene.deviceState.getStarted() && (s->sm->frame - s->started_frame) > 10 * UI_FREQ) {
const uint64_t cs_frame = s->sm->rcv_frame("controlsState");
if (cs_frame < s->started_frame) {
// car is started, but controlsState hasn't been seen at all

4
selfdrive/ui/ui.hpp
View File

@ -108,10 +108,10 @@ typedef struct UIScene {
float alert_blinking_rate;
cereal::ControlsState::AlertSize alert_size;
cereal::HealthData::PandaType pandaType;
cereal::PandaState::PandaType pandaType;
NetStatus athenaStatus;
cereal::ThermalData::Reader thermal;
cereal::DeviceState::Reader deviceState;
cereal::RadarState::LeadData::Reader lead_data[2];
cereal::CarState::Reader car_state;
cereal::ControlsState::Reader controls_state;

4
tools/carcontrols/debug_controls.py
View File

@ -46,7 +46,7 @@ def steer_thread():
if joystick is not None:
axis_3 = clip(-joystick.testJoystick.axes[3] * 1.05, -1., 1.) # -1 to 1
actuators.steer = axis_3
actuators.steerAngle = axis_3 * 43. # deg
actuators.steeringAngleDeg = axis_3 * 43. # deg
axis_1 = clip(-joystick.testJoystick.axes[1] * 1.05, -1., 1.) # -1 to 1
actuators.gas = max(axis_1, 0.)
actuators.brake = max(-axis_1, 0.)
@ -67,7 +67,7 @@ def steer_thread():
CC.actuators.gas = actuators.gas
CC.actuators.brake = actuators.brake
CC.actuators.steer = actuators.steer
CC.actuators.steerAngle = actuators.steerAngle
CC.actuators.steeringAngleDeg = actuators.steeringAngleDeg
CC.hudControl.visualAlert = hud_alert
CC.hudControl.setSpeed = 20
CC.cruiseControl.cancel = pcm_cancel_cmd

4
tools/nui/FileReader.cpp
View File

@ -98,8 +98,8 @@ void LogReader::mergeEvents(int dled) {
// hack
// TODO: rewrite with callback
if (event.which() == cereal::Event::ENCODE_IDX) {
auto ee = event.getEncodeIdx();
if (event.which() == cereal::Event::ROAD_ENCODE_IDX) {
auto ee = event.getRoadEncodeIdx();
eidx_local.insert(ee.getFrameId(), qMakePair(ee.getSegmentNum(), ee.getSegmentId()));
}

6
tools/nui/Unlogger.cpp
View File

@ -153,14 +153,14 @@ void Unlogger::process() {
auto ee = msg.getRoot<cereal::Event>();
ee.setLogMonoTime(nanos_since_boot());
if (e.which() == cereal::Event::FRAME) {
auto fr = msg.getRoot<cereal::Event>().getFrame();
if (e.which() == cereal::Event::ROAD_CAMERA_STATE) {
auto fr = msg.getRoot<cereal::Event>().getRoadCameraState();
// TODO: better way?
auto it = eidx.find(fr.getFrameId());
if (it != eidx.end()) {
auto pp = *it;
//qDebug() << fr.getFrameId() << pp;
//qDebug() << fr.getRoadCameraStateId() << pp;
if (frs->find(pp.first) != frs->end()) {
auto frm = (*frs)[pp.first];

8
tools/replay/camera.py
View File

@ -35,7 +35,7 @@ def ui_thread(addr, frame_address):
camera_surface = pygame.surface.Surface((_FULL_FRAME_SIZE[0] * SCALE, _FULL_FRAME_SIZE[1] * SCALE), 0, 24).convert()
frame = messaging.sub_sock('frame', conflate=True)
frame = messaging.sub_sock('roadCameraState', conflate=True)
img = np.zeros((_FULL_FRAME_SIZE[1], _FULL_FRAME_SIZE[0], 3), dtype='uint8')
imgff = np.zeros((_FULL_FRAME_SIZE[1], _FULL_FRAME_SIZE[0], 3), dtype=np.uint8)
@ -46,10 +46,10 @@ def ui_thread(addr, frame_address):
# ***** frame *****
fpkt = messaging.recv_one(frame)
yuv_img = fpkt.frame.image
yuv_img = fpkt.roadCameraState.image
if fpkt.frame.transform:
yuv_transform = np.array(fpkt.frame.transform).reshape(3, 3)
if fpkt.roadCameraState.transform:
yuv_transform = np.array(fpkt.roadCameraState.transform).reshape(3, 3)
else:
# assume frame is flipped
yuv_transform = np.array([[-1.0, 0.0, _FULL_FRAME_SIZE[0] - 1],

16
tools/replay/ui.py
View File

@ -53,9 +53,9 @@ def ui_thread(addr, frame_address):
camera_surface = pygame.surface.Surface((640, 480), 0, 24).convert()
top_down_surface = pygame.surface.Surface((UP.lidar_x, UP.lidar_y), 0, 8)
frame = messaging.sub_sock('frame', addr=addr, conflate=True)
frame = messaging.sub_sock('roadCameraState', addr=addr, conflate=True)
sm = messaging.SubMaster(['carState', 'longitudinalPlan', 'carControl', 'radarState', 'liveCalibration', 'controlsState',
'liveTracks', 'modelV2', 'liveMpc', 'liveParameters', 'lateralPlan', 'frame'], addr=addr)
'liveTracks', 'modelV2', 'liveMpc', 'liveParameters', 'lateralPlan', 'roadCameraState'], addr=addr)
img = np.zeros((480, 640, 3), dtype='uint8')
imgff = None
@ -109,7 +109,7 @@ def ui_thread(addr, frame_address):
# ***** frame *****
fpkt = messaging.recv_one(frame)
rgb_img_raw = fpkt.frame.image
rgb_img_raw = fpkt.roadCameraState.image
num_px = len(rgb_img_raw) // 3
if rgb_img_raw and num_px in _FULL_FRAME_SIZE.keys():
@ -134,15 +134,15 @@ def ui_thread(addr, frame_address):
w = sm['controlsState'].lateralControlState.which()
if w == 'lqrState':
angle_steers_k = sm['controlsState'].lateralControlState.lqrState.steerAngle
angle_steers_k = sm['controlsState'].lateralControlState.lqrState.steeringAngleDeg
elif w == 'indiState':
angle_steers_k = sm['controlsState'].lateralControlState.indiState.steerAngle
angle_steers_k = sm['controlsState'].lateralControlState.indiState.steeringAngleDeg
else:
angle_steers_k = np.inf
plot_arr[:-1] = plot_arr[1:]
plot_arr[-1, name_to_arr_idx['angle_steers']] = sm['carState'].steeringAngle
plot_arr[-1, name_to_arr_idx['angle_steers_des']] = sm['carControl'].actuators.steerAngle
plot_arr[-1, name_to_arr_idx['angle_steers']] = sm['carState'].steeringAngleDeg
plot_arr[-1, name_to_arr_idx['angle_steers_des']] = sm['carControl'].actuators.steeringAngleDeg
plot_arr[-1, name_to_arr_idx['angle_steers_k']] = angle_steers_k
plot_arr[-1, name_to_arr_idx['gas']] = sm['carState'].gas
plot_arr[-1, name_to_arr_idx['computer_gas']] = sm['carControl'].actuators.gas
@ -195,7 +195,7 @@ def ui_thread(addr, frame_address):
info_font.render("LONG CONTROL STATE: " + str(sm['controlsState'].longControlState), True, YELLOW),
info_font.render("LONG MPC SOURCE: " + str(sm['longitudinalPlan'].longitudinalPlanSource), True, YELLOW),
None,
info_font.render("ANGLE OFFSET (AVG): " + str(round(sm['liveParameters'].angleOffsetAverage, 2)) + " deg", True, YELLOW),
info_font.render("ANGLE OFFSET (AVG): " + str(round(sm['liveParameters'].angleOffsetAverageDeg, 2)) + " deg", True, YELLOW),
info_font.render("ANGLE OFFSET (INSTANT): " + str(round(sm['liveParameters'].angleOffset, 2)) + " deg", True, YELLOW),
info_font.render("STIFFNESS: " + str(round(sm['liveParameters'].stiffnessFactor * 100., 2)) + " %", True, YELLOW),
info_font.render("STEER RATIO: " + str(round(sm['liveParameters'].steerRatio, 2)), True, YELLOW)

8
tools/replay/unlog_segment.py
View File

@ -27,11 +27,11 @@ def replay(route, loop):
lr = LogReader(f"cd:/{route}/rlog.bz2")
fr = FrameReader(f"cd:/{route}/fcamera.hevc", readahead=True)
# Build mapping from frameId to segmentId from encodeIdx, type == fullHEVC
# Build mapping from frameId to segmentId from roadEncodeIdx, type == fullHEVC
msgs = [m for m in lr if m.which() not in IGNORE]
msgs = sorted(msgs, key=lambda m: m.logMonoTime)
times = [m.logMonoTime for m in msgs]
frame_idx = {m.encodeIdx.frameId: m.encodeIdx.segmentId for m in msgs if m.which() == 'encodeIdx' and m.encodeIdx.type == 'fullHEVC'}
frame_idx = {m.roadEncodeIdx.frameId: m.roadEncodeIdx.segmentId for m in msgs if m.which() == 'roadEncodeIdx' and m.roadEncodeIdx.type == 'fullHEVC'}
socks = {}
lag = 0
@ -45,11 +45,11 @@ def replay(route, loop):
start_time = time.time()
w = msg.which()
if w == 'frame':
if w == 'roadCameraState':
try:
img = fr.get(frame_idx[msg.frame.frameId], pix_fmt="rgb24")
img = img[0][:, :, ::-1] # Convert RGB to BGR, which is what the camera outputs
msg.frame.image = img.flatten().tobytes()
msg.roadCameraState.image = img.flatten().tobytes()
except (KeyError, ValueError):
pass

22
tools/replay/unlogger.py
View File

@ -49,9 +49,9 @@ class UnloggerWorker(object):
poller = zmq.Poller()
poller.register(commands_socket, zmq.POLLIN)
# We can't publish frames without encodeIdx, so add when it's missing.
if "frame" in pub_types:
pub_types["encodeIdx"] = None
# We can't publish frames without roadEncodeIdx, so add when it's missing.
if "roadCameraState" in pub_types:
pub_types["roadEncodeIdx"] = None
# gc.set_debug(gc.DEBUG_LEAK | gc.DEBUG_OBJECTS | gc.DEBUG_STATS | gc.DEBUG_SAVEALL |
# gc.DEBUG_UNCOLLECTABLE)
@ -86,11 +86,11 @@ class UnloggerWorker(object):
continue
# **** special case certain message types ****
if typ == "encodeIdx" and msg.encodeIdx.type == fullHEVC:
# this assumes the encodeIdx always comes before the frame
if typ == "roadEncodeIdx" and msg.roadEncodeIdx.type == fullHEVC:
# this assumes the roadEncodeIdx always comes before the frame
self._frame_id_lookup[
msg.encodeIdx.frameId] = msg.encodeIdx.segmentNum, msg.encodeIdx.segmentId
#print "encode", msg.encodeIdx.frameId, len(self._readahead), route_time
msg.roadEncodeIdx.frameId] = msg.roadEncodeIdx.segmentNum, msg.roadEncodeIdx.segmentId
#print "encode", msg.roadEncodeIdx.frameId, len(self._readahead), route_time
self._readahead.appendleft((typ, msg, route_time, cookie))
def _send_logs(self, data_socket):
@ -98,7 +98,7 @@ class UnloggerWorker(object):
typ, msg, route_time, cookie = self._readahead.pop()
smsg = msg.as_builder()
if typ == "frame":
if typ == "roadCameraState":
frame_id = msg.frame.frameId
# Frame exists, make sure we have a framereader.
@ -135,7 +135,7 @@ class UnloggerWorker(object):
self._lr = MultiLogIterator(route.log_paths(), wraparound=True)
if self._frame_reader is not None:
self._frame_reader.close()
if "frame" in pub_types or "encodeIdx" in pub_types:
if "roadCameraState" in pub_types or "roadEncodeIdx" in pub_types:
# reset frames for a route
self._frame_id_lookup = {}
self._frame_reader = RouteFrameReader(
@ -313,8 +313,8 @@ def absolute_time_str(s, start_time):
def _get_address_mapping(args):
if args.min is not None:
services_to_mock = [
'thermal', 'can', 'health', 'sensorEvents', 'gpsNMEA', 'frame', 'encodeIdx',
'model', 'features', 'liveLocation',
'deviceState', 'can', 'pandaState', 'sensorEvents', 'gpsNMEA', 'roadCameraState', 'roadEncodeIdx',
'modelV2', 'liveLocation',
]
elif args.enabled is not None:
services_to_mock = args.enabled

20
tools/sim/bridge.py
View File

@ -32,7 +32,7 @@ REPEAT_COUNTER = 5
PRINT_DECIMATION = 100
STEER_RATIO = 15.
pm = messaging.PubMaster(['frame', 'sensorEvents', 'can'])
pm = messaging.PubMaster(['roadCameraState', 'sensorEvents', 'can'])
sm = messaging.SubMaster(['carControl','controlsState'])
class VehicleState:
@ -59,13 +59,13 @@ def cam_callback(image):
img = np.reshape(img, (H, W, 4))
img = img[:, :, [0, 1, 2]].copy()
dat = messaging.new_message('frame')
dat.frame = {
dat = messaging.new_message('roadCameraState')
dat.roadCameraState = {
"frameId": image.frame,
"image": img.tostring(),
"transform": [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
}
pm.send('frame', dat)
pm.send('roadCameraState', dat)
frame_id += 1
def imu_callback(imu):
@ -81,18 +81,18 @@ def imu_callback(imu):
dat.sensorEvents[1].gyroUncalibrated.v = [imu.gyroscope.x, imu.gyroscope.y, imu.gyroscope.z]
pm.send('sensorEvents', dat)
def health_function():
pm = messaging.PubMaster(['health'])
def panda_state_function():
pm = messaging.PubMaster(['pandaState'])
while 1:
dat = messaging.new_message('health')
dat = messaging.new_message('pandaState')
dat.valid = True
dat.health = {
dat.pandaState = {
'ignitionLine': True,
'pandaType': "blackPanda",
'controlsAllowed': True,
'safetyModel': 'hondaNidec'
}
pm.send('health', dat)
pm.send('pandaState', dat)
time.sleep(0.5)
def fake_gps():
@ -197,7 +197,7 @@ def go(q):
vehicle_state = VehicleState()
# launch fake car threads
threading.Thread(target=health_function).start()
threading.Thread(target=panda_state_function).start()
threading.Thread(target=fake_driver_monitoring).start()
threading.Thread(target=fake_gps).start()
threading.Thread(target=can_function_runner, args=(vehicle_state,)).start()

10
tools/streamer/streamerd.py
View File

@ -35,7 +35,7 @@ def receiver_thread():
context = zmq.Context()
s = messaging.sub_sock(context, 9002, addr=addr)
frame_sock = messaging.pub_sock(context, service_list['frame'].port)
frame_sock = messaging.pub_sock(context, service_list['roadCameraState'].port)
ctx = av.codec.codec.Codec('hevc', 'r').create()
ctx.decode(av.packet.Packet(start.decode("hex")))
@ -64,10 +64,10 @@ def receiver_thread():
#print 'ms to make yuv:', (t1-t2)*1000
#print 'tsEof:', ts
dat = messaging.new_message('frame')
dat.frame.image = yuv_img
dat.frame.timestampEof = ts
dat.frame.transform = map(float, list(np.eye(3).flatten()))
dat = messaging.new_message('roadCameraState')
dat.roadCameraState.image = yuv_img
dat.roadCameraState.timestampEof = ts
dat.roadCameraState.transform = map(float, list(np.eye(3).flatten()))
frame_sock.send(dat.to_bytes())
if PYGAME: