From 05eb44115e2733481ab941a09121929f8ec8fd57 Mon Sep 17 00:00:00 2001 From: Adeeb Shihadeh Date: Tue, 16 Feb 2021 21:39:32 -0800 Subject: [PATCH] cereal cleanup part 2 (#20092) * car stuff * thermal * Revert "car stuff" This reverts commit 77fd1c65ebd01abfa8493ae12c9e6b14f7ada976. * 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 old-commit-hash: 312b681a46b8153314a8420611b6479dd6f70dfc --- cereal | 2 +- selfdrive/athena/athenad.py | 6 +- selfdrive/athena/tests/test_athenad.py | 12 +- selfdrive/athena/tests/test_athenad_old.py | 12 +- selfdrive/boardd/boardd.cc | 108 ++++++++-------- selfdrive/boardd/panda.cc | 20 +-- selfdrive/boardd/panda.h | 8 +- selfdrive/boardd/tests/boardd_old.py | 14 +-- selfdrive/boardd/tests/fuzzer.py | 20 --- .../boardd/tests/test_boardd_loopback.py | 4 +- selfdrive/camerad/cameras/camera_common.cc | 4 +- .../camerad/cameras/camera_frame_stream.cc | 2 +- selfdrive/camerad/cameras/camera_qcom.cc | 6 +- selfdrive/camerad/cameras/camera_qcom2.cc | 6 +- selfdrive/camerad/cameras/camera_webcam.cc | 10 +- selfdrive/camerad/snapshot/snapshot.py | 4 +- selfdrive/camerad/test/check_skips.py | 4 +- selfdrive/camerad/test/frame_test.py | 6 +- selfdrive/camerad/test/test_camerad.py | 8 +- selfdrive/car/chrysler/carstate.py | 4 +- selfdrive/car/ford/carcontroller.py | 4 +- selfdrive/car/ford/carstate.py | 2 +- selfdrive/car/gm/carstate.py | 2 +- selfdrive/car/honda/carstate.py | 4 +- selfdrive/car/honda/interface.py | 2 +- selfdrive/car/hyundai/carcontroller.py | 2 +- selfdrive/car/hyundai/carstate.py | 4 +- selfdrive/car/hyundai/interface.py | 4 +- selfdrive/car/mazda/carstate.py | 4 +- selfdrive/car/mock/interface.py | 2 +- selfdrive/car/nissan/carcontroller.py | 4 +- selfdrive/car/nissan/carstate.py | 2 +- selfdrive/car/subaru/carstate.py | 2 +- selfdrive/car/toyota/carcontroller.py | 2 +- selfdrive/car/toyota/carstate.py | 10 +- selfdrive/car/volkswagen/carstate.py | 4 +- selfdrive/controls/controlsd.py | 60 ++++----- selfdrive/controls/lib/events.py | 2 +- selfdrive/controls/lib/latcontrol_indi.py | 14 +-- selfdrive/controls/lib/latcontrol_lqr.py | 10 +- selfdrive/controls/lib/latcontrol_pid.py | 14 +-- selfdrive/controls/lib/lateral_planner.py | 10 +- .../controls/lib/longitudinal_planner.py | 2 +- selfdrive/controls/tests/test_startup.py | 8 +- selfdrive/debug/cycle_alerts.py | 12 +- selfdrive/debug/internal/check_alive_valid.py | 2 +- .../internal/measure_steering_accuracy.py | 2 +- .../internal/sounds/test_sound_stability.py | 4 +- selfdrive/debug/live_cpu_and_temp.py | 10 +- selfdrive/debug/test_fw_query_on_routes.py | 4 +- selfdrive/debug/uiview.py | 10 +- selfdrive/hardware/eon/hardware.py | 4 +- selfdrive/hardware/pc/hardware.py | 4 +- selfdrive/hardware/tici/hardware.py | 4 +- selfdrive/locationd/locationd.py | 2 +- selfdrive/locationd/paramsd.py | 31 +++-- selfdrive/locationd/test/ubloxd.py | 4 +- .../locationd/test/ubloxd_regression_test.py | 4 +- selfdrive/locationd/ublox_msg.cc | 4 +- selfdrive/loggerd/loggerd.cc | 16 +-- selfdrive/loggerd/uploader.py | 6 +- selfdrive/manager.py | 10 +- selfdrive/modeld/modeld.cc | 4 +- selfdrive/sensord/sensors_qcom.cc | 4 +- .../test/longitudinal_maneuvers/plant.py | 22 ++-- .../test/process_replay/camera_replay.py | 6 +- selfdrive/test/process_replay/inject_model.py | 6 +- .../test/process_replay/process_replay.py | 4 +- selfdrive/test/test_car_models.py | 2 +- selfdrive/test/test_models.py | 2 +- selfdrive/test/test_sounds.py | 8 +- selfdrive/thermald/power_monitoring.py | 28 ++--- .../thermald/tests/test_power_monitoring.py | 70 +++++------ selfdrive/thermald/thermald.py | 116 +++++++++--------- selfdrive/ui/sidebar.cc | 50 ++++---- selfdrive/ui/ui.cc | 24 ++-- selfdrive/ui/ui.hpp | 4 +- tools/carcontrols/debug_controls.py | 4 +- tools/nui/FileReader.cpp | 4 +- tools/nui/Unlogger.cpp | 6 +- tools/replay/camera.py | 8 +- tools/replay/ui.py | 16 +-- tools/replay/unlog_segment.py | 8 +- tools/replay/unlogger.py | 22 ++-- tools/sim/bridge.py | 20 +-- tools/streamer/streamerd.py | 10 +- 86 files changed, 500 insertions(+), 515 deletions(-) delete mode 100755 selfdrive/boardd/tests/fuzzer.py diff --git a/cereal b/cereal index 39d80be841..a034837924 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 39d80be84108820a42fc4b1b1817e8e3b73c89fb +Subproject commit a0348379249f77b85765846f0ebc5dcdb358821d diff --git a/selfdrive/athena/athenad.py b/selfdrive/athena/athenad.py index 8f08c390ba..c3645ed8d6 100755 --- a/selfdrive/athena/athenad.py +++ b/selfdrive/athena/athenad.py @@ -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(): diff --git a/selfdrive/athena/tests/test_athenad.py b/selfdrive/athena/tests/test_athenad.py index 503aacb032..42eb5ca318 100755 --- a/selfdrive/athena/tests/test_athenad.py +++ b/selfdrive/athena/tests/test_athenad.py @@ -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() diff --git a/selfdrive/athena/tests/test_athenad_old.py b/selfdrive/athena/tests/test_athenad_old.py index 8fdcad3a1a..739dc7efa7 100644 --- a/selfdrive/athena/tests/test_athenad_old.py +++ b/selfdrive/athena/tests/test_athenad_old.py @@ -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() diff --git a/selfdrive/boardd/boardd.cc b/selfdrive/boardd/boardd.cc index e317339bc4..ba0ba0257c 100644 --- a/selfdrive/boardd/boardd.cc +++ b/selfdrive/boardd/boardd.cc @@ -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 fault_bits(health.faults); - auto faults = healthData.initFaults(fault_bits.count()); + std::bitset 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 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()) { diff --git a/selfdrive/boardd/panda.cc b/selfdrive/boardd/panda.cc index 2fceb3c64c..84697965f1 100644 --- a/selfdrive/boardd/panda.cc +++ b/selfdrive/boardd/panda.cc @@ -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); } diff --git a/selfdrive/boardd/panda.h b/selfdrive/boardd/panda.h index a96af8a6af..26bd263605 100644 --- a/selfdrive/boardd/panda.h +++ b/selfdrive/boardd/panda.h @@ -53,7 +53,7 @@ class Panda { ~Panda(); std::atomic 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> get_firmware_version(); std::optional 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::Reader can_data_list); int can_receive(kj::Array& out_buf); diff --git a/selfdrive/boardd/tests/boardd_old.py b/selfdrive/boardd/tests/boardd_old.py index 4fd2350bff..017fde6dd5 100755 --- a/selfdrive/boardd/tests/boardd_old.py +++ b/selfdrive/boardd/tests/boardd_old.py @@ -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()) diff --git a/selfdrive/boardd/tests/fuzzer.py b/selfdrive/boardd/tests/fuzzer.py deleted file mode 100755 index bb58f8b5e0..0000000000 --- a/selfdrive/boardd/tests/fuzzer.py +++ /dev/null @@ -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) diff --git a/selfdrive/boardd/tests/test_boardd_loopback.py b/selfdrive/boardd/tests/test_boardd_loopback.py index 422fed8db3..3144274bdf 100755 --- a/selfdrive/boardd/tests/test_boardd_loopback.py +++ b/selfdrive/boardd/tests/test_boardd_loopback.py @@ -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 diff --git a/selfdrive/camerad/cameras/camera_common.cc b/selfdrive/camerad/cameras/camera_common.cc index 68ef891841..dc37b6ae6c 100644 --- a/selfdrive/camerad/cameras/camera_common.cc +++ b/selfdrive/camerad/cameras/camera_common.cc @@ -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); } diff --git a/selfdrive/camerad/cameras/camera_frame_stream.cc b/selfdrive/camerad/cameras/camera_frame_stream.cc index e982416c59..1998001472 100644 --- a/selfdrive/camerad/cameras/camera_frame_stream.cc +++ b/selfdrive/camerad/cameras/camera_frame_stream.cc @@ -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(); } diff --git a/selfdrive/camerad/cameras/camera_qcom.cc b/selfdrive/camerad/cameras/camera_qcom.cc index bb7cd21b25..dd80393895 100644 --- a/selfdrive/camerad/cameras/camera_qcom.cc +++ b/selfdrive/camerad/cameras/camera_qcom.cc @@ -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; diff --git a/selfdrive/camerad/cameras/camera_qcom2.cc b/selfdrive/camerad/cameras/camera_qcom2.cc index 38c39bd9a1..dde7256f1a 100644 --- a/selfdrive/camerad/cameras/camera_qcom2.cc +++ b/selfdrive/camerad/cameras/camera_qcom2.cc @@ -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); diff --git a/selfdrive/camerad/cameras/camera_webcam.cc b/selfdrive/camerad/cameras/camera_webcam.cc index 1543cc5d15..0befea22ad 100644 --- a/selfdrive/camerad/cameras/camera_webcam.cc +++ b/selfdrive/camerad/cameras/camera_webcam.cc @@ -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) { diff --git a/selfdrive/camerad/snapshot/snapshot.py b/selfdrive/camerad/snapshot/snapshot.py index 2e7542a36a..97adace070 100755 --- a/selfdrive/camerad/snapshot/snapshot.py +++ b/selfdrive/camerad/snapshot/snapshot.py @@ -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) diff --git a/selfdrive/camerad/test/check_skips.py b/selfdrive/camerad/test/check_skips.py index c6efb90ca0..0814ce44ff 100755 --- a/selfdrive/camerad/test/check_skips.py +++ b/selfdrive/camerad/test/check_skips.py @@ -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 diff --git a/selfdrive/camerad/test/frame_test.py b/selfdrive/camerad/test/frame_test.py index 36e6733654..39198e19da 100755 --- a/selfdrive/camerad/test/frame_test.py +++ b/selfdrive/camerad/test/frame_test.py @@ -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() diff --git a/selfdrive/camerad/test/test_camerad.py b/selfdrive/camerad/test/test_camerad.py index b405f58962..e2333b3839 100755 --- a/selfdrive/camerad/test/test_camerad.py +++ b/selfdrive/camerad/test/test_camerad.py @@ -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 diff --git a/selfdrive/car/chrysler/carstate.py b/selfdrive/car/chrysler/carstate.py index 5994a813c2..2d3c1a2ec7 100644 --- a/selfdrive/car/chrysler/carstate.py +++ b/selfdrive/car/chrysler/carstate.py @@ -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. diff --git a/selfdrive/car/ford/carcontroller.py b/selfdrive/car/ford/carcontroller.py index 648fa07741..41ce2fd8a0 100644 --- a/selfdrive/car/ford/carcontroller.py +++ b/selfdrive/car/ford/carcontroller.py @@ -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: diff --git a/selfdrive/car/ford/carstate.py b/selfdrive/car/ford/carstate.py index 7f34acc9df..2a776b45db 100644 --- a/selfdrive/car/ford/carstate.py +++ b/selfdrive/car/ford/carstate.py @@ -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 diff --git a/selfdrive/car/gm/carstate.py b/selfdrive/car/gm/carstate.py index 7f45e7ea9e..7ec7679f8c 100644 --- a/selfdrive/car/gm/carstate.py +++ b/selfdrive/car/gm/carstate.py @@ -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. diff --git a/selfdrive/car/honda/carstate.py b/selfdrive/car/honda/carstate.py index 16d85892a6..410d34a730 100644 --- a/selfdrive/car/honda/carstate.py +++ b/selfdrive/car/honda/carstate.py @@ -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'] diff --git a/selfdrive/car/honda/interface.py b/selfdrive/car/honda/interface.py index e2eb5439a8..112bfdbc29 100755 --- a/selfdrive/car/honda/interface.py +++ b/selfdrive/car/honda/interface.py @@ -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 diff --git a/selfdrive/car/hyundai/carcontroller.py b/selfdrive/car/hyundai/carcontroller.py index a9001ca8d5..e71ba6d6e8 100644 --- a/selfdrive/car/hyundai/carcontroller.py +++ b/selfdrive/car/hyundai/carcontroller.py @@ -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: diff --git a/selfdrive/car/hyundai/carstate.py b/selfdrive/car/hyundai/carstate.py index 951b615e07..b0803698bd 100644 --- a/selfdrive/car/hyundai/carstate.py +++ b/selfdrive/car/hyundai/carstate.py @@ -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']) diff --git a/selfdrive/car/hyundai/interface.py b/selfdrive/car/hyundai/interface.py index 5569aa6ff7..e8965a2e21 100644 --- a/selfdrive/car/hyundai/interface.py +++ b/selfdrive/car/hyundai/interface.py @@ -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 diff --git a/selfdrive/car/mazda/carstate.py b/selfdrive/car/mazda/carstate.py index bc6c6d0eb9..7b8293570d 100644 --- a/selfdrive/car/mazda/carstate.py +++ b/selfdrive/car/mazda/carstate.py @@ -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'] diff --git a/selfdrive/car/mock/interface.py b/selfdrive/car/mock/interface.py index e497ab96e4..8fe7c74d95 100755 --- a/selfdrive/car/mock/interface.py +++ b/selfdrive/car/mock/interface.py @@ -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() diff --git a/selfdrive/car/nissan/carcontroller.py b/selfdrive/car/nissan/carcontroller.py index 2804ec86cb..da69b86c07 100644 --- a/selfdrive/car/nissan/carcontroller.py +++ b/selfdrive/car/nissan/carcontroller.py @@ -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 diff --git a/selfdrive/car/nissan/carstate.py b/selfdrive/car/nissan/carstate.py index fcaa878a01..2b66bcd158 100644 --- a/selfdrive/car/nissan/carstate.py +++ b/selfdrive/car/nissan/carstate.py @@ -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"]) diff --git a/selfdrive/car/subaru/carstate.py b/selfdrive/car/subaru/carstate.py index 3be516ed33..1ad6bc75f7 100644 --- a/selfdrive/car/subaru/carstate.py +++ b/selfdrive/car/subaru/carstate.py @@ -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] diff --git a/selfdrive/car/toyota/carcontroller.py b/selfdrive/car/toyota/carcontroller.py index a3a04dcc9a..7152aa73ac 100644 --- a/selfdrive/car/toyota/carcontroller.py +++ b/selfdrive/car/toyota/carcontroller.py @@ -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): diff --git a/selfdrive/car/toyota/carstate.py b/selfdrive/car/toyota/carstate.py index ba660934a2..4c110bdff4 100644 --- a/selfdrive/car/toyota/carstate.py +++ b/selfdrive/car/toyota/carstate.py @@ -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)) diff --git a/selfdrive/car/volkswagen/carstate.py b/selfdrive/car/volkswagen/carstate.py index a994130929..73a83bb6c8 100644 --- a/selfdrive/car/volkswagen/carstate.py +++ b/selfdrive/car/volkswagen/carstate.py @@ -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 diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 5764265c8e..919a188684 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -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. diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 7cf010a2d1..49336c45dc 100644 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -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", diff --git a/selfdrive/controls/lib/latcontrol_indi.py b/selfdrive/controls/lib/latcontrol_indi.py index 277f0e0779..59deb53950 100644 --- a/selfdrive/controls/lib/latcontrol_indi.py +++ b/selfdrive/controls/lib/latcontrol_indi.py @@ -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) diff --git a/selfdrive/controls/lib/latcontrol_lqr.py b/selfdrive/controls/lib/latcontrol_lqr.py index cb54e52ebd..1d9356be48 100644 --- a/selfdrive/controls/lib/latcontrol_lqr.py +++ b/selfdrive/controls/lib/latcontrol_lqr.py @@ -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 diff --git a/selfdrive/controls/lib/latcontrol_pid.py b/selfdrive/controls/lib/latcontrol_pid.py index 173dcd58f7..be521b0cf6 100644 --- a/selfdrive/controls/lib/latcontrol_pid.py +++ b/selfdrive/controls/lib/latcontrol_pid.py @@ -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 diff --git a/selfdrive/controls/lib/lateral_planner.py b/selfdrive/controls/lib/lateral_planner.py index 9ca67fe3a5..160c43b460 100644 --- a/selfdrive/controls/lib/lateral_planner.py +++ b/selfdrive/controls/lib/lateral_planner.py @@ -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 diff --git a/selfdrive/controls/lib/longitudinal_planner.py b/selfdrive/controls/lib/longitudinal_planner.py index b812bc48a0..d7c7aff3a9 100755 --- a/selfdrive/controls/lib/longitudinal_planner.py +++ b/selfdrive/controls/lib/longitudinal_planner.py @@ -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 diff --git a/selfdrive/controls/tests/test_startup.py b/selfdrive/controls/tests/test_startup.py index 541b11cb2f..70d1e08c6a 100755 --- a/selfdrive/controls/tests/test_startup.py +++ b/selfdrive/controls/tests/test_startup.py @@ -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: diff --git a/selfdrive/debug/cycle_alerts.py b/selfdrive/debug/cycle_alerts.py index 4e3cd7663c..9999d8657c 100755 --- a/selfdrive/debug/cycle_alerts.py +++ b/selfdrive/debug/cycle_alerts.py @@ -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) diff --git a/selfdrive/debug/internal/check_alive_valid.py b/selfdrive/debug/internal/check_alive_valid.py index 51ad862745..6a64e5d270 100755 --- a/selfdrive/debug/internal/check_alive_valid.py +++ b/selfdrive/debug/internal/check_alive_valid.py @@ -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: diff --git a/selfdrive/debug/internal/measure_steering_accuracy.py b/selfdrive/debug/internal/measure_steering_accuracy.py index 706af73989..a9040a6d4a 100755 --- a/selfdrive/debug/internal/measure_steering_accuracy.py +++ b/selfdrive/debug/internal/measure_steering_accuracy.py @@ -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 diff --git a/selfdrive/debug/internal/sounds/test_sound_stability.py b/selfdrive/debug/internal/sounds/test_sound_stability.py index 2b86622081..ba6c59220b 100755 --- a/selfdrive/debug/internal/sounds/test_sound_stability.py +++ b/selfdrive/debug/internal/sounds/test_sound_stability.py @@ -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) diff --git a/selfdrive/debug/live_cpu_and_temp.py b/selfdrive/debug/live_cpu_and_temp.py index 6c55a40842..de854f070e 100755 --- a/selfdrive/debug/live_cpu_and_temp.py +++ b/selfdrive/debug/live_cpu_and_temp.py @@ -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'] diff --git a/selfdrive/debug/test_fw_query_on_routes.py b/selfdrive/debug/test_fw_query_on_routes.py index cf1db6348a..1c7f0077e4 100755 --- a/selfdrive/debug/test_fw_query_on_routes.py +++ b/selfdrive/debug/test_fw_query_on_routes.py @@ -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 diff --git a/selfdrive/debug/uiview.py b/selfdrive/debug/uiview.py index e31d6cf3c3..7b619805c6 100755 --- a/selfdrive/debug/uiview.py +++ b/selfdrive/debug/uiview.py @@ -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] diff --git a/selfdrive/hardware/eon/hardware.py b/selfdrive/hardware/eon/hardware.py index 96be2116a0..db0c590e66 100644 --- a/selfdrive/hardware/eon/hardware.py +++ b/selfdrive/hardware/eon/hardware.py @@ -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): diff --git a/selfdrive/hardware/pc/hardware.py b/selfdrive/hardware/pc/hardware.py index 2dc1d06254..f7b003f3b7 100644 --- a/selfdrive/hardware/pc/hardware.py +++ b/selfdrive/hardware/pc/hardware.py @@ -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): diff --git a/selfdrive/hardware/tici/hardware.py b/selfdrive/hardware/tici/hardware.py index e5f8e9eabc..e6dbef1f3b 100644 --- a/selfdrive/hardware/tici/hardware.py +++ b/selfdrive/hardware/tici/hardware.py @@ -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 diff --git a/selfdrive/locationd/locationd.py b/selfdrive/locationd/locationd.py index 896786a28e..d061452f57 100755 --- a/selfdrive/locationd/locationd.py +++ b/selfdrive/locationd/locationd.py @@ -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: diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index c0afe16974..263f5cc142 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -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)) diff --git a/selfdrive/locationd/test/ubloxd.py b/selfdrive/locationd/test/ubloxd.py index ad0d742a69..6e26c12e72 100755 --- a/selfdrive/locationd/test/ubloxd.py +++ b/selfdrive/locationd/test/ubloxd.py @@ -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'], } diff --git a/selfdrive/locationd/test/ubloxd_regression_test.py b/selfdrive/locationd/test/ubloxd_regression_test.py index 29210bf545..4f496ccd6f 100644 --- a/selfdrive/locationd/test/ubloxd_regression_test.py +++ b/selfdrive/locationd/test/ubloxd_regression_test.py @@ -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) diff --git a/selfdrive/locationd/ublox_msg.cc b/selfdrive/locationd/ublox_msg.cc index 61b0f7dfb2..154200c088 100644 --- a/selfdrive/locationd/ublox_msg.cc +++ b/selfdrive/locationd/ublox_msg.cc @@ -192,7 +192,7 @@ kj::Array 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 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); } diff --git a/selfdrive/loggerd/loggerd.cc b/selfdrive/loggerd/loggerd.cc index d4d1704789..200e34e06a 100644 --- a/selfdrive/loggerd/loggerd.cc +++ b/selfdrive/loggerd/loggerd.cc @@ -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(); 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(); } diff --git a/selfdrive/loggerd/uploader.py b/selfdrive/loggerd/uploader.py index 1537676120..8fdb2a77a8 100644 --- a/selfdrive/loggerd/uploader.py +++ b/selfdrive/loggerd/uploader.py @@ -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" diff --git a/selfdrive/manager.py b/selfdrive/manager.py index 3fb37317e1..e1db3c04f6 100755 --- a/selfdrive/manager.py +++ b/selfdrive/manager.py @@ -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] diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 356812156d..c43e9588b3 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -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; diff --git a/selfdrive/sensord/sensors_qcom.cc b/selfdrive/sensord/sensors_qcom.cc index a4ebf8bff3..77e670d3f4 100644 --- a/selfdrive/sensord/sensors_qcom.cc +++ b/selfdrive/sensord/sensors_qcom.cc @@ -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); diff --git a/selfdrive/test/longitudinal_maneuvers/plant.py b/selfdrive/test/longitudinal_maneuvers/plant.py index 67f4cc46dd..01c90c27f7 100755 --- a/selfdrive/test/longitudinal_maneuvers/plant.py +++ b/selfdrive/test/longitudinal_maneuvers/plant.py @@ -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 diff --git a/selfdrive/test/process_replay/camera_replay.py b/selfdrive/test/process_replay/camera_replay.py index c906bdf3e7..4871ae3022 100755 --- a/selfdrive/test/process_replay/camera_replay.py +++ b/selfdrive/test/process_replay/camera_replay.py @@ -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) diff --git a/selfdrive/test/process_replay/inject_model.py b/selfdrive/test/process_replay/inject_model.py index 1ebc0f8e50..b5ccf264e4 100755 --- a/selfdrive/test/process_replay/inject_model.py +++ b/selfdrive/test/process_replay/inject_model.py @@ -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) diff --git a/selfdrive/test/process_replay/process_replay.py b/selfdrive/test/process_replay/process_replay.py index 5962665c92..c100137d51 100755 --- a/selfdrive/test/process_replay/process_replay.py +++ b/selfdrive/test/process_replay/process_replay.py @@ -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, diff --git a/selfdrive/test/test_car_models.py b/selfdrive/test/test_car_models.py index 37b3726f28..185703822f 100755 --- a/selfdrive/test/test_car_models.py +++ b/selfdrive/test/test_car_models.py @@ -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") diff --git a/selfdrive/test/test_models.py b/selfdrive/test/test_models.py index ad101e2078..39c04c1d67 100755 --- a/selfdrive/test/test_models.py +++ b/selfdrive/test/test_models.py @@ -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']} diff --git a/selfdrive/test/test_sounds.py b/selfdrive/test/test_sounds.py index d9c150e029..574eb32adf 100755 --- a/selfdrive/test/test_sounds.py +++ b/selfdrive/test/test_sounds.py @@ -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]}") diff --git a/selfdrive/thermald/power_monitoring.py b/selfdrive/thermald/power_monitoring.py index a291b15145..d34659eaea 100644 --- a/selfdrive/thermald/power_monitoring.py +++ b/selfdrive/thermald/power_monitoring.py @@ -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 diff --git a/selfdrive/thermald/tests/test_power_monitoring.py b/selfdrive/thermald/tests/test_power_monitoring.py index be2e4a09e4..78b31c5659 100755 --- a/selfdrive/thermald/tests/test_power_monitoring.py +++ b/selfdrive/thermald/tests/test_power_monitoring.py @@ -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() diff --git a/selfdrive/thermald/thermald.py b/selfdrive/thermald/thermald.py index 2edbf5d4be..92cac2afdf 100755 --- a/selfdrive/thermald/thermald.py +++ b/selfdrive/thermald/thermald.py @@ -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 diff --git a/selfdrive/ui/sidebar.cc b/selfdrive/ui/sidebar.cc index df609d2bbc..0d05dade05 100644 --- a/selfdrive/ui/sidebar.cc +++ b/selfdrive/ui/sidebar.cc @@ -26,36 +26,36 @@ static void draw_home_button(UIState *s) { } static void draw_network_strength(UIState *s) { - static std::map 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 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 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 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 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 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"; } diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index a11aed8585..b60146dff0 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -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 diff --git a/selfdrive/ui/ui.hpp b/selfdrive/ui/ui.hpp index c0570f6d43..f184ffc5d2 100644 --- a/selfdrive/ui/ui.hpp +++ b/selfdrive/ui/ui.hpp @@ -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; diff --git a/tools/carcontrols/debug_controls.py b/tools/carcontrols/debug_controls.py index 595147048f..9264b2d768 100755 --- a/tools/carcontrols/debug_controls.py +++ b/tools/carcontrols/debug_controls.py @@ -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 diff --git a/tools/nui/FileReader.cpp b/tools/nui/FileReader.cpp index 13b7f78078..84176e70d6 100644 --- a/tools/nui/FileReader.cpp +++ b/tools/nui/FileReader.cpp @@ -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())); } diff --git a/tools/nui/Unlogger.cpp b/tools/nui/Unlogger.cpp index ee8327eda2..6455750d77 100644 --- a/tools/nui/Unlogger.cpp +++ b/tools/nui/Unlogger.cpp @@ -153,14 +153,14 @@ void Unlogger::process() { auto ee = msg.getRoot(); ee.setLogMonoTime(nanos_since_boot()); - if (e.which() == cereal::Event::FRAME) { - auto fr = msg.getRoot().getFrame(); + if (e.which() == cereal::Event::ROAD_CAMERA_STATE) { + auto fr = msg.getRoot().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]; diff --git a/tools/replay/camera.py b/tools/replay/camera.py index 8a552785eb..9c1e9fb4bd 100755 --- a/tools/replay/camera.py +++ b/tools/replay/camera.py @@ -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], diff --git a/tools/replay/ui.py b/tools/replay/ui.py index d910dd5732..be58412948 100755 --- a/tools/replay/ui.py +++ b/tools/replay/ui.py @@ -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) diff --git a/tools/replay/unlog_segment.py b/tools/replay/unlog_segment.py index 94520b3baa..ac9f0686ff 100755 --- a/tools/replay/unlog_segment.py +++ b/tools/replay/unlog_segment.py @@ -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 diff --git a/tools/replay/unlogger.py b/tools/replay/unlogger.py index df137acdfd..8941283c62 100755 --- a/tools/replay/unlogger.py +++ b/tools/replay/unlogger.py @@ -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 diff --git a/tools/sim/bridge.py b/tools/sim/bridge.py index 242e87f1a1..6ec74bc23c 100755 --- a/tools/sim/bridge.py +++ b/tools/sim/bridge.py @@ -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() diff --git a/tools/streamer/streamerd.py b/tools/streamer/streamerd.py index 7bfaeb38d5..b26e0eafc9 100755 --- a/tools/streamer/streamerd.py +++ b/tools/streamer/streamerd.py @@ -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: